腿-輪復(fù)合機器人全方位運動控制器的設(shè)計與仿真

辛亞先 ，李貽斌 ，榮學(xué)文 ，柴 匯 ，李 彬

（1.山東大學(xué)控制科學(xué)與工程學(xué)院，濟(jì)南 250061；2.山東大學(xué)機器人研究中心，濟(jì)南 250061；3.齊魯工業(yè)大學(xué)（山東省科學(xué)院）數(shù)學(xué)與統(tǒng)計學(xué)院，濟(jì)南 250353）

1 引 言

隨著科技的發(fā)展，越來越多的自主偵察、運輸、作戰(zhàn)平臺或智能機器人走向戰(zhàn)場，代替士兵進(jìn)入復(fù)雜地形環(huán)境實施作戰(zhàn)或偵察任務(wù)。目前的移動式作戰(zhàn)平臺均采用履帶式、輪式或腿足式移動方式行走。履帶式和輪式移動平臺的移動均需連續(xù)的支撐條件，適應(yīng)地形變化的能力較差。而腿足式移動平臺在平坦地面上的移動速度較慢，能耗較高。腿-輪復(fù)合移動機器人在結(jié)構(gòu)化地形下運動速度快、效率高，同時能夠工作在非結(jié)構(gòu)環(huán)境下（崎嶇不平的山地、叢林和冰雪地面等）。

目前，國內(nèi)對腿-輪復(fù)合移動機器人的研究尚處在起始階段，相對國外腿-輪復(fù)合移動機器人的研究存在較大差距。美國波士頓動力公司在軍方資助下于2017年推出的仿人型腿-輪機器人Handle不僅能夠在平坦地面快速移動，而且能夠跳過垂直障礙、平穩(wěn)通過左右陡直障礙、上下臺階等復(fù)雜地形環(huán)境，還具有較強的負(fù)重能力和受到外部干擾后的快速反應(yīng)能力，證明了腿-輪機器人更好的實用性。另外，兩輪機器人平臺的研究開展較早，目前已有較為成熟的應(yīng)用，如2002年Grasser F等人［1］研制了JOE兩輪移動倒立擺平臺，可實現(xiàn)自平衡與轉(zhuǎn)彎；日本慶應(yīng)大學(xué)搭建了兩輪機械手平臺，并提出諸多非線性控制方法［2-6］；2014年Kook A J等人［7］搭建了擁有兩個機械手的兩輪機器人平臺，并對機器人與外界環(huán)境的交互力進(jìn)行了分析與控制。迄今為止，將雙足與兩輪平臺相結(jié)合的平臺并不多見，2009年Lee H等人研制出BalBot Ⅴ［8］，將雙足機器人固定于兩輪移動平臺上，通過遙控控制雙足機器人踝關(guān)節(jié)轉(zhuǎn)動，引起質(zhì)心位置的前后變化，從而間接控制機器人行進(jìn)速度。已有的兩輪機器人平臺大多不能調(diào)整平臺高度，工作空間受到限制，身體或者機械臂均搭建在車架上，不可以改變身體姿態(tài)的冗余自由度，兩輪之間、車架與身體之間相對位置固定，不利于通過崎嶇不平地面，不利于身體的隔振。

兩輪自平衡機器人平臺作為典型的欠驅(qū)動非線性系統(tǒng)受到廣泛關(guān)注，研究人員利用其對眾多線性、非線性方法進(jìn)行檢驗［9-15］，但在實際應(yīng)用中，將機器人動力學(xué)模型在平衡點附近進(jìn)行線性化，然后采用線性控制方法如LQR進(jìn)行控制是最為常見的［16，17］，另外，利用不基于模型的PID控制方法對兩輪自平衡機器人進(jìn)行控制也獲得了良好性能［18-21］。

本文對高動態(tài)、高效率的小型腿-輪復(fù)合移動平臺的整體建模與控制進(jìn)行了研究，采用模塊化策略，對每一模塊分別進(jìn)行運動學(xué)與動力學(xué)分析，利用虛擬模型控制方法，將機器人整體虛擬為兩輪一級倒立擺，各模塊通過運動學(xué)解算與坐標(biāo)變換得到倒立擺的虛擬質(zhì)心，通過控制兩輪輸入力矩，實現(xiàn)平衡、行進(jìn)與轉(zhuǎn)向；通過控制腿部伸縮，實現(xiàn)平臺的升高和降低；通過控制手臂關(guān)節(jié)，使末端執(zhí)行器到達(dá)目標(biāo)任務(wù)點；并進(jìn)行了仿真實驗以驗證所提控制器的有效性。

