基于backstepping方法的兩輪移動(dòng)機(jī)器人軌跡跟蹤控制

陳 瀅，陳 煒*，楊賀賀，孫玉超，吳 航

（1.天津理工大學(xué)a.天津市先進(jìn)機(jī)電系統(tǒng)設(shè)計(jì)與智能控制重點(diǎn)實(shí)驗(yàn)室，b.機(jī)電工程國(guó)家級(jí)實(shí)驗(yàn)教學(xué)示范中心，天津300384；2.天津理工大學(xué)中環(huán)信息學(xué)院，天津300380；

3.軍事科學(xué)院 系統(tǒng)工程研究院衛(wèi)勤保障技術(shù)研究所，天津300161）

移動(dòng)機(jī)器人學(xué)是機(jī)器人學(xué)中的一個(gè)重要分支[1]。如今，移動(dòng)機(jī)器人在軍事、航海等領(lǐng)域能代替人類完成大量的危險(xiǎn)任務(wù)，例如掃雷，海底勘測(cè)，無(wú)人駕駛，煤礦和地下工作等[2-5]。移動(dòng)機(jī)器人也在人類的生產(chǎn)生活中發(fā)揮著重要的作用。在過去的20年中，由于理論和實(shí)際應(yīng)用的需要，移動(dòng)機(jī)器人的控制問題一直受到人們廣泛關(guān)注。其控制問題一般可分為點(diǎn)鎮(zhèn)定和軌跡跟蹤問題。

隨著科學(xué)水平不斷提高，移動(dòng)機(jī)器人軌跡跟蹤問題受到越來(lái)越多研究者的關(guān)注。由于移動(dòng)機(jī)器人是一種非完整約束的系統(tǒng)[6]，該系統(tǒng)不可積，所以移動(dòng)機(jī)器人軌跡規(guī)劃、鎮(zhèn)定等問題較為復(fù)雜。近年來(lái)，輪式移動(dòng)機(jī)器人已經(jīng)在運(yùn)動(dòng)學(xué)控制方面進(jìn)行了廣泛的研究。例如，文獻(xiàn)[7-10]首先對(duì)移動(dòng)機(jī)器人的物理模型進(jìn)行推導(dǎo)研究，并在機(jī)器人具有物理模型的基礎(chǔ)下，根據(jù)不同控制方法對(duì)移動(dòng)機(jī)器人的靜態(tài)軌跡跟蹤進(jìn)行了大量的研究。

文獻(xiàn)[11]中沒有考慮移動(dòng)機(jī)器人系統(tǒng)的非線性，特別是系統(tǒng)消除未知參數(shù)線性化的假設(shè)，忽略建模參數(shù)，使用模糊自適應(yīng)神經(jīng)網(wǎng)絡(luò)研究控制，并直接在線研究系統(tǒng)模型。這對(duì)復(fù)雜機(jī)器人的研究具有深遠(yuǎn)的意義。但是由于移動(dòng)機(jī)器人的特性，直接忽略機(jī)器人的非線性特性在實(shí)際操作中是不可取的。文獻(xiàn)[12]提出了一種運(yùn)用計(jì)算力矩法對(duì)移動(dòng)機(jī)器人方程進(jìn)行反饋線性化，并使用滑膜控制對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)行跟蹤。但是并沒有對(duì)模型不確定性的移動(dòng)機(jī)器人的動(dòng)力學(xué)進(jìn)行建模。

文獻(xiàn)[13]設(shè)計(jì)了一種滑膜軌跡跟蹤控制器，盡管該控制方法可以較好的處理模型不確定性及外部干擾，但是卻存在抖振問題。文獻(xiàn)[14]考慮到移動(dòng)機(jī)器人運(yùn)動(dòng)的物理約束條件，提出了一種結(jié)合基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)優(yōu)化的模型預(yù)測(cè)控制方法來(lái)實(shí)現(xiàn)兩輪驅(qū)動(dòng)機(jī)器人的軌跡跟蹤控制。但是該方法主要問題是計(jì)算量過大。文獻(xiàn)[15]提出了一種與以往控制不同的控制器，一種以運(yùn)動(dòng)學(xué)和動(dòng)態(tài)線性級(jí)聯(lián)的形式提供類似反饋控制，是一種具有更簡(jiǎn)單和模塊化的控制結(jié)構(gòu)。由上述文獻(xiàn)看出近年來(lái)對(duì)移動(dòng)機(jī)器人控制的研究主要集中在運(yùn)動(dòng)學(xué)上。但是在大多數(shù)情況下，動(dòng)力學(xué)問題不容忽視。本文的目的是分析和設(shè)計(jì)包括運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)在內(nèi)的移動(dòng)機(jī)器人的控制算法。

