余杰先 燕偉

摘要：在調(diào)研分析當(dāng)前足式機(jī)器人的研究進(jìn)展的基礎(chǔ)上，提出一種基于旋量理論的四足機(jī)器人運動學(xué)建模方法。運用旋量理論建立其運動學(xué)模型，建立各個關(guān)節(jié)的速度螺旋后根據(jù)羅德里格斯公式推導(dǎo)出該四足機(jī)器人的正運動學(xué)顯式解析解，運用空間幾何方法推導(dǎo)出該四足機(jī)器人的逆運動學(xué)顯式解析解。最后運用專業(yè)多體動力學(xué)仿真軟件 Adams對所設(shè)計的四足機(jī)器人進(jìn)行運動學(xué)仿真分析，驗證了運動學(xué)理論推導(dǎo)的正確性及改四足機(jī)器人設(shè)計的合理性。提出四足機(jī)器人運動學(xué)算法的顯式解析解，特別適用于對四足機(jī)器人控制有實時性要求的應(yīng)用場景，為足式機(jī)器人的實時控制打下堅實的算法基礎(chǔ)。該四足機(jī)器人運動學(xué)算法對后續(xù)的四足機(jī)器人甚至足式機(jī)器人的動力學(xué)、軌跡規(guī)劃、步態(tài)規(guī)劃研究都具有重要的參考意義。

關(guān)鍵詞：旋量理論;足式機(jī)器人;運動學(xué);仿真

中圖分類號：TP24文獻(xiàn)標(biāo)志碼：A

文章編號：1009-9492（2021）11-0153-04

開放科學(xué)（資源服務(wù)）標(biāo)識碼（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 引言

近十幾年來，對移動機(jī)器人的研究收到越來越多的重視，足式移動機(jī)器人相對于輪式移動機(jī)器人具有很多有優(yōu)點，因此足式移動機(jī)器人成為了當(dāng)今移動機(jī)器人研究的一個熱點，出現(xiàn)了很多研究足式機(jī)器人的團(tuán)隊，比如波士頓動力的液壓雙足 atlas[1]和四足bigdog[2]，這是世界上目前運動性能最好的足式機(jī)器人。IIT 的液壓四足HyQ[3]和 ETH 的ANYmal[4]，上海交通大學(xué)高峰團(tuán)隊的大型六足機(jī)器人可承載1 t的重物[5-6]。這些團(tuán)隊從機(jī)器人的某一方面入手，取得的很好的研究成果和演示效果。

足式機(jī)器人在行走過程中需要面對極其復(fù)雜多變的外部環(huán)境。這就要求機(jī)器人需要具備快速感知外部環(huán)境并作出決策的能力，來適應(yīng)在不同地形下的行走。很多專家學(xué)者提出了特別好的控制方法和策略。 Marc H Raibert[7]研究了單腿跳躍的虛擬腿模型，通過控制跳躍高度，前進(jìn)速度和空中姿態(tài)來使單足穩(wěn)定，然后又推廣到了四足的控制[8]實現(xiàn)了四足跳躍。ETH[9]使用 SEA （Series Elastic Actuation）的方法設(shè)計了一款具有高順應(yīng)性的腿，極大地提高了四足的地面適應(yīng)性和抗沖擊能力，但是這對于機(jī)器人本身的控制性能提出了挑戰(zhàn)。MIT 設(shè)計了具有本體感知能力的驅(qū)動模塊來實現(xiàn)快速動態(tài)的腿式移動[10]。并使用該驅(qū)動模塊開發(fā)了 cheetah2和 cheetah3[11-12]。一直以來，對于四足的建模都采用了經(jīng)典的 SLIP模型[13]，很多學(xué)者使用該模型取得了很好的研究成果。但是該模型是對四足的簡化，并不能完全表現(xiàn)出機(jī)器人本身的物理特性。

旋量理論是一種被廣泛引用于機(jī)器人運動學(xué)與動力學(xué)建模的方法，一些學(xué)者使用旋量理論進(jìn)行機(jī)構(gòu)分析[14-17]，針對不同機(jī)構(gòu)都能對機(jī)構(gòu)本身的運動學(xué)和動力學(xué)特性進(jìn)行完整地闡述。對于四足機(jī)器人，CHEN X等[18]分析了四足機(jī)器人在有部分機(jī)構(gòu)故障時的運動學(xué)問題，提出了容錯步態(tài)。目前還未建立四足機(jī)器人的全動力學(xué)模型，主要是由于動力學(xué)模型需要大量的計算，這會延長模型的求解時間，對于足式機(jī)器人來講必須具備較高動態(tài)性能，在很短的時間內(nèi)完成計算和規(guī)劃，旋量理論可以簡化機(jī)器人坐標(biāo)系的建立，使求解更加快速。

