一种动态行走的四足探测机器人及控制方法与流程

1.本发明属于深空探测技术领域，涉及一种动态行走的四足探测机器人及控制方法。


背景技术：

2.我国深空探测、载人登月任务的最终目标是在月球和火星表面建立永久性基地，而地外天体基地建设是一项前所未有的重大工程，实施前需要充分的准备工作，其中最关键的一环就是研制能够适应月球火星表面环境的空间机器人，为建立基地以至为载人登陆开道铺路。
3.在复杂的外星球环境，足式移动机器人与生俱来的“悬架结构”将机体与地形环境相分离，仅需依靠离散的落脚点便能够稳定、连续地移动，使得其在非规则地形下表现出明显的通过性优势，被认为是科学探测、紧急搜救、物资运输、侦察巡逻等作业的最佳移动平台。四足机器人兼具运动的灵活性和稳定性，可以实现动态行走，是高速移动足式机器人研究的主要对象。
4.四足机器人的动步态行走过程存在少于三条腿支撑的过程，即不能静态稳定，因此保证其行走过程的稳定性就是最重要的目标。现有方法步态的规划大多是基于时间规划各部分运动轨迹，但是这种方式在受到外部干扰时很难保持预定的轨迹或者恢复稳定；此外许多现有方法在行走控制时不基于系统动力学模型，使实际轨迹与规划轨迹间存在较大而进一步影响稳定性；在超过2条腿支撑时各腿形成闭链，规划和控制的误差将导致腿间形成较大内力，从而影响行走控制甚至造成器件损坏。


技术实现要素：