1 兩輪移動(dòng)機(jī)器人的建模

兩輪移動(dòng)機(jī)器人建模包括運(yùn)動(dòng)學(xué)建模和動(dòng)力學(xué)建模。運(yùn)動(dòng)學(xué)建模解決了速度和位置之間的問題。動(dòng)力學(xué)模型描述了速度與輸入力之間的關(guān)系。兩輪移動(dòng)機(jī)器人的原理圖如圖1所示。點(diǎn)p是兩輪移動(dòng)機(jī)器人的中心，點(diǎn)G是兩輪移動(dòng)機(jī)器人的重要點(diǎn)。點(diǎn)p與點(diǎn)G重合。兩輪移動(dòng)機(jī)器人由全局坐標(biāo)系中的向量表示。（x，y，θ）是移動(dòng)平臺(tái)轉(zhuǎn)換的參考坐標(biāo)。兩個(gè)驅(qū)動(dòng)輪的半徑是r，兩個(gè)輪子之間的距離是l。

圖1 移動(dòng)輪式機(jī)器人原理圖Fig.1 Structure of the two-wheel mobile robot

1.1 運(yùn)動(dòng)學(xué)建模

建立兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型的目的是了解其運(yùn)動(dòng)狀態(tài)。兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型如圖1所示。

令p=（x，y，θ）T，q=（v，w）T，其中（x，y）為兩輪移動(dòng)機(jī)器人的位置坐標(biāo)，θ為轉(zhuǎn)向角，是兩輪移動(dòng)機(jī)器人行進(jìn)方向和x正半軸的夾角，v和w分別為兩輪移動(dòng)機(jī)器人的速度和角速度。

假設(shè)兩輪移動(dòng)機(jī)器人在地面只做純滾動(dòng)運(yùn)動(dòng)，無(wú)打滑，則兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型[16]為

根據(jù)坐標(biāo)變化，引入跟蹤誤差的概念，得到兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)誤差方程

其中pe為理想位姿。

對(duì)兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)控制，就是通過控制輸入q=（v，w）T，來(lái)得到任意時(shí)刻的位姿誤差。

1.2 動(dòng)力學(xué)建模

假設(shè)兩輪移動(dòng)機(jī)器人在平面內(nèi)移動(dòng)[17]，設(shè)定全局坐標(biāo)xoy為固定坐標(biāo)系，則兩輪移動(dòng)機(jī)器人的動(dòng)力學(xué)方程推導(dǎo)如下。

對(duì)于移動(dòng)平臺(tái)，由力矩平衡原理[18]，平臺(tái)轉(zhuǎn)動(dòng)角度=動(dòng)力轉(zhuǎn)矩-阻力轉(zhuǎn)矩，得

由牛頓第二定律，得

其中，IV是機(jī)器人重心的轉(zhuǎn)動(dòng)慣量；Fl和Fr是左右輪的驅(qū)動(dòng)力；l為輪子到移動(dòng)平臺(tái)重心的距離；φ，v分別為移動(dòng)平臺(tái)的位姿角和線速度。

對(duì)于移動(dòng)平臺(tái)的車輪，根據(jù)力矩平衡原理，左右輪動(dòng)態(tài)特性方程分別如下：IW為輪子的轉(zhuǎn)動(dòng)慣量，c為摩擦系數(shù)，k為驅(qū)動(dòng)增益，u為驅(qū)動(dòng)輸入，θ、r分別指車輪轉(zhuǎn)角和半徑。

由兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)，得

由式（6）可得

由式（5）可得

則

由式（7）可得

則

將上式分別代入（4）和（5），得

則可分別得到

將式（16）和（17）寫成兩輪移動(dòng)機(jī)器人動(dòng)力學(xué)狀態(tài)方程為

2 設(shè)計(jì)控制規(guī)律

本文采用backstepping控制分別對(duì)兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)進(jìn)行控制設(shè)計(jì)。

