Open Access
Issue
JNWPU
Volume 37, Number 6, December 2019
Page(s) 1285 - 1293
DOI https://doi.org/10.1051/jnwpu/20193761285
Published online 11 February 2020

© 2019 Journal of Northwestern Polytechnical University. All rights reserved.

Licence Creative Commons
This is an Open Access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/4.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

双足机器人腿部运动由于其混合动力学、接触力的单向性约束以及双足机器人一般动力学的高维和非线性,使其步态规划与平衡控制相当复杂。一般双足的步态规划分为离线规划和实时在线规划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) 机器人的质量被集中于质心COM点;

2) 质心保持在恒定的高度zc

3) 机器人踝关节无力矩输入,看成自由状态。

在二维x-z平面中, COM的位置坐标为Pcom=[x, zc]T, 支点P的位置坐标为P=[p, 0]T, 点P等效于机器人的ZMP点[11], y-z平面同理可得。

可以列写出倒立摆的动力学模型

解出

如果F可控, 使其满足Fcosθ=mg, 则。又因为zc=const, LIP模型x轴方向上线性化为

式中,

系统写成的二阶微分方程形式, 选择不同的p和系统初值都会得到一组x(t), 如果将多次摆动拼接在一起, 选择合适的p和系统初值可以组成一组连续的倒立摆行走步态。将(3)式写成状态空间形式

式中,求出上式的解析解为

式中,

thumbnail 图1

平面二维倒立摆模型

1.2 DCM的定义与推导

根据CP点的定义得

得到

将(3)式进行拉氏变换得到

系统传递函数具有右半平面的极点, 系统不是渐进稳定。将G(s)改写为(9)式并画出系统框图(见图 2)

根据稳定部分传递函数G2(s)求出ξx的关系

对比(7)式和(10)式, 可以发现ξ和CP点具有相同的数学表达形式。图 2x追踪ξ过程稳定, 而ξ追踪p过程不稳定, 故ξ被称为运动发散分量(DCM)。由于x追踪ξ过程稳定, 所以双足步态规划时只要ξ不发散, 机器人就可以稳定摆动下去。

thumbnail 图2

LIP模型动力学拆分

2 DCM参考轨迹生成及跟踪控制

2.1 DCM参考轨迹的生成

选择xξ作为系统状态变量, 系统动力学方程写成状态空间形式为

式中,σ=[x  ξ]T

机器人一步摆动中支点 p不变, p点即为机器人的行走脚印。DCM参考轨迹的生成需要一组规划好的脚印坐标 pk, 人为设定双足机器人多步连续行走的最后一步结束时刻ξ点正好和p点重合, 误差为0。然后依据G1(s)就可反推回第一步, 那么双足整个行走步态中ξ都是不发散的。

将(11)式中的第二行即G1(s)写成离散状态方程ξk=F(ξk+1, pk)形式

给出第k步的落脚点pk和第k+1步ξ的起始值ξk+1, 依据规则F可以算出一个时间跨度T前第kξ的起始值ξk, 以此类推。第kξ的终止值等于第k+1步ξ的起始值(见图 3)。求出了每一步ξ的起始值和落脚点pk依据G1(s)可以推出ξkk+1(t), 这里求出的每一个T时间段内的ξkk+1(t)都受控。

将(11)式中的第一行即G2(s)写成离散状态方程xk+1=H(xk, ξk)形式

依据xkξk就可以推出T时间后的xk+1, 求出了每一步x的起始值xk依据G2(s)可以推出xkk+1(t)。

概括基于后项递推方法规划DCM和COM轨迹的算法为:

1) 给出目标落脚点(脚印)P=[p1pkpn];

2) 由ξk+1pk反推ξk, ξk=F(ξk+1, pk), k≥1;

3) 根据第k步初值ξk, pkG1(s), 求出ξkk+1(t);

4) 由xkξk推出xk+1, xk+1=H(xk, ξk);

5) 根据xkξkk+1(t)求出xkk+1(t)。

ξk=F(ξk+1, pk)还可以推出ξk, ξk+1, ξkk+1(t)和pk时刻都在同一条直线上, 由G2(s)可知, 即ξ-x同向, x(t)的速度方向始终指向ξk (t), 大小等于ω|ξ-x|, 如图 4所示。

