基于ZMP的四足仿生机器人反应式行为控制策略研究

闫 曈， 许 威， 苏 波

(中国北方车辆研究所，北京 100072)

现今，智能移动平台正朝着高机动性、高通过性和高稳定性方向发展，因此移动机器人对复杂环境的适应随之成为运动控制领域的研究热点，足式机器人也因其结构复杂而对控制系统提出了更高的要求.一方面，足式机器人移动平台在未知的崎岖地形行走时会受到足端与地面非预期碰撞而产生的冲击力，影响平台稳定；另一方面，足式机器人也会由于机身与外界碰撞或搭载载荷而受到外力扰动，影响平台位姿.因此减少外力冲击对四足机器人稳定行走的影响十分重要.

为实现四足机器人的柔顺步态行走，一般在腿部设计被动柔顺机构或采用合适的主动柔顺控制策略.被动柔顺结构对四足机器人稳定性的提高有限，主动柔顺控制主要通过对四足机器人模型施加力控制算法来实现.上世纪80年代，Raibert使用弹簧负载倒立摆模型(SLIP)[1]实现单足机器人Hopper的稳定控制，后续很多四足机器人的控制策略包括著名的BigDog[2]都是在该模型的基础上发展而来的.

虚拟模型控制(Virtual Model Control，VMC)是一种描述足式机器人与周围环境交互的力行为语言.首先确定系统需要控制的自由度，然后在该自由度上构建恰当的虚拟构件以产生合适的虚拟力，建立整体质量-弹簧-阻尼虚拟模型，通过雅可比矩阵计算期望的关节扭矩.将机器人的移动轨迹用作用于机体质心的虚拟力去表征，并将质心虚拟力通过足力分配策略转换为关节扭矩，从而把四足机器人整机的移动映射为关节的扭矩.通过实时在线调整虚拟力，提高移动平台在复杂环境下的适应能力.HyQ[3]、StarlETH[4]等机器人均采用了虚拟模型控制.文献[5]设计了一种基于虚拟模型的四足机器人对角小跑步态控制器，在仿真环境中验证了四足机器人应对不平坦地形能力.文献[6]扩展了VMC，实现了围绕支撑对角线机体旋转的控制.文献[7]提出了一种CPG(Central Pattern Generator)结合VMC的生物启发运动控制器，实现四足机器人在仿真环境下的崎岖地面行走.日本的Matsuoka研究了生物神经细胞的适应现象，构成了Matsuoka神经元振荡器模型[8].日本的大多数研究者在四足机器人控制中采用的都是Matsuoka模型.

零力矩点ZMP(Zero Moment Point)由国外学者Vukobratovic提出，是地面上机器人的反应力矩与净惯性矩之和为零的点.当ZMP落在支撑面内，可保证机器人运动过程的稳定性，所以ZMP可以作为机器人的动态稳定判据.Kurazume等人基于ZMP方法提出了躯干的三维摆动曲线，在TITAN-VIII机器人上实现了稳定的对角小跑运动[9].文献[10]将ZMP理论与CPG控制系统结合在了一起，提出了一种用于仿真机器人的混合CPG-ZMP控制器.但目前ZMP方法下机器人运动速度偏慢，且抗扰性较差.本研究针对这一问题设计了一种基于ZMP判据的反应式行为策略，该策略采用一种基于非线性干扰观测器的滑模角度跟踪控制策略，增大了四足机器人在受到外力突变时的抗干扰能力.

1 基于ZMP判据的反应式行为策略设计

从运动学角度来看，负载突变主要影响四足机器人机身的高度和机身在空间中的姿态.若从动力学角度看，负载突变作用于机身可以分解为扰动力和力矩.很多传统的ZMP判据并没有引入机身动力学信息，将机身动力学信息引入ZMP判据中来，结合动力学模型，机器人在负载突变下的机身动力学同样可以描述为

(1)

式中：m代表机身的质量；Θ是机身的惯性张量；g是重力加速度；Fext和Text是负载突变下作用在质心处的合力和合力矩；a和ω分别是质心的线性加速度和角加速度。上标变量I指惯性坐标系，上标变量B指机身坐标系.

