球形機(jī)器人軌跡跟蹤算法

汪 磊，戰(zhàn) 強(qiáng)

（北京航空航天大學(xué) 機(jī)械工程及自動(dòng)化學(xué)院，北京 100191）

0 引言

與傳統(tǒng)的輪式機(jī)器人相比，球形機(jī)器人有其固有的優(yōu)勢(shì)：滾動(dòng)中的球自然會(huì)選擇阻力最小的路徑，因此球形可以令機(jī)器人在不同障礙或地形中運(yùn)動(dòng)［1］。在過去的幾十年里提出過的一些典型球形機(jī)器人模型存在很多阻礙球形機(jī)器人大規(guī)模投入實(shí)用的問題，其中最主要的就是運(yùn)動(dòng)控制的復(fù)雜性，目前仍然沒有有效的球形機(jī)器人控制方法。本文對(duì)BHQ－1球形機(jī)器人的軌跡跟蹤問題［2］進(jìn)行了探討，著眼于結(jié)構(gòu)如BHQ－1的球形機(jī)器人軌跡跟蹤控制的解決方法［3］。

1 球形機(jī)器人模型

1．1 物理系統(tǒng)描述

球形機(jī)器人BHQ－1的機(jī)械結(jié)構(gòu)如圖1所示，它由電機(jī)1和2、空心軸3、重塊4、攝像頭5和一副球殼6組成。兩個(gè)電機(jī)的轉(zhuǎn)動(dòng)可使機(jī)器人重心改變?nèi)缓螽a(chǎn)生一個(gè)重力轉(zhuǎn)矩驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)，因此可以通過控制兩個(gè)電機(jī)來控制球形機(jī)器人的運(yùn)動(dòng)。

圖1 BHQ－1球形機(jī)器人結(jié)構(gòu)

1．2 運(yùn)動(dòng)學(xué)模型

當(dāng)球形移動(dòng)機(jī)器人在平面上運(yùn)動(dòng)時(shí)，球殼的運(yùn)動(dòng)模型可簡(jiǎn)化為如圖2所示。其中ΣO｛X，Y，Z｝為慣性坐標(biāo)系與地面固聯(lián)，Σb｛Xb，Yb，Zb｝為球形移動(dòng)機(jī)器人的載體坐標(biāo)系，其中Ob與機(jī)器人球殼的中心固聯(lián)，球殼的半徑為R，球殼與地面的接觸點(diǎn)Pc在慣性坐標(biāo)下的坐標(biāo)為（xc，yc），φ、β、ψ分別為繞 Xb、Yb、Zb旋轉(zhuǎn)的角度，采用ZYX 歐拉角表示為（ψ，β，φ），τφ、τβ分別為繞Xb、Yb旋轉(zhuǎn)的剪切應(yīng)力。球形機(jī)器人的運(yùn)動(dòng)學(xué)方程可寫成：

圖2 球形機(jī)器人運(yùn)動(dòng)學(xué)模型示意圖

可見球形機(jī)器人系統(tǒng)運(yùn)動(dòng)學(xué)方程是一個(gè)2輸入5輸出的一階非線性方程，并且速度項(xiàng)不可積分，針對(duì)輪式機(jī)器人提出的非完整性約束系統(tǒng)控制方法無(wú)法直接應(yīng)用于球形移動(dòng)機(jī)器人。

2 軌跡跟蹤算法設(shè)計(jì)

2．1 動(dòng)態(tài)軌跡跟蹤

機(jī)器人的軌跡跟蹤與路徑跟隨不同，軌跡跟蹤的理想軌跡是一條與時(shí)間呈一定關(guān)系的幾何曲線f（xr（t），yr（t），θr（t））。

機(jī)器人軌跡跟蹤的誤差模型如圖3所示，機(jī)器人的位姿坐標(biāo)為（x，y，θ），跟隨目標(biāo)的位姿坐標(biāo)為（xr，yr，θr），得到的誤差模型可寫成：

其中：de為目標(biāo)參考點(diǎn)位置與機(jī)器人位置的絕對(duì)距離差；θe為目標(biāo)參考點(diǎn)至機(jī)器人連線與此時(shí)航向角的差。

圖3 軌跡跟蹤誤差模型示意圖

位置跟蹤控制器的期望輸入位移差為0，當(dāng)目標(biāo)點(diǎn)與機(jī)器人的位移差增大，則PID控制輸出足夠大的前進(jìn)速度指令用于追趕目標(biāo)點(diǎn)以縮小位移差。設(shè)ud為位置跟蹤控制器的輸出值：

其中：kp、kd、ki分別為比例、微分和積分系數(shù)。

航向跟蹤控制器與位置跟蹤控制器類似，通過航向誤差信號(hào)θe，由控制器輸出角速度控制指令用于修正機(jī)器人的航向角，從而使機(jī)器人與目標(biāo)點(diǎn)航向保持一致。設(shè)uθ為航向跟蹤控制器的輸出值：針對(duì)不同的輸入變量在同一個(gè)參數(shù)的作用下并不能輸出最優(yōu)的控制策略，尤其針對(duì)不同軌跡的跟蹤將表現(xiàn)出不同的控制性能。普通的PID控制器并不能有效地抑制外界擾動(dòng)，當(dāng)擾動(dòng)過大甚至可能造成控制失效。為此對(duì)上述提出的軌跡跟蹤算法提出如下的改進(jìn)方式［4］。

2．2 模糊自適應(yīng)控制方法