2.1 運(yùn)動(dòng)學(xué)控制規(guī)律

考慮式（2）所示的運(yùn)動(dòng)學(xué)系統(tǒng)，設(shè)計(jì)軌跡為

其中xd，yd，θd表示移動(dòng)機(jī)器人的理想位姿。

backstepping控制器的設(shè)計(jì)分為兩步：

1）令虛擬輸入α，根據(jù)式（2），取

令Lyapunov函數(shù)[19]為

由式（2）和式（19）式可得

通過設(shè)計(jì)虛擬量α，使得

則

將線速度和虛控制律設(shè)計(jì)為

2）令e=α-θ，定義Lyapunov函數(shù)為

則

設(shè)計(jì)角速度控制律

則

則t→∞時(shí)，xe→0，ye→0，θ→θd且以指數(shù)形式收斂。兩輪移動(dòng)機(jī)器人動(dòng)力學(xué)控制器結(jié)構(gòu)如圖2所示。

圖2 兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)控制結(jié)構(gòu)Fig.2 The process of kinematics control

2.2 動(dòng)力學(xué)控制規(guī)律

兩輪移動(dòng)機(jī)器人動(dòng)力學(xué)狀態(tài)方程（18）為線性方程。在設(shè)計(jì)動(dòng)力學(xué)控制規(guī)律前，需要將式（18）進(jìn)行線性解耦。

將式（29）代入式（27）可得到如下兩個(gè)控制方程：

式（31）設(shè)計(jì)控制規(guī)律為：

假設(shè)理想速度為vd，速度誤差為ve=vd-v。定義Lyapunov函數(shù)

則

設(shè)計(jì)控制規(guī)律為

式（32）設(shè)計(jì)控制規(guī)律為：

假設(shè)理想角速度為φd，誤差為z1=φ-φd，給定控制變量

其中c2＞0。

定義Lyapunov函數(shù)

則

定義Lyapunov函數(shù)

則

設(shè)計(jì)控制規(guī)律為

得到

則V（t）≤e-2CmtV（0），當(dāng)且僅當(dāng)t→∞時(shí)，φ˙→設(shè)計(jì)好的動(dòng)力學(xué)控制器結(jié)構(gòu)如圖3所示。

圖3 兩輪移動(dòng)機(jī)器人動(dòng)力學(xué)控制結(jié)構(gòu)Fig.3 The process of dynamics control

3 數(shù)值仿真

為驗(yàn)證控制規(guī)律設(shè)計(jì)的有效性，分別對(duì)兩輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)進(jìn)行仿真，并且進(jìn)行路徑為圓形的軌跡跟蹤。

3.1 運(yùn)動(dòng)學(xué)仿真

為驗(yàn)證該方法的有效性，給出兩輪移動(dòng)機(jī)器人的理想運(yùn)動(dòng)軌跡，跟蹤移動(dòng)機(jī)器人預(yù)設(shè)位置。設(shè)兩輪移動(dòng)機(jī)器人的初始位置為［1 1 0］，其中位姿為x=cost，y=sint。控制律為式（24）和（27）。控制參數(shù)設(shè)c1=c2=c3=10，仿真結(jié)果如圖4～圖8所示。

圖4 x軸方向位置跟蹤Fig.4 The trajectory in x axis

圖6 速度控制輸入Fig.6 The linear velocity input of the vehicle

圖7 角速度控制輸入Fig.7 The angular velocity input of the vehicle

圖8 位置軌跡跟蹤Fig.8 The trajectory of the vehicle using kinematics control

從圖4運(yùn)動(dòng)曲線看出，兩輪移動(dòng)機(jī)器人在x軸方向按照給定參考軌跡移動(dòng)時(shí)，兩輪移動(dòng)機(jī)器人位置的瞬時(shí)值在初始狀態(tài)下，與初始值偏差較大，在backstepping控制器的控制下，兩輪移動(dòng)機(jī)器人能夠在0.50 s的時(shí)間內(nèi)，能夠迅速縮小和參考軌跡的誤差。在0.5～20 s時(shí)間段內(nèi)，實(shí)際軌跡和參考軌跡幾乎重合。同理可得圖5，兩輪移動(dòng)機(jī)器人在0.65 s后，實(shí)際軌跡與參考軌跡幾乎重合。并且由圖5看出，機(jī)器人運(yùn)動(dòng)軌跡平滑，無(wú)跳躍，說明該控制器在控制運(yùn)動(dòng)學(xué)有著良好的全局穩(wěn)定性。