在一个平整的路面上，零力矩点(ZMP点)被定义为由重力和惯性力的合力投影到地面上的点。假设nI为地面平面的法向量，结合式(1)，可以得到式(2)关系式.

.

(2)

式中：RIB是从惯性坐标系到机身坐标系的欧拉旋转矩阵，r和rZMP表示质心和ZMP的位置.

使惯性坐标系的z轴和平面法向量的方向重合，式(2)中ZMP在平面上的分量为：

,

(3)

其中，xy(·)指一个量在xy平面上的分量，xyL由式(4)给出.

.

(4)

式中：zB是rI在z轴上的分量；RIB，i指RIB的第i行.

当机器人处于行走步态时，至少有3条腿接触地面构成支撑面.此时只有当ZMP落入到支撑面上才能保证行走的稳定性.但是当四足机器人处于一种迈腿频率更快的对角小跑(trot)步态时，同时只有两条腿接触地面，构成支撑线.只有当ZMP落在这条支撑线上，才可以认为此时机器人处于行走稳定状态.

将机器人的左前、右前、左后、右后4条腿足端与地面接触点Pi的位置表示为(xi，yi，zi)，i=1，…，4.假设下一次足端落地点Pi′坐标为(xi+δx，yi，zi)，即Pi沿着惯性坐标系x轴平移了δx，那么变化后的足端支撑线在xy平面可以表示为式(5).

(5)

根据trot步态的相位关系可知，i=1时j=4，或i=2时j=3.

为了使零力矩点落在机器人的支撑线上，根据点与直线的判断条件，将ZMP的坐标式(6)代入支撑线方程式(5)，得到式(7).

(6)

(7)

如图1(a)所示，计算出δx的值以后，就可以使ZMP落在调整后的支撑线上.但上述过程明显存在不足，那就是只调节落地点x轴方向上的位置.实际上，trot步态是一种不稳定步态，若受到外力扰动时，从x和y方向一起来调节足端落地点会使机器人反应更加迅速，提升机器人的抗扰性能，使机器人更快地恢复平稳.

假设足端落地点Pi′(xi+δx1，yi+δy1，zi)为上一周期的足端触地点Pi沿着机身坐标系x轴方向平移了δx1，又沿着y轴方向平移了δy1后得到，如图1(b)所示，此时相当于把上一周期足端支撑线l平移到了l′的位置，并且使支撑线经过ZMP.图1中θ是足端支撑线和机器人前进方向的夹角，x轴方向是机器人前进方向，y轴方向是机器人侧向方向.

由图1的几何关系可以得出：

δx1=δxsin2θ，δy1=δxsinθcosθ.

(8)

图1 两种调整支撑线的方法示意图

假设规划出的足端落地点都能符合机器人腿部本身的运动学约束，那么只要通过规划出足端下一次的落地点Pi′的位置(xi+δx1，yi+δy1，zi)，就能使足端落地后，机器人的ZMP落在支撑线上.若足端的调整位置超出了足端工作空间，则应在满足机器人运动学约束的情况下，尽可能地落在足端工作空间边界上使足端调整量达到最大.由机器人设计的尺寸和其正常行走实验可知θ<45°，此时满足δx1+δy1<δx.从x轴和y轴方向一起来调节足端落地点，其调整量总和比在单一的在x轴方向上调整的调整量减小，从而使修正后足端位置最大可能地满足足端工作空间的限制，进而提升足端位置调整后的抗扰性能并使机器人能够应对更大幅度的外力扰动.

四足机器人的trot步态对角线上的两条腿步态一致.假设T是运动周期，S是单腿的步距，H为髋部到足端最低点的距离，h为髋部到足端最高点的距离，δx1和δy1是基于ZMP的步态调整量，则经过改进后的单腿在3个方向上的足端轨迹如式(9)～式(11)所示.

(9)

(10)

.

(11)

足端落地点调整量还需要满足工作空间约束：

0≤S+δx1≤Xmax,h≤Ymax

.

(12)

式中：Xmax和Ymax为机器人足端在x轴和y轴方向上可以到达的极限距离.

