四足机器人在负载突变下基于ZMP理论的反应式鲁棒控制
方法
技术领域
[0001] 本
发明属于机器人控制技术，具体为一种四足机器人在负载突变下基于 ZMP理论的反应式鲁棒控制方法。
背景技术
[0002] 在波士顿动
力研发出BIGDOG后，四足机器人受到了来自越来越多研究界的关注。与传统的轮式和
履带式机器人不同的是，四足机器人行走时与地面的
接触是点式的，所以可以在崎岖不平的山路、泥沼或阶梯等复杂恶劣的环境下移动，且在移动速度、负载能力、
稳定性等方面表现出诸多优势。四足机器人在实际环境中往往会面对各种各样复杂的情况，当四足机器人受到负载突变扰动时，
机身高度降低，机身
姿态倾斜，影响机器人的行走稳定性，严重的时候甚至会造成机身的倾覆，其鲁棒控制问题比较复杂，至今仍没有有效的控制方法被提出。
发明内容
[0003] 本发明的目的在于提出了一种
四足机器人在负载突变下基于ZMP理论的反应式鲁棒控制方法，以保证机器人的稳定行走。
[0004] 实现本发明的技术解决方案为：一种四足机器人在负载突变下基于ZMP理论的反应式鲁棒控制方法，具体步骤为：
[0005] 步骤1、建立四足机器人带扰动项的腿部动力学模型和机身动力学模型，通过机身与四条腿的耦合点，将腿部动力学模型和机身动力学模型结合起来，建立带有扰动项的完整四足机器人动力学模型；
[0006] 步骤2、根据步骤1中的机身动力学模型确定负载突变下ZMP的计算公式；
[0007] 步骤3、基于ZMP规划出四足机器人在负载突变过程中的稳定对
角小跑步态；并根据规划出的稳定对角小跑步态，通过逆运动学计算出期望关节角；
[0008] 步骤4、根据期望关节角，设计带有非线性干扰观测器的滑模
跟踪控制器，观测器观测出干扰值，通过滑模控制对干扰进行补偿，实现关节的跟踪控制。
[0009] 优选地，步骤1建立的四足机器人带扰动项的单腿动力学模型为：
[0010]
[0011] 其中， 是
关节角度向量， 是对称半正定惯性矩阵， 是
向心力和科氏力项， 是重力项， 是关节力矩， 是扰动力矩， 是
足端接触力雅克比矩阵， 是足端和地面的接触力向量， 是腿
部和机身的耦合点到关节空间的雅克比矩阵， 是腿部和机身接触点的耦合力，若单腿处于摆动相时地面接触力Fg等于零。
[0012] 优选地，步骤1建立的四足机器人机身动力学模型为：
[0013]
[0014] 其中， 表示身体的
位置/姿态矢量， 为广义力矢量， 为对称半正定惯性矩阵， 为科氏力和向心力矩阵， 为重力矢量。
[0015] 优选地，步骤1建立的带有扰动项的完整四足机器人动力学模型为：
[0016]
[0017] 将 定义为 其中Joi(xo)是从机身
坐标系到第i条腿关节1的雅可比矩阵，令L＝Jo+(xo)Je(q)，则上述模型中的参数为：
[0018]
[0019]
[0020]
[0021] 优选地，步骤2中四足机器人在负载突变下的机身动力学等价为：
[0022]
[0023] 其中，m代表机身的
质量，Θ是机身的惯性张量，g是重力向量，Fext和Text是负载突变下作用在CoM处的合力和合力矩，a和ω分别是CoM的线性
加速度和
角加速度，所有变量的上标中，坐标系I是惯性坐标系，坐标系B是机身坐标系。
[0024] 优选地，根据负载突变下的机身动力学确定如下关系：
[0025]
[0026] 其中，RIB是欧拉旋转矩阵从惯性坐标系到机身坐标系，r和rZMP是CoM和ZMP 的位置；
[0027] 使惯性坐标系的z轴和平面法向量的方向重合，得到负载突变下ZMP在x和 y上的计算公式为：
[0028]
[0029] 其中，zB是rI在z轴上的分量，xy(·)指一个量在某个平面的分量，xyL由下式给出：
[0030]
[0031] 其中，RIB,i指RIB的第i行元素。
[0032] 优选地，步骤3中基于ZMP规划出四足机器人在负载突变过程中的稳定步态，具体步骤如下：
[0034] 机器人的四条腿从左前、右前、左后、右后腿分别编号为1到4，第i条腿足端与地面接触点PiI的位置为(xi,yi,zi)(i＝1,...4)，假设下一次足端落地点PiI′坐标为(xi+δx,yi,zi)，即PiI沿着惯性坐标系x轴平移了δx，zi为机身CoM距离地面的高度，变化后的足端支撑线在xy平面表示为：
[0035]
[0036] 步骤32、根据点在直线上定理，求解位移量，具体为：
[0037] 将ZMP的坐标代入足端支撑线方程，如下式：
[0038]
[0039] 解得位移量：
[0040]
[0041] 步骤33、根据位移量，优化足端轨迹与落足点，具体为：
[0042] 假设足端落地点Pi(xi+δx1,yi+δy1,zi)既沿着机身坐标系x轴方向平移了δx1，又沿着y轴方向平移了δy1，则由几何关系，有以下关系：
[0043] δx1＝sin2θδx，δy1＝sinθcosθδx。
[0044] 四足机器人的trot步态对角线上的两条腿步态一致，T是运动周期，S是单腿的步距，H为髋部到足端最低点的距离，h为髋部到足端最高点的距离，δx1和δy1是基于ZMP的调整量，规划出的足端落地点都能符合机器人腿部本身的运动学约束，只要通过规划出足端下一次的落地点Pi′的位置 (xi+δx1,yi+δy1,zi)，就能使足端落地后，机器人的ZMP落在支撑线上，从而保证机器人的稳定行走，Trot步态在三个方向上的足端轨迹如下：
[0045]
[0046]
[0047]
[0048] 优选地，步骤3中的期望关节角为：
[0049]
[0050]
[0051]
[0052] 其中，L1、L2、L3为四足机器人单腿
连杆1、连杆2和连杆3的长度，b＝-2XtrotL2，
[0053] 优选地，步骤4根据期望关节角，设计带有非线性干扰观测器的滑模跟踪控制器，观测器观测出干扰值，通过滑模控制对干扰进行补偿，实现关节的跟踪控制具体过程如下：
[0054] 步骤41、设计非线性干扰观测器，具体为：
[0055]
[0056]
[0057]
[0058] 步骤42、通过线性矩阵不等式求解非线性干扰观测器的增益矩阵X，具体为：
[0059] 令Y＝X-1，根据Schur补定理，LMI求解式为：
[0060]
[0061] 其中，ζ为 的上界，Γ为一个对称正定矩阵，X＝Y-1。
[0062] 步骤43、设计带有扰动补偿的滑模控制器，具体为：
[0063] 令Λ＝λ1I12×12，定义滑模函数为：
[0064]
[0065] 假设关节的期望输入角度为qd，定义跟踪误差e＝q-qd，取辅助变量即有 设计的带有扰动补偿的滑模控制器为：
[0066]
[0067] 其中， K＝λ2I12×12。
[0068] 本发明与
现有技术相比，其显著优点为：本发明对负载突变这一特定扰动具有较强的针对性，使得四足机器人在负载突变下，能恢复平衡并且继续平稳的以 trot步态行走，控制策略有较强鲁棒性。
[0069] 下面结合
附图对本发明做进一步详细的描述。
附图说明
[0070] 图1是四足机器人结构和单腿坐标系。
[0072] 图3是δx分解为δx1和δy1示意图。蓝色的点是足端触地点，红色的点是 ZMP，θ是足端支撑线和机器人前进方向的夹角。
[0073] 图4是三种控制策略下机器人
横滚角的绝对值曲线。
[0074] 图5是三种控制策略下机器人
俯仰角的绝对值曲线。
[0075] 图6是
机器人关节1期望角度和实际跟踪曲线。
[0076] 图7是机器人关节2期望角度和实际跟踪曲线。
[0077] 图8是机器人关节3期望角度和实际跟踪曲线。
[0078] 图9是MATLAB和ADAMS联合仿真中的机器人虚拟样机截图。
具体实施方式
[0079] 一种四足机器人在负载突变下基于零力矩点(Zero Moment Point，ZMP)理论的反应式鲁棒控制方法，具体步骤为：
[0080] 步骤1、建立带扰动项的四足机器人完整动力学模型；
[0081] 如图1，考虑四足机器人的每条腿有三个
自由度，机器人突加负载时候受到的外力扰动可以部分折算到关节上的扰动力矩，带有扰动项的第i条腿 (i＝1,...k)的动力学模型可以描述为：
[0082]
[0083] 其中， 是关节角度向量， 是对称半正定惯性矩阵， 是向心力和科氏力项， 是重力项， 是关节力矩， 是扰动力矩，
是足端接触力雅克比矩阵， 是足端和地面的接触力向量， 是
腿部和机身的耦合点到关节空间的雅克比矩阵， 是腿部和机身接触点的耦合力，若单腿处于摆动相时地面接触力Fg等于零。
[0084] 所有腿的动力学可以表示为：
[0085]
[0086] 其中，
[0087] 令L1、L2、L3为四足机器人单腿连杆1、连杆2和连杆3的长度，m1、m2、 m3为连杆1、连杆2、连杆3的质量。模型具体参数如下：
[0088]
[0089]
[0090]
[0091]
[0092]
[0093]
[0094]
[0095]
[0096]
[0097]
[0098] J12＝L2cosθi2+L3cos(θi2+θi3)
[0099] J13＝L3cos(θi2+θi3)
[0100] J21＝cosθi1(L1+L2cosθi2+L3cos(θi2+θi3))
[0101] J22＝-sinθi1(L2sinθi2+L3sin(θi2+θi3))
[0102] J23＝-L3sinθi1sin(θi2+θi3)
[0103] J31＝sinθi1(L1+L2cosθi2+L3cos(θi2+θi3))
[0104] J32＝cosθi1(L2sinθi2+L3sin(θi2+θi3))
[0105] J23＝L3cosθi1sin(θi2+θi3)
[0106] 用 表示身体的位置/姿态矢量，四足机器人机身的动力学方程由作用在机身质心(COM)的广义力矢量 包括机器人负载突变下作用在CoM 处的合外力和合外力矩，对称半正定惯性矩阵 科氏力和向心力矩阵 以及重力矢量 组成。机身动力学方程描述为：
[0107]
[0108] 机身动力学的具体参数如下：
[0109] Gb＝[0 0 -mg 0 0 0]T其中，Mφ＝ATIbA，Ib是机身质心的
转动惯量，m是机身质量， 其中φB＝
[φx,φy,φz]T分别代表了机身的横滚，俯仰，
偏航角。
[0110] 将 定义为 其中Joi(xo)是从机身坐标系到第i条腿关节1的雅可比矩阵。由相互作用力可知，Fo可写为：
[0111] Fo＝-JoT(xo)Fe (1.4)
[0112] 由式(1.4)可知，腿部支撑力Fe可表示为：
[0113] Fe＝-JoT(xo)+Fo (1.5)
[0114] 其中 是JoT(xo)的伪逆矩阵。将(1.5)代入(1.2)，我们得到
[0115]
[0116] 令 为机身与第i条腿的耦合点的位置矢量。由雅可比矩阵Jei(qi)可以得到xie与qi的关系为：
[0117]
[0118] 同理， 与 的关系为：
[0119]
[0120] 结合(1.7)和(1.8)，可以获得第i条腿的关节速度与机身
位姿速度之间的关系：
[0121]
[0122] 假设腿
工作空间足够合理以保证雅可比矩阵Jei(qi)存在逆矩阵。考虑所有腿同时作用在机身，可以得到：
[0123]
[0124] 式(1.10)关于时间t求微分得到：
[0125]
[0126] 将式(1.10)和(1.11)代入到式(1.3)得到：
[0127]
[0128] 令L＝Jo+(xo)Je(q)，则 在式(1.12)两边均左乘LT，得到：
[0129]
[0130] 将式(1.13)和式(1.6)合并，可以得到完整的动力学方程为：
[0131]
[0132] 其中：
[0133]
[0134]
[0135]
[0136] 步骤2、结合机身动力学推导出负载突变下ZMP的计算公式；
[0137] 从运动学角度来看，负载突变主要影响四足机器人机身的高度和机身在空间中的姿态。若从动力学角度看，负载突变作用于机身可以分解为扰动力和力矩。将机身动力学信息引入ZMP判据中来，结合四足机器人动力学模型，式(1.3)中机器人在负载突变下的机身动力学同样可以描述为：
[0138]
[0139] 其中，m代表机身的质量，Θ是机身的惯性张量，g是重力向量，Fext和Text是负载突变下作用在CoM处的合力和合力矩，a和ω分别是CoM的线性加速度和角加速度。所有变量的上标中，坐标系I是惯性坐标系，坐标系B是机身坐标系。
[0140] 在一个平整的路面上，ZMP被定义为由重力和
惯性力合力投影到地面上的点。假设地面平面的法向量nI，结合式(2.1)，可以得到以下关系：
[0141]
[0142] 其中，RIB是欧拉旋转矩阵从惯性坐标系到机身坐标系，r和rZMP是CoM和ZMP 的位置。使惯性坐标系的z轴和平面法向量的方向重合，式(2.2)中ZMP在x和y 上的分量可以得出
[0143]
[0144] 其中，zB是rI在z轴上的分量，xy(·)指一个量在某个平面的分量，xyL由下式给出[0145]
[0146] 其中，RIB,i指RIB的第i行。
[0147] 步骤3、基于ZMP规划出四足机器人在负载突变过程中的稳定的步态；
[0148] 步骤31、trot步态设计；
[0149] 步态是各腿在时间和空间上的运动顺序，平整地面上多为周期步态。当腿与地面接触时，腿的状态为支撑相，腿在空中摆动时状态为摆动相。对角小跑步态 (Trot)是稳定性极高的动步态，其对角线上的腿运动一致。
[0150] 对角小跑步态占空比为0.5，即每个时刻都有两条腿处于着地状态，另外两条腿处于空中摆动状态。
[0151] Trot步态时，对角线上两足运动状态一致，支撑相两足相对于各自髋部的速度是一致的，足端轨迹采用多项式形式，四足机器人单腿摆动相轨迹如下：
[0152]
[0153]
[0154]
[0155] 其中，T是运动周期，S是单腿的步距，H为髋部到足端最低点的距离，h为髋部到足端最高点的距离
[0156] 步骤32、基于ZMP稳定判据的落地点选择；
[0157] 四足机器人处于trot步态时，只有两条腿同时接触地面，构成支撑线。只有当ZMP点落在这条支撑线上，才可以认为此时机器人现在处于行走稳定状态。
[0158] 机器人的四条腿从左前、右前、左后、右后腿分别编号为1到4，第i条腿足端与地面接触点PiI的位置为(xi,yi,zi)(i＝1,...4)。假设下一次足端落地点PiI′坐标为(xi+δx,yi,Izi)，即Pi沿着惯性坐标系x轴平移了δx。因为在行走过程中 ZMP和机器人足端支撑线都在地面，所以zi就是机身CoM距离地面的高度。变化后的足端支撑线在xy平面可以表示为式[0159]
[0160] 根据trot步态的
相位关系可知，i＝1时j＝4，或i＝2时j＝3。
[0161] 为了使ZMP落在机器人的支撑线上，根据点与直线的判断条件，将ZMP 的坐标代入支撑线方程，如下式
[0162]
[0163] 解得
[0164]
[0165] 这样，就可以计算出δx的值，从而使ZMP点落在支撑线上。但上述过程明显存在不足，那就是只需要调节落地点x轴方向上的位置，实际的trot步态中，需要从x和y方向一起来调节足端落地点。假设足端落地点Pi(xi+δx1,yi+δy1,zi)既沿着机身坐标系x轴方向平移了δx1，又沿着y轴方向平移了δy1，如图2(b)。图2 的几何关系可以得出：
[0166] δx1＝sin2θδx (3.7)
[0167] δy1＝sinθcosθδx (3.8)
[0168] 假设规划出的足端落地点都能符合机器人腿部本身的运动学约束，只要通过规划出足端下一次的落地点Pi′的位置(xi+δx1,yi+δy1,zi)，就能使足端落地后，机器人的ZMP在支撑线上，基于ZMP四足机器人负载突变下稳定的trot步态轨迹为：
[0169]
[0170]
[0171]
[0172] 步骤33四足机器人逆运动学
[0173] 根据机器人的D-H坐标系建立规则，建立四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型。四足机器人各条腿的D-H坐标系如图1 所示。通过齐次坐标变换可以得到第i条腿足端位置和关节角度关系如下：
[0174]
[0175] 若已知足端规划的轨迹，通过联立上面三个方程，可以通过逆运动学解出三个关节的期望角度：
[0176]
[0177] 其中， b＝-2XtrotL2，
[0178] 步骤4、设计非线性干扰观测器，利用观测器测出的干扰值，通过滑模控制对干扰进行补偿，实现关节角的跟踪控制；
[0179] 提出了一种基于非线性干扰观测器的滑模角度跟踪控制策略。图3描述了用于抑制扰动的非线性干扰观测器和计算力矩法的滑模控制的结构框图。四足机器人先规划好步态，给出关节期望的角度与
角速度。再根据与实际关节角度跟踪的误差，建立了一个滑模面。然后通过非线性干扰观测器尽可能的估计关节上存在的扰动，通过滑模补偿，来抵消扰动或者尽可能降低扰动带来的影响。最后基于 ZMP修正并规划出下一周期的步态。
[0180] 步骤41、干扰观测器的设计与稳定性分析
[0181] 非线性干扰观测器(Nonlinear Disturbance Observer，NLDO)设计为[0182]
[0183] 干扰观测器的增益矩阵取
[0184]
[0185] 其中，X∈R3k×3k是一个常数可逆的矩阵。
[0186] 由于角加速度
信号的难以获得，向量 可以由改进的观测器增益矩阵 L(q)得到：
[0187]
[0188] 结合式(4.2)和(4.3)，可得：
[0189]
[0190] 从而，式(4.1)、(4.2)和(4.4)构成了完整的非线性干扰观测器。
[0191] 设计李雅普诺夫函数
[0192]
[0193] 由于 为对称正定矩阵，X也是可逆矩阵，所以推出 也是正定的。从而李雅普诺夫函数V0也是正定的。观测误差定义为
[0194] 取式(4.5)对时间的导数，得到
[0195]
[0196] 由观测器式(4.1)可得
[0197]
[0198] 假设干扰力矩d为常值扰动，则有 在机器人学的文献中，这样的假设很常见。因而可以通过式(4.7)得到观测误差方程为
[0199]
[0200] 这等价于：
[0201]
[0202] 将式(4.9)代入式(4.6)，得到
[0203]
[0204] 假设存在一个对称正定的矩阵Γ满足如下不等式：
[0205]
[0206] 于是
[0207]
[0208] 由式(4.10)和(4.11)可知， 对于所有的观测误差 都负定，即：对于都有
[0209] 继续考虑李雅普诺夫函数的收敛速度问题，由式(4.5)可以得出：
[0210]
[0211] 定义 的下确界为σ1，上确界为σ2，则 由式(4.13)可以得出如下方程：
[0212]
[0213] 同理，由式(4.12)可以得出：
[0214]
[0215] 联立(4.14)与(4.15)，得到
[0216]
[0217] 所以，李雅普诺夫标量函数V0的最小收敛速率为
[0218] 由式(4.5)可以得到：
[0219]
[0220] 联立式(4.16)、(4.17)，可得：
[0221]
[0222] 式(4.18)左右两边同时开根号，就可以得出干扰观测器跟踪信号误差最小收敛速率为λmin(Γ)/2σ1||X||2。由此得出，λmin(Γ)越大，||X||越小，观测器跟踪误差最小收敛速率越大。
[0223] 步骤42、LMI不等式的求解；
[0224] 由不等式(4.11)可见，式中含有非线性项，必须转化为线性矩阵不等式(Linear Matrix Inequation，LMI)才能求解。令Y＝X-1，将YT＝(X-1)T和Y＝X-1分别乘以式(4.11)的左右两边，得
[0225]
[0226] 即
[0227]
[0228] 定义 的上界为ζ，则 可以推出 则式(4.20)成立的充分条件为
[0229] YT+Y-ζI-YTΓY≥0 (4.21)
[0230] 根据Schur补定理，则上式(4.21)等价为
[0231]
[0232] 通过MATLAB下的LMI控制工具箱求解式(4.22)，便可以得到Y，从而得到X。
[0233] 步骤43、计算力矩法的滑模控制器设计
[0234] 采用观测器式(4.1)观测干扰d，在滑模控制(Sliding-mode Control，SMC) 中对干扰进行补偿，可有效地降低切换增益，从而有效地降低抖振。
[0235] 假设关节的期望输入角度为qd，定义跟踪误差e＝q-qd，取辅助变量即有
[0236] 其中，Λ＝λ1I12×12，λ1＞0。定义滑模函数为
[0237]
[0238] 于是
[0239]
[0240] 设计控制器为
[0241]
[0242] 从而有
[0243]
[0244] 其中， K＝λ2I12×12，λ2＞0。
[0245] 由于 为正定阵，设计
闭环系统Lyapunov函数为
[0246]
[0247] 由于干扰观测器指数收敛，则 取 则有
[0248]
[0249] 由于 为一个反对称矩阵，则有
[0250]
[0251] 由上式可得
[0252]
[0253] 其中， λKmin和 分别为K和 的最小和最大特征值。
[0254] 采用不等式求解式(4.30)，不等式方程 的解为
[0255]
[0256] 从而可得，当t→∞时，滑模函数s趋近于零，即 且指数收敛，收敛速度取决于参数μ的值。
[0257] 通过本发明提出了一种负载突变情况下的四足机器人反应式鲁棒控制方法，通过计算基于ZMP稳定判据的落足点，来保证四足机器人的稳定性，建立如图 3所示的控制结构，实现规划腿部运动轨迹的跟踪。负载在仿真第4s坠落撞击到机身。图4、5为三种控制方式对姿态信息的影响，从图中可以看出，采用本发明的基于ZMP的反应式鲁棒控制方法，姿态角在扰动影响下变化程度更小，说明机器人稳定性也越好。图6、图7、图8为四足机器人左前腿三关节的期望角度和实际跟踪角度。每个关节期望的角度是通过基于ZMP规划出的步态和逆运动学求解出的，关节跟踪控制器采用带有非线性干扰观测器的滑模控制器。从三幅图中可以看出，三个关节角跟踪性能较好，当机器人受到负载突变时，只有关节3的跟踪误差较大，但之后关节迅速跟踪上期望关节角信号。图9通过对负载突变下的四足机器人进行MATLAB和ADAMS联合仿真，验证了本文所提出的控制策略的有效性。
