一种双足机器人偏摆力矩补偿方法与流程

1.本发明涉及机器人技术领域，具体涉及一种双足机器人偏摆力矩补偿方法。


背景技术：

2.在目前的双足机器人研究领域，对其稳定性分析时，多采用基于zmp的稳定性判据，该方法较为成熟，在众多机器人产品得到了较好的应用。但zmp稳定性判据存在一定的弊端，根据zmp的定义：在机器人步行过程中，脚掌与地面的接触会产生来自地面的作用力，将作用力的合力等效为某一点处，且在该点沿水平面内的力矩为零，则该点即为零力矩点(zmp)。在双足机器人动态步行时，若其zmp始终保持在支撑多边形区域内，则步行是稳定的。该判据保证了机器人在水平面内的力矩为零，但对于机器人绕竖直方向的力矩，该判据未对其进行研究。在对机器人研究时，默认机器人竖直方向的力矩由地面提供的摩擦力矩完全补偿，基于以上条件完成对机器人稳定性的研究。
3.在机器人与地面摩擦系数较大、步行过程偏摆力矩较小的情况下，以上假设是成立的，摩擦力矩完全可以补偿机器人加、减速过程产生的偏摆力矩。但在机器人实际步行过程中，由于运动速度相对较快，地面摩擦力提供的摩擦力矩有限，无法完全补偿偏摆力矩，此时脚掌相对地面会发生旋转滑动，影响机器人的稳定性，当旋转滑动程度较大时，机器人甚至会发生摔倒的现象。目前，大多数双足机器人物理样机在踝关节处均未配备z轴方向的偏航自由度，故当机器人全身相对于支撑脚踝部的偏摆力矩增大时，会造成踝关节的磨损，对机器人零部件造成损伤。因此，如何控制机器人的上半身来降低偏摆力矩产生的影响，是进一步提高机器人步行稳定性的关键。
4.在目前常用的双足机器人步态规划研究中，由于双足机器人是一个多刚体、变结构、强耦合的复杂动力学系统，运动学模型、力学控制模型等多为高阶非线性方程，在线步态规划方法应用到双足机器人领域有较大难度，对步行过程的在线优化耗时较长，对机器人硬件系统提出较高求，故目前的双足机器人步行方式多为离线步态规划，有的则采用离线规划加在线调整的控制方式完成机器人的步行过程。基于以上机器人步行方式的提出，对于其偏摆力矩的补偿，应在离线步态规划阶段完成，求解步态规划过程中所产生的偏摆力矩，提出有效的补偿措施。
5.申请号为201611194751.7的中国专利公开一种仿人机器人偏摆力矩控制方法，通过控制踝关节抵消偏摆力矩的影响。该方法通过建立踝关节处的力优化函数，基于迭代方法的得到踝关节位置补偿量。而机器人是一个多自由度系统，运动过程中驱动电机存在磨损，导致各关节角的控制精度变低，由于机器人脚掌与地面的冲击碰撞，导致各组成结构件发生一定程度的形变。若通过对踝关节进行位置补偿，一方面由于的电机磨损导致的控制精度变低，对偏摆力矩的控制作用无法达到预期要求；另一方面由于机器人各连接件的形变，导致踝关节理论计算补偿量和实际所需补偿量存在较大差异，最终无法使机器人偏摆力矩控制在摩擦力矩范围内。


技术实现要素：

