智能小車的路徑跟蹤控制算法研究

楊 權(quán)，蔡 勇

（西南科技大學(xué) 制造科學(xué)與工程學(xué)院，四川 綿陽(yáng) 621010）

0 引言

在路徑跟蹤中，有時(shí)需要跟蹤一條與時(shí)間無(wú)關(guān)的幾何路徑（不帶時(shí)間參數(shù)的二維幾何路徑），比如地上的電纜、有色亮帶等等，這些都屬于信標(biāo)路徑，需要人為事先進(jìn)行實(shí)物的鋪設(shè)。針對(duì)這種基于路標(biāo)信息的路徑跟蹤［1］，可以把該幾何路徑劃分成若干個(gè)路標(biāo)節(jié)點(diǎn)，而各路標(biāo)節(jié)點(diǎn)就構(gòu)成了此路徑的全部情況，即小車從初始位姿出發(fā)，跟蹤的不是一條幾何路徑，而是一個(gè)一個(gè)路標(biāo)節(jié)點(diǎn)，直至最后到達(dá)終點(diǎn)，完成路徑跟蹤的目的。

本文以3輪式智能小車為例，建立極坐標(biāo)方式的位姿誤差模型，利用李雅普諾夫（Lyapunov）直接法設(shè)計(jì)跟蹤控制律，通過(guò)MATLAB平臺(tái)仿真，驗(yàn)證此方法的有效性。

1 路徑跟蹤問(wèn)題描述

首先建立笛卡爾直角坐標(biāo)系XOY，為方便描述智能小車的位姿，建立極坐標(biāo)系，如圖1所示。圖1（a）中是簡(jiǎn)化的3輪式智能小車，ρ表示小車當(dāng)前位姿與目標(biāo)位姿之間的距離，α表示小車初始位姿方向與ρ之間的角度，其中以初始點(diǎn)到目標(biāo)點(diǎn)的方向?yàn)闃O軸方向，定義逆時(shí)針?lè)较蜣D(zhuǎn)動(dòng)為α正值。圖1（b）中，φ表示小車初始位姿方向角與目標(biāo)點(diǎn)方向角的角度差，極軸方向?yàn)樾≤囋谀繕?biāo)點(diǎn)時(shí)的朝向，定義逆時(shí)針?lè)较蜣D(zhuǎn)動(dòng)φ為正值，即φ∈［－π，π］。

在直角坐標(biāo)系下給定任意一個(gè)目標(biāo)位姿qr＝［xryrθr］T，其中，xr，yr，θr均是常數(shù)。設(shè)小車的當(dāng)前位姿為q＝［xyθ］T，在全局坐標(biāo)系XOY下描述全局位姿誤差qe，可得：

對(duì)式（2）中第一個(gè)方程求導(dǎo)：

對(duì)式（2）中第二個(gè)方程求導(dǎo)：

其中：v為小車的線速度；ω為小車的角速度。

對(duì)式（2）中第三個(gè)方程求導(dǎo)：

因此，由式（2）所求得的位姿誤差微分方程為：

圖1 用極坐標(biāo)表示的智能小車位姿誤差分析示意圖

2 路徑跟蹤控制器設(shè)計(jì)

根據(jù)本文所述設(shè)計(jì)的姿態(tài)跟蹤控制系統(tǒng)的方框圖如圖2所示。

圖2 姿態(tài)跟蹤控制系統(tǒng)方框圖

利用李雅普諾夫直接法設(shè)計(jì)跟蹤控制律，取極坐標(biāo)變量ρ和α為設(shè)計(jì)對(duì)象，構(gòu)造李雅普諾夫函數(shù)：

從式（4）中明顯看出，V≥0，當(dāng)且僅當(dāng)［ρα］T＝0時(shí)，V＝0。對(duì)式（4）求導(dǎo)來(lái)確定系統(tǒng)的控制律：

根據(jù)式（5）取系統(tǒng)的控制律：

其中：k1，k2均為大于零的常數(shù)。下面根據(jù)取得的系統(tǒng)控制律來(lái)判斷系統(tǒng)的穩(wěn)定性，把控制律（6）代入構(gòu)造的李雅普諾夫函數(shù)得：