4条腿的轨迹只要根据trot步态不同的相位就可以得到.只要保证机器人足端沿着基于ZMP规划的步态行走，就能保证机器人的稳定行走.通过机器人逆运动学就能求出期望的关节角度、角速度和角加速度.

2 关节角鲁棒跟踪控制策略

图2为控制系统结构框图.控制系统由外环和内环构成.外环基于ZMP规划出了四足机器人在负载突变下的稳定步态，并通过逆运动学计算出期望关节角信息.内环则为了实现期望关节角的跟踪.本研究采用了一种基于非线性干扰观测器的滑模角度跟踪控制策略.首先根据实际关节角度与给定的期望关节角度的误差，建立了一个滑模面；然后通过非线性干扰观测器尽可能地估计关节上存在的扰动，通过滑模补偿，来抵消扰动或者尽可能降低扰动带来的影响，从而提升关节角跟踪性能.

非线性干扰观测器设计为

.

(13)

为了克服观测器的不足之处，取

L(θ)=X-1M-1(θ)

(14)

.

(15)

式中：X为可逆矩阵，可通过线性矩阵不等式来求.

由式(14)、式(15)可求得

.

(16)

式(13)、式(14)和式(15)构成了非线性干扰观测器.

设计Lyapunov函数为

(17)

式中：M(θ)为对称正定矩阵.

对式(17)求导，可得

.

(18)

.

(19)

.

(20)

将式(14)代入式(20)，可得

(21)

将式(21)代入式(18)，可得

.

(22)

构造不等式：

.

(23)

式中：Γ为对称正定阵.

于是

.

(24)

可见，干扰观测器指数收敛，收敛速度取决于λΓmin值.λΓmin为Γ的最小特征值，λΓmin值越大，收敛速度越快.

由不等式(23)可见，式中含有非线性项，必须转化为线性矩阵不等式才能求解.令Y=X-1，将YT=(X-1)T和Y=X-1分别乘以式(23)的左右两边，得

(25)

即

(26)

YT+Y-YTΓY-I>ζI

.

(27)

.

(28)

通过Matlab下的YALMIP工具箱求解式(28)，便可以得到Y，从而得到X.能否求解该不等式取决于ζ和Γ值.ζ越小、Γ越小，越容易得到可行解.

采用观测器式(18)观测干扰d.在滑模控制中对干扰进行补偿，可有效地降低切换增益，从而有效地降低抖振.

(29)

对式(29)求导，可得

(30)

设计控制器为

(31)

将式(31)代入式(30)，并整理可得

(32)

(33)

(34)

(35)

由式(35)可得

(36)

V(t)a≤e-μ(t-t0)V(t0)

.

(37)

3 实验仿真

仿真先对比常规滑模控制、带有非线性干扰观测器的滑模控制和基于ZMP的带有非线性干扰观测器的滑模控制3种机制下，受到负载突变扰动的机器人机身姿态角的调整曲线；然后给出了基于ZMP的带有非线性干扰观测器的滑模控制策略作用下，机器人腿部关节角的跟踪曲线，最后给出了Adams虚拟样机仿真动画截图.

由于需要同时对多个参数变量进行实时检测、计算和调整并且需要模拟实际系统的动力学特性，控制过程相对复杂，因此选择使用Matlab2016a和Adams2017联合仿真技术来进行算法仿真，仿真环境设置如下：

第一步，在Adams里建立四足机器人的模型，四足机器人有4条结构一样的腿，每条腿有3个自由度.仿真中关节1代表髋关节侧摆关节，关节2代表髋关节前摆关节，关节3代表膝关节前摆关节.使4条腿关节1的初始位置为0°，关节2的初始位置为-30°，关节3的初始位置为60°，在每个关节处添加旋转副，在足端设置与地面的接触.

第二步，设置虚拟样机环境与时间触发函数.机器人0～1 s的时候处于四腿着地的稳定静止状态，1～8 s处于对角小跑步态(trot)正常行走.在距离机身右上方约0.5 m高的地方放置一个长方体重物，在第4 s的时候，触发重物释放函数，重物会呈自由落体砸到机身上.