下面结合具体算例, 仿真算法的实际效果。在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, 使用递推算法, 算出ξkk+1(t)和xkk+1(t)轨迹如图 5所示, 实线为COM轨迹, 虚线为DCM轨迹。DCM是分段曲线,每一段表示一次摆动。可以看出图中ξkk+1(t)一直处于可控状态, 经过一个时间跨度T趋于发散时进入下一摆。

thumbnail 图3

DCM规划方法

thumbnail 图4

三维DCM和COM的基本移动

thumbnail 图5

二维DCM和COM步态间的移动

2.2 一步DCM终值不变抗扰动控制

针对LIP动力学方程的不稳定部分, 有必要设计增强系统鲁棒性的负反馈控制器, 使双足系统在第k步受到扰动后依然能在一个时间跨度T内重回规划的ξk+1点处。

使用图 6来说明一步DCM终值不变抗扰动控制的思想。开始阶段pkpkk+1(0)处, 依据pk保持不变规划出的ξkξk+1pk在同一条直线上如图 6左侧实线所示。如果系统不受外界扰动ξkk+1(t)将随时间的推移沿直线从ξk运动到ξk+1处。但是如果系统受到了外界的扰动, ξkk+1(t)偏移了左侧实线。扰动撤除后, 系统如果仍然希望ξkk+1(t)能够在一个时间跨度T内运动到ξk+1, 那么pk必然需要移动以保持ξkk+1(t)、ξk+1pkk+1(t)在同一条直线上, 如图 6所示。在这个pkk+1(t)作用下, ξkk+1(t)将会随时间的推移运动回ξk+1处, 与不受扰动相比能以相同的ξ初值进入下一摆, 下一摆系统继续依照规划运行下去。

依据G1(s)求出, 解出其通解为

ξkk+1(t)用ξk+1代替, ξk用当前实测系统的ξx(t)代替, (14)式进而可以改写为

式中,ξk+1为第k步系统期望到达的终值, 为一常值, ξx(t)为系统实时反馈ξ值, b=eω(T-t), T为一步所需时间跨度。可以算出目标pkk+1(t)值, 也即是期望ZMP轨迹, 控制ZMP点跟踪该pkk+1(t)即可保证ξkk+1(t)随着时间的推移仍归于ξk+1。画出系统的闭环系统框图如图 7所示。

采用2.1节算例, matlab仿真得到

以第一步x方向为例, ξ1→2(t)从0.003运动到0.253完成一次x方向摆动, 如果系统受到外界扰动, 扰动力f′沿x轴正方向作用, 系统的动力学方程变为

最终体现在对支点p的扰动。

应用DCM终值不变闭环控制算法, 使LIP在x, y正方向仍均受到相同大小的外界扰动力。按(15)式设计框图 7所示的控制器。仿真结果如图 8所示, 可以看到:受到扰动力后, DCM轨迹偏离规划,接入控制器后的DCM随着时间的推移重回规划。曲线最终回到了ξx, 2=0.253处。在x-y平面上DCM的轨迹如图 9所示, 在图中同时做出了未受扰动时的曲线, 可以看到一步结束时刻ξkk+1(t)随着时间的推移仍归于规划的ξk+1

thumbnail 图6

一步DCM终值不变抗扰动控制思想

thumbnail 图7

一步DCM终值不变抗扰动控制框图

thumbnail 图8

闭环控制前后x方向DCM波形

thumbnail 图9

闭环控制前后DCM和COM波形

2.3 实时DCM轨迹跟踪闭环控制

相对于2.2节, 实时DCM轨迹跟踪闭环控制思想体现在:在一个行走步态中保证反馈ξx(t)实时跟踪规划的ξkk+1(t)。(14)式中令ξkk+1(t)保持不变, ξk使用反馈ξx(t)代替, 求出