2 機器人結(jié)構(gòu)

機器人的結(jié)構(gòu)示意圖如圖1所示，可將整體分為輪、腿、雙臂、頭部四部分，腿、雙臂與頭部通過驅(qū)動關(guān)節(jié)固連于軀干上。其中，兩輪為軸線平行的驅(qū)動輪，通過施加適當(dāng)驅(qū)動力實現(xiàn)平臺的平衡、移動；每條腿配置兩個俯仰關(guān)節(jié)，并通過一個欠驅(qū)動關(guān)節(jié)與驅(qū)動輪相連，可以通過控制腿部伸縮實現(xiàn)移動平臺的升高和降低；本文暫不考慮手部末端執(zhí)行器，每條手臂配置一個橫滾關(guān)節(jié)，兩個俯仰關(guān)節(jié)，可在手部目標(biāo)位置進(jìn)入手部工作空間后靈活執(zhí)行任務(wù)；頭部通過一個俯仰關(guān)節(jié)與軀干固連，本文未對其進(jìn)行控制，未來將通過加入頭部關(guān)節(jié)的控制實現(xiàn)對各種擾動與故障的反應(yīng)式補償。

由于機器人整體結(jié)構(gòu)較復(fù)雜，控制難度大，本文采用解耦的分布式控制架構(gòu)，腿與雙臂根據(jù)平臺高度要求與末端執(zhí)行器目標(biāo)位置的要求，作為兩個子控制系統(tǒng)進(jìn)行控制；虛擬倒立擺子系統(tǒng)通過控制輪的力矩，使得機器人在保持平衡的同時達(dá)到期望位置。針對以上三個控制模塊，展開各模塊的運動學(xué)與動力學(xué)建模，根據(jù)各模塊控制任務(wù)的特點，構(gòu)造適當(dāng)?shù)目刂破鳌?/p>

圖1 腿-輪復(fù)合式機器人結(jié)構(gòu)示意圖

本文僅考慮機器人姿態(tài)的矢狀面運動，將雙臂的橫滾關(guān)節(jié)鎖死，且左右手臂姿態(tài)相同，左右腿姿態(tài)相同，因此只在矢狀面中展開運動學(xué)、動力學(xué)建模與控制。文中涉及到的機器人參數(shù)及其定義如表1所示。

3 系統(tǒng)建模

3.1 腿部運動學(xué)與動力學(xué)建模

腿部運動學(xué)與動力學(xué)分析在軀干坐標(biāo)系中進(jìn)行。兩條腿結(jié)構(gòu)相同，簡化模型如圖2所示。采用D-H方法，在每一連桿上固接一個坐標(biāo)系，通過坐標(biāo)系之間的平移、旋轉(zhuǎn)關(guān)系描述連桿之間的相對位置。

表1 機器人參數(shù)及定義表

圖2 以軀干坐標(biāo)系為基坐標(biāo)系的腿部簡化模型

足端位置即腿部運動學(xué)方程為：

足端在y軸不存在運動，兩腿的雅可比矩陣相同：

采用拉格朗日方法對腿部子系統(tǒng)進(jìn)行動力學(xué)分析。腿部子系統(tǒng)的總動能可表示為：

3.2 雙臂運動學(xué)與動力學(xué)建模

手臂作為一個子系統(tǒng)，通過控制肩關(guān)節(jié)與肘關(guān)節(jié)的運動，使手臂末端到達(dá)期望位置。手臂的運動學(xué)與動力學(xué)分析在軀干坐標(biāo)系中進(jìn)行，其示意圖如圖3所示。

圖3 以軀干坐標(biāo)系為基坐標(biāo)系的手臂簡化模型

手臂正運動學(xué)方程為：

手臂雅可比矩陣為：

手臂總質(zhì)心位置坐標(biāo)為：

仍然采用拉格朗日方法建立動力學(xué)模型，并整理為如下標(biāo)準(zhǔn)形式

3.3 頭部運動學(xué)與動力學(xué)建模

頭部通過一個俯仰關(guān)節(jié)與軀干相連，作為一個子系統(tǒng)進(jìn)行控制。在軀干坐標(biāo)系中分析該子系統(tǒng)的運動學(xué)與動力學(xué)，如圖4所示。