圖5 y軸方向位置跟蹤Fig.5 The trajectory in y axis

3.2 動(dòng)力學(xué)仿真

為驗(yàn)證backstepping控制規(guī)律在兩輪移動(dòng)機(jī)器人動(dòng)力學(xué)運(yùn)動(dòng)中的有效性，本文采用兩輪移動(dòng)機(jī)器人使用控制器前后的動(dòng)力學(xué)進(jìn)行比較。取線速度為vd=1.0，角度φd=-cost。采用式（34）、（41）作為動(dòng)力學(xué)控制規(guī)律，取參數(shù)c1=c2=c3=10。

從仿真圖9看出，未采用控制之前，兩輪移動(dòng)機(jī)器人實(shí)際轉(zhuǎn)向角度與理想轉(zhuǎn)向角度值相差較遠(yuǎn)。轉(zhuǎn)矩曲線圖10中，未采用控制器的兩輪移動(dòng)機(jī)器人左右輪轉(zhuǎn)矩曲線波動(dòng)明顯。對(duì)比圖11和圖12，采用backstepping控制器后，兩輪移動(dòng)機(jī)器人的轉(zhuǎn)動(dòng)實(shí)際角度與期望角度在0～2 s有較大偏差，2 s后實(shí)際角度與理想角度曲線重合，成給定正弦曲線變化。采用控制器后的輸出左右輪轉(zhuǎn)矩曲線相較于未采用之前，曲線波動(dòng)在1 s后逐漸趨于平穩(wěn)光滑。

圖9 未用backstepping控制器的兩輪移動(dòng)機(jī)器人的角度跟蹤Fig.9 Angle tracking of the vehicle without backstepping controller

圖10 未使用backstepping控制器時(shí)兩輪移動(dòng)機(jī)器人的轉(zhuǎn)矩跟蹤Fig.10 Torque tracking of the vehicle without backstepping controller

圖11 基于backstepping方法的兩輪移動(dòng)機(jī)器人的角度跟蹤Fig.11 Angle tracking of the vehicle based on backstepping controller

圖12 基于backstepping控制器時(shí)兩輪移動(dòng)機(jī)器人的轉(zhuǎn)矩跟蹤Fig.12 Torque tracking of the vehicle based on backstepping controller

從仿真結(jié)果可以看出，本文所設(shè)計(jì)的控制律均能使各誤差參數(shù)快速收斂于0。該控制規(guī)律有以下優(yōu)點(diǎn)：引入虛擬量，采用backstepping控制器對(duì)運(yùn)動(dòng)學(xué)的線速度和角速度進(jìn)行控制，對(duì)動(dòng)力學(xué)的角度和轉(zhuǎn)矩進(jìn)行控制。如圖8和圖13所示，該控制器具有良好的穩(wěn)定性和魯棒性，采用Lyapunov函數(shù)提高了跟蹤控制精度，實(shí)現(xiàn)了兩輪移動(dòng)機(jī)器人快速有效跟蹤參考軌跡。

圖13 位置軌跡跟蹤Fig.13 The trajectory of the vehicle using dynamics control

4 結(jié)論

本文在兩輪移動(dòng)機(jī)器人的模型基礎(chǔ)上，設(shè)計(jì)了backstepping控制算法對(duì)兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)行了控制，通過仿真驗(yàn)證了該控制方法的準(zhǔn)確性。首先對(duì)其運(yùn)動(dòng)學(xué)進(jìn)行軌跡跟蹤控制，針對(duì)兩輪移動(dòng)機(jī)器人的運(yùn)動(dòng)特性，對(duì)其軌跡跟蹤進(jìn)行了深入的研究。其次對(duì)其動(dòng)力學(xué)進(jìn)行控制算法的設(shè)計(jì)，使輸入量位置和理論位置保持在一定范圍內(nèi)，使機(jī)器人運(yùn)動(dòng)軌跡有很好的平滑性。全文采用Lyapunov穩(wěn)定定理保證了兩輪移動(dòng)機(jī)器人系統(tǒng)的全局漸進(jìn)穩(wěn)定性。