一种双轮足机器人单边越障控制方法与流程

1.本发明涉及机器人运动控制领域，尤其是一种双轮足机器人单边越障控制方法。


背景技术：

2.双轮足机器人为足式、轮式复合行走的一种机器人，区别于现有的双轮平衡小车，双轮足机器人兼具轮式高效移动和足式复杂环境行走的优势，国内外研究机构已开展相应的研究，比较典型的包括，美国波士顿动力的handle轮足机器人可实现单边越障、跳跃、下楼梯、下坡等复杂动作，具体涉及的技术尚未公开，瑞士苏黎世理工大学研制的ascento双轮足机器人能够实现跳跃、单边越障、连续上楼梯等灵活动作，单边越障控制主要采用全身动力学的方法实现。
3.双轮足机器人包括机体、两条支撑腿、两个驱动轮，每条支撑腿配有一个驱动轮，每条支撑腿包含大腿和小腿，大腿和机体的连接部分成为髋部关节，大腿和小腿的连接部分称为膝关节，现有的双轮足机器人单边越障控制技术大部分采用全身动力学的方法，该方法需要对每一个刚体和关节进行复杂动力学建模，同时借助优化的控制方式进行实现，因此，针对每一个刚体，要求具有更高的建模精度，同时对整个系统算力也具有较大的挑战。


技术实现要素：