頭部質(zhì)心在軀干坐標(biāo)系中的位置為：

頭部子系統(tǒng)的動力學(xué)描述為：

圖4 以軀干坐標(biāo)系為基坐標(biāo)系的頭部簡化模型

3.4 倒立擺運動學(xué)與動力學(xué)建模

虛擬倒立擺子系統(tǒng)是最為重要的一個子系統(tǒng)，它是保持機器人平衡的關(guān)鍵，是實現(xiàn)機器人整體運動的基礎(chǔ)，其簡化示意圖如圖5所示?？紤]到機器人采用分塊控制，利用雙腿、雙臂以及頭部運動學(xué)計算出位于軀干坐標(biāo)系中虛擬質(zhì)心的位置

坐標(biāo)系{oB}到{ow}的齊次變換矩陣為

此時，將身體各個連桿的質(zhì)心集中于一點，作為一級倒立擺的虛擬質(zhì)心，在輪軸坐標(biāo)系中對虛擬質(zhì)心位置進(jìn)行計算，利用公式（13）和公式（14）得到輪軸與虛擬質(zhì)心之間的連桿長度及虛擬俯仰角。

圖5 機器人簡化為虛擬倒立擺模型

假設(shè)該虛擬倒立擺的驅(qū)動輪始終與地面保持接觸，并且不產(chǎn)生滑動，其運動學(xué)滿足：

假設(shè)該虛擬倒立擺的驅(qū)動輪始終與地面保持接觸，并且不產(chǎn)生滑動，根據(jù)運動學(xué)關(guān)系得到如下三個非完整約束：

為得到這一約束方程組的零空間，即公式（17）的可行解空間，首先將該約束整理為的形式，其中，

將該約束條件利用拉格朗日乘數(shù)法引入動力學(xué)方程，動力學(xué)方程變?yōu)椋?/p>

4 控制器設(shè)計

由于系統(tǒng)的自由度較高，整體動力學(xué)系統(tǒng)的建立非常復(fù)雜，并且不利于滿足機器人控制的實時性?？紤]到工程應(yīng)用中對控制器執(zhí)行性能的要求，本文將整體機器人系統(tǒng)分為三個控制模塊，既相互獨立又互相影響，其關(guān)系框圖如圖6所示。

4.1 腿部與手臂的控制

腿部與手臂的控制有許多相似之處，都為控制兩個驅(qū)動俯仰關(guān)節(jié)使末端到達(dá)期望位置，本文對腿部與手臂的控制采用相同的控制器——加入前饋補償?shù)谋壤⒎址答伨€性化控制器。

其中，τff為前饋力矩，τfb為反饋力矩。狀態(tài)變量q為關(guān)節(jié)空間的變量，對于機器人腿與手臂的運動控制，運動聚焦在操作空間，因此期望值一般為末端位置，而不是關(guān)節(jié)角度值。利用公式（22）進(jìn)行非線性狀態(tài)坐標(biāo)變換：

其中，p為末端位置坐標(biāo)。解得：

圖6 平臺總體控制框圖

將公式（25）、（26）帶入公式（21）

此時，式（27）的動力學(xué)系統(tǒng)狀態(tài)變量為末端位置向量，輸入仍為關(guān)節(jié)力矩。在此基礎(chǔ)上構(gòu)造控制器保證末端位置p、速度、加速度分別漸進(jìn)收斂至期望位置軌跡pd、期望速度軌跡與期望加速度軌跡即

反饋線性化是處理非線性系統(tǒng)較為常見的一種控制方法，由內(nèi)環(huán)控制率與外環(huán)控制率組成，內(nèi)環(huán)控制率為：

其中，v1，v2為外環(huán)控制率的輸出：

所提控制方法所產(chǎn)生的輸入力矩為：

其控制框圖如圖7所示。

圖7 腿部與手臂控制框圖

4.2 虛擬倒立擺運動控制器

為推導(dǎo)方便，采用符號形式表示虛擬倒立擺動力學(xué)公式：

由公式（32）可看出子系統(tǒng)-5θ與子系統(tǒng)是耦合的，子系統(tǒng)-6θ則與另外兩個子系統(tǒng)完全解耦。整理得到如下形式的動力學(xué)方程：

4.2.1 狀態(tài)變量的獲取