6.针对现有技术中所存在的不足，本发明提出了一种双足机器人偏摆力矩补偿方法。本发明对于当前zmp稳定性判据存在的弊端，导致机器人在快速步行过程发生的偏转滑移现象，采用机器人上半身关节运动控制的方式，产生用于平衡偏摆力矩的补偿力矩，通过腰关节或臂关节的运动模式，基于能量消耗最优考虑，实现机器人运动过程偏摆力矩的最优补偿，进一步提高机器人的步行稳定性。因机器人为一个多自由度、强耦合、混杂态的混合动力学系统，对其进行动力学建模时，采取常规的求解方式较为复杂，故需将其简化为多连杆模型，对于不同的运动模式可获得相应的动力学方程，针对腰关节或臂关节的运动模式，可获得相应的能量函数。
7.本发明的目的是通过以下技术方案来实现的：一种双足机器人偏摆力矩补偿方法，该方法具体步骤如下：
8.(1)将双足机器人模型抽象为多连杆模型，基于机器人动力学分析方法，建立机器人步行过程力矩平衡方程，并求解zmp位置；
9.(2)根据机器人zmp稳定性判据条件，结合步骤(1)建立的平衡方程，得到机器人运动过程中的偏摆力矩；
10.(3)当偏摆力矩超过地面和双足机器人足部的最大摩擦力矩时需要对偏摆力矩进行补偿，具体过程为：取时间间隔δt＝t/n，t表示运动周期，n表示离散点的数目，对偏摆力矩m
z
进行采样，获得离散力矩m＝[m1、m2、
……
、m
n
]
t
，建立机器人上半身关节运动的角轨迹与离散力矩的映射关系，基于时间
‑
角位置多项式插值方法，结合设置的边界条件，对双足机器人的腰部扭转角度或者双臂摆动角度进行设计，实现对偏摆力矩的补偿效果。
[0011]
(4)根据能量消耗函数对偏摆力矩补偿结果进行评估；具体如下：
[0012]
(4.1)针对头部配备图像采集的双足机器人，只进行双臂摆动角度设计，在周期运动阶段所消耗的能量函数如下：
[0013][0014]
其中，n表示离散点的数目，δt＝t/n表示在整个运动周期的采样时间间隔，i2为机器人双臂绕肩关节轴中心的惯性矩，m
armz
表示手臂的质量，l
armz
表示手臂的长度，l表示机器人髋关节的间距，m
j
表示第j个采样时刻对应的偏摆力矩，c
i
为手臂摆动阶段对应的单位线位置。
[0015]
(4.2)针对头部未配备图像采集的双足机器人，在周期运动阶段所消耗的能量函数如下：
[0016]
设计腰部扭转角度时，周期运动阶段所消耗的能量函数为：
[0017][0018]
其中，n表示离散点的数目，δt＝t/n表示在整个运动周期的采样时间间隔，i1为机器人上半身整体绕腰关节中心轴的惯性矩，m
j
表示第j个采样时刻对应的偏摆力矩。
[0019]
设计双臂摆动角度时，周期运动阶段所消耗的能量函数为：
[0020][0021]
其中，n表示离散点的数目，δt＝t/n表示在整个运动周期的采样时间间隔，i2为机器人双臂绕肩关节轴中心的惯性矩，m
j
表示第j个采样时刻对应的偏摆力矩，c
i
为手臂摆动阶段对应的单位线位置。
[0022]
比较δe1和δe2的能量消耗大小，选择最佳的偏摆力矩补偿方式。
[0023]
(5)判断步骤(4)的能量函数计算结果是否满足偏摆力矩补偿要求，若是，则完成偏摆力矩补偿，若否，返回步骤(3)，重新设计腰部扭转角度或者双臂摆动角度，并改变相应的边界约束条件，直至实现所设计的运动形式满足偏摆力矩补偿要求。
[0024]
进一步地，步骤(1)中所建立的力矩平衡方程，可根据机器人动力学分析进行求解，结合机器人实际连杆物理模型，可获得其力矩平衡方程为：
[0025][0026]
其中，m
i
表示编号为i的连杆质量，r
i
，和mci分别表示连杆i的质心位置矢量、加速度矢量和力矩。p为支撑脚与地面接触点的位置矢量，g为重力加速度向量，t
p
为地面对机器人的力矩，k为机器人连杆总数。
[0027]
进一步地，在步骤(3)中，对于机器人偏摆力矩的采样过程，应根据其传感器的实际传输速度，来决定采样频率的大小，为最大限度获得机器人实际运动阶段所产生的偏摆力矩，取采样时间间隔为传感器数据发送的时间间隔。
[0028]
进一步地，在步骤(3)中，针对时间
‑
角位置多项式插值方法，多项式次数的高低与边界条件的数目相对应，为避免次数过高导致的龙格现象，其边界条件应选择性约束。
[0029]
进一步地，在步骤(4)中，对于机器人双臂摆动角度设计，应保证其左、右臂在摆动阶段为对称式运动，且大小相等、方向相反；对于机器人腰部扭转角度设计，应考虑其最大限定转动角的约束，防止出现机器人各零部件之间的运动干涉情况。
[0030]
进一步地，针对步骤(4)所建立的双臂摆动角度设计所消耗的能量函数，以及腰部扭转角度设计所消耗的能量函数，是建立在机器人上半身关节运动轨迹设计完成之后的评估函数，用来确定最佳的偏摆力矩补偿方案。
[0031]
本发明的优点在于：通过控制机器人上半身运动的模式，基于对机器人腰关节或肩关节的控制，来实现对机器人步行过程偏摆力矩的有效补偿，且不影响原zmp水平面内的稳定性状态，适用于不同类型的双足机器人样机，对于提高机器人的稳定性具有重要意义。
附图说明
[0032]
图1是本发明的双足机器人结构自由度配置图；
[0033]
图2是本发明的双足机器人简化七连杆模型示意图；
[0034]
图3是本发明的双足机器人九连杆模型示意图；
[0035]
图4是图3中机器人局部肩关节运动示意图；
[0036]
图5是机器人在单周期运动情况下，采用基于双臂摆动的偏摆力矩补偿方式状态下的步行过程；
[0037]
图6是机器人在单周期运动情况下，采用腰关节转动方式实现偏摆力矩补偿作用的步行过程；
[0038]
图7是机器人在多个周期运动条件下，机器人采用双臂摆动的偏摆力矩补偿方式条件下，机器人的力矩变化情况；
[0039]
图8是一种双足机器人偏摆力矩补偿方法所实现的整体流程图；
具体实施方式
[0040]
为了使本发明的目的更加明确、技术路线更为清晰，以下将结合附图及具体实施例对本发明做进一步详细说明。提出的实施例仅用于说明本发明内容，并不用于限制本发明。
[0041]
本发明提供了一种双足机器人偏摆力矩补偿方法，如图1所示为双足机器人的整体自由度分布图，其各关节术语已在图中标注，可将机器人结构简化为七连杆模型，如图2所示，用于研究双足机器人步行过程偏摆力矩产生的影响。为表示各连杆之间的位置关系，可将机器人上半身(包括躯干及双臂)简化为“t”型连杆，机器人下半身大腿、小腿、脚踝均表示为质量分布均匀的直杆。
[0042]
进一步地，对于以上建立的双足机器人七连杆模型，分析机器人步行过程产生的偏摆力矩，具体为：
[0043]
从双足机器人的运动模式来看，为保证双足机器人实现拟人运动，减少能量消耗，通常保证机器人的质心位置在竖直方向上保持高度恒定，在机器人步行稳定时，在笛卡尔坐标系o
‑
xyz下，其力矩平衡方程为：
[0044][0045]
其中，m
i
表示编号为i的连杆质量，r
i
，和m
ci
分别表示连杆i的质心位置矢量、加速度矢量和力矩。p为支撑脚与地面接触点的位置矢量，g为重力加速度向量，t
p
为地面对机器人的力矩，n为机器人连杆总数。
[0046]
根据机器人zmp稳定性判据内容可知，当机器人步行稳定时，机器人从地面产生的水平面上的反作用力矩为零，即绕x、y轴方向的力矩为零：
[0047]
t
px
＝0
[0048]
t
py
＝0
[0049]
对于z轴方向的偏摆力矩，未对其进行研究。因此，满足zmp稳定条件仅仅可以保证机器人在x、v轴方向的稳定，是否发生z轴方向的滑转则取决于偏摆力矩和z轴方向摩擦力矩t
pz
之间的大小关系。
[0050]
机器人全身在运动过程中产生的偏摆力矩，在假设条件下，由地面完全补偿时，即存在：
[0051]
m
z
t
pz
＝0
[0052]
地面摩擦力矩所能提供的极限值即为|t
pz
|
max
：
[0053]
|t
pz
|
max
＝f(μ，f
z
，s)
[0054]
其中，μ为机器人脚掌与地面的摩擦系数，f
z
为法向反力，s为脚的大小及形状。
[0055]
假设机器人的zmp为p点，可求出机器人由于加、减速过程产生的绕zmp的力矩m，其中下角标z表示z轴分量，即m
z
为偏摆力矩：
[0056][0057]
其中，p点的位置矢量为p＝[x
zmp
，y
zmp
，0]
t
。
[0058]
当m
z
足够大时，超过地面摩擦力所能提供的最大摩擦力矩时，即：
[0059]
|m
z
|＞|t
pz
|
max
[0060]
机器人将发生绕支撑脚z轴方向的旋转运动，对机器人的步行方向及稳定性产生影响，当机器人偏转程度较大时，甚至会发生失稳甚至摔倒的现象。
[0061]
基于以上分析，需考虑设计机器人上半身运动模式，产生偏摆力矩的反力矩，以降低偏摆力矩带来的影响。在完整的运动周期中，对时间间隔δt＝t/n，对力矩m
z
进行采样，可获得离散力矩m＝[m1、m2、
……
、m
n
]
t
，建立机器人上半身关节运动的角轨迹与离散力矩的映射关系，基于时间
‑
角位置多项式插值方法，结合设置的边界条件，对双足机器人的腰部扭转角度或者双臂摆动角度进行设计，实现对偏摆力矩的补偿效果。针对时间
‑
角位置多项式插值方法，多项式次数的高低与边界条件的数目相对应，为避免次数过高导致的龙格现象，其边界条件应选择性约束。对于机器人偏摆力矩的采样过程，应根据其传感器的实际传输速度，来决定采样频率的大小，为最大限度获得机器人实际运动阶段所产生的偏摆力矩，取采样时间间隔为传感器数据发送的时间间隔。
[0062]
机器人上半身具有多个关节，一方面，可采用机器人的腰关节实现旋转运动，产生补偿力矩，以控制偏摆力矩保持在最大摩擦力矩之内；另一方面，机器人具有臂关节，可采用双臂摆动的形式，根据双臂摆动产生的补偿力矩，实现对步行过程的控制。两者控制方式均可实现对偏摆力矩的有效补偿，但从能量消耗角度进行分析，期望可实现能量消耗最优，故需对以上两种偏摆力矩补偿方式进行分析，获得能量最优。
[0063]
进一步地，首先对机器人腰关节转动运动形式进行分析，通过机器人上半身整体绕其腰关节中心轴旋转以补偿偏摆力矩，可得到：
[0064][0065][0066][0067]
其中，m
t1
为机器人上半身在不产生绕z轴的旋转运动所产生的力矩，m
t2
为机器人上半身平移运动及腰部旋转运动所产生的力矩，r
t
及为机器人上半身质心相对于zmp的位置矢量和加速度矢量，i1为机器人上半身整体绕腰关节中心轴的惯性矩，为自旋角加速度，将m
t
＝
‑
m代入以上公式，可求得：
[0068][0069]
基于以上分析，假设机器人腰关节初始角度为ψ1＝ψ
max
，其扭转最大角表示为ψ
max
，取初始角速度为可进行求解得到：
[0070][0071][0072]
其中，为周期时间内的机器人上半身的自旋角速度，ψ＝[ψ1，ψ2，...，ψ
n
]
t
为自旋角，e
n
为n
×
1的向量，可表示为e
n
＝[1，1，...，1]
t
，p为n
×
n的下三角矩阵，可以表示为：
[0073][0074]
可获得机器人腰关节旋转情况下周期时间t以内的能量消耗：
[0075][0076]
对以上公式综合求解，可以得出δe1的最终形式：
[0077][0078]
进一步地，可讨论机器人双臂摆动运动情况下，由此产生的补偿力矩对偏摆力矩的补偿效果，分析其能量消耗情况，与腰关节旋转运动的补偿力矩进行能耗比较，实现机器人偏摆力矩补偿的能量消耗最优。
[0079]
如图3所示，为研究机器人双臂摆动对偏摆力矩的补偿作用，可将机器人上半身简化为三连杆模型，即机器人双臂简化为绕肩关节转动的连杆，实现双臂前后摆动，与下半身共同组成九连杆机构。
[0080]
自然状态下，机器人双臂与地面垂直，在未采用双臂摆动控制策略时，双臂与躯干一起做平移运动，此时可以求出左、右臂对zmp的力矩：
[0081][0082][0083]
其中，m
′
l
和m
′
r
分别表示左臂和右臂保持平移运动时产生的力矩，m
arm
为机器人手臂的质量，r
w
＝[r
wx
，r
wy
，r
wz
]
t
和分别表示机器人运动时髋关节中心相对于zmp的位置矢量和加速度矢量。r
al
＝[r
ax
，r
ay
，r
az
]
t
、r
ar
＝[r
ax
，
‑
r
ay
，r
az
]
t
、分别表示当左、右臂垂直地面时，相对于髋关节中心的位置矢量和加速度矢量。
[0084]
由于步行过程双臂未摆动，左、右臂相对于躯干位置一直保持不变，故均为0。
[0085]
在机器人的双臂实现摆动的情况下，摆动平面为xoz平面，摆动规律为左、右臂摆动方向相反、大小相等时，根据机器人zmp求解公式可知，其zmp位置不受摆臂的影响，在其
他运动条件保持相同的情况下，可以获取双臂摆动后左、右臂对zmp的力矩：
[0086][0087][0088]
其中，i
arm
和分别表示手臂绕肩摆动时的转动惯量和角加速度，r
l
＝[r
x
，0，r
z
]
t
、r
r
＝[
‑
r
x
，0，r
z
]
t
、分别表示左、右臂摆动时手臂质心相对于未摆动垂直状态下的位置矢量和加速度矢量。
[0089]
机器人左、右臂的摆动具有对称性，大大减少了方程求解的难度。通过以上分析，可以获得双臂摆动控制策略提供的总力矩：
[0090]
m
arm
＝m
l
‑
m
′
l
m
r
‑
m
′
r
[0091][0092]
手臂摆动产生的绕z轴的力矩为在双臂摆动过程中，其摆动模型如图4所示，手臂总是在垂直线附近旋转，手臂质心沿z轴方向的位置分量r
z
和加速度分量相对较小，摆臂运动对机器人全身绕x轴和y轴的力矩影响也较小。故手臂摆动产生的力矩m
armz
可用来补偿机器人步行过程产生的偏摆力矩m，使其控制在地面所能提供的最大摩擦力矩范围内。
[0093]
参考图4所示的手臂摆动模型，θ表示手臂绕肩旋转的角度，和分别表示手臂摆动过程中的法向加速度和切向加速度，可求出手臂在x轴方向质心的加速度向量：
[0094][0095]
其中，x方向用e
x
＝[1，0，0]
t
表示，且存在：
[0096][0097]
其中，l
arm
表示手臂的长度，设机器人双肩间距为l，由于机器人结构对称布置，可获得r
ay
和l之间的关系：r
ay
＝0.5l。由此可获得手臂摆动产生的补偿力矩：
[0098][0099]
对以上公式进行简化处理可得：
[0100][0101]
为保证偏摆力矩得到补偿效果，需保证m
armz
＝
‑
m，参考手臂摆动模型，即需要实现：
[0102][0103]
其中，θ＝[θ1，θ2，...，θ
n
]
t
为在周期时间t内手臂摆动角度的离散化数据，sinθ＝
[sinθ1，sinθ2，...，sinθ
n
]
t
。
[0104]
设定手臂摆动的初始位置θ1＝θ
max
，其初始角速度设置为由此可获得机器人运动过程，手臂摆动阶段对应的单位线位置：
[0105][0106]
其中，c＝[c1，c2，...，c
n
]。由此可求解手臂对应的角位置：
[0107]
θ＝arcsin(c)
[0108]
进一步地，可求解得到机器人双臂摆动对应的动能方程：
[0109][0110]
机器人在周期步行阶段，手臂摆动呈周期变化，从起始运动阶段到运动结束阶段，手臂的重力势能未发生改变。故在周期时间t内，手臂摆动所消耗的能量为：
[0111][0112][0113]
进一步地，为比较腰关节旋转方式和双臂摆动运动方式两种方法对机器人偏摆力矩补偿效果能量消耗的最优，需定义比例系数σ：
[0114][0115]
进一步地，根据双足机器人相关参数，可以实现对以上两种偏摆力矩补偿状态下的能量消耗求解。基于比例系数σ的大小，可以得到两种偏摆力矩条件下的能量消耗对比，从而可以选取较优的方式，实现对机器人偏摆力矩的有效补偿。
[0116]
判断能量函数计算结果是否满足偏摆力矩补偿要求，若是，则完成偏摆力矩补偿，若否，则重新设计腰部扭转角度或者双臂摆动角度，并改变相应的边界约束条件，直至实现所设计的运动形式满足偏摆力矩补偿要求。根据所获得的最佳偏摆力矩补偿方案，判定最终是否满足偏摆力矩补偿的预期效果，根据判定结论的与否，来确定最终方案的输出，整体实现流程如图8所示。
[0117]
因机器人的手臂长度将直接影响偏摆力矩的补偿效果，故可考虑对机器人的手臂进行讨论，在不同的手臂长度下，双臂摆动的力矩平衡方法和腰关节旋转的力矩平衡方法所消耗的能量也将存在较大差异。因此可以通过对σ取值为1，获取手臂长度的临界值，确定两者偏摆力矩的最优情况。
[0118]
机器人手臂长度增加的同时，机器人系统的稳定性会提升，可实现的操作空间也会变大，但手臂质量的增加，会给驱动装置带来负担，导致机器人变得较为笨重，机器人能耗也会随着增加。因此，对于机器人手臂质量的选取，应综合考虑。
[0119]
在考虑到机器人头部相机的图像采集效果，应优先选用双臂摆动的力矩补偿形式；在考虑到对机器人的手臂轻量化设计，或当机器人不具备手臂结构时，应选用腰关节旋
转的方式实现偏摆力矩补偿。当机器人具备全身自由度时，不考虑其他外界因素，以能量消耗最优为优化目标时，应根据σ的数值来确定偏摆扭矩的补偿方式。对于机器人双臂摆动角度设计，应保证其左、右臂在摆动阶段为对称式运动，且大小相等、方向相反；对于机器人腰部扭转角度设计，应考虑其最大限定转动角的约束，防止出现机器人各零部件之间的运动干涉情况。
[0120]
以机器人双臂摆动补偿偏摆力矩为例，如图5所示，基于手臂在不同时刻的摆动情况下，可实现对机器人在不同时刻的偏摆力矩补偿。基于仿真实验条件，设定机器人与地面的最大摩擦力矩为0.2n
·
m，可得到机器人在无力矩补偿及有力矩补偿条件下的偏摆力矩变化，如图7所示，未采用基于双臂摆动的偏摆力矩补偿方案时，机器人在某些时刻的偏摆力矩值超出最大摩擦力矩，此时机器人将发生偏转滑移，对机器人的使用寿命及运动稳定性将产生不利影响。采用基于双臂摆动的偏摆力矩补偿方案时，将机器人运动过程的偏摆力矩控制在最大摩擦力矩范围以内，克服机器人运动过程偏摆力矩的影响，显著提高机器人的运动稳定性。
[0121]
同理，对于腰关节旋转的偏摆力矩补偿方式，分析方法和步骤与机器人双臂摆动补偿偏摆力矩相同，其步行过程的行走状态如图6所示，在不考虑机器人头部相机的采集效果时，可采用此方法，对机器人实现偏摆力矩补偿。
[0122]
上述实施案例仅用于说明本发明内容的技术方案，针对不同类型的双足机器人物理样机具有普适性。对于机器人领域的研究人员，可对上述理论方法进行修改改进，或者应用替换到相近的机器人领域；但对于以上方法的应用，均属于本发明权利要求的保护范围。