余杰先 燕伟

摘要：在调研分析当前足式机器人的研究进展的基础上，提出一种基于旋量理论的四足机器人运动学建模方法。运用旋量理论建立其运动学模型，建立各个关节的速度螺旋后根据罗德里格斯公式推导出该四足机器人的正运动学显式解析解，运用空间几何方法推导出该四足机器人的逆运动学显式解析解。最后运用专业多体动力学仿真软件 Adams对所设计的四足机器人进行运动学仿真分析，验证了运动学理论推导的正确性及改四足机器人设计的合理性。提出四足机器人运动学算法的显式解析解，特别适用于对四足机器人控制有实时性要求的应用场景，为足式机器人的实时控制打下坚实的算法基础。该四足机器人运动学算法对后续的四足机器人甚至足式机器人的动力学、轨迹规划、步态规划研究都具有重要的参考意义。

关键词：旋量理论;足式机器人;运动学;仿真

中图分类号：TP24文献标志码：A

文章编号：1009-9492（2021）11-0153-04

开放科学（资源服务）标识码（OSID）：

The Kinematic Analysis of Four-legged Robot Base on Screw Theory Yu Jiexian，Yan Wei

（Department of Mechanical and Energy Engineering， Southern University of Science and Technology， Shenzhen， Guangdong 518000， China）

Abstract： Through the investigation and analysis of the current research progress of four-legged robot， a kinematic modeling and method of four-legged robot base on screw theory was proposed. According to the screw theory， the kinematic model of four-legged robot was first established. The forward kinematics explicit solution was derived by rodriguez formula after establishing the twist of each four-legged robot joint. Simultaneously the inverse kinematics explicit solution was derived by Geometric method of space. Finally making the kinematic simulation analysis of this four-legged robot by using the professional multi-body dynamics simulation software of Adams， the work was the verification of the kinematics screw's derivation validity and the design reasonability of the four-legged robot. The explicit solution of the four-legged robot kinematics algorithm that presented by the paper， especially appropriate to the application scenarios that require a real-time of the four-legged robot， and the kinematics algorithm has lay a solid foundation for legged-robot's real-time control. The kinematics algorithm has asignificant reference value for thesubsequent research of dynamics， trajectory planning， gait planning of four-legged robot even legged-robot.

Key words： screw theory; legged-robot; kinematics; simulation

0 引言

近十幾年来，对移动机器人的研究收到越来越多的重视，足式移动机器人相对于轮式移动机器人具有很多有优点，因此足式移动机器人成为了当今移动机器人研究的一个热点，出现了很多研究足式机器人的团队，比如波士顿动力的液压双足 atlas[1]和四足bigdog[2]，这是世界上目前运动性能最好的足式机器人。IIT 的液压四足HyQ[3]和 ETH 的ANYmal[4]，上海交通大学高峰团队的大型六足机器人可承载1 t的重物[5-6]。这些团队从机器人的某一方面入手，取得的很好的研究成果和演示效果。

足式机器人在行走过程中需要面对极其复杂多变的外部环境。这就要求机器人需要具备快速感知外部环境并作出决策的能力，来适应在不同地形下的行走。很多专家学者提出了特别好的控制方法和策略。 Marc H Raibert[7]研究了单腿跳跃的虚拟腿模型，通过控制跳跃高度，前进速度和空中姿态来使单足稳定，然后又推广到了四足的控制[8]实现了四足跳跃。ETH[9]使用 SEA （Series Elastic Actuation）的方法设计了一款具有高顺应性的腿，极大地提高了四足的地面适应性和抗冲击能力，但是这对于机器人本身的控制性能提出了挑战。MIT 设计了具有本体感知能力的驱动模块来实现快速动态的腿式移动[10]。并使用该驱动模块开发了 cheetah2和 cheetah3[11-12]。一直以来，对于四足的建模都采用了经典的 SLIP模型[13]，很多学者使用该模型取得了很好的研究成果。但是该模型是对四足的简化，并不能完全表现出机器人本身的物理特性。