機器人利用三個傳感器，分別為左右輪上的編碼器與軀干幾何中心處的慣性測量單元，獲取左右輪弧度信息與軀干俯仰角度。通過運動學(xué)推導(dǎo)得到虛擬倒立擺的俯仰角與質(zhì)心高度。但是該控制器中的狀態(tài)變量不能由傳感器直接獲得，需要根據(jù)機體位置約束，利用傳感器信息實時計算狀態(tài)變量，其中，俯仰角5θ為計算得到，由5θ差分得到。其他狀態(tài)量的計算如下：

其中，pc為機器人左右輪輪軸連線中點所行駛的總路程，對應(yīng)地，為行駛速度。

4.2.2 控制器設(shè)計

分別給出俯仰角、俯仰角速度、偏航角、偏航角速度、小車行駛路程、小車線速度期望值θ5d、構(gòu)造控制器，使得

設(shè)計一個PD控制器，兩個PID控制器，分別對俯仰角、偏航角、小車路程進(jìn)行控制。

兩個驅(qū)動輪的輸入力矩為：

5 仿真實驗

5.1 實驗設(shè)置

本文提出的控制方法利用仿真軟件Webots進(jìn)行仿真，仿真軟件運行環(huán)境為Core i7，采樣時間為100Hz。仿真模型如圖8所示，腿部兩個俯仰關(guān)節(jié)、手臂兩個俯仰關(guān)節(jié)以及頭部一個俯仰關(guān)節(jié)處安裝有位置傳感器，驅(qū)動輪裝有編碼器，軀干的幾何中心處裝有慣性測量單元。為驗證文中所提控制方法的有效性，分別進(jìn)行了平衡實驗、直行實驗、轉(zhuǎn)彎實驗、姿態(tài)調(diào)整實驗，并對數(shù)據(jù)進(jìn)行分析。

5.2 平衡控制

圖8 于Webots環(huán)境中搭建的機器人仿真平臺

該機器人的控制模塊中，作為其他控制模塊的前提，最為重要的是虛擬倒立擺的平衡控制，為了驗證所提方法的有效性與抗外擾能力，分別利用20kg的擺錘與70kg的擺錘垂直擊打處于平衡狀態(tài)的機器人后側(cè)，選取增益分別為k1=-25，k2=-5，此時，機器人能夠迅速回復(fù)到平衡狀態(tài)而不會失穩(wěn)。如圖9所示，分別展示了初始俯仰角與俯仰角速度都為0時，受到20kg與70kg擺錘擊打時，機器人恢復(fù)穩(wěn)定過程中的虛擬俯仰角度與虛擬俯仰角速度曲線?？煽闯鍪艿?0kg擺錘擊打時，虛擬俯仰角峰值達(dá)到11度，受到70kg擺錘擊打時，虛擬俯仰角峰值達(dá)到25.6度，都能夠在4s內(nèi)恢復(fù)平衡狀態(tài)，未出現(xiàn)震蕩。

5.3 直線運動控制

在成功實現(xiàn)平衡的前提下，希望機器人擺脫遙控操作，擺脫對參考位置曲線、參考速度曲線的要求，到達(dá)期望位置的同時達(dá)到期望速度。首先，對直行運動進(jìn)行實驗，選取增益為k3=-0.06，k4=-0.001，k5=-0.23，只給出期望位置pcd=5，期望速度機器人由世界坐標(biāo)系的原點出發(fā)，根據(jù)當(dāng)前位置、當(dāng)前速度，自主規(guī)劃下一時刻速度，最終使得實驗數(shù)據(jù)如圖10所示，機器人自主規(guī)劃平滑的速度曲線，先加速，后減速，當(dāng)速度降為0時，恰好到達(dá)期望位置。由虛擬俯仰角曲線圖可看出，機器人的加速度與俯仰角的方向和大小有關(guān)，當(dāng)機器人虛擬質(zhì)心前傾，存在前向加速度，且角度越大加速度越大，當(dāng)機器人虛擬質(zhì)心位于輪軸上方時，加速度為0，當(dāng)虛擬質(zhì)心后傾時，產(chǎn)生后向加速度。當(dāng)機器人到達(dá)期望位置時，虛擬俯仰角恰好恢復(fù)到平衡位置，未發(fā)生抖動。

5.4 曲線運動控制