為了提高算法的自適應(yīng)性及魯棒性，采用模糊數(shù)學(xué)方法將規(guī)則條件、控制策略用模糊集合表示，并將這些模糊規(guī)則及策略存儲(chǔ)在嵌入式控制系統(tǒng)中，同時(shí)控制器根據(jù)外界實(shí)際的輸入變量運(yùn)用模糊推理在線對(duì)PID的參數(shù)進(jìn)行自適應(yīng)整定，根據(jù)不同軌跡的輸入變量自適應(yīng)調(diào)節(jié)PID參數(shù)。自適應(yīng)模糊PID控制器以誤差e和誤差的變化率ec作為輸入，可以實(shí)現(xiàn)在線根據(jù)不同e和ec情況對(duì)PID參數(shù)自整定［5］。

對(duì)于模糊控制器，誤差信號(hào)與微分信號(hào)的輸入效果十分重要。為了更好地獲得一個(gè)信號(hào)的最佳逼近同時(shí)獲取該信號(hào)的微分，本課題采用非線性跟蹤微分器（NTD）來實(shí)現(xiàn)［6］。跟蹤微分器的動(dòng)態(tài)系統(tǒng)可描述為［7］：

由于PID控制器是一個(gè)參數(shù)敏感的控制方式，其

其中：r為跟蹤速度因子，決定了跟蹤效果的快慢，當(dāng)r值較大時(shí)跟蹤速度越快；T 為采樣周期；fst［x1（k），x2（k），v（k），r，T］為用于跟蹤信號(hào)v（k）的二階導(dǎo)數(shù)，其定義如下：

其中：v（t）為輸入信號(hào)；輸出信號(hào)x1為v（t）的跟蹤信號(hào)；x2為v（t）的微分信號(hào)。

為了更好地實(shí)現(xiàn)嵌入式系統(tǒng)，將其進(jìn)行離散化處理，得到：

其中：δ為跟蹤信號(hào)v（k）的變化量；

y（k）＝x1（k）－v（k）＋Tx2（k）。

根據(jù)各參數(shù)修正的模糊規(guī)則進(jìn)行在線推理，得到了相應(yīng)的模糊輸出子集，由于被控對(duì)象只能接收一個(gè)精確的控制量，無(wú)法接收這種模糊控制量，因此必須經(jīng)過清晰化處理將其轉(zhuǎn)換為精確量，本設(shè)計(jì)采用加權(quán)平均法進(jìn)行去模糊處理。處理后的控制變量乘以一個(gè)量化因子α后作用在相應(yīng)的PID參數(shù)上，即模糊自適應(yīng)PID控制器的參數(shù)可表示為：

其中：Kp、Ki、Kd分別為去模糊處理后的修正比例、積分、微分系數(shù)；Kp0、Ki0、Kd0分別為上述系數(shù)的初始值。

3 仿真研究

為了驗(yàn)證設(shè)計(jì)算法的可行性，以BHQ－1球形機(jī)器人為模型對(duì)象，針對(duì)式（1）球形機(jī)器人運(yùn)動(dòng)學(xué)模型，對(duì)直線軌跡下的軌跡跟蹤問題進(jìn)行了仿真，球形機(jī)器人原始位姿為（x＝0，y＝0，ψ＝－π／2，β＝0，φ＝0）。目標(biāo)跟蹤軌跡為x＝0．2t，y＝0，即目標(biāo)點(diǎn)跟蹤速度為0．2m／s。普通和變參數(shù)PID控制器的仿真曲線分別如圖4、圖5所示。

圖4 普通PID控制器的直線軌跡跟蹤仿真曲線

采用了模糊自適應(yīng)PID控制器后，機(jī)器人能夠迅速地跟蹤上目標(biāo)路徑，調(diào)節(jié)時(shí)間小于1s，并且系統(tǒng)超調(diào)微小。

4 結(jié)論

雖然傳統(tǒng)的動(dòng)態(tài)軌跡跟蹤算法對(duì)于非完整移動(dòng)機(jī)器人具有良好的跟蹤性能，但仿真結(jié)果表明，這并不適合2自由度擺球形機(jī)器人。采用經(jīng)典PID控制策略雖然能夠有效地跟蹤上目標(biāo)曲線，但是由于各個(gè)PID參數(shù)固定，針對(duì)不同路徑的跟蹤表現(xiàn)出的性能不盡相同。而模糊自適應(yīng)PID控制器能夠根據(jù)機(jī)器人與目標(biāo)位置的誤差信號(hào)實(shí)時(shí)調(diào)整PID參數(shù)，使機(jī)器人的位姿得到最優(yōu)的調(diào)整。

圖5 變參數(shù)PID控制器的直線軌跡跟蹤仿真曲線

［1］ Armour Ｒ Ｈ，Vincent Ｊ Ｆ Ｖ．Rolling in nature and robotics：a review［J］．Journal of Bionic Engineering，2006，3：195－208．

［2］ Zhan Q，Liu Z，Cai Y．A back－stepping based trajectory tracking controller for a non－chained nonholonomic spherical robot［J］．Chinese Journal of Aeronautics，2008，21：472－480．

［3］ Chen M，Zhan Q，Liu Z．Optimal trajectory planning based on Hamiltonian function of a spherical mobile robot［J］．Chinese High Technology Letters，2008，14：289－293．

［4］ 曹承志．微型計(jì)算機(jī)控制新技術(shù)［M］．北京：機(jī)械工業(yè)出版社，2000．

［5］ 孫增圻，鄧志東，張?jiān)倥d．智能控制理論與技術(shù)［M］．北京：清華大學(xué)出版社；南寧：廣西科學(xué)技術(shù)出版社，2006．

［6］ 韓京清，王偉．非線性跟蹤—微分器［J］．系統(tǒng)科學(xué)與數(shù)學(xué)，1994，14（2）：177－183．

［7］ 解永清，鄭恩讓．帶有微分－跟蹤器的無(wú)模型自適應(yīng)控制研究［J］．電氣自動(dòng)化，2009，31（2）：16－19．