基于四足机器人的全向运动控制与仿真

张 博 何海龙 王 明 丁 雪

(西安电子工程研究所 西安 710100)

0 引言

四足机器人的研究开始于20世纪60年代，在20世纪80年代初期四足机器人就已经实现了一定的智能化[1]。21世纪以来，随着微电子技术的发展和硬件运算能力的提升，更加优秀的控制算法和建模手段被不断提出，计算机视觉技术和高精度传感器的应用，进一步提高了机器人的姿态调整能力和环境适应能力[2-4]。随着机器学习和深度学习技术的发展，四足机器人已经能够根据自身实时运动状态，以及复杂地形条件实现自我调整，平稳行进，并对外界突发的作用力进行快速判断和补偿控制。使其成为一个稳定可靠的机器运动载体。

本文介绍并分析了四足机器人的足端轨迹规划和全向运动的基本控制算法和动态仿真方法。

1 四足机器人的运动学研究

四足机器人的运动学主要研究机器人在空间中的位置、姿态和足端相对身体之间的运动关系。主要分为从关节角度控制量解算足端运动特性的正运动学和根据足端运动规划求解关节角度驱动的逆运动学分析。同时包括位置、姿态、速度、加速度等方面的分析[5-6]。

1.1 动力学建模

通过对四足哺乳动物身体结构的仿生学等效得出了简单四足机器人结构如图1所示。每条腿由大腿、小腿和足三个部分组成，大腿与机体间设置相对机体横向的和纵向的两个自由度驱动作为髋关节，并在小腿和大腿间设置相对机体纵向的自由度驱动作为膝关节。每条腿有三个主动自由度。对其使用Denavit-Hartenberg(D-H)方法[7]建模，得出四足机器人右前腿的运动学模型如图2所示。其中的坐标系Ow代表世界坐标系，Ob代表机体坐标系，四条腿坐标系建立方法和右前腿一致。图中运动方向与身体坐标系X轴正方向同向，L(Left)代表左肢、R(Right)代表右肢、F(Front)代表前肢、H(Hind)代表后肢，右前腿可表示为RF。

图1 仿生学等效模型

坐标系(Xb,Yb,Zb)(如图2所示)，原点Ob为机体质心位置，设Xb轴方向为机体前向运动方向，Yb轴为反重力方向，Zb轴指向机体右侧，其他坐标系都按Z轴指向轴向，X轴指向前一个连杆的反向延长线的规则建立。坐标系0、1、2分别代表髋部侧向运动、髋部前后运动和膝关节运动，原点都建立在关节中心。θ1、θ2和θ3分别代表各关节运动的角度。l1、l2、l3为侧摆、大腿、小腿的长度。图2中坐标系3代表右前腿足端坐标系。

图2 四足机器人连杆模型

1.2 运动学分析

(1)

用齐次变换矩阵可以描述各连杆坐标系间的位置关系。如图2中的右前腿D-H参数表如表1所示。

表1 单足D-H参数

(2)

(3)

(4)

(5)

式(2～5)中，si=sinθi,ci=cosθi,则可得到足端相对于机体坐标的变换矩阵为

(6)

式(6)中，si.j=sin(θi+θj),ci.j=cos (θi+θj) 。已知关节驱动求足端坐标的过程被称作动力学正解过程，如式(7)所示。

(7)

式(7)中，(p′xp′yp′z)表示足端相对于机体坐标系b的坐标。已知足端坐标求解各关节驱动角的过程，称为动力学逆解，如式(8)所示。

(8)

(9)

式(8～9)中，(pxpypz)表示足端相对于坐标系0的坐标，由于坐标系0相对于坐标系b位置固定，运动学逆解通常使用坐标系0做参考坐标系。

2 四足机器人全向运动规划

用D-H法分析机器人运动问题，通常使用轨迹约束的方法，先确定足端的运动曲线，再通过运动学逆解，求解各关节转动驱动的输出，从而实现机器人的运动。文献[8]提出了一种零冲击的足端运动曲线。可以使足端进入支撑相时，速度和加速度都为零，如式(10)所示。

(10)

式(10)中，Tb代表足端处于摆动相的周期；e=(exeyez)T是足端在世界坐标系下的位置坐标；ej=(ejxejyejz)T是j个运动周期后足端的位置；Sj=ejx-e(j-1)x表示足端在第j个运动周期相对于第j-1个运动周期的前向位移；Dj=ejz-e(j-1)z表示足端j周期的横向位移；Hj表示足端抬起高度。

2.1 四足机器人的直线运动