5.本发明解决的技术问题是：克服现有技术的不足，提出一种动态行走的四足探测机器人及控制方法，显著提高四足机器人对角小跑动态行走的稳定性，并具备良好的速度调节、斜坡自适应、干扰自适应的能力。
6.本发明解决技术的方案是：
7.一种动态行走的四足探测机器人，包括机身、第一移动腿、第二移动腿、第三移动腿和第四移动腿；其中，机身为锥台状结构；第一移动腿、第二移动腿、第三移动腿和第四移动腿均匀固定安装在机身的周向侧壁处；第一移动腿、第二移动腿、第三移动腿和第四移动腿为结构完全相同的移动腿。
8.在上述的一种动态行走的四足探测机器人，在机身的上表面建立机身本体坐标系oxyz；其中，o点位于机身的上表面的中心；x轴正方向指向四足探测机器人的前进方向；z轴正方向垂直于机身竖直向上；y轴由右手定则确定；
9.第一移动腿和第二移动腿设置在x轴的一侧；第三移动腿和第四移动腿设置在x轴的另一侧；第一移动腿、第二移动腿、第三移动腿和第四移动腿沿顺时针依次设置；且第一移动腿与第四移动腿相对于x轴对称；第二移动腿与第三移动腿相对于x轴对称。
10.在上述的一种动态行走的四足探测机器人，每个移动腿的腿根处与机身的侧壁连接形成腿根关节；腿根关节相对于机身具有俯仰旋转自由度和滚转旋转自由度；每个移动腿具有1个膝关节，膝关节具有俯仰旋转自由度。
11.一种动态行走的四足探测机器人的控制方法，包括如下步骤：
12.步骤一、测量第i个移动腿的各关节角速度qi＝[q
i_1 q
i_2 q
i_3
]和角速度测量机身本体坐标系oxyz中z轴的姿态欧拉角γ、y轴的姿态欧拉角β和x轴的姿态欧拉角α；其中，i为移动腿的序号；
[0013]
步骤二、根据正运动学算法获得第i个移动腿的足端在机身本体坐标系oxyz中的位置xi＝[x
i y
i zi]和速度建立惯性坐标系o1x1y1z1，计算机身本体坐标系oxyz相对于惯性坐标系o1x1y1z1的旋转矩阵ic0；
[0014]
步骤三、定义四足探测机器人的运动姿态：初始时，以第二移动腿、第四移动腿为支撑腿；第一移动腿和第三移动腿为摆动腿，同时向x轴正方向运动，其后交替进行；定义前支撑腿为f_sp，后支撑腿为b_sp，前摆动腿为f_sw，后摆动腿为b_sw；
[0015]
步骤四、计算四足探测机器人的x方向的行走速度和y方向的行走速度
[0016]
步骤五、规划前摆动腿的落点位置和后摆动腿的落点位置
[0017]
步骤六、规划摆动腿的足端轨迹；
[0018]
步骤七、规划支撑腿的足端轨迹；
[0019]
步骤八、规划机身的加速度f
*
；计算4个移动腿的前馈力
[0020]
步骤九、采用带动力学前馈的阻抗控制对足端进行控制，获得四足探测机器人控制方程：
[0021][0022]
式中，τ
id
为第i条移动腿的期望关节力矩向量；
[0023]ji
为第i条移动腿的雅克比矩阵；
[0024]fid
为第i条移动腿的前馈力；
[0025]
x
di
为第i条移动腿期望的足端轨迹；
[0026]
当第i条移动腿为前摆动腿时，将x
f_sw
代入x
di
；
[0027]
当第i条移动腿为后摆动腿时，将x
b_sw
代入x
di
；
[0028]
当第i条移动腿为前支撑腿时，将x
f_sp
代入x
di
；
[0029]
当第i条移动腿为后支撑腿时，将x
b_sp
代入x
di
；
[0030]
k为正定的刚度矩阵；
[0031]
b为正定的阻尼矩阵；
[0032]
xi为第i个移动腿的足端在机身本体坐标系oxyz中的位置；
[0033]
根据控制方程实现对四足探测机器人的控制。
[0034]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤二中，惯性坐标系o1x1y1z1的建立方法为：
[0035]
在初始未移动的四足探测机器人的机身上建立惯性坐标系o1x1y1z1；其中，原点o1位于机身上表面的中心；z1轴正向竖直向上；x1轴和y1轴可在过o1点切垂直于z1的平面上任意选取，满足右手定则即可；
[0036]
旋转矩阵ic0为：
[0037][0038]
式中，α为机身本体坐标系oxyz中x轴的姿态欧拉角；
[0039]
β为机身本体坐标系oxyz中y轴的姿态欧拉角。
[0040]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤四中，x方向的行走速度的计算方法为：
[0041][0042]
式中，为前支撑腿相对于机身坐标系x方向的速度；
[0043]
为后支撑腿相对于机身坐标系x方向的速度；
[0044]
y方向的行走速度的计算方法为：
[0045][0046]
式中，为前支撑腿相对于机身坐标系y方向的速度；
[0047]
为后支撑腿相对于机身坐标系y方向的速度。
[0048]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤五中，前摆动腿的落点位置为：
[0049][0050]
后摆动腿的落点位置为：
[0051][0052]
式中，x
t,f_sw
为预先设计的前摆动腿的期望落足点；
[0053]
x
t,b_sw
为预先设计的后摆动腿的期望落足点；
[0054]
δd为落足点前进方向调整量；
[0055]
δs为落足点侧向调整量。
[0056]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤六中，规划摆动腿的足端轨迹的具体方法为：
[0057]
s61、计算四足探测机器人的行走进程参考变量θ：
[0058][0059]
式中，x
f_sp
为前支撑腿在机身坐标系下x方向的坐标；
[0060]
x
f_sw
为后支撑腿在机身坐标系下x方向的坐标；
[0061]
s62、对行走进程参考变量θ进行归一化处理，获得归一化的进度参考量s：
[0062][0063]
式中，s为归一化的进度参考量，s∈[0,1]；
[0064]
θ