1 坐標(biāo)系定義

如圖1所示，四足機(jī)器人具有4條完全一樣的機(jī)械腿，每條機(jī)械腿具有3個自由度，每條腿的3個驅(qū)動關(guān)節(jié)是串聯(lián)構(gòu)型，本章將介紹四足機(jī)器人的相關(guān)坐標(biāo)系定義，為運動學(xué)分析求解建立基礎(chǔ)。

連桿的速度和慣量在不同的坐標(biāo)系下的表達(dá)是不同的，因此定義初始時刻的位姿以及坐標(biāo)系是非常必要的。在四足機(jī)器人控制的底層，需要完成對身體和腳尖的軌跡規(guī)劃，那么定義該軌跡的參考坐標(biāo)系為地面坐標(biāo)系，是固定不動的，而身體和腳的坐標(biāo)系是相對運動的，為浮動坐標(biāo)系。在求解逆運動學(xué)時，可以使用以下公式計算出腳尖相對于腿坐標(biāo)系的坐標(biāo)，進(jìn)而求得關(guān)節(jié)轉(zhuǎn)角。在求解正運動學(xué)時，以身體坐標(biāo)系為參考坐標(biāo)系，求出所有關(guān)節(jié)和連桿在身體坐標(biāo)系下的位姿，為后邊求解動力學(xué)做出鋪墊。為了便于計算，本文定義四足機(jī)器人的初始位姿和相關(guān)坐標(biāo)系如下：

2 運動學(xué)逆解

運動學(xué)逆解是已知足尖在腿坐標(biāo)系下的表達(dá)（x， y ，z ），求解關(guān)節(jié)轉(zhuǎn)角（ q1，q2，q3）。在求解之前，根據(jù)實際機(jī)器人的行走情況作了限定來簡化計算。初始位置定義，連桿參數(shù)以及表達(dá)如圖2所示，假設(shè)腿不會運動到身體所在平面的上半部分，即：

計算時先將結(jié)構(gòu)投影到y(tǒng)Oz平面分析，計算出 q1，然后由于連桿2和3始終在一個平面上，將坐標(biāo)系原點轉(zhuǎn)移到關(guān)節(jié)2處分析計算，將問題轉(zhuǎn)化為求解平面兩連桿機(jī)構(gòu)的逆運動學(xué)，最后如圖3所示分類討論得出 q2和 q3。

在yOz平面內(nèi)。點C 的投影點為點D ，由幾何關(guān)系得：

式中：α為 DO 與z 軸的夾角;β為AO 與 DO 的夾角。

根據(jù)結(jié)構(gòu)可以分析得知，不論 q1轉(zhuǎn)過多少角度。連桿2和3始終保持在同一個平面上，為此，將該平面作為基準(zhǔn)面建立新的坐標(biāo)系來求解 q2和 q3。建立如圖4所示的新坐標(biāo)系xyz′。在新坐標(biāo)系下末端的坐標(biāo)轉(zhuǎn)換公式為：

根據(jù)三角函數(shù)關(guān)系可得：

式中： ?為AC 與x 軸之間的夾角; φ為AC 與AB之間的夾角。

由圖5可知，當(dāng)末端在同一個位置時由于曲腿的方式不同，會有不同的解，定義當(dāng)機(jī)器人為外曲腿模式時，為圖5中虛線部分，此時 q3>0。

q3>0時：

q3<0時

3 運動學(xué)正解

在旋量理論中，速度螺旋 v 和螺旋軸 S 由以下兩個向量定義：

螺旋軸 S 定義了轉(zhuǎn)動關(guān)節(jié)的位置以及指向。向量 T 描述其在坐標(biāo)系 B 中的位置，向量 R 描述其在坐標(biāo)系 B 中的指向。速度螺旋 v 描述剛體在坐標(biāo)系 B 原點處的線速度以及角速度。