式中,ξkk+1(t)为第k步系统规划出的DCM轨迹, 为一时间序列, ξx(t)为实时系统反馈ξ值, b=edT, dT为系统跟踪上目标ξ值所需要的时间, 注意dT和一步所需时间跨度T的区别。将图 7中的控制器部分写成数学公式的形式会发现:从ξkk+1(t)到ξx(t)转变成了一个一阶惯性环节

式中,

所以由于一阶惯性环节的存在, 不管ω′取何值, ξx(t)在同一时刻都无法跟踪上ξkk+1(t), ξx(t)滞后ξkk+1(t)dT时间。因此为了保证一步时间跨度T后, 不影响下一步的摆动, 可以将ξkk+1(t+dT)作为t时刻的指令值, 即使系统滞后dT, 也能保证t时刻反馈ξx(t)=ξkk+1(t)。

图 10说明了2.1节算例的第2步x方向加入扰动信号后控制器的控制效果。这里取b=1.1, 推出dT=0.021 5 s。可以看到:受到扰动后ξx(t)开始偏离ξkk+1(t), 但是扰动去除后系统很快会在控制器的作用下重回规划。图中还给出了pkk+1(t)的轨迹, 算出的pkk+1(t)受到扰动偏离了pk点, 然后扰动去除后又重回pk点。

thumbnail 图10

实时跟踪控制前后x方向DCM轨迹

3 指定COM目标轨迹逆运动学数值解法

在本节中, 将寻找依据规划出的COM轨迹和双脚运动轨迹求解机器人逆运动学的方法, 机器人模型如图 11所示。

选择机器人髋关节中的一个坐标系作为机器人随体基坐标系o。世界坐标系固定于地面上某一点, 则基坐标系o相对于世界坐标系的位置为ro。将从坐标系o分出的2个运动链分别称为第一肢体和第二肢体, 单腿支撑阶段支撑腿称为第一肢体, 另一条腿称为第二肢体。双脚支撑阶段任选一条腿为第一肢体。第i肢体上具有ni≥6个活动关节。

i肢体末端坐标系在世界坐标系中的位置为

式中, riR3×1, RoR3×3是坐标系o到世界坐标系的转移矩阵, roR3×1是坐标系o在世界坐标系中的位置。

i肢体末端在坐标系o中的速度

上标o表示相对于坐标系o而言, oJiRni是雅克比矩阵, , 是第i肢体的关节角速度向量。

thumbnail 图11

用于动态行走的双足机器人

3.1 肢体运动约束条件

接下来求取第i个肢体末端在世界坐标系中的速度

式中, [(·)×]是矢量积的反对称矩阵, 将其归一化写成

是坐标系o相对于世界坐标系的速度。

R6×6, , I303分别是三维单位阵和三维0阵。

对于机器人不同的肢体, 都应该具有相同的坐标系o相对于世界坐标系的速度, 换句话说, 从(22)式可以看出, 所有的肢体都应该满足坐标系o速度相同的约束条件, 因此, 第i肢体和第j肢体肢体应该满足以下关系

特别的有

所以

式中,JiJi的Moore-Penrose广义逆矩阵

所以, 基于(25)式找到的关系可以求出其他肢体的逆运动学解, 而与坐标系o的运动没有关系。

3.2 指定质心运动轨迹的逆运动学求解

在世界坐标系中全身质心坐标可以表示为

式中,cR3, oci表示第i肢体的质心在o坐标系中的坐标。接下来求取质心在世界坐标系中的速度

式中,, oJci表示第i肢体的质心相对于o坐标的雅克比矩阵, 令

显然

式中,oci, kR3表示第i肢体中第k连杆的质心在o中的坐标向量。又根据雅克比矩阵的定义

式中

mi, k表示第i肢体中第k连杆的质量。并将(25)式带入(28)式, 化简得

又因为

式中,ωo表示坐标系o的旋转, ωi表示坐标系i的旋转, Jv1Jω1分别为在世界坐标系上表示的基肢(第一肢体)雅可比J1的线速度部分和角速度部分。所以

又因为基肢, 所以(34)式简化为

改写(35)式

式中

将旋转增广入(36)式中得

这样就找到了基肢关节空间与质心坐标空间之间的关系, 结合(25)式, 即可求出在指定的质心运动轨迹下的逆解。