为一步中θ的最大值；
[0065]
θ-为一步中θ的最小值；
[0066]
s63、对前摆动腿足端轨迹x
f_sw
(θ)和后摆动腿足端轨迹x
b_sw
(θ)采用5阶b
é
zier曲线进行规划：
[0067][0068][0069]
式中，p
f_sw,j
为前摆动腿在b
é
zier曲线的第j个参数；
[0070]
p
b_sw,j
为后摆动腿在b
é
zier曲线的第j个参数；
[0071]
为排列组合公式；
[0072]
完成摆动腿的足端轨迹规划。
[0073]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤七中，支撑腿的足端轨迹的规划方法为：
[0074]
s71、采用5阶b
é
zier曲线来规划机身相对于支撑轨足端的轨迹x
body
(θ)：
[0075][0076]
式中，p
body,j
为机身在b
é
zier曲线的第j个参数；
[0077]
s72、根据上式解析出速度
[0078][0079]
s73、机身的姿态采用速度级规划，规划姿态角速度ω
body
与姿态偏差成比例，即：
[0080][0081]
式中，αc为期望滚转角；
[0082]
βc为期望俯仰角；
[0083]kα
为设置的关于α的正常数；
[0084]kβ
为设置的关于β的正常数；
[0085]
s74、根据机身相对支撑腿的速度规划解得支撑足的速度：
[0086][0087][0088]
式中，为前支撑腿的速度；
[0089]
为后支撑腿的速度；
[0090]
s75、对前支撑腿速度和后支撑腿速度积分，获得前支撑腿相对机身坐标系的轨迹x
f_sp
和后支撑腿相对机身坐标系的轨迹x
b_sp
。
[0091]
在上述的一种动态行走的四足探测机器人的控制方法，所述步骤八中，机身加速度f
*
为：
[0092][0093]
式中，m为机身的惯性矩阵；
[0094]
c为机身科氏力和离心力矩阵；
[0095]
g为机身重力项向量；
[0096]
计算速度变换矩阵it0：
[0097][0098]
式中，i为单位矩阵；
[0099]
o为零；
[0100]
xi为第i个移动腿的足端在机身本体坐标系oxyz中的位置；
[0101]
(xi)
×
为xi的叉乘矩阵；
[0102]
计算力分解矩阵φ：
[0103][0104]
式中，当移动腿i为支撑腿时，si＝1，否则si＝0；
[0105]
则各移动腿的前馈力为：
[0106][0107]
式中，φ为力分解矩阵；
[0108]
φ

