2. 陇东学院 机械工程学院, 甘肃 庆阳 745000;
3. 西北工业大学 机电学院, 陕西 西安 710072
双足机器人腿部运动由于其混合动力学、接触力的单向性约束以及双足机器人一般动力学的高维和非线性，使其步态规划与平衡控制相当复杂。一般双足的步态规划分为离线规划和实时在线规划2种方式，由于实时规划具有更好的鲁棒性，所以目前表现突出的知名机器人都采用实时规划进行运动控制。实时规划一个常用的思想是主要关注机器人的COM动力学。依据该思想，较常用的方法是将双足的走动简化成线性倒立摆(LIP)[1]的摆动。LIP模型较好地描述了双足步行的“宏观”动力学行为。基于LIP模型的COM动力学规划方法实现了几种双足步行机器人的有效行走[2-3]。
Vukobratovic等[4]1972年提出了ZMP(zero moment point)的概念，ZMP作为脚与地之间的垂直压力分布的抽象描述，可以有效地判定双足行走的稳定性。结合LIP模型，Wieber[5]提出了一种无轨迹模型预测控制器(MPC)，该控制器最小化COM抖动，实现了双足从强扰动中恢复。
根据LIP的动力学方程，可以计算特定的立足点，使双足COM的运动停止，系统动能等于0，双足处于完全静止状态，这一点被Pratt等人称为“CP”(caption point)[6]。CP又被Takenaka和Englsberger等[7-8]称作运动发散分量DCM。Takenaka提出了一种基于LIP动力学特征值分解的实时行走模式生成方法，给出了“收敛”和“发散”分量的定义，证明了“发散”分量等价于CP。Hopkins等[9]又将DCM的概念扩展到时变系统。目前COM的轨迹规划普遍采用事先样条插值将其插值出来[10]。本文基于DCM动力学，找到了一套从行走脚印到COM轨迹，再到关节角度求解的在线规划方法。具体完成了以下工作：①基于LIP模型推导了DCM动力学方程，在此方程的基础上，设计了2种抗干扰控制器，使运动发散分量在机器人行走过程中以及受到扰动后都不发散。②基于不发散的DCM，规划出了双足行走过程中的COM轨迹，在此轨迹约束下采用三次样条插值法又规划出了摆动腿末端的摆动轨迹。③基于规划出的COM和两腿末端轨迹，推导出了使用广义逆计算逆运动学的方法，行之有效地求出了在以上约束下的关节角度向量。基于LIP模型的DCM方法抓住了双足行走的主要矛盾，进而设计出相对简单的控制器，结合高效的逆运动学求解方法完成了双足的整套行走规划。本文的研究使双足步态规划的方法体系化，最后在一个普适机器人模型上验证了方法的有效性。
1 LIP动力学与DCM的推导 1.1 LIP模型将机器人抽象成图 1所示的平面二维倒立摆，倒立摆的摆杆长度可以伸缩并保持整体质心高度zc不变。应用如下假设：
|
| 图 1 平面二维倒立摆模型 |
1) 机器人的质量被集中于质心COM点；
2) 质心保持在恒定的高度zc；
3) 机器人踝关节无力矩输入，看成自由状态。
在二维x-z平面中, COM的位置坐标为Pcom=[x, zc]T, 支点P的位置坐标为P=[p, 0]T, 点P等效于机器人的ZMP点[11], y-z平面同理可得。
可以列写出倒立摆的动力学模型
|
(1) |
解出
|
(2) |
如果F可控, 使其满足Fcosθ=mg, 则
|
(3) |
式中, 
系统写成的二阶微分方程形式, 选择不同的p和系统初值都会得到一组x(t), 如果将多次摆动拼接在一起, 选择合适的p和系统初值可以组成一组连续的倒立摆行走步态。将(3)式写成状态空间形式
|
(4) |
式中，
|
(5) |
式中，
根据CP点的定义得
|
(6) |
得到
|
(7) |
将(3)式进行拉氏变换得到
|
(8) |
系统传递函数具有右半平面的极点, 系统不是渐进稳定。将G(s)改写为(9)式并画出系统框图(见图 2)
|
(9) |
|
| 图 2 LIP模型动力学拆分 |
根据稳定部分传递函数G2(s)求出ξ和x的关系
|
(10) |
对比(7)式和(10)式, 可以发现ξ和CP点具有相同的数学表达形式。图 2中x追踪ξ过程稳定, 而ξ追踪p过程不稳定, 故ξ被称为运动发散分量(DCM)。由于x追踪ξ过程稳定, 所以双足步态规划时只要ξ不发散, 机器人就可以稳定摆动下去。
2 DCM参考轨迹生成及跟踪控制 2.1 DCM参考轨迹的生成选择x和ξ作为系统状态变量, 系统动力学方程写成状态空间形式为
|
(11) |
式中，σ=[x ξ]T。
机器人一步摆动中支点p不变, p点即为机器人的行走脚印。DCM参考轨迹的生成需要一组规划好的脚印坐标pk, 人为设定双足机器人多步连续行走的最后一步结束时刻ξ点正好和p点重合, 误差为0。然后依据G1(s)就可反推回第一步, 那么双足整个行走步态中ξ都是不发散的。
将(11)式中的第二行即G1(s)写成离散状态方程ξk=F(ξk+1, pk)形式
|
(12) |
给出第k步的落脚点pk和第k+1步ξ的起始值ξk+1, 依据规则F可以算出一个时间跨度T前第k步ξ的起始值ξk, 以此类推。第k步ξ的终止值等于第k+1步ξ的起始值(见图 3)。求出了每一步ξ的起始值和落脚点pk依据G1(s)可以推出ξk→k+1(t), 这里求出的每一个T时间段内的ξk→k+1(t)都受控。
|
| 图 3 DCM规划方法 |
将(11)式中的第一行即G2(s)写成离散状态方程xk+1=H(xk, ξk)形式
|
(13) |
依据xk和ξk就可以推出T时间后的xk+1, 求出了每一步x的起始值xk依据G2(s)可以推出xk→k+1(t)。
概括基于后项递推方法规划DCM和COM轨迹的算法为:
1) 给出目标落脚点(脚印)P=[p1…pk…pn];
2) 由ξk+1和pk反推ξk, ξk=F(ξk+1, pk), k≥1;
3) 根据第k步初值ξk, pk和G1(s), 求出ξk→k+1(t);
4) 由xk和ξk推出xk+1, xk+1=H(xk, ξk);
5) 根据xk和ξk→k+1(t)求出xk→k+1(t)。
由ξk=F(ξk+1, pk)还可以推出ξk, ξk+1, ξk→k+1(t)和pk时刻都在同一条直线上, 由G2(s)可知
|
| 图 4 三维DCM和COM的基本移动 |
下面结合具体算例, 仿真算法的实际效果。在x-y平面中选择系统摆动5步, 取脚印pk=[(0.0, 0.0), (0.25, 0.3), (0.5, 0.0), (0.75, 0.3), (1.0, 0.0), (1.25, 0.3)](m), T=1 s, ω=4.427, ξ6=(1.25, 0.3)带入ξk=F(ξk+1, pk)可以求出ξ1→5, 使用递推算法, 算出ξk→k+1(t)和xk→k+1(t)轨迹如图 5所示, 实线为COM轨迹, 虚线为DCM轨迹。DCM是分段曲线，每一段表示一次摆动。可以看出图中ξk→k+1(t)一直处于可控状态, 经过一个时间跨度T趋于发散时进入下一摆。
|
| 图 5 二维DCM和COM步态间的移动 |
针对LIP动力学方程的不稳定部分, 有必要设计增强系统鲁棒性的负反馈控制器, 使双足系统在第k步受到扰动后依然能在一个时间跨度T内重回规划的ξk+1点处。
使用图 6来说明一步DCM终值不变抗扰动控制的思想。开始阶段pk在pk→k+1(0)处, 依据pk保持不变规划出的ξk、ξk+1和pk在同一条直线上如图 6左侧实线所示。如果系统不受外界扰动ξk→k+1(t)将随时间的推移沿直线从ξk运动到ξk+1处。但是如果系统受到了外界的扰动, ξk→k+1(t)偏移了左侧实线。扰动撤除后, 系统如果仍然希望ξk→k+1(t)能够在一个时间跨度T内运动到ξk+1, 那么pk必然需要移动以保持ξk→k+1(t)、ξk+1和pk→k+1(t)在同一条直线上, 如图 6所示。在这个pk→k+1(t)作用下, ξk→k+1(t)将会随时间的推移运动回ξk+1处, 与不受扰动相比能以相同的ξ初值进入下一摆, 下一摆系统继续依照规划运行下去。
|
| 图 6 一步DCM终值不变抗扰动控制思想 |
依据G1(s)求出
|
(14) |
将ξk→k+1(t)用ξk+1代替, ξk用当前实测系统的ξx(t)代替, (14)式进而可以改写为
|
(15) |
式中，ξk+1为第k步系统期望到达的终值, 为一常值, ξx(t)为系统实时反馈ξ值, b=eω(T-t), T为一步所需时间跨度。可以算出目标pk→k+1(t)值, 也即是期望ZMP轨迹, 控制ZMP点跟踪该pk→k+1(t)即可保证ξk→k+1(t)随着时间的推移仍归于ξk+1。画出系统的闭环系统框图如图 7所示。
|
| 图 7 一步DCM终值不变抗扰动控制框图 |
采用2.1节算例, matlab仿真得到
|
|
|
|
以第一步x方向为例, ξ1→2(t)从0.003运动到0.253完成一次x方向摆动, 如果系统受到外界扰动, 扰动力f′沿x轴正方向作用, 系统的动力学方程变为
|
(16) |
最终体现在对支点p的扰动。
应用DCM终值不变闭环控制算法, 使LIP在x, y正方向仍均受到相同大小的外界扰动力。按(15)式设计框图 7所示的控制器。仿真结果如图 8所示, 可以看到:受到扰动力后, DCM轨迹偏离规划，接入控制器后的DCM随着时间的推移重回规划。曲线最终回到了ξx, 2=0.253处。在x-y平面上DCM的轨迹如图 9所示, 在图中同时做出了未受扰动时的曲线, 可以看到一步结束时刻ξk→k+1(t)随着时间的推移仍归于规划的ξk+1。
|
| 图 8 闭环控制前后x方向DCM波形 |
|
| 图 9 闭环控制前后DCM和COM波形 |
相对于2.2节, 实时DCM轨迹跟踪闭环控制思想体现在:在一个行走步态中保证反馈ξx(t)实时跟踪规划的ξk→k+1(t)。(14)式中令ξk→k+1(t)保持不变, ξk使用反馈ξx(t)代替, 求出
|
(17) |
式中，ξk→k+1(t)为第k步系统规划出的DCM轨迹, 为一时间序列, ξx(t)为实时系统反馈ξ值, b=edT, dT为系统跟踪上目标ξ值所需要的时间, 注意dT和一步所需时间跨度T的区别。将图 7中的控制器部分写成数学公式的形式会发现:从ξk→k+1(t)到ξx(t)转变成了一个一阶惯性环节
|
(18) |
式中，
所以由于一阶惯性环节的存在, 不管ω′取何值, ξx(t)在同一时刻都无法跟踪上ξk→k+1(t), ξx(t)滞后ξk→k+1(t)dT时间。因此为了保证一步时间跨度T后, 不影响下一步的摆动, 可以将ξk→k+1(t+dT)作为t时刻的指令值, 即使系统滞后dT, 也能保证t时刻反馈ξx(t)=ξk→k+1(t)。
图 10说明了2.1节算例的第2步x方向加入扰动信号后控制器的控制效果。这里取b=1.1, 推出dT=0.021 5 s。可以看到:受到扰动后ξx(t)开始偏离ξk→k+1(t), 但是扰动去除后系统很快会在控制器的作用下重回规划。图中还给出了pk→k+1(t)的轨迹, 算出的pk→k+1(t)受到扰动偏离了pk点, 然后扰动去除后又重回pk点。
|
| 图 10 实时跟踪控制前后x方向DCM轨迹 |
在本节中, 将寻找依据规划出的COM轨迹和双脚运动轨迹求解机器人逆运动学的方法, 机器人模型如图 11所示。
|
| 图 11 用于动态行走的双足机器人 |
选择机器人髋关节中的一个坐标系作为机器人随体基坐标系o。世界坐标系固定于地面上某一点, 则基坐标系o相对于世界坐标系的位置为ro。将从坐标系o分出的2个运动链分别称为第一肢体和第二肢体, 单腿支撑阶段支撑腿称为第一肢体, 另一条腿称为第二肢体。双脚支撑阶段任选一条腿为第一肢体。第i肢体上具有ni≥6个活动关节。
第i肢体末端坐标系在世界坐标系中的位置为
|
(19) |
式中, ri∈R3×1, Ro∈R3×3是坐标系o到世界坐标系的转移矩阵, ro∈R3×1是坐标系o在世界坐标系中的位置。
第i肢体末端在坐标系o中的速度
|
(20) |
上标o表示相对于坐标系o而言, oJi∈R6×ni是雅克比矩阵, 
接下来求取第i个肢体末端在世界坐标系中的速度
|
(21) |
式中, [(·)×]是矢量积的反对称矩阵, 将其归一化写成
|
(22) |
|
∈R6×6, 
对于机器人不同的肢体, 都应该具有相同的坐标系o相对于世界坐标系的速度, 换句话说, 从(22)式可以看出, 所有的肢体都应该满足坐标系o速度相同的约束条件, 因此, 第i肢体和第j肢体肢体应该满足以下关系
|
(23) |
特别的有
|
(24) |
所以
|
(25) |
式中，Ji†为Ji的Moore-Penrose广义逆矩阵
|
(26) |
所以, 基于(25)式找到的
在世界坐标系中全身质心坐标可以表示为
|
(27) |
式中，c∈R3, oci表示第i肢体的质心在o坐标系中的坐标。接下来求取质心在世界坐标系中的速度
|
(28) |
式中，
显然
|
(29) |
式中，oci, k∈R3表示第i肢体中第k连杆的质心在o中的坐标向量。又根据雅克比矩阵的定义
|
(30) |
式中
|
mi, k表示第i肢体中第k连杆的质量。并将(25)式带入(28)式, 化简得
|
(31) |
又因为
|
(32) |
|
(33) |
式中，ωo表示坐标系o的旋转, ωi表示坐标系i的旋转, Jv1和Jω1分别为在世界坐标系上表示的基肢(第一肢体)雅可比J1的线速度部分和角速度部分。所以
|
(34) |
又因为基肢
|
(35) |
改写(35)式
|
(36) |
式中
|
|
将旋转
|
(37) |
这样就找到了基肢关节空间与质心坐标空间之间的关系, 结合(25)式, 即可求出在指定的质心运动轨迹下的逆解。
特别的对于双足步行机器人, 其只有2个运动链, 且质心保持平动, n1=n2=6, ωo=0有
|
(38) |
直接求解(38)式是很难完成的。这里采用数值解法, 将其写成差分形式
|
(39) |
对于给定了质心和双腿末端运动轨迹的双足机器人, 第一肢体末端与地面接触并保持不动, 由第二肢体参考轨迹和COM参考轨迹应用图 12可以快速求解出q1和q2。
|
| 图 12 双足机器人逆运动学的数值解法 |
本节将会在无扰动的情况下(有扰动时同理可得)对上述方法进行全面的仿真。运动学模型采用改进D-H法则建立, 具体结构尺寸参数以及质量属性见表 1。
| 部位 | 坐标系 | αi-1/rad | ai-1/mm | di/mm | θi/rad | 质心在相应连杆处的位置(x, y, z)/mm | 质量/kg |
| 右 腿 |
0(末端) | 0 | 0 | 0 | 0 | (-58.421, 0, 78.212)(脚) | 0.972 |
| 1(踝关节) | 0 | 80 | 0 | q1 | |||
| 2(踝关节) | π/2 | 0 | 0 | q2 | (259.056, 27.210, 0)(小腿) | 3.941 | |
| 3(膝关节) | π | 430.6 | 0 | q3 | (224.999, 12.271, 0)(大腿) | 4.47 | |
| 4(髋关节) | 0 | 440 | 0 | q4 | |||
| 5(髋关节) | π/2 | 0 | 0 | q5 | |||
| 6(髋关节) | 90 | 0 | 124.9 | q6 | (150.135, 0, 0)(身板) | 7.757 | |
| 左 腿 |
7(髋关节) | 0 | 300 | -124.9 | q7 | ||
| 8(髋关节) | π/2 | 0 | 0 | q8 | |||
| 9(髋关节) | π/2 | 0 | 0 | q9 | (150.135, 0, 0)(大腿) | 4.47 | |
| 10(膝关节) | π | 440 | 0 | q10 | (150.135, 0, 0)(小腿) | 3.941 | |
| 11(踝关节) | π | 430.6 | 0 | q11 | |||
| 12(踝关节) | -π/2 | 0 | 0 | q12 | |||
| 13(末端) | 0 | 80 | 0 | 0 | (150.135, 0, 0)(脚) | 0.972 |
依据第2节方法, 从pk=[(0.0, 0.0), (0.3, 0.3), (0.6, 0.0), (0.9, 0.3), (1.2, 0.0)](m)脚印得到了DCM轨迹, 进而生成了x-y平面COM轨迹, 对于步行单脚支撑阶段, 第一肢体末端与地面接触, 并固定不动, 第二肢体末端轨迹采用三次样条插值法生成, 所以在空间中会有如图 13所示参考轨迹。
|
| 图 13 双足机器人步行目标轨迹 |
规划出了所需目标轨迹后, 就可以应用第3节图 12所示逆运动学的数值解法, 求出具体的关节角度, 由于双足的行走具有周期性, 所以不妨以一次摆动为例见图 14。图中实线为参考轨迹, 点画线展示了通过逆运动学求解得到关节角度后计算正运动学在操作空间中的实际COM和摆动脚轨迹。可以看到任意给出一个系统初始姿态, 通过几次迭代后, 2条曲线都收敛到参考轨迹上。
|
| 图 14 一步行走逆运动学求解轨迹跟踪曲线 |
本文详细研究了一种基于DCM动力学的双足机器人步行规划与控制方法。DCM方法直观地将LIP描述为2个级联的一阶系统，通过设置期望的行走脚印和期望的DCM步尾位置，规划出期望的DCM轨迹，然后设计了2种DCM闭环控制器求出了期望的ZMP轨迹。利用ZMP作为控制信号，可以稳定双足机器人，并将DCM移动到所需位置。讨论研究了基于规划的COM轨迹求解机器人逆运动学的问题。最终方法解决了机器人步行从脚印到关节角度的整套求解问题。
| [1] | KAJITA S, KANEHIRO F, KANEKO K, et al. The 3D Linear Inverted Pendulum Mode: a Simple Modeling for a Biped Walking Pattern Generation[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, Hawaii, 2001: 239-246 |
| [2] | CHOI Y, KIM D, OH Y, et al. Posture/Walking Control for Humanoid Robot Based on Kinematic Resolution of COM Jacobian with Embedded Motion[J]. IEEE Trans on Robotics, 2007, 23(6): 1285-1293. DOI:10.1109/TRO.2007.904907 |
| [3] | KAJITA S, KANEHIRO F, KANEKO K, et al. Biped Walking Pattern Generation by Using Preview Control of Zero-Moment Point[C]//IEEE International Conference on Robotics & Automation, Taipei, 2003: 1620-1626 |
| [4] | VUKOBRATOVIC M, STEPANENKO J. On the Stability of Anthropomorphic Systems[J]. Mathematical Biosciences, 1972, 15(1): 1-37. |
| [5] | WIEBER P B. Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations[C]//IEEE-RAS International Conference on Humanoid Robots, Genova, 2006: 137-142 |
| [6] | PRATT J, CARFF J, DRAKUNOV S, et al. Capture Point: a Step Toward Humanoid Push Recovery[C]//IEEE-RAS International Conference on Humanoid Robots. Genova, 2006: 200-207 |
| [7] | TAKENAKA T, MATSUMOTO T, YOSHIIKE T, et al. Real Time Motion Generation and Control for Biped Robot, 1st Report: Walking Gait Pattern Generation[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, 2009: 1084-1091 |
| [8] | ENGLSBERGER J, OTT C. Three-Dimensional Bipedal Walking Control Using Divergent Component of Motion[J]. IEEE Trans on Robotics, 2015, 31(2): 355-368. DOI:10.1109/TRO.2015.2405592 |
| [9] | HOPKINS M A, HONG D W, LEONESSA A. Humanoid Locomotion on Uneven Terrain Using the Time-Varying Divergent Component of Motion[C]//IEEE-RAS International Conference on Humanoid Robots, Madrid, Spain, 2014: 266-272 |
| [10] |
于昂可.液压驱动双足机器人机构设计及其步态仿真[D].杭州: 浙江大学, 2015 YU Angke. Mechanism Design and Gait Simulation of Hydraulically Driven Biped Robot[D]. Hangzhou, Zhejiang University, 2015(in Chinese) |
| [11] | ENGLSBERGER J, OTT C. Integration of Vertical COM Motion and Angular Momentum in an Extended Capture Point Tracking Controller for Bipedal Walking[C]//IEEE-RAS International Conference on Humanoid Robots, Osaka, 2012: 183-189 |
2. College of Mechanical Engineering, Longdong University, Qingyang 745000, China;
3. School of Mechanical Engineering, Northwestern Polytechnical University, Xi'an 710072, China
