基于落足点调整的四足机器人对角步态柔顺控制方法

1.本发明涉及电驱动四足机器人，尤其是涉及一种基于落足点调整的四足机器人对角步态柔顺控制方法。


背景技术：

2.机器人技术是一门涉及机械制造、电子学、计算机科学、人工智能等学科的交叉学科。机器人可以帮助人们进行一些危险、机械重复等方面的工作。研究机器人技术具有很高的应用价值和社会价值。
3.相比于轮式、履带式机器人，足式机器人可在不平坦、非结构化的环境灵活行走，其具有更好的地形适应能力和灵活运动性能。基于灵活的运动能力，足式机器人可以参与复杂工况的工作任务如野外未知环境负载仪器勘探、灾后(如地震、火灾等)危险环境救援物质运输、长时间的野外行军运输、核电站作业(如核泄漏)等完成特殊工况的应用。
4.足式机器人按照足的数量划分，一般有双足、四足、六足等足式机器人。相较于双足机器人，四足机器人具有更灵活的运动能力；相较于六足机器人，四足机器人的机械结构、控制算法等更简单。综上，四足机器人在较简单的机械结构、控制算法的基础上能够实现灵活的运动能力。
5.四足机器人的步态指的是有时序地进行足端轨迹规划，使得四足机器人能够有节律地运动。四足机器人的运动分为支撑相和摆动相，按照每条腿之间的相位差，步态大致可以爬行(crawl)步态、对侧(pace)步态、对角小跑(trot)步态、跳跃(bound)步态、疾驰(gallop)步态等。
6.虚拟模型控制就是假设在质心、足端等位置有弹簧阻尼模型与外部作用点连接，进而产生对四足机器人的虚拟力。虚拟力通过一定的映射关系转化为关节力矩，进而实现对四足机器人的控制。
7.柔顺控制分为被动柔顺控制和主动柔顺控制。被动柔顺控制就是在四足机器人的机体(如足端)配备上用于减震缓冲的弹簧等，进而减小外部冲击实现柔顺性能。主动柔顺控制就是通过控制算法实现柔顺性能。相比于被动柔顺控制，主动柔顺控制具有机械结构简单、控制灵活性高等优点。
8.目前，基于落足点调整的电驱动四足机器人在足端轨迹规划方面主要采取静态的规划方法，不能很好地适应地形；在对角小跑步态规划方面，目前关于起步姿态与行走过渡的研究有所欠缺，不利于四足机器人起步时的稳定性；在对角小跑步态状态转换方面，只有两对虚拟腿之间的状态转换，但实际可能发生一对虚拟腿一条腿先着地、一条腿后着地的情况；在产生及转化虚拟力、虚拟力矩方面，一般的方法是在质心处产生六自由度的虚拟力和虚拟力矩，再通过空间映射矩阵和雅可比矩阵转化到各个关节，计算复杂度高、算法复杂。


技术实现要素：

