基于旋量理論的四足機(jī)器人運(yùn)動(dòng)學(xué)分析

余杰先 燕偉

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

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

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

文章編號(hào)：1009-9492（2021）11-0153-04

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

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

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

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

1 坐標(biāo)系定義

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

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

2 運(yùn)動(dòng)學(xué)逆解

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

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

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

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

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

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

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

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

q3>0時(shí)：

q3<0時(shí)

3 運(yùn)動(dòng)學(xué)正解

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語(yǔ)

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