特别的对于双足步行机器人, 其只有2个运动链, 且质心保持平动, n1=n2=6, ωo=0有

直接求解(38)式是很难完成的。这里采用数值解法, 将其写成差分形式

对于给定了质心和双腿末端运动轨迹的双足机器人, 第一肢体末端与地面接触并保持不动, 由第二肢体参考轨迹和COM参考轨迹应用图 12可以快速求解出q1q2

thumbnail 图12

双足机器人逆运动学的数值解法

4 仿真试验

本节将会在无扰动的情况下(有扰动时同理可得)对上述方法进行全面的仿真。运动学模型采用改进D-H法则建立, 具体结构尺寸参数以及质量属性见表 1

依据第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所示参考轨迹。

规划出了所需目标轨迹后, 就可以应用第3节图 12所示逆运动学的数值解法, 求出具体的关节角度, 由于双足的行走具有周期性, 所以不妨以一次摆动为例见图 14。图中实线为参考轨迹, 点画线展示了通过逆运动学求解得到关节角度后计算正运动学在操作空间中的实际COM和摆动脚轨迹。可以看到任意给出一个系统初始姿态, 通过几次迭代后, 2条曲线都收敛到参考轨迹上。

表1

机器人机构质量属性表

thumbnail 图13

双足机器人步行目标轨迹

thumbnail 图14

一步行走逆运动学求解轨迹跟踪曲线

5 结论

本文详细研究了一种基于DCM动力学的双足机器人步行规划与控制方法。DCM方法直观地将LIP描述为2个级联的一阶系统,通过设置期望的行走脚印和期望的DCM步尾位置,规划出期望的DCM轨迹,然后设计了2种DCM闭环控制器求出了期望的ZMP轨迹。利用ZMP作为控制信号,可以稳定双足机器人,并将DCM移动到所需位置。讨论研究了基于规划的COM轨迹求解机器人逆运动学的问题。最终方法解决了机器人步行从脚印到关节角度的整套求解问题。

References

  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 [Google Scholar]
  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 [Article] [CrossRef] [Google Scholar]
  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 [Google Scholar]
  4. Vukobratovic M, Stepanenko J. On the Stability of Anthropomorphic Systems[J]. Mathematical Biosciences, 1972, 15(1): 1–37 [Article] [CrossRef] [Google Scholar]
  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 [Google Scholar]
  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 [Google Scholar]
  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 [Google Scholar]
  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 [Article] [CrossRef] [Google Scholar]
  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 [Google Scholar]
  10. Yu Angke. Mechanism Design and Gait Simulation of Hydraulically Driven Biped Robot[D]. Hangzhou, Zhejiang University, 2015 (in Chinese) [Google Scholar]
  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 [Google Scholar]

All Tables

表1

机器人机构质量属性表

All Figures

thumbnail 图1

平面二维倒立摆模型

In the text
thumbnail 图2

LIP模型动力学拆分

In the text
thumbnail 图3

DCM规划方法

In the text
thumbnail 图4

三维DCM和COM的基本移动

In the text
thumbnail 图5

二维DCM和COM步态间的移动

In the text
thumbnail 图6

一步DCM终值不变抗扰动控制思想

In the text
thumbnail 图7

一步DCM终值不变抗扰动控制框图

In the text
thumbnail 图8

闭环控制前后x方向DCM波形

In the text
thumbnail 图9

闭环控制前后DCM和COM波形

In the text
thumbnail 图10

实时跟踪控制前后x方向DCM轨迹

In the text
thumbnail 图11

用于动态行走的双足机器人

In the text
thumbnail 图12

双足机器人逆运动学的数值解法

In the text
thumbnail 图13

双足机器人步行目标轨迹

In the text
thumbnail 图14

一步行走逆运动学求解轨迹跟踪曲线

In the text

Current usage metrics show cumulative count of Article Views (full-text article views including HTML views, PDF and ePub downloads, according to the available data) and Abstracts Views on Vision4Press platform.

Data correspond to usage on the plateform after 2015. The current usage metrics is available 48-96 hours after online publication and is updated daily on week days.

Initial download of the metrics may take a while.