第三步，建立Adams和Simulink交互的数据，Adams的输入就是Simulink的输出，Adams的输出就是Simulink的输入.设置Adams的输入为12个关节的力矩，Adams的输出为12个关节的角度和角速度、机身的姿态角和姿态角速度、机身的高度和在惯性坐标系下x轴和y轴方向的加速度.

第四步，设置仿真步长为0.001 s，仿真求解器为ode15s，动画模式为交互式，仿真模型为离散型，仿真时间为8 s.

控制器参数选取如下：Λ1=3I12×12，KD=5I12×12，ζ=2.6，即X=0.385.

为了突出基于ZMP的步态重规划和引入非线性干扰观测器对抵抗负载突变扰动的有效性，图3给出了负载突变下的机器人在3种不同控制机制下横滚角和俯仰角的调整曲线.仿真开始后，机器人正常行走，常规滑模控制在姿态角调整性能上略差于另外两种控制策略.第4 s负载落下，4.3 s落到机身，可以看出横滚角和俯仰角在4.3 s之后产生明显抖变，机器人发生倾覆，仅使用常规滑模控制策略无法保证机器人姿态稳定.而在另外两种控制策略作用下，机器人遇到负载突变扰动后都能保持较为稳定的行走.

图3(a)中，对比曲线2和曲线3，可以发现曲线3距离平衡位置的最大偏离值为3°，而曲线2距离平衡位置的最大偏移值有7.5°；曲线3的振荡幅值在4.3 s负载突变以后并不会有太大变化，而曲线2的振荡幅值在负载突变后振荡幅值变大，也即机身不稳定程度加深.对比可知曲线3所对应的控制策略更优，此仿真结果证明基于ZMP规划的步态使机器人在负载突变后姿态稳定性增强.

图3 3种控制策略下机器人的横滚角俯仰角曲线对比

图4给出了机器人在基于ZMP的四足仿生机器人反应式行为控制策略作用下左前腿三关节的期望角度和实际跟踪角度.每个关节的期望角度是通过基于ZMP理论规划出的步态和逆运动学求解出的.关节跟踪控制器为带有非线性干扰观测器的滑模控制器，可以看出该控制策略下关节角跟踪性能较好，在第4.3 s机器人负载突变时，由于负载突变给关节3带来急剧增大的压力，使得关节3的角度跟踪误差较大，误差达到0.1 rad，但之后实际关节角又能迅速跟踪上期望关节角信号，这表明所提出的控制策略是有效的.

在每个关节处施加一个幅值为5 N·m、周期为5 s的方波信号，对比在有无扰动观测器时的角度跟踪情况.实际得出的结论是，具有干扰观测器比没有干扰观测器的跟踪效果精度要高很多.

图4 机器人关节期望角度和实际跟踪曲线

图5给出了四足机器人Adams虚拟样机的仿真过程图，坠落负载的质量为11.7 kg，坠落高度为0.3 m，负载坠落位置在机器人机身质心前方.

从图5可以明显看出机器人正常行走的时候姿态平稳，在遭到负载撞击后姿态相对于正常行走时有小幅变化.通过观察足端轨迹曲线图，可明显看出机器人负载突变造成了四条腿的位置不对称.但0.5 s以内机器人便能恢复平稳，背负负载继续正常行走.

上述仿真结果表明了所提出的基于ZMP的步态重规划和关节角鲁棒控制策略在四足机器人应对负载突变的有效性，证明了组合控制策略鲁棒性优于任何一种单一的控制策略.

图5 负载突变下的四足机器人虚拟样机抗扰仿真过程图

4 结 论

针对四足机器人行进间负载突变下的稳定性问题，设计了一种基于ZMP稳定判据的步态自适应规划和底层带有非线性干扰观测器的关节角鲁棒滑模控制的反应式控制策略.Adams和Matlab联合仿真结果表明该方案的有效性.后续会在本方法的基础上，考虑研究加速减速等其他特殊工况下受到较大侧向干扰力等多应用场景下的四足机器人抗干扰能力规划与控制策略设计.