为力分解矩阵φ的广义逆矩阵。
[0109]
本发明与现有技术相比的有益效果是：
[0110]
(1)本发明提出的足式四足探测机器人动步态规划与控制方法通过基于系统状态的步态规划、落足点调整、动力学控制等方法，显著提高四足机器人对角小跑动态行走的稳定性，并具备良好的速度调节、斜坡自适应、干扰自适应的能力；
[0111]
(2)本发明运动学计算部分计算足端位置、速度、姿态旋转矩阵，估计行走的速度；落足点规划部分规划摆动腿落足点位置，以实现速度调节和抗干扰调节的作用；摆动腿足端轨迹规划部分基于系统状态规划摆动腿足端的三维轨迹；支撑腿足端轨迹规划首先规划机身的位置和姿态角，然后通过速度级逆解求得支撑足的足端轨迹；基于动力学控制部分计算系统的动力学前馈力并采用笛卡尔空间的阻抗控制完成步态的跟踪控制，有效减小支撑腿闭链中的内力。
附图说明
[0112]
图1为本发明四足探测机器人示意图。
具体实施方式
[0113]
下面结合实施例对本发明作进一步阐述。
[0114]
本发明提供一种动态行走的四足探测机器人及控制方法，运动学计算部分计算足端位置、速度、姿态旋转矩阵，估计行走的速度；落足点规划部分规划摆动腿落足点位置，以实现速度调节和抗干扰调节的作用；摆动腿足端轨迹规划部分基于系统状态规划摆动腿足端的三维轨迹；支撑腿足端轨迹规划首先规划机身的位置和姿态角，然后通过速度级逆解求得支撑足的足端轨迹；基于动力学控制部分计算系统的动力学前馈力并采用笛卡尔空间的阻抗控制完成步态的跟踪控制，有效减小支撑腿闭链中的内力。
[0115]
四足探测机器人，如图1所示，包括机身、第一移动腿、第二移动腿、第三移动腿和第四移动腿；其中，机身为锥台状结构；第一移动腿、第二移动腿、第三移动腿和第四移动腿均匀固定安装在机身的周向侧壁处；第一移动腿、第二移动腿、第三移动腿和第四移动腿为结构完全相同的移动腿。
[0116]
在机身的上表面建立机身本体坐标系oxyz；其中，o点位于机身的上表面的中心；x轴正方向指向四足探测机器人的前进方向；z轴正方向垂直于机身竖直向上；y轴由右手定则确定；
[0117]
第一移动腿和第二移动腿设置在x轴的一侧；第三移动腿和第四移动腿设置在x轴的另一侧；第一移动腿、第二移动腿、第三移动腿和第四移动腿沿顺时针依次设置；且第一移动腿与第四移动腿相对于x轴对称；第二移动腿与第三移动腿相对于x轴对称。
[0118]
每个移动腿的腿根处与机身的侧壁连接形成腿根关节；腿根关节相对于机身具有
俯仰旋转自由度和滚转旋转自由度；每个移动腿具有1个膝关节，膝关节具有俯仰旋转自由度。
[0119]
四足探测机器人的控制方法，包括如下步骤：
[0120]
步骤一、测量第i个移动腿的各关节角速度qi＝[q
i_1 q
i_2 q
i_3
]和角速度测量机身本体坐标系oxyz中z轴的姿态欧拉角γ、y轴的姿态欧拉角β和x轴的姿态欧拉角α；其中，i为移动腿的序号。
[0121]
步骤二、根据正运动学算法获得第i个移动腿的足端在机身本体坐标系oxyz中的位置xi＝[x
i y
i zi]和速度建立惯性坐标系o1x1y1z1，计算机身本体坐标系oxyz相对于惯性坐标系o1x1y1z1的旋转矩阵ic0；惯性坐标系o1x1y1z1的建立方法为：
[0122]
在初始未移动的四足探测机器人的机身上建立惯性坐标系o1x1y1z1；其中，原点o1位于机身上表面的中心；z1轴正向竖直向上；x1轴和y1轴可在过o1点切垂直于z1的平面上任意选取，满足右手定则即可；
[0123]
旋转矩阵ic0为：
[0124][0125]
式中，α为机身本体坐标系oxyz中x轴的姿态欧拉角；
[0126]
β为机身本体坐标系oxyz中y轴的姿态欧拉角。
[0127]
步骤三、定义四足探测机器人的运动姿态：初始时，以第二移动腿、第四移动腿为支撑腿；第一移动腿和第三移动腿为摆动腿，同时向x轴正方向运动，其后交替进行；定义前支撑腿为f_sp，后支撑腿为b_sp，前摆动腿为f_sw，后摆动腿为b_sw。
[0128]
步骤四、计算四足探测机器人的x方向的行走速度和y方向的行走速度采用支撑足端相对机身惯性系的x方向平均速度的相反数经低通滤波作为行走速度估计，x方向的行走速度的计算方法为：
[0129][0130]
式中，为前支撑腿相对于机身坐标系x方向的速度；
[0131]
为后支撑腿相对于机身坐标系x方向的速度；
[0132]
同样，侧向速度采用支撑足端相对机身惯性系的y方向平均速度的相反数经低通滤波作为估计，y方向的行走速度的计算方法为：
[0133][0134]
式中，为前支撑腿相对于机身坐标系y方向的速度；
[0135]
为后支撑腿相对于机身坐标系y方向的速度。
[0136]
步骤五、规划前摆动腿的落点位置和后摆动腿的落点位置前摆动腿的落点位置为：
[0137][0138]
后摆动腿的落点位置为：
[0139][0140]
式中，x
t,f_sw
为预先设计的前摆动腿的期望落足点；
[0141]
x
t,b_sw
为预先设计的后摆动腿的期望落足点；
[0142]
δd为落足点前进方向调整量；
[0143]
δs为落足点侧向调整量。
[0144]
通过x
t,f_sw
、x
t,b_sw
，即不存在速度偏差和干扰下的期望落足点。δd、δs用于调节落脚点相对位置，通过设计δd、δs可实现速度调节和抗干扰功能。
[0145]
采用调解律：
[0146][0147][0148]
步骤六、规划摆动腿的足端轨迹；规划摆动腿的足端轨迹的具体方法为：
[0149]
s61、计算四足探测机器人的行走进程参考变量θ：
[0150][0151]
式中，x
f_sp
为前支撑腿在机身坐标系下x方向的坐标；
[0152]
x
f_sw
为后支撑腿在机身坐标系下x方向的坐标；
[0153]
上式的物理意义即为支持足连线中心(记为s)相对机身原点在行进方向的位置。
[0154]
s62、对行走进程参考变量θ进行归一化处理，获得归一化的进度参考量s：
[0155][0156]
式中，s为归一化的进度参考量，s∈[0,1]；
[0157]
θ

为一步中θ的最大值；
[0158]
θ-为一步中θ的最小值；
[0159]
θ

和θ-也是初态值和终态值。为了使规划的步态是连续的，在每一步初始时由式(9)计算θ