本文中，所有關(guān)節(jié)及連桿參數(shù)都在身體坐標(biāo)系 B 下描述，因此，假設(shè)在求解運動學(xué)的時候身體坐標(biāo)系是相對靜止的。正運動學(xué)問題是已知在坐標(biāo)系 B 下各個關(guān)節(jié)的輸入角度θij，求解末端在坐標(biāo)系 B 下的位置。求解前，機(jī)器人的初始位置應(yīng)該是確定的，以明確各個關(guān)節(jié)的初始速度螺旋。為了方便計算，本文定義四足機(jī)器人的初始位置如圖1所示。

四足機(jī)器人各個螺旋軸定義所示：

通過螺旋軸Sij可以計算得到各個關(guān)節(jié)的原始速度螺旋，這里單位螺旋用表示。例如腿1的第一個關(guān)節(jié)表示為： =（0，0，0， 1，0，0）。

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

式中：i=1，2，3 ，4是四足機(jī)器人的機(jī)械腿的編號;P 為在關(guān)節(jié)轉(zhuǎn)角θ下的齊次變換矩陣;Jijvso為通過螺旋軸Sij計算得到的關(guān)節(jié)速度螺旋。

4 仿真結(jié)果與分析

為了驗證上述運動學(xué)算法的正確性，本文利用專業(yè)多體動力學(xué)軟件 Adams對該運動學(xué)算法進(jìn)行可視化的運動仿真，但因為 Adams軟件并不擅長三維建模，因此需要借助專業(yè)的三維建模軟件對上述仿生四足機(jī)器人進(jìn)行三維建模，本文選用 SolidWorks軟件進(jìn)行三維建模。首先在 SolidWorks中對該四足機(jī)器人進(jìn)行建模，對機(jī)器人的每一個部件都賦上真實的材料或質(zhì)量屬性，力求模型的真實性，然后測量各個部件的質(zhì)量、重心、慣量等信息并記錄下來。將 SolidWorks里的四足機(jī)器人三維模型保存為中間格式，并導(dǎo)入 Adams軟件中，在 Adams中對各個部件設(shè)置上述在 SolidWorks里測量得到的質(zhì)量、重心、慣量等信息，并添加相關(guān)運動副及驅(qū)動，并設(shè)置驅(qū)動函數(shù)即可進(jìn)行仿真，最后在后處理屆面讀取相關(guān)仿真信息。

如圖6所示，該踏步運動仿真驗證了以上運動學(xué)算法的正確性，同時為該機(jī)器人的動力學(xué)、實時控制建立了堅實的算法基礎(chǔ)。仿真時的要點是要保證機(jī)器人的重心始終處于腳觸點的支撐平面范圍內(nèi)，以防止機(jī)器人傾倒。圖中運動仿真過程中的一些快照。仿真結(jié)果顯示機(jī)器人利用上述求解的運動學(xué)算法，可以走出準(zhǔn)確的步態(tài)，驗證上述的運動學(xué)算法是正確的。

5 結(jié)束語

本文通過利用旋量理論建立四足機(jī)器人的運動學(xué)模型，并由各個關(guān)節(jié)的速度螺旋運用羅德里格斯公式（ Ro- driguez formula）推導(dǎo)出該四足機(jī)器人的正運動學(xué)的顯式解析解，逆運動學(xué)的顯式解析解則是通過空間幾何法進(jìn)行推導(dǎo)。最后將四足機(jī)器人在 Adams中進(jìn)行仿真，通過對比分析機(jī)器人運動仿真，驗證了本文運動學(xué)算法推導(dǎo)的理論正確性以及機(jī)器人機(jī)構(gòu)設(shè)計的合理性。旋量理論在并聯(lián)及混聯(lián)機(jī)構(gòu)的運動學(xué)分析有明顯的優(yōu)勢，能大大減少數(shù)學(xué)模型的坐標(biāo)系定義，有利于簡化正逆運動學(xué)的求解。而運動學(xué)顯式解析解能夠滿足式機(jī)器人控制系統(tǒng)對實時性的要求。本文提出的基于旋量理論的四足機(jī)器人運動學(xué)分析對四足機(jī)器人乃至足式機(jī)器人的實時控制、動力學(xué)、軌跡規(guī)劃、步態(tài)規(guī)劃等方面的研究都具有重要的參考意義。

參考文獻(xiàn)：

[1] KUINDERSMAS，DEITSR，F(xiàn)ALLONM，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-