輪式移動機器人軌跡跟蹤與避障研究

張立陽，陳奕梅

（天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津 300387）

輪式移動機器人軌跡跟蹤與避障研究

張立陽，陳奕梅

（天津工業(yè)大學(xué) 電氣工程與自動化學(xué)院，天津 300387）

針對非完整輪式移動機器人軌跡跟蹤與避障問題，在跟蹤誤差微分方程的基礎(chǔ)上提出了軌跡跟蹤控制律，運用Lyapunov判定其穩(wěn)定性；與模糊控制方法結(jié)合，將傳統(tǒng)的人工勢場法進行改進，實時改變斥力系數(shù)和引力系數(shù)，并引出虛擬目標(biāo)點；插值相鄰跟蹤點，與軌跡跟蹤控制律結(jié)合，充分發(fā)揮軌跡跟蹤和人工勢場法的優(yōu)點，實現(xiàn)同時軌跡跟蹤與避障任務(wù)。仿真和試驗結(jié)果顯示，該機器人能夠有效跟蹤軌跡并能避開障礙物，表明該控制方法的有效性。

輪式移動機器人；軌跡跟蹤；人工勢場法；虛擬目標(biāo)點

輪式移動機器人WMR是典型的非完整動力學(xué)系統(tǒng)，具有廣泛的實際應(yīng)用背景，其軌跡跟蹤控制因此得到廣泛的研究。大體而言，控制的方法主要有模糊方法[1-2]、神經(jīng)網(wǎng)絡(luò)[3-4]、滑模方法[5]、自適應(yīng)[6]以及極坐標(biāo)表示下的控制律[7]等。然而，人們不僅希望機器人完成軌跡跟蹤任務(wù)，還想與避障算法結(jié)合，同時完成軌跡跟蹤和避障任務(wù)。

人工勢場法是由Khatib[8]提出的一種虛擬力法，該算法因其原理簡單、結(jié)構(gòu)簡潔、生成路徑平滑等特點，在機器人避障路徑規(guī)劃中有著廣泛的應(yīng)用，非常適合于對路徑生成實時性和安全性要求較高的規(guī)劃任務(wù)。該方法得到的規(guī)劃路徑雖然不一定是最短的，但卻是最平滑和最安全的。

在此，基于機器人的軌跡跟蹤誤差微分方程設(shè)計出軌跡跟蹤控制律，運用李雅譜諾夫第二法判定其穩(wěn)定性，對傳統(tǒng)的人工勢場法進行改進，利用模糊系統(tǒng)計算引力系數(shù)和斥力系數(shù)，進而計算合力并引出虛擬目標(biāo)點。采用插值法計算相鄰跟蹤點并應(yīng)用于軌跡跟蹤控制律，最后仿真并試驗驗證了該方法的有效性。

1 機器人的運動學(xué)模型

機器人及參考軌跡如圖1所示，輪式移動機器人在坐標(biāo)系為X-Y的空間平面中，θ為機器人方向角；v為機器人的線速度；ω為機器人的角速度。機器人的位姿可用q=[x，y，θ ]T表示。輪式移動機器人的運動學(xué)模型為

圖1 機器人及參考軌跡Fig.1 Robot and reference trajectory

5 個聲吶位于機器人前部，夾角互為 30°。 Pr（xr，yr）為參考軌跡上的點，以 qr=[xr，yr，θr]T作為機器人的期望位姿，以 ur=[vr，ωr]T作為機器人的期望速度控制。

同樣，機器人在期望位姿符合的運行學(xué)模型為

根據(jù)坐標(biāo)變換公式，可得局部坐標(biāo)系Xe-Ye下，用直角坐標(biāo)描述的非完整性移動機器人跟蹤誤差qe為

進一步可得機器人的軌跡跟蹤誤差微分方程[9]為

2 軌跡跟蹤及避障算法設(shè)計

2.1 軌跡跟蹤算法設(shè)計

針對非完整輪式移動機器人運動學(xué)模型的軌跡跟蹤控制問題，可以描述為對于式（4）所描述的系統(tǒng)，在任意初始誤差的條件下，尋找控制輸入u=，使有界，，并且對此，軌跡跟蹤控制律設(shè)計如下：

式中：kx>0，ky>0，kθ>0。

將式（5）代入式（4），可得軌跡跟蹤控制系統(tǒng)的狀態(tài)空間方程

式中：xe∈R，ye∈R，θe∈（-π，π］。 選取李雅普諾夫函數(shù)為

對式（7）求導(dǎo)，并代入式（6），化簡后得