四足机器人的直线行走作为周期运动，可以看成是四条腿按照特定的顺序依次完成摆动周期和支撑周期的过程。比如常见的步态trot(对角线的腿同时运动)，可看作右前腿和左后腿同时开始自己的运动周期，经过一个摆动到达支撑位置时，左前腿和右后退再开始自己的运动周期。每条腿的运动周期T=Tz+Tb,是摆动周期Tb和支撑周期Tz的和。支撑周期Tz占运动周期T的比例可以用负载因子β来表示。

2.1.1 四足机器人直线trot运动trot步态是哺乳动物经常使用的小跑步态，对角线上的两腿可以看成一组，组内两腿运动同步，组间相差一个相位差。两个组如果看成两个虚腿，很像双足动物的行走动作，但是由于每条虚腿支撑点和跨度很大，相比于双足动物，更容易实现平衡，稳定性更好。

式(10)表示了足端在世界坐标系下的轨迹，按此轨迹可以保证足端于地面接触时的冲击最小，同时获得地面最大的摩擦力。

在直线运动时，可以假设身体在世界坐标系下相当于以Sj做前向匀速运动，并保持离地高度不变。就好像人在跑步机上跑步一样，传送带匀速向后运动，人在上面保持身体不动地奔跑，当这个匀速运动的传送带撤掉，人便可以维持那种步态匀速前进。从而得出足端相对于身体坐标系的轨迹如图3所示。可见相对于身体坐标系，Y轴最低点落足的同时，X轴负向运动速度最快的点，并向相对于身体坐标系向后，推动身体向前运动。对角足相位相差半个周期就可以实现直线行走步态。

图3 足端轨迹曲线图

2.1.2 四足机器人直线walk运动

Walk运动指的是四足机器人更自由地分配四足的触底时间，按顺序向前迈步,每条腿之间相差四分之一个周期的相位差。相对于trot运动，walk运动稳定性更高。

2.2 四足机器人的转向运动

相比于直线运动，四足机器人的转向运动可以看作是直线运动足端轨迹发生的转动变换。如图4所示。

图4 转动运动变换

身体转动角度为a，质心转动半径为R，世界坐标系{W}建立在转动前的机体质心处。转动前的机体坐标系为{B},转动后的机体坐标系为{B}′，则在转动过程中，坐标系{B}′相对于世界坐标系，产生了X轴方向上的平移和Y轴方向上的平移，以及沿Z轴发生的旋转，体坐标系的变换矩阵Gt，可用式(11)表示。

(11)

通过式(10)和式(11)可求出相对于转动运动机体坐标系下的各足端轨迹。可通过(8)求得各关节的驱动输出。

2.3 四足机器人的斜向运动

四足机器人做斜向运动时同样可以通过体坐标系的变换，求解各足端的运动轨迹，从而各关节的驱动输出。如图5所示，斜向角为Ф，机身横向运动距离为H2,纵向移动距离为H1，世界坐标系{W}建立在转动前的机体质心处。转动前的机体坐标系为{B},转动后的机体坐标系为{B}′，则在斜向过程中，坐标系{B}′相对于世界坐标系，产生了X轴方向上的平移和Y轴方向上的平移，体坐标系的变换矩阵Gt，可用式(12)表示。

图5 斜向运动变换

(12)

3 联合仿真

ADAMS是美国MDI公司发布的三维虚拟软件，提供了对Matlab的接口。本文通过SolidWorks软件建立了一个用于仿真的四足机器人模型如图6所示，并导入到ADAMS软件中设置材料、连接关系、摩擦力等参数和系统单元。再导入Matlab的Simulink中如图8所示，通过S函数的编写和Simulink内部模块的连接(如图7所示)，实现了对四足机器人的动态联合仿真。

图6 用于仿真的模型

图7 Simulink连接图

图8 adams在simulink下的模型

4 仿真分析

本文在Simulink下分别对四足机器人的足端相对于身体质心坐标系下的位置进行了数据录取。直行步态、转向步态、斜向步态的三维图分别如图9、图10、图11所示。

图9 直行足端位置

图10 转向足端位置

图11 斜向足端位置

以上数据反应了四足机器人在两个运动周期内的四足足端相对于身体的位置变化情况。可以明显看出运动趋势和轨迹变化情况。

5 结束语

本文对四足机器人足端轨迹规划运动方法进行了仿真。从Adams和Simulink的交互式动态仿真效果上看，已经圆满实现了全向运动的控制目的。但是运动中呈现明显的晃动和不平衡现象。要实现平稳运动控制，还需要考虑机器人的平衡稳定问题。