令機器人跟隨一條曲線軌跡時，可將跟隨任務(wù)分解為直行與轉(zhuǎn)向，該實驗結(jié)合上文中的直行控制器，驗證航向角控制器的有效性。期望軌跡軌跡各點處弧度均不相同，其弧度曲線如圖11（b）中的期望軌跡所示，航向角控制器增益為k6=65，k7= 0 .0075，k8=3.4。此時，機器人能夠?qū)崿F(xiàn)位置與航向角的同時跟隨，由世界坐標(biāo)系的原點處出發(fā)，實際運行軌跡如圖12所示，左右輪的輸入力矩如圖13所示。

圖9 機器人受到外力沖擊時的虛擬俯仰角、俯仰角速度曲線

圖10 機器人直線運動時的實驗數(shù)據(jù)曲線

圖11 機器人轉(zhuǎn)彎運動的實驗數(shù)據(jù)曲線

圖12 機器人的行進(jìn)軌跡曲線

圖13 左右輪力矩曲線

5.5 身體姿態(tài)控制

同時進(jìn)行腿部、手臂位置跟隨實驗以驗證所提控制器的有效性。本實驗同時控制手臂末端與足端跟隨規(guī)劃的期望軌跡，規(guī)劃、控制、數(shù)據(jù)收集均在軀干坐標(biāo)系中進(jìn)行；平衡控制器對腿部、手臂運動產(chǎn)生的虛擬質(zhì)心前后移動所造成的機器人不穩(wěn)定進(jìn)行控制，對虛擬質(zhì)心位置的計算在輪軸坐標(biāo)系中進(jìn)行；位置控制器保證了機器人姿態(tài)變化時的位置跟隨能力，對機器人位置的數(shù)據(jù)收集在世界坐標(biāo)系中進(jìn)行?？刂谱笥沂直?、左右腿分別進(jìn)行相同運動，且無外擺，因此，只對手臂末端與足端的前后位置與上下位置進(jìn)行規(guī)劃即可。手臂控制器的增益為Kd1=800，Kp1=1000，Kp2=3000，手臂末端初始位置為（0,0.8），期望位置為（0.5,0.1），對兩點進(jìn)行三次樣條差值，得到圖14 （a）、（b）中的期望軌跡。腿部控制器的增益為Kd1=900，Kp1=1000，Kp2=4000，足端初始位置為（0,-0.94），期望位置為（0.01,-0.6），對兩點進(jìn)行三次樣條差值，得到圖14（c）、（d）中的期望軌跡。圖15（a）為虛擬倒立擺模型的身體質(zhì)心位置曲線，可見身體姿態(tài)的變化引起虛擬質(zhì)心的實時變化，由質(zhì)心位置所計算出的虛擬俯仰角的曲線如15（b）所示，俯仰角的變化范圍為［-0.015178，0.02756］rad，機器人處于穩(wěn)定狀態(tài)。其次，虛擬俯仰角的前后波動對機器人位置產(chǎn)生了影響，其變化范圍為［-0.100366，0.288195］ m，處于允許范圍。15（d）為同時保證機器人的平衡與位置輪的輸出力矩曲線，變化范圍為［-1.168327，1.681358］N·m。這一實驗在驗證了手臂、腿部控制器有效性的同時，也驗證了平衡控制單元與位置控制單元的抗持續(xù)外擾能力。

圖14 手臂與腿部姿態(tài)變化曲線

圖15 機器人姿態(tài)調(diào)節(jié)的相關(guān)實驗數(shù)據(jù)曲線

6 結(jié) 論

本文提出一種腿-輪復(fù)合機器人，并對平臺進(jìn)行運動學(xué)、動力學(xué)建模與控制?？紤]到整體建模與控制的復(fù)雜度，將平臺分為手臂子系統(tǒng)、腿部子系統(tǒng)、頭部子系統(tǒng)以及虛擬倒立擺子系統(tǒng)，構(gòu)造基于笛卡爾狀態(tài)空間的加入前饋補償?shù)谋壤鲆娣答伨€性化控制器對手臂子系統(tǒng)與腿部子系統(tǒng)進(jìn)行控制；將各子系統(tǒng)的質(zhì)心映射為虛擬倒立擺子系統(tǒng)的桿件質(zhì)心，并作為一級倒立擺進(jìn)行控制，實現(xiàn)平臺的前后移動與轉(zhuǎn)彎運動。