顯然 V（qe）≥0，當(dāng)且僅當(dāng) qe=0 時，等式成立；又vr＞0，由式（8）可知V˙（qe）≤0。 令 V（qe）=0，得超曲面方程為，對 m（qe）求導(dǎo)，得，故（qe）不恒為零。 當(dāng)‖qe‖→∞ 時，有 V（qe）→∞，由李雅譜諾夫第二法可知控制律（5）是全局漸近穩(wěn)定的。

2.2 聲吶避障算法設(shè)計

根據(jù)Khatibd的人工勢場法模型[10]，可知障礙物對機器人的斥力Foi為

式中：ko為斥力增益系數(shù)：loi為第i個聲吶與障礙物的距離：los為障礙物的影響距離。

聲吶避障原理如圖2所示，由5個聲吶分別產(chǎn)生的斥力為Fo1-Fo5，則5個斥力的斥力和Fo為

式中：Fox為斥力和X軸的分量；Foy為斥力和Y軸的分量。

圖2 聲吶避障Fig.2 Sonar obstacle avoidance

同時，由目標(biāo)點Pr以及機器人的坐標(biāo)，可得目標(biāo)點對機器人的引力及其各分量為

式中：ka為引力系數(shù)。由斥力和以及引力，可以計算合力Ft及其各分量，即

斥力系數(shù)ko和引力系數(shù)ka均采用單輸入單輸出系統(tǒng)。其中，輸入變量分別為障礙物距離loi和目標(biāo)點距離li，并且定義其5個模糊子集為｛距離很近，距離近，距離中，距離遠，距離很遠｝；輸出變量分別為斥力增益系數(shù)ko和引力增益系數(shù)ka，模糊子集為｛增益很小，增益小，增益中，增益大，增益很大｝。該模糊系統(tǒng)的模糊規(guī)則見表1。

表1 模糊規(guī)則Tab.1 Fuzzy rule

為了將人工勢場法和軌跡跟蹤結(jié)合，在此引入虛擬目標(biāo)點Pr′，使機器人受到的合力等效為虛擬目標(biāo)點 Pr′對機器人的引力，于是由式（13）和式（14）可得

從而，可得虛擬目標(biāo)點 Pr′（xr′，yr′）的坐標(biāo)，即

在機器人的避障過程中，為了利用軌跡跟蹤控制律，需要在機器人坐標(biāo)和虛擬點之間進行插值，插值方法采用線性插值，并且限制插值點的距離。

避障中插值原理如圖3所示，原相鄰之間的距離lr為

當(dāng)機器人在障礙物的影響范圍之內(nèi)時，在機器人坐標(biāo) M（x，y）和虛擬目標(biāo)點 Pr′（xr′，yr′）坐標(biāo)之間進行線性插值，插值節(jié)點xini分別為

圖3 避障中插值Fig.3 Interpolation in the obstacle avoidance

而當(dāng)機器人在障礙物的影響距離之外時，此時，應(yīng)使機器人迅速回歸軌跡，因此在 M（x，y）和Pr+1（xr+1，yr+1）點之間進行線性插值，插值節(jié)點分別為

根據(jù)插值節(jié)點可得 Pin1，Pin2，Pin3，Pin4的坐標(biāo)。利用機器人坐標(biāo)以及插值點可以計算避障時候的vr，ωr。

3 試驗驗證

3.1 Matlab仿真

假設(shè)給定的參考軌跡為

令影響距離los=1.6 m，并且2個模糊系統(tǒng)的輸入均采用梯形隸屬函數(shù)，輸出為三角形隸屬函數(shù)，并采用面積重心法去模糊化方法。調(diào)節(jié)系數(shù)kx=1，ky=0.9，kθ＝1.2。 機器人的初始位姿為 q=（-1，0，3）T，利用Matlab進行仿真。圖4為李雅普諾夫函數(shù)V和跟蹤誤差 qe=[xe，ye，θe]T隨時間的變化，如圖可見跟蹤誤差能夠快速趨于零。參考軌跡及機器人的軌跡如圖5所示。

在坐標(biāo)為（4，5），（5，0），（-5，-2）的位置上分別放置3個障礙物，則機器人軌跡以及參考軌跡如圖6所示。從仿真結(jié)果可以看出，機器人能夠有效避開障礙物并繼續(xù)進行跟蹤任務(wù)。

圖4 李雅普諾夫函數(shù)及位姿誤差Fig.4 Lyapunov function，error of position and posture

圖5 參考軌跡和機器人的軌跡Fig.5 Reference trajectory and robot trajectory

圖6 加入障礙物后的機器人軌跡Fig.6 Robot trajectory with obstacle

3.2 機器人試驗