9.本发明的目的在于提供一种包括刚体建模、姿态获取、足端轨迹规划、对角小跑步态规划、状态机设计、阻抗控制器和改进型虚拟模型控制器设计的基于落足点调整的四足机器人对角步态柔顺控制方法，以实现电驱动四足机器人基于落足点调整的对角小跑步态更加稳定且柔顺地行走。
10.本发明包括以下步骤：
11.1)建立四足机器人简化的刚体模型；
12.2)根据简化后的刚体模型，结合电机编码器、足底激光距离传感器、关节力传感器、imu等传感器数据计算得到关节角、触地信号、髋部运动状态、机体质心姿态；
13.3)进行落足点调整、抬腿高度自适应的足端轨迹规划；
14.4)设计起步姿态与行走过渡，进行对角小跑步态规划；
15.5)设计对角小跑步态切换的状态机；
16.6)建立四足机器人运动的阻抗控制器和虚拟模型控制器。
17.在步骤1)中，所述建立四足机器人简化的刚体模型，是在空间中建立世界坐标系、机体坐标系、关节坐标系；为了建模和控制的简单，忽略四条腿的质量和转动惯量，建立四足机器人简化的刚体模型，得到模型的运动学方程、腿部雅可比矩阵。
18.在步骤3)中，所述进行落足点调整、抬腿高度自适应的足端轨迹规划的具体步骤如下：
19.步骤3.1，给定摆动相x、y、z三个方向的足端轨迹方程；
20.步骤3.2，根据地形情况，适当地调整足端的抬腿高度。
21.在步骤4)中，所述设计起步姿态与行走过渡，进行对角小跑步态规划的具体步骤如下：
22.在踏步的基础上先以半步长运动半个步态周期，然后切换成全步长进行对角小跑运动，这样一方面可以保证机身速度的平稳过渡，不会造成机身速度的突变，另一方面也可以保证足端相对于髋关节的位置保持对称状态，有利于运动的稳定性。
23.在步骤5)中，所述设计对角小跑步态切换的状态机的具体步骤如下：
24.步骤5.1，设计对角小跑步态切换的状态机集合。
25.步骤5.2，结合每条腿的触地信息，进行对角小跑状态切换；考虑到实际更多触地情况的状态机切换方法，包括摆动腿着地的先后问题，用于调整机身姿态、提高机身稳定性的四足支撑相。
26.在步骤6)中，所述建立四足机器人运动的阻抗控制器和虚拟模型控制器的具体实现过程如下：
27.步骤6.1，将每条腿的状态区分为支撑相和摆动相；
28.步骤6.2，对于处于摆动相的腿，设计阻抗控制器；
29.步骤6.3，对于处于支撑相的腿，在其髋部假设三个平动方向的虚拟弹簧阻尼模型计算虚拟力，在质心处假设三个转动自由度的虚拟弹簧阻尼模型计算虚拟力矩进而转化为支撑腿的虚拟力和虚拟力矩；将髋部的虚拟力通过腿部的雅可比矩阵转化为三个关节力矩。
30.与现有技术相比，本发明具有以下突出的优点和技术效果：
31.在足端轨迹规划方面，相较于之前静态的规划方法，提出一种落足点调整、抬腿高度、支撑相时间自适应的足端轨迹规划方法，可以比较好适应地形，提高运动稳定性。
32.在对角小跑步态规划方面，目前关于起步姿态与行走过渡的研究有所欠缺，提出了一种起步方式：在踏步的基础上先以半步长运动半个步态周期，然后切换成全步长进行对角小跑运动，这样一方面可以保证机身速度的平稳过渡，不会造成机身速度的突变，另一方面也可以保证足端相对于髋关节的位置保持对称状态，有利于运动的稳定性。
33.在对角小跑步态状态切换方面，相较于之前只有两对虚拟腿之间的状态转换，考虑到由于地形等原因实际可能发生一对虚拟腿一条腿先着地、一条腿后着地等更复杂的情况，提出了根据足底触地信号的状态机切换方法，考虑到更多状态的可能性。
34.此外，给定足够的四足支撑相时间可以用于调整机身姿态、提高机身稳定性。在产生及转化虚拟力、虚拟力矩方面，相较于之前一般在质心处产生六自由度的虚拟力和虚拟力矩，再通过空间映射矩阵和雅可比矩阵转化到各个关节的方法，设计一种摆动相的阻抗控制器和支撑相的虚拟模型控制器，计算复杂度更低、算法简单，可以柔顺且稳定地实现四足机器人的运动。
附图说明
35.图1为本发明实施例的实施流程示意图。
36.图2为足式机器人整体结构示意图。
37.图3为状态机切换图。
38.图4为阻抗控制框架示意图。
具体实施方式
39.为了使本发明的目的、技术方案及优点更加清楚明白，以下实施例将结合附图对本发明进行作进一步的说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本发明有更好的了解，以下对本发明的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本发明。
40.实施例1
41.本实施例的实施步骤如图1所示：
42.步骤一，建立四足机器人简化的刚体模型；
43.步骤二，根据简化后的刚体模型，结合电机编码器、足底激光距离传感器、关节力传感器、imu等传感器数据计算得到关节角、触地信号、髋部运动状态、机体质心姿态；
44.步骤三，进行落足点调整、抬腿高度自适应的足端轨迹规划；
45.步骤四，设计起步姿态与行走过渡，进行对角小跑步态规划；
46.步骤五，设计对角小跑步态切换的状态机；
47.步骤六，建立四足机器人的阻抗控制器和虚拟模型控制器。
48.下面对上述的各步骤做详细的描述。
49.步骤一，建立四足机器人简化的刚体模型，见图2。
50.具体地，在空间中建立世界坐标系、机体坐标系、关节坐标系。为了建模和控制的简单，由于腿部质量相对于机身质量可以大致忽略不计，故而忽略四条腿的质量和转动惯量。建立四足机器人简化的刚体模型，得到模型的运动学方程、腿部雅可比矩阵等。
51.步骤二，根据简化后的刚体模型，结合电机编码器、足底激光距离传感器、关节力传感器、imu等传感器数据计算得到关节角、触地信号、髋部运动状态、机体质心姿态。
52.具体地，电机编码器测量12个关节的关节角θi和关节角速度其中i∈{1，2，...，11，12}，顺序为左前腿、右前腿、右后腿、左后腿，每条腿内部顺序为髋关节、大腿关节、小腿关节，例如右前腿的髋关节关节角为θ4；足底激光距离传感器测量每条腿的触地状态，设定一个阈值δd
foot
，将测量值d
foot，i
与阈值比较，若d
foot，i
≥δd
foot
，则表示足底未触地，若d
foot，i
＜δd
foot
，则表示足底触地，其中i∈{1，2，3，4}，顺序为左前腿、右前腿、右后腿、左后腿；关节力传感器测量每条腿关节处的力矩τ
joint，ij
，其中i∈{1，2，3，4}，j∈{1，2，3}，如τ
joint，23
表示右前腿的小腿关节力矩；imu测量机体质心的实际姿态角速度和xyz三个方向的实际加速度通过电机编码器和imu的数据，结合腿部正运动学等计算得出髋部运动状态(x
coxa
，y
coxa
，z
coxa
)和以及机体质心姿态(θ
pitch
，θ
roll
，θ
yaw
)。
53.步骤三，进行落足点调整、抬腿高度自适应的足端轨迹规划。具体包括：
54.步骤3.1，给定摆动相x、y、z三个方向的足端轨迹方程。
55.具体地：首先，根据步骤二中获得的机体速度反馈确定x、y方向的落足点位置可求得步长s
x
＝x
f-x0，sy＝y
f-y0其中x0，y0为上一步态周期支撑相结束时的位置。其次，通过插值确定足端轨迹方程，插值方程可以为摆线函数、三角函数多项式、高阶多项式等。基于x、y方向零冲击的特性(摆动腿触地时足端相对于髋关节的速度、加速度为0)，插值多项式选取五次多项式x＝a
x
t5 b
x
t4 c
x
t3 d
x
t2 e
x
t f
x
，y＝ayt5 byt4 cyt3 dyt2 eyt fy。结合位置、速度、加速度约束可以求得插值方程相关参数。最后，足端轨迹算法在每一个周期不断进行迭代，基于落足点的在线调整实现对机体速度的实时控制。注意，需给定合适的运动周期t、支撑相时间t
st
等足端轨迹参数，其中规划的支撑相时间t
st
不是一成不变的，而是等于上一周期反馈的实际的支撑相时间。
56.步骤3.2，根据地形情况，适当地调整足端的抬腿高度δh。
57.具体地，首先确定抬腿高度δh0；其次根据反馈的机身姿态进行高度的直觉调节；最后限制抬腿高度区间为[δh
max
,δh
min
]。
[0058]
步骤四，设计起步姿态与行走过渡，进行对角小跑步态规划。具体包括：
[0059]
步骤4.1，设计起步姿态与行走过渡。
[0060]
首先，以规划的足端高度δh0原地踏步；其次，在踏步的基础上先以半步长运动半个步态周期最后，切换成全步长s进行对角小跑运动。这样一方面可以保证机身速度的平稳过渡，不会造成机身速度的突变，另一方面又可以保证足端相对于髋关节的位置保
持对称状态，有利于运动的稳定性。
[0061]
步骤4.2，进行对角小跑步态规划。
[0062]
左前腿、右后腿为一对虚拟腿，右前腿、左后腿为一对虚拟腿。两对虚拟腿的相位差φ规划为0.5，占空比(支撑相站整个步态周期的百分比)规划为55％，以保证四足机器人在运动过程中具有四足支撑相的状态，增强运动的稳定性。每一条腿的初始摆动位置可以根据四足机器人具体的姿态进行微调，以期达到更稳定的状态。
[0063]
步骤五，设计对角小跑步态切换的状态机，见图3。具体包括：
[0064]
步骤5.1，设计对角小跑步态切换的状态机集合。
[0065]
具体地，集合包括九种状态，分别为状态1——四足同时着地；状态2——左前腿、右后腿着地的同时右前腿、左后腿在空中摆动；状态3——右前腿、左后腿着地的同时左前腿、右后腿在空中摆动；状态4——状态2的时候，右前腿、左后腿同时触地；状态4.1——状态2的时候，右前腿先于左后腿触地；状态4.2——状态2的时候，左后腿先于右前腿触地；状态5——状态3的时候，左前腿、右后腿同时触地；状态5.1——状态3的时候，左前腿先于右后腿触地；状态5.2——状态3的时候，右后腿先于左前腿触地。
[0066]
步骤5.2，结合每条腿的触地信息，进行对角小跑状态切换。
[0067]
具体地，首先初始状态为状态1——四足同时着地。一定时间待机体稳定后，即机身质心水平速度小于阈值后开始摆腿，先摆动右前腿、左后腿这对虚拟腿，状态切换到状态2。接下来如果右前腿、左后腿同时触地，状态切换到状态4；如果右前腿先于左后腿触地，状态切换到状态4.1，此时右前腿停止摆动，等待左后腿触地，然后状态切换到状态4；如果左后腿先于右前腿触地，状态切换到状态4.2，此时左后腿停止摆动，等待右前腿触地，然后状态切换到状态4。当状态切换到状态4，四足机器人此时处于四足支撑相，令四足支撑相时间t
st,4
等于整个运动周期t(包括支撑相周期t
st
和摆动相周期t
sw
)的10％。接着状态切换到状态3，摆动左前腿、右后腿这对虚拟腿。接下来如果左前腿、右后腿同时触地，状态切换到状态5；如果左前腿先于右后腿触地，状态切换到状态5.1，此时左前腿停止摆动，等待右后腿触地，然后状态切换到状态5；如果右后腿先于左前腿触地，状态切换到状态5.2，此时右后腿停止摆动，等待左前腿触地，然后状态切换到状态5。当状态切换到状态5，四足机器人此时处于四足支撑相，令四足支撑相时间t
st,4
等于整个运动周期t的10％。接着状态切换到状态2，摆动右前腿、左后腿这对虚拟腿。至此，状态机形成了一个循环，接下来便按照状态机继续循环往复，实现对角小跑步态行走。
[0068]
步骤六，建立四足机器人的阻抗控制器和虚拟模型控制器。具体包括：
[0069]
步骤6.1，将每条腿的状态区分为支撑相和摆动相。
[0070]
具体地，根据足底激光距离传感器判断该腿处于支撑相还是摆动相。
[0071]
步骤6.2，对于处于摆动相的腿，设计阻抗控制器，见图4。
[0072]
具体地，设计外环为阻抗控制器，内环为力控制器。外环的阻抗控制器本质上为低增益的pd控制器，将足端轨迹的期望值xd、和实际值的误差x、作为输入，将足端虚拟力fd作为输出，再结合单腿动力学模型产生虚拟力矩τd作为内环力控制器的输入。这种内外环的基于力的阻抗控制器，可以动态地调节力与位置的关系，柔顺地跟随足端轨迹，减小足端触地时的冲击力，增强机体运动时的稳定性。
[0073]
特别地，外环的pd控制参数可以结合关节力传感器的反馈值τ
joint,ij
进行模糊规则
的调节。通常来说，摆动腿的pd参数会小于支撑腿的pd参数，达到更加柔顺的效果，从而增强机体的稳定性。
[0074]
步骤6.3，对于处于支撑相的腿，在其髋部假设三个平动方向的虚拟弹簧阻尼模型计算虚拟力，在质心处假设三个转动自由度的虚拟弹簧阻尼模型计算虚拟力矩，进而转化为支撑腿的虚拟力和虚拟力矩。
[0075]
具体地，对于处于支撑相的腿，将xyz三个平动方向运动所需要的力等效在髋部，根据期望状态与实际状态计算可得
[0076][0077]
其中，f为三维力，pd、p、分别为期望三维位置、实际三维位置、期望三维速度、实际三维速度，以为例，k
st
、c
st
为pd控制参数。
[0078]
在质心处根据虚拟弹簧阻尼模型结合姿态角计算虚拟力矩。其中，俯仰角θ
pitch
通过调节前后足髋部的高度进行间接控制，不直接进行控制。滚转力矩τ
roll
和偏航力矩τ
yaw
计算过程如下：
[0079][0080][0081]
其中，θ
rolld
、θ
roll
、分别为期望滚转角、实际滚转角、期望滚转角速度、实际滚转角速度；θ
yawd
、θ
yaw
、分别为期望偏航角、实际偏航角、期望偏航角速度、实际偏航角速度，k
roll
、c
roll
、k
yaw
、c
yaw
为pd控制参数。
[0082]
然后，将质心处得到的虚拟偏航力矩τ
yaw
分解到髋部的前进方向，即x方向：
[0083]
δf
x
＝τ
yaw
/w
[0084]
其中，w为机身的宽度。
[0085]
作用在髋部的总虚拟力：
[0086]fsum
＝f [δf
x
；0；0]
[0087]
将髋部的虚拟力通过腿部的雅可比矩阵转化为三个关节力矩。
[0088]
τ＝-j
′fsum
[0089]
其中，j为单腿的雅可比矩阵。
[0090]
最后，将虚拟滚转力矩τ
roll
分解到髋关节处：
[0091][0092]
由于实际关节电机有输出力矩限制，注意给定力矩阈值τ
sum,max
。
[0093]
本发明的技术关键点如下：
[0094]
1、基于落足点调整的四足机器人对角步态柔顺控制方法，包括：建立四足机器人简化的刚体模型；根据简化后的刚体模型，结合电机编码器、足底激光距离传感器、关节力传感器、imu等传感器数据计算得到关节角、触地信号、髋部运动状态、机体质心姿态；进行落足点调整、抬腿高度自适应的足端轨迹规划；设计起步姿态与行走过渡，进行对角小跑步
态规划；设计对角小跑步态切换的状态机；建立四足机器人的阻抗控制器和虚拟模型控制器。
[0095]
2、建立四足机器人简化的刚体模型。在空间中建立世界坐标系、机体坐标系、关节坐标系。为了建模和控制的简单，由于腿部质量相对于机身质量可以大致忽略不计，故而忽略四条腿的质量和转动惯量。建立四足机器人简化的刚体模型，得到模型的运动学方程、腿部雅可比矩阵等。
[0096]
3、根据简化后的刚体模型，结合电机编码器、足底激光距离传感器、关节力传感器、imu等传感器数据计算得到关节角、触地信号、髋部运动状态、机体质心姿态。
[0097]
4、落足点调整、抬腿高度、支撑相时间自适应的足端轨迹规划方法。给定摆动相x、y、z三个方向的足端轨迹方程；根据地形情况，适当地调整足端的抬腿高度。可以比较好适应地形，提高运动稳定性。
[0098]
5、设计起步姿态与行走过渡，进行对角小跑步态规划。在踏步的基础上先以半步长运动半个步态周期，然后切换成全步长进行对角小跑运动，这样一方面可以保证机身速度的平稳过渡，不会造成机身速度的突变，另一方面也可以保证足端相对于髋关节的位置保持对称状态，有利于运动的稳定性。
[0099]
6、设计对角小跑步态切换的状态机。设计对角小跑步态切换的状态机集合；结合每条腿的触地信息，进行对角小跑状态切换。考虑到实际更多触地情况的状态机切换方法，包括摆动腿着地的先后问题，用于调整机身姿态、提高机身稳定性的四足支撑相等。
[0100]
7、摆动相的阻抗控制器与支撑相的虚拟模型控制器。将每条腿的状态区分为支撑相和摆动相；对于处于摆动相的腿，设计阻抗控制器；对于处于支撑相的腿，在其髋部假设三个平动方向的虚拟弹簧阻尼模型计算虚拟力，在质心处假设三个转动自由度的虚拟弹簧阻尼模型计算虚拟力矩进而转化为支撑腿的虚拟力和虚拟力矩；将髋部的虚拟力通过腿部的雅可比矩阵转化为三个关节力矩。
[0101]
上述实施例仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明申请范围所作的均等变化与改进等，均应仍归属于本发明的专利涵盖范围之内。