由于k1，k2是大于0的常數(shù)，所以可得≤0，因此V（t）滿足全局一致漸近穩(wěn)定的條件。由此可以得到V為連續(xù)可微正定函數(shù)且有界，為負(fù)半定且一致連續(xù)的函數(shù)，根據(jù)Barbalat引理［4］可以得到，當(dāng)t→＋∞，時(shí)，間接可以得到ρ→0且α→0，滿足設(shè)計(jì)要求。

3 MATLAB的仿真與分析

為了驗(yàn)證此方法的有效性和收斂性，在MATLAB 7.0軟件平臺(tái)上利用所設(shè)計(jì)的跟蹤控制律，編寫(xiě)仿真程序進(jìn)行仿真實(shí)驗(yàn)。

本節(jié)仿真所考慮的路徑跟蹤是一個(gè)一個(gè)的節(jié)點(diǎn)，所以在進(jìn)行仿真時(shí)，針對(duì)一個(gè)目標(biāo)點(diǎn)，從最開(kāi)始的位置出發(fā)進(jìn)行跟蹤仿真實(shí)驗(yàn)，從得出的跟蹤效果圖來(lái)分析此控制律的有效性，從極坐標(biāo)變量的變化曲線來(lái)分析坐標(biāo)變量是否收斂。

為了跟蹤給定的目標(biāo)點(diǎn)，小車有可能產(chǎn)生過(guò)大的速度，其帶來(lái)的過(guò)大加速度會(huì)破壞小車的非完整約束，容易造成小車橫向的移動(dòng)或者縱向的滑動(dòng)。為了避免上述問(wèn)題，必須對(duì)小車的速度進(jìn)行一個(gè)速度限制策略，取vmax＝1.5m/s，ωmax＝15rad/s。經(jīng)過(guò)多次實(shí)驗(yàn)之后，取控制器參數(shù)k1＝0.2，k2＝1.2，下面針對(duì)控制律式（6）進(jìn)行目標(biāo)點(diǎn)的跟蹤仿真實(shí)驗(yàn)。

圖3 目標(biāo)點(diǎn)跟蹤效果圖

圖4 速度變化曲線

從圖3中可以看出：小車有效地跟蹤到了目標(biāo)點(diǎn)。從圖4中可以看出：小車的線速度和角速度最大值都在限制范圍之內(nèi)；小車的角速度在3s內(nèi)便快速地收斂到零。從圖5中可以看出：坐標(biāo)變量ρ和α最終都會(huì)收斂于0；而φ并沒(méi)有收斂于0，而是收斂于一個(gè)常數(shù)。分析其原因在于，φ表示小車目標(biāo)位姿的方向角與小車當(dāng)前位姿的方向角的差值，當(dāng)小車跟蹤目標(biāo)點(diǎn)時(shí)，由ρ和α決定跟蹤狀態(tài)，φ并不能決定小車是否鎮(zhèn)定于目標(biāo)點(diǎn)，它只代表跟蹤到目標(biāo)點(diǎn)時(shí)一個(gè)數(shù)學(xué)意義上的差值，當(dāng)進(jìn)入到下一個(gè)跟蹤狀態(tài)時(shí)，會(huì)重新賦予一個(gè)ρ和α的數(shù)值，所以構(gòu)造函數(shù)時(shí)以坐標(biāo)變量ρ和α為設(shè)計(jì)對(duì)象是正確的。由此可以看出，在控制律式（6）作用下，小車能夠很好地實(shí)現(xiàn)目標(biāo)點(diǎn)的跟蹤，證明了此控制律是有效的。

圖5 極坐標(biāo)下坐標(biāo)變量變化曲線

［1］樊鑫峰.基于無(wú)線傳感器網(wǎng)絡(luò)的機(jī)器人路徑跟蹤的研究［D］.合肥：安徽大學(xué)，2011：17-18.

［2］Kanayama Y，Kimura Y，Miyazaki F，et al.A stable tracking control method for an autonomous mobile robot［C］//Proceedings of the IEEE International Conference on Robotics and Automation.Cincinnati：IEEE，1990：384-389.

［3］Brockett R W.Asymptotic stability and feedback stabilization in differential geometric control theory［M］.Boston：Birkhauser，1983：181-191.

［4］閔穎穎，劉允剛.Barbalat引理及其在系統(tǒng)穩(wěn)定性分析中的應(yīng)用［J］.山東大學(xué)學(xué)報(bào)，2007，37（1）：51-55.