4.为了解决由于复杂建模方式所导致的双轮足机器人单边越障控制方式复杂的问题，本发明提供一种双轮足机器人单边越障控制方法，在线性化、离散化的双轮足简化动力学模型的基础上，引入虚拟腿的概念，将两条腿化简为一条虚拟腿，简化后的模型中，两个轮之间相互独立，虚拟腿与两个轮连接，本发明通过控制驱动轮的驱动力距实现被控对象的平衡控制，即控制被控对象不发生前后倾斜；通过分别控制两个支撑腿的关节角度实现姿态控制，确保被控对象不发生侧倾。本发明融合虚拟腿建立状态反馈控制模型，最后对支撑腿进行单边越障姿态补偿实现稳定控制。
5.本发明首先建立含有机器人虚拟腿姿态角度、虚拟腿姿态角速度、机体偏航姿态角度、机体偏航姿态角速度、机体前向位移，以及机体前向线速度六个状态量的双轮足简化动力学模型，并进行线性化、离散化处理，其次引入虚拟腿概念，针对离散的虚拟腿长度进行状态反馈矩阵元素的多项式插值，采用卡尔曼滤波器实现各状态观测，利用线性二次型调节器进行全状态反馈，产生两个驱动轮的驱动力矩，实现状态反馈平衡控制，最后，在状态反馈平衡控制的基础上，对支撑腿进行侧倾角姿态补偿，通过支撑腿运动学逆解产生髋部关节角度和膝关节角度，实现单边越障的姿态控制，该方法简单易实现，且控制效果较好。
6.具体技术方案如下：
7.步骤(1)、建立含有机器人虚拟腿姿态角度、虚拟腿姿态角速度、机体偏航姿态角度、机体偏航姿态角速度、机体前向位移，以及机体前向线速度六个状态量的双轮足简化动
力学模型，作为控制的模型基础，并进行线性化、离散化处理；
8.步骤(2)可变虚拟腿长度下的双轮足机器人平衡控制，为单边越障的虚拟腿变化做支撑；
9.步骤(3)单边越障下的姿态稳定适应，完成单边越障稳定控制。
10.进一步的，步骤(2)包括以下步骤，
11.步骤21：求解当前时刻的状态反馈矩阵：
12.通过状态反馈矩阵离线计算，获取离散虚拟腿长度下的状态反馈矩阵kd，进而利用离散的虚拟腿长度和对应的kd，通过5次多项式插值，拟合状态反馈矩阵k，最终根据当前时刻可变虚拟腿长度得到当前时刻的状态反馈矩阵，所述的拟合得到的状态反馈矩阵k用于适应可变虚拟腿各种长度下的状态反馈控制；
13.步骤22：根据双轮足机器人的离散模型，采用卡尔曼滤波器估计当前时刻的系统状态
14.步骤23：融合步骤21的状态反馈矩阵k和步骤22的系统状态建立期望状态xd与估计的当前时刻系统状态之间的状态误差反馈控制模型，得到当前时刻轮子的驱动力矩uk，实现可变虚拟腿长度下的平衡控制。
15.进一步的，步骤(3)包括以下步骤，
16.步骤31：实时测量机体侧倾姿态角度φ；
17.步骤32：建立机体侧倾角比例控制项和积分控制项；
18.步骤33：利用步骤32建立的比例控制项与积分控制项分别对左右支撑腿当前时刻的高度进行姿态补偿，将补偿后的两条支撑腿的高度位置通过逆运动学转换为每条支撑腿的关节角度，通过驱动关节实现姿态的稳定适应，和步骤23的状态误差反馈模型，共同作用实现单边越障稳定控制。
19.有益效果：
20.现有的双轮足机器人控制方法，在机器人前行、转向等平衡控制上进行了较多的研究，针对机体的位置变化状态反馈控制精度不足，且在单边越障工况下大部分采用全身动力学的控制方法解决姿态控制的问题，控制过程复杂，。相比于现有技术，本发明具有以下优势：
21.(1)建立全状态的动力学方程，可以有效控制机器人的位姿；(2)采用虚拟腿思想，并将虚拟腿长度作为状态反馈矩阵元素插值的输入，可有效实现不同虚拟腿长度下的稳定性；(3)状态矩阵元素进行五次多项式插值，可提高状态反馈控制的精度；(4)针对支撑腿高度进行侧倾角度姿态补偿，可有效控制机器人单边越障，控制方法简单有效；(5)本发明模型简单，控制方法简单有效。
附图说明
22.图1是双轮足机器人模型。
23.图2是双轮足机器人单边越障模型。
24.图3是虚拟腿在竖直方向的角度示意图。
具体实施方式
25.下面结合附图和具体实施例对一种双轮足机器人单边越障控制方法进行详细描述。
26.同时，在这里做以说明的是，为了使实施例更加详尽，下面的实施例为最佳、优选实施例，对于一些公知技术，本领域技术人员也可采用其他替代方式而进行实施；而且附图部分仅是为了更具体的描述实施例，并不旨在对本发明进行具体的限定。
27.本发明涵盖任何在本发明的精髓和范围上做的替代、修改、等效方法以及方案。为了使公众对本发明有彻底的了解，在以下本发明优选实施例中详细说明了具体的细节，而对本领域技术人员来说没有这些细节的描述也可以完全理解本发明。
28.如图1所示，步骤1，双轮足动力学建模，作为控制的模型基础。具体包括：
29.步骤11：采用拉格朗日方程建立含有机器人虚拟腿姿态角度/姿态角速度、偏航姿态/姿态角速度、前向位移/线速度六个状态量的双轮足动力学模型。
[0030][0031]
式中，l为拉格朗日函数，q为广义坐标，q为广义力。
[0032]
系统的动能：
[0033][0034]
式中，t为系统动能，θ为虚拟腿在竖直方向的角度，γ为机体的偏航角度，x为机器人的前向位移，i
xx
为机体沿x轴的转动惯量，i
yy
为机体沿y轴的转动惯量，i
zz
机体沿z轴的转动惯量，m轮子质量，m机体质量，l虚拟腿长度，d轮子之间宽度，r轮子半径，
[0035]
系统的势能：
[0036]
v＝mglcosθ
[0037]
l＝t-v
[0038]
步骤12：双轮足动力学线性化：
[0039][0040][0041]
其中：
[0042]
p1＝[(2m m)i
yy
r2 2i
yyiw
] 2mml2r2 2ml2iw[0043]
p2＝(ml2 i
yy
)(mr2 2mr2 2iw)-m2l2r2[0044]
p3＝2d2r(m iw/r2) ri
zz
[0045]
式中，iw轮子沿转轴的转动惯量，τ
l
、τr分别为左右轮的主动驱动力矩。
[0046]
步骤13：双轮足连续动力学模型离散化。
[0047][0048]
式中，δt为离散时间。
[0049]
步骤2，可变虚拟腿长度下的双轮足机器人平衡控制，为单边越障的虚拟腿变化做支撑。具体包括：
[0050]
步骤21：求解当前时刻的状态反馈矩阵。通过状态反馈矩阵离线计算，获取离散虚拟腿长度下的状态反馈矩阵元素，根据离散的虚拟腿长度及矩阵元素，进行5次多项式插值；实时测量两条支撑腿的髋部关节和膝关节角度，根据腿部运动学关系计算虚拟腿长度，并根据拟合后的5次多项式，计算当前时刻的状态反馈矩阵。
[0051]
其中，状态反馈矩阵离线计算，采用黎卡夫riccati方程求解，求解如下：
[0052][0053]
式中，正定矩阵p由以下方程得到，
[0054][0055]
式中，q为半正定实对称矩阵，r为正定实对称矩阵，ad和bd矩阵的元素包含虚拟腿长度，且由步骤13的公式中获取。
[0056]
其中，状态反馈矩阵5次多项式插值，通过设置离散的虚拟腿长度，并根据q、r矩阵设置，求解对应的k矩阵元素，以适应可变虚拟腿长度下的状态反馈控制，五次多项式插值形式如下：
[0057]kij
＝k
5_ij
l5 k
4_ij
l4 k
3_ij
l3 k
2_ij
l2 k
1_ij
l k
0_ij
[0058]
式中k
ij
为k矩阵的第i行第j列元素，k
n_ij
为对应k
ij
的多项式系数。
[0059]
其中，所述的虚拟腿长度是通过实时测量两条支撑腿的髋部关节和膝关节角度，并根据腿部运动学关系计算而来，求解如下：
[0060][0061]
p
f1x
＝-l1sinθ
11-l2sin(θ
11
θ
12
)
[0062]
p
f1z
＝-l1cosθ
11-l2cos(θ
11
θ
12
)
[0063]
p
f2x
＝-l1sinθ
21-l2sin(θ
21
θ
22
)
[0064]
p
f2z
＝-l1cosθ
21-l2cos(θ
21
θ
22
)
[0065]
[0066][0067]
式中，p
fi
为第i个腿的足位置，p
fix
为第i个腿的足x方向位置，p
fiz
为第i个腿的足z方向位置，l1大腿长度，l2小腿长度，θ
11
左腿髋关节角度，θ
12
左腿膝关节角度，θ
21
右腿髋关节角度，θ
22
右腿膝关节角度。
[0068]
步骤22：根据步骤13双轮足机器人的离散模型，采用卡尔曼滤波器估计当前时刻的系统状态具体如下：
[0069][0070]
其中，
[0071][0072][0073][0074]
式中，为上一个离散时间的系统状态，为新的系统状态，为当前时间估计的系统状态，p
k-1
为上一个离散时间的协方差矩阵，为新的协方差矩阵，pk为当前的协方差矩阵，y为系统输出，u
k-1
为上一个离散时间的控制输入，ad、bd、从双轮足简化动力学离散化模型中获取，c矩阵从系统输出方程中获取，p
k-1
初始设定为单位矩阵，初始设置为零向量，u
k-1
初始设置为零向量。
[0075]
步骤23：融合步骤21的状态反馈矩阵和步骤22的系统状态，建立状态误差反馈控制模型，产生轮子驱动力矩，实现可变虚拟腿长度下的平衡控制，控制模型如下：
[0076][0077]
式中，uk为当前离散时间的轮子驱动力矩输入，xd为期望状态。
[0078]
如图2所示，步骤3，单边越障下的姿态稳定适应，保证被控对象不侧翻，完成单边越障稳定控制。具体包括：
[0079]
步骤31：通过机载惯性测量单元实时检测机体的侧倾角度φ。
[0080]
步骤32：采用比例积分的方式控制侧倾角φ，建立机体侧倾角比例控制项与积分控制项如下：
[0081]
比例项p
kp
＝k
pφ
(φ
d-φ)，积分项p
ki
＝k
iφ
∑(φ
d-φ)δt，
[0082]
式中，k
pφ
为比例系数，k
iφ
为积分系数，φd为期望的机体侧倾角，φ为实际的机体侧倾角，δt为时间间隔。
[0083]
步骤33：根据左右侧腿的极性方向，对支撑腿高度进行姿态补偿，产生新的足端位置；由于足端位置和每个关节角度存在计算关系，故可以通过腿部逆运动学，由新的足端位置得到每个关节的角度，进而控制关节角度实现姿态适应，所述的姿态适应和步骤23的状态误差反馈模型共同作用，实现单边越障稳定控制。
[0084][0085][0086]
θ
ij
＝ik(p
i_new
)
[0087]
式中，左侧腿α＝-1，右侧腿α＝1，为第i支撑腿的高度补偿量，为第i支撑腿新的高度位置，p
i_new
为第i支撑腿新的足端位置，ik表示腿的逆运动学计算，θ
ij
表示第i支撑腿第j个关节的角度。
[0088]
本发明用于控制双轮足机器人运动，具体通过设定期望的机器人虚拟腿姿态角度、虚拟腿姿态角速度、机体偏航姿态角度、机体偏航姿态角速度、机体前向位移，机体前向线速度六个状态量，以及期望的机体侧倾角，保证机器人实现稳定单边越障。比如设定期望的机体前向线速度为1km/h，期望的偏航速度和偏航角度设置为0，期望的虚拟腿姿态和姿态角速度设置为0，期望前向位移会一直根据速度和时间进行累加，此时机器人就会根据期望的机体前向线速度行驶；当前进路上遇到斜坡，控制机器人单边越障，即一个轮子骑在坡上，另一个轮子保持水平路面行驶，此时，首先通过步骤(2)保证机器人不发生前后倾，然后通过比例积分控制使机体侧倾角达到期望的机体侧倾角，保证不侧翻，从而实现平稳单边越障。