試驗中，使用編譯軟件VC 6.0，編程語言為C++。機器人采用里程計定位。將尺寸為0.37 mm×0.2 mm×0.3 mm的紙箱作為障礙物放置在坐標(biāo)（5，-1）的位置上，機器人及障礙物如圖7所示。

機器人的初始位姿為qe=[0，0，0 ]T，調(diào)節(jié)系數(shù)kx=0.051，ky=0.060，kθ＝0.065。在 MFC 的窗口中畫出參考軌跡、機器人軌跡以及實時檢測出的障礙物，如圖8所示。從MFC窗口可以看出試驗效果較為理想。

圖7 機器人及障礙物Fig.7 Robot and obstacle

圖8 MFC窗口中機器人軌跡Fig.8 Robot trajectory in MFC window

4 結(jié)語

軌跡跟蹤控制是移動機器人運動控制的重要問題。針對非完整輪式移動機器人運動學(xué)模型，提出了軌跡跟蹤控制律，該控制律方法計算簡單，收斂速度快，具有很好的穩(wěn)定性。同時，根據(jù)人工勢場法提出了虛擬目標(biāo)點，并與軌跡跟蹤結(jié)合，充分發(fā)揮了軌跡跟蹤和人工勢場法的優(yōu)點，使器人既能跟蹤軌跡又能有效避開障礙物。所研究的問題對今后的同時軌跡跟蹤與避障問題具有一定的參考價值。

[1] Hou Z G，Zou A M，Cheng L，et al.Adaptive control of an electrically driven nonholonomic mobile robot via backstepping and fuzzy approach[J].IEEE Transactions on Control Systems Technology，2009，17（4）：803-815.

[2] Das T，Kar I N.Design and implementation of an adaptive fuzzy logic-based controller for wheeled mobile robots[J].IEEE Transactions on Control Systems Technology，2006，14（3）：501-510.

[3] Rossomando F G，Soria C，CarelliR.Neuralnetwork-based compensation control ofmobilerobotswith partially known structure[J].Iet Control Theory& Applications，2012，6（12）：1851-1860.

[4] Fierro R，Lewis F L.Control of a nonholonomic mobile robot using neural networks[M].IEEE Press，1998.

[5] Yue M，Hu P，Sun W.Path followingofaclassofnonholonomic mobile robot with underactuated vehicle body[J].Iet Control Theory&Applications，2010，4（10）：1898-1904.

[6] Fukao T，Nakagawa H，Adachi N.Adaptive tracking control of a nonholonomic mobile robot[J].IEEE Transactions on Robotics&Automation，2000，16（5）：609-615.

[7] Park B S，Yoo S J，Jin B P，et al.Adaptive neural sliding mode controlofnonholonomic wheeled mobile robotswith model uncertainty[J].IEEE Transactionson ControlSystems Technology，2009，17（1）：207-214.

[8] Khatib O.Real time obstacle avoidance for manipulators and mobile robots[J].International Journal of Robotics and Research，1986，5（1）：90-98.

[9] Chen X，Jia Y，Matsuno F.Tracking control of nonholonomic mobile robotswith velocity and acceleration constraints[J].American Control Conference，2014，18（6）：880-884.

[10]張殿富，劉福.基于人工勢場法的路徑規(guī)劃方法研究及展望[J].計算機工程與科學(xué)，2013，35（6）：88-95.

Research on Trajectory Tracking and Obstacle Avoidance of Wheeled Mobile Robot

ZHANG Li-yang，CHEN Yi-mei

（College of Electrical Engineering and Automation，Tianjin Polytechnic University，Tianjin 300387，China）

Aiming at the problem of trajectory tracking and obstacles avoidance of nonholonomic wheeled mobile robots.An trajectory tracking control law is proposed on the basis of the differential equation of the tracking error，and its stability is determined by Lyapunov.Combined with the fuzzy control method，the traditional artificial potential field algorithm is improved.The coefficients of repulsion and gravitation are changed in real time，and virtual target points are extracted.Interpolate the adjacent tracking point and combine trajectory tracking control law，in this way can create maximum value of trajectory tracking and artificial potential field algorithm to realize trajectory tracking and obstacle avoidance tasks at the same time.In conclusion，the simulation and experimental results validate the effeciveness of this control law.Robot can track the trajectory and can avoid obstacles effectively.

wheeled mobile robot（WMR）；trajectory tracking；artificial potential field algorithm；virtual target point

TP242

A

1001-9944（2017）11-0072-05

10.19557/j.cnki.1001-9944.2017.11.017

2017-06-08；

2017-09-04

張立陽（1990—），男，碩士研究生，研究方向為機器人控制；陳奕梅（1972—），女，博士，副教授，研究方向為機器人控制技術(shù)。