張立陽，陳奕梅

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

輪式移動(dòng)機(jī)器人軌跡跟蹤與避障研究

張立陽，陳奕梅

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

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

輪式移動(dòng)機(jī)器人；軌跡跟蹤；人工勢(shì)場(chǎng)法；虛擬目標(biāo)點(diǎn)

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

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

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

1 機(jī)器人的運(yùn)動(dòng)學(xué)模型

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

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

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

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

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

進(jìn)一步可得機(jī)器人的軌跡跟蹤誤差微分方程[9]為

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

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

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

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

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

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

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

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

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

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

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

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

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

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

同時(shí)，由目標(biāo)點(diǎn)Pr以及機(jī)器人的坐標(biāo)，可得目標(biāo)點(diǎn)對(duì)機(jī)器人的引力及其各分量為

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

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

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

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

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

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

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

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

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

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

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

3 試驗(yàn)驗(yàn)證

3.1 Matlab仿真

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

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

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

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

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

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

3.2 機(jī)器人試驗(yàn)

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

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

圖7 機(jī)器人及障礙物Fig.7 Robot and obstacle

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

4 結(jié)語

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

[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]張殿富，劉福.基于人工勢(shì)場(chǎng)法的路徑規(guī)劃方法研究及展望[J].計(jì)算機(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—），男，碩士研究生，研究方向?yàn)闄C(jī)器人控制；陳奕梅（1972—），女，博士，副教授，研究方向?yàn)闄C(jī)器人控制技術(shù)。