旋量理论是一种被广泛引用于机器人运动学与动力学建模的方法，一些学者使用旋量理论进行机构分析[14-17]，针对不同机构都能对机构本身的运动学和动力学特性进行完整地阐述。对于四足机器人，CHEN X等[18]分析了四足机器人在有部分机构故障时的运动学问题，提出了容错步态。目前还未建立四足机器人的全动力学模型，主要是由于动力学模型需要大量的计算，这会延长模型的求解时间，对于足式机器人来讲必须具备较高动态性能，在很短的时间内完成计算和规划，旋量理论可以简化机器人坐标系的建立，使求解更加快速。

1 坐标系定义

如图1所示，四足机器人具有4条完全一样的机械腿，每条机械腿具有3个自由度，每条腿的3个驱动关节是串联构型，本章将介绍四足机器人的相关坐标系定义，为运动学分析求解建立基础。

连杆的速度和惯量在不同的坐标系下的表达是不同的，因此定义初始时刻的位姿以及坐标系是非常必要的。在四足机器人控制的底层，需要完成对身体和脚尖的轨迹规划，那么定义该轨迹的参考坐标系为地面坐标系，是固定不动的，而身体和脚的坐标系是相对运动的，为浮动坐标系。在求解逆运动学时，可以使用以下公式计算出脚尖相对于腿坐标系的坐标，进而求得关节转角。在求解正运动学时，以身体坐标系为参考坐标系，求出所有关节和连杆在身体坐标系下的位姿，为后边求解动力学做出铺垫。为了便于计算，本文定义四足机器人的初始位姿和相关坐标系如下：

2 运动学逆解

运动学逆解是已知足尖在腿坐标系下的表达（x， y ，z ），求解关节转角（ q1，q2，q3）。在求解之前，根据实际机器人的行走情况作了限定来简化计算。初始位置定义，连杆参数以及表达如图2所示，假设腿不会运动到身体所在平面的上半部分，即：

计算时先将结构投影到yOz平面分析，计算出 q1，然后由于连杆2和3始终在一个平面上，将坐标系原点转移到关节2处分析计算，将问题转化为求解平面两连杆机构的逆运动学，最后如图3所示分类讨论得出 q2和 q3。

在yOz平面内。点C 的投影点为点D ，由几何关系得：

式中：α为 DO 与z 轴的夹角;β为AO 与 DO 的夹角。

根据结构可以分析得知，不论 q1转过多少角度。连杆2和3始终保持在同一个平面上，为此，将该平面作为基准面建立新的坐标系来求解 q2和 q3。建立如图4所示的新坐标系xyz′。在新坐标系下末端的坐標转换公式为：

根据三角函数关系可得：

式中： ?为AC 与x 轴之间的夹角; φ为AC 与AB之间的夹角。

由图5可知，当末端在同一个位置时由于曲腿的方式不同，会有不同的解，定义当机器人为外曲腿模式时，为图5中虚线部分，此时 q3>0。

q3>0时：

q3<0时

3 运动学正解

在旋量理论中，速度螺旋 v 和螺旋轴 S 由以下两个向量定义：

螺旋轴 S 定义了转动关节的位置以及指向。向量 T 描述其在坐标系 B 中的位置，向量 R 描述其在坐标系 B 中的指向。速度螺旋 v 描述刚体在坐标系 B 原点处的线速度以及角速度。

本文中，所有关节及连杆参数都在身体坐标系 B 下描述，因此，假设在求解运动学的时候身体坐标系是相对静止的。正运动学问题是已知在坐标系 B 下各个关节的输入角度θij，求解末端在坐标系 B 下的位置。求解前，机器人的初始位置应该是确定的，以明确各个关节的初始速度螺旋。为了方便计算，本文定义四足机器人的初始位置如图1所示。

四足机器人各个螺旋轴定义所示：

通过螺旋轴Sij可以计算得到各个关节的原始速度螺旋，这里单位螺旋用表示。例如腿1的第一个关节表示为： =（0，0，0， 1，0，0）。

然后可以得到末端位置如下：

式中：i=1，2，3 ，4是四足机器人的机械腿的编号;P 为在关节转角θ下的齐次变换矩阵;Jijvso为通过螺旋轴Sij计算得到的关节速度螺旋。

4 仿真结果与分析