，θ-为一步中设定的θ，设步长为d，则取
[0160][0161]
s63、对前摆动腿足端轨迹x
f_sw
(θ)和后摆动腿足端轨迹x
b_sw
(θ)采用5阶b
é
zier曲
线进行规划：
[0162][0163][0164]
式中，p
f_sw,j
为前摆动腿在b
é
zier曲线的第j个参数；
[0165]
p
b_sw,j
为后摆动腿在b
é
zier曲线的第j个参数；
[0166]
为排列组合公式；
[0167]
式中p0为迈腿初始时的x
i_sw
值，p5为期望落足点，在落足点规划部分确定。完成摆动腿的足端轨迹规划。
[0168]
步骤七、规划支撑腿的足端轨迹；先规划机身六维速度，采用速度级逆运动学计算法支撑足端轨迹。支撑腿的足端轨迹的规划方法为：
[0169]
s71、采用5阶b
é
zier曲线来规划机身相对于支撑轨足端的轨迹x
body
(θ)：
[0170][0171]
式中，p
body,j
为机身在b
é
zier曲线的第j个参数；
[0172]
s72、根据上式解析出速度
[0173][0174]
s73、机身的姿态采用速度级规划，规划姿态角速度ω
body
与姿态偏差成比例，即：
[0175][0176]
式中，αc为期望滚转角；
[0177]
βc为期望俯仰角；
[0178]kα
为设置的关于α的正常数；
[0179]kβ
为设置的关于β的正常数；
[0180]
s74、根据机身相对支撑腿的速度规划解得支撑足的速度：
[0181][0182][0183]
式中，为前支撑腿的速度；
[0184]
为后支撑腿的速度；
[0185]
s75、对前支撑腿速度和后支撑腿速度积分，获得前支撑腿相对机身坐标系的轨迹x
f_sp
和后支撑腿相对机身坐标系的轨迹x
b_sp
。
[0186]
步骤八、规划机身的加速度f
*
；计算4个移动腿的前馈力机身加速度f
*
为：
[0187][0188]
式中，m为机身的惯性矩阵；
[0189]
c为机身科氏力和离心力矩阵；
[0190]
g为机身重力项向量；
[0191]
计算速度变换矩阵it0：
[0192][0193]
式中，i为单位矩阵；
[0194]
o为零；
[0195]
xi为第i个移动腿的足端在机身本体坐标系oxyz中的位置；
[0196]
(xi)
×
为xi的叉乘矩阵；
[0197]
计算力分解矩阵φ：
[0198][0199]
式中，当移动腿i为支撑腿时，si＝1，否则si＝0；
[0200]
则各移动腿的前馈力为：
[0201][0202]
式中，φ为力分解矩阵；
[0203]
φ

为力分解矩阵φ的广义逆矩阵。
[0204]
步骤九、由规划的支撑足、摆动足段轨迹得到各足端的期望轨迹，采用带动力学前馈的阻抗控制对足端进行控制，获得四足探测机器人控制方程：
[0205][0206]
式中，τ
id
为第i条移动腿的期望关节力矩向量；
[0207]ji
为第i条移动腿的雅克比矩阵；
[0208]fid
为第i条移动腿的前馈力；
[0209]
x
di
为第i条移动腿期望的足端轨迹；
[0210]
当第i条移动腿为前摆动腿时，将x
f_sw
代入x
di
；
[0211]
当第i条移动腿为后摆动腿时，将x
b_sw
代入x
di
；
[0212]
当第i条移动腿为前支撑腿时，将x
f_sp
代入x
di
；
[0213]
当第i条移动腿为后支撑腿时，将x
b_sp
代入x
di
；
[0214]
k为正定的刚度矩阵；
[0215]
b为正定的阻尼矩阵；
[0216]
xi为第i个移动腿的足端在机身本体坐标系oxyz中的位置；
[0217]
根据控制方程实现对四足探测机器人的控制。
[0218]
对于支撑腿，竖直方向采用较高的刚度保证精度，水平方向采用较低的刚度，以减小各支撑腿形成的闭链系统间的内力。
[0219]
触地检测与换腿时，根据关节角度和角速度的反馈以及关节力矩命令对足端力进行估计，当竖直方向力超过阈值时判定为足端触地，先触地后的摆动腿转变为支撑腿进行规划。当后触地的摆动腿触地后，支撑腿与摆动腿互换，重复上述步骤即可。
[0220]
本发明所提出的足式四足探测机器人动步态规划与控制方法通过基于系统状态的步态规划、落足点调整、动力学控制等方法，显著提高四足机器人对角小跑动态行走的稳定性，并具备良好的速度调节、斜坡自适应、干扰自适应的能力。
[0221]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。