为了验证上述运动学算法的正确性，本文利用专业多体动力学软件 Adams对该运动学算法进行可视化的运动仿真，但因为 Adams软件并不擅长三维建模，因此需要借助专业的三维建模软件对上述仿生四足机器人进行三维建模，本文选用 SolidWorks软件进行三维建模。首先在 SolidWorks中对该四足机器人进行建模，对机器人的每一个部件都赋上真实的材料或质量属性，力求模型的真实性，然后测量各个部件的质量、重心、惯量等信息并记录下来。将 SolidWorks里的四足机器人三维模型保存为中间格式，并导入 Adams软件中，在 Adams中对各个部件设置上述在 SolidWorks里测量得到的质量、重心、惯量等信息，并添加相关运动副及驱动，并设置驱动函数即可进行仿真，最后在后处理届面读取相关仿真信息。

如图6所示，该踏步运动仿真验证了以上运动学算法的正确性，同时为该机器人的动力学、实时控制建立了坚实的算法基础。仿真时的要点是要保证机器人的重心始终处于脚触点的支撑平面范围内，以防止机器人倾倒。图中运动仿真过程中的一些快照。仿真结果显示机器人利用上述求解的运动学算法，可以走出准确的步态，验证上述的运动学算法是正确的。

5 结束语

本文通过利用旋量理论建立四足机器人的运动学模型，并由各个关节的速度螺旋运用罗德里格斯公式（ Ro- driguez formula）推导出该四足机器人的正运动学的显式解析解，逆运动学的显式解析解则是通过空间几何法进行推导。最后将四足机器人在 Adams中进行仿真，通过对比分析机器人运动仿真，验证了本文运动学算法推导的理论正确性以及机器人机构设计的合理性。旋量理论在并联及混联机构的运动学分析有明显的优势，能大大减少数学模型的坐标系定义，有利于简化正逆运动学的求解。而运动学显式解析解能够满足式机器人控制系统对实时性的要求。本文提出的基于旋量理论的四足机器人运动学分析对四足机器人乃至足式机器人的实时控制、动力学、轨迹规划、步态规划等方面的研究都具有重要的参考意义。

參考文献：

[1] KUINDERSMAS，DEITSR，FALLONM，etal. Optimiza- tion-based locomotion planning， estimation， and control design for the atlas humanoid robot [J]. Autonomous Robots， 2015， 40（3）：429-455.

[2] MARC RAIBERT K B， GABRIEL NELSON， ROB PLAYTER AND THE BIGDOG TEAM. BigDog， the Rough-Terrain Quadru- ped Robot [J]. The International Federation of Automatic Con- trol， 2008，

[3] SEMINI C， TSAGARAKIS N G， GUGLIELMINO E， et al. Design of HyQ-a hydraulically and electrically actuated quadruped ro- bot [J]. Proceedings of the Institution of Mechanical Engineers， Part I： Journal of Systems and Control Engineering，2011，225（6）：831-849.

[4] M Hutter， C Gehring， A Lauber， et al. ANYmal-toward legged robots for harsh environments[J]. Advanced Robotics， 2017，31（17）：918-931.

[5] Fu J， Gao F， Chen W， et al. Kinematic accuracy research of a novelsix-degree-of-freedomparallelrobot withthreelegs[J]. Mechanism & Machine Theory，2016（102）：86-102.

[6] PAN Y，GAOF. Positionmodelcomputationalcomplexity of walking robot with different parallel leg mechanism topology pat- terns [J]. Mechanism and Machine Theory， 2017（107）：324-337.

[7] Raibert M H， Brown H B， Chepponis M. Experiments in Balance With a 2D One-Legged Hopping Machine[J]. Trans Asme，1984， 106（2）：75-92.

[8] Raibert M H， Chepponis M， Jr H. Running on four legs as though they were one[J]. IEEE Journal on Robotics & Automation， 1986（2）：70-82.

[9] Hutter M， Remy C D， Hoepflinger M A ， et al. Efficient and Ver- satile Locomotion With Highly Compliant Legs[J]. IEEE/ASME Transactions on Mechatronics，2013， 18（2）：449-458.

[10] Seok S ，Wang A ，Otten D ，et al. Actuator Design for High Force Proprioceptive Control in Fast Legged Locomotion[C]//Proceed-