復雜環(huán)境下移動機器人路徑規(guī)劃與跟隨

王 影，楊家鑫，劉 麒，徐姣姣

(吉林化工學院 信息與控制工程學院，吉林 吉林 132022)

移動，對于人類來說不過是從一個地方到達另一個地方的簡單任務，但對于機器人來說這卻是一個非常大的挑戰(zhàn)[1].近幾十年來，越來越多的機器人走進人類的生活中，涉及農(nóng)業(yè)、工業(yè)、服務、軍事等方面[2].鑒于機器人在生活中起到的作用越來越重要，利用人工智能算法，設計一套行之有效的控制方法解決機器人最基本的移動問題顯得尤為重要[3].例如，在地震或者火災這類危險的環(huán)境中，需要機器人預先進入其中搜索受害者并采集受害者的身體信息，但在這類復雜的環(huán)境中，往往外部通信設備欠佳，無法對機器人進行實時有效的控制，這就要求機器人擁有自主移動的能力.機器人想要擁有自主移動的能力需要經(jīng)過兩個過程，一是，需要從起點規(guī)劃出一條到達終點的路徑，這條路徑需要擁有一定的評判標準且是可行的，這一過程稱之為路徑規(guī)劃；二是，在已知路徑的情況下，通過控制機器人的速度和方向使得機器人能夠沿著這條路徑行走，這個過程叫作路徑跟隨.

近年來，相關學者對移動機器人的路徑規(guī)劃與跟隨做了大量的研究.在路徑規(guī)劃方面，陳繼清等人[4]為解決障礙物分布對生成路徑的影響，將傳統(tǒng)的A*和人工勢場法進行結合，提高了傳統(tǒng)A*算法的搜索能力.邱碩涵等人[5]研究了巡檢機器人在D*和Bug2兩種算法下的路徑規(guī)劃效果，得出D*算法在運算速度和環(huán)境適應性上的優(yōu)越性.Iram Noreen等人[6]為提高機器人的路徑規(guī)劃速度，在RRT*的基礎上改變隨機采樣的邊界，使得原算法變得更具有目的性.同樣的，在路徑跟隨方面，張旋武等人[7]介紹了目前較為常用的6種路徑跟隨控制方法，強調(diào)了分層控制與強化學習等控制方法在未來路徑跟隨方面的重要性.馬艷彤等人[8]提出了一種基于水下機器人的雙層PID控制器，跟隨效果達到了分米級.張立偉等人[9]設計了一種適用于無軌列車的PID控制算法，在曲率突變的情況下跟蹤效果較為優(yōu)異.

在前人的所有研究中，路徑規(guī)劃和跟隨被當作兩個獨立的問題.本文利用路徑規(guī)劃生成期望路徑，利用路徑跟隨通過控制器控制機器人的姿態(tài)生成實際路徑，這樣就將其統(tǒng)一為一個控制問題.同樣，在前人的研究中，其路徑規(guī)劃和跟隨研究的都是較為簡單的環(huán)境，復雜環(huán)境下的研究較少，而日常生活中卻存在著非常多的復雜環(huán)境，如倉儲物流、港口運輸.因此本文將研究重點放在復雜的環(huán)境下.

本文的其余部分組織如下：第1節(jié)基于A*算法研究了移動機器人的路徑規(guī)劃問題；第2節(jié)建立了機器人運動學模型并設計了控制器；第3節(jié)設計了實驗并對實驗結果進行了分析總結；第4節(jié)對論文進行了總結.

1 路徑規(guī)劃

目標是在障礙物已知的二維柵格地圖上找到一條從起點到終點的無碰撞路徑，其評判標準是符合實際情況、最少的路徑轉折點以及高平滑性.首先進行環(huán)境建模，之后在現(xiàn)有的地圖環(huán)境中進行路徑規(guī)劃，最后對規(guī)劃的路徑進行平滑處理.

1.1 環(huán)境建模

一張完整的柵格地圖上包含起點、終點、可行區(qū)域、不可行區(qū)域、邊界這5個基本要素.在傳統(tǒng)的柵格法建模過程中，將整個環(huán)境空間劃分為具有一定分辨率的小柵格，其中可行區(qū)域用1來表示，不可行區(qū)域用0來表示，但是由于大部分路徑考慮的都是數(shù)學上的最小值，即最短路徑，這樣就導致規(guī)劃出來的路徑往往距離障礙物非常近，甚至是緊挨著障礙物的.這種規(guī)劃的路徑在實際情況中是不存在的.因此，本文采用擴展障礙物的方法在傳統(tǒng)的建模方法上進行改進，其改進思路如下：

通過提前設定一個安全閾值r來對環(huán)境中的障礙物進行拓展.在開始的時候，只有原障礙物所在的柵格值為0，之后以單位柵格的膨脹速度從障礙物的邊界開始對障礙物進行拓展，拓展范圍內(nèi)的柵格值設為0，拓展的距離記為d.如果d

1.2 算法改進

采用A*算法來進行路徑規(guī)劃.就原始的A*算法而言，其規(guī)劃出來的路徑存在著許多的路徑轉折點，而有些轉折點在實際情況中是非必要的，同時轉折點的增多無疑增加了移動機器人控制的復雜性，針對這個問題，對傳統(tǒng)的A*算法進行了改進.其改進思路如下：

步驟一：直線判斷.對非終點和非起點的路徑節(jié)點進行判斷，假設當前節(jié)點為Qi(xi,yi)，取其上次迭代的節(jié)點Qi-1(xi-1,yi-1)和上上次迭代的節(jié)點Qi-2(xi-2,yi-2).根據(jù)公式(1)進行直線判斷，如果k=1，說明當前節(jié)點和前兩個迭代的節(jié)點在一條直線上，反之k=0.

(1)

其中i≥2，xi和yi為第i個節(jié)點的橫坐標和縱坐標.

步驟二：增加權重因子W.在前一個步驟的基礎上，引入一個權重因子W.若此時k=1，說明當前的路徑是直線的，較為理想，評價函數(shù)為：

f(n)=g(n)+h(n).

(2)

若k=0，說明當前路徑進行了轉折，可能存在非必要的轉折點，評價函數(shù)為：

f(n)=g(n)+h(n)+W.

(3)

其中，0

算法2多轉折點優(yōu)化輸入:遍歷后的最優(yōu)節(jié)點信息Ni,遍歷的節(jié)點個數(shù)p輸出:權重W1:fori=2:p2: k←lineJudge(Ni,Ni-1) % 判斷當前節(jié)點和上一節(jié)點是否在同一條直線上3: if k ≠ 14: W =0.25: else6: W = 07: end if8:end for

現(xiàn)對上述改進A*算法的方法進行實驗驗證.如圖1所示的是在不規(guī)則障礙物下的原始A*算法和改進后的A*算法進行規(guī)劃所產(chǎn)生的路徑.如圖1(a)所示的路徑第1個關鍵轉折點A的坐標為[7，1]；如圖1(b)所示的路徑第1個關鍵轉折點C的坐標為[9，1]，因為原始的A*算法規(guī)劃的路徑的過早轉折，所以在后續(xù)的路徑中產(chǎn)生了4個非必要的轉折點.同樣的，在第2個關鍵轉折B點也是因為規(guī)劃路徑的過早轉折，造成后續(xù)產(chǎn)生了兩個非必要的轉折點，而改進后的算法則沒有這樣的缺陷.相同的路徑長度下，原始A*算法規(guī)劃的路徑共有8個路徑轉折點，而改進后的A*算法只有2個路徑轉折點，從中可以說明改進后的A*算法在減少路徑轉折點方面的優(yōu)越性.

1.3 路徑優(yōu)化

移動機器人在運動過程中遇到障礙物要進行規(guī)避時，在某個指定的位置停下轉向后再進行避障的情況是非常少的.一般情況下是在行走的過程中進行避障，所以折線形的路徑并不適合機器人后期的路徑跟隨，因此本文使用四階三次均勻B樣條曲線對路徑進行優(yōu)化.

由n+1個控制點p0，p1，…，Pn可以確定的四階三次均勻B樣條曲線是[10-11]：

(4)

其中，0≤u≤1，i=0，1，…，n-3.

在平面的曲線規(guī)劃中，機器人的坐標可以表示為：

(5)

其中，0≤s≤1，s為歸一化路程.

(6)

速度可以表示為：

(7)

加速度可以表示為：

(8)

直接用四階三次均勻B樣條曲線進行規(guī)劃，其產(chǎn)生的路徑并不經(jīng)過起點與終點，因此采用拓展控制點的方法對四階三次均勻B樣條曲線進行調(diào)整.具體方法如下：

先將起點S(x,y)拓展為3個節(jié)點s1、s2、s3，其中s1=S-VsL，s2=S,s3=S+VsL.其中，Vs表示移動機器人過起點時的速度，L為常數(shù).終點拓展同理.

為說明上述方法的正確性，現(xiàn)對其進行證明.證明如下：

設與起點S連續(xù)的控制點為C，那么對起點進行拓展后的曲線方程為：

在同一個屬性條件下，第k個方案和第i個方案之間的排序關系可由決策矩陣中元素的大小關系來表示。ak等價于ai?rkj=rij;ak優(yōu)于ai?rkj>rij;ak劣于ai?rkj

(9)

(10)

(11)

將s=0帶入公式(9)、(10)、(11)中，可得：

(12)

(13)

(14)

得證起點經(jīng)節(jié)點擴展后得到的曲線過起點，同理可證，終點經(jīng)節(jié)點擴展后得到的曲線過終點.

2 路徑跟隨

本節(jié)的目標是在已經(jīng)規(guī)劃好的路徑上使得移動機器人能夠沿著這條規(guī)劃的路徑行走.首先對兩輪差速移動機器人進行建模，之后設計控制器使得移動機器人能夠沿著既定的路徑行走.

2.1 機器人運動學建模

(15)

考慮到對移動機器人進行控制時，使用單積分器速度命令會更加直觀，但由于單積分器模型是全向機器人的簡化模型[12]，普通的兩輪差速機器人存在著運動學約束，其無法直接遵循單積分器命令(即無法側向行駛).這個問題的一個有效的解決方案是在單積分器模型和移動機器人運動學模型之間使用近似恒等微分同胚(NID)[13].具體實現(xiàn)步驟如下[14]：

(16)

其中，0<|∈R.

前視點的瞬時速度不受差速驅動平臺非完整約束的限制.為了直觀表達，考慮機器人的右手局部坐標系，其中X方向與圖2中的線速度矢量v在相同的方向上.給定一個純旋轉運動的速度命令，前視點將在局部坐標系的Y方向上運動.同樣，如果給定一個純直線運動的速度命令，前視點將在局部坐標系的X方向上運動，如此就符合單積分器動力學.

對p′求導，得到基于前視點的移動機器人運動學方程：

(17)

2.2 控制器設計

ex=xr-x-lcosθ，

(18)

Y軸方向的誤差：

ey=yr-y-lsinθ，

(19)

可得位置誤差追蹤方程：

(20)

(21)

(22)

(23)

設計PID控制器:

(24)

(25)

可得：

(26)

(27)

故可得控制律：

(28)

3 仿真實驗

本節(jié)在MATLAB中共設計了4個實驗來驗證前文的理論論述.實驗3.1對應第1.1、1.2小節(jié)；實驗3.2對應第1.3小節(jié)；實驗3.3對應第2節(jié)；實驗3.4驗證了在本文的理論基礎上兩輪差速移動機器人在復雜環(huán)境下路徑規(guī)劃與跟隨的效果.

3.1 路徑規(guī)劃實驗

設計了6張不同的地圖來驗證所提出的算法.每張地圖上都分布著不同的障礙物，且柵格大小統(tǒng)一設置為500×500.在前5張地圖中，將起點和終點統(tǒng)一設置為[10，10]和[490，490].第6張地圖中起點設置為[250，250]，終點設置為[490，300].由于目前柵格地圖搜索方法以4鄰域、8鄰域、24鄰域為主，故就這幾種搜索方法分別進行實驗，用來說明改進后的A*算法在處理多拐點問題方面的優(yōu)越性.代價用歐式距離進行計算，算法評判標準為：

(29)

其中，Mi代表第i個轉折點.

通過比較3種不同搜索方式下A*算法和改進A*算法在6張地圖中的不同路徑，可以很清晰地看出改進后的算法在處理多拐點方面的優(yōu)異性和普適性.如表1所示，在不同鄰域搜索下改進A*算法產(chǎn)生的路徑相對于A*算法距離障礙物擁有更大的安全距離，轉折點更少.如圖3所示，在4鄰域搜索方式下，改進后的算法相較于原算法轉折點減少了45.9%.在8鄰域搜索方式下，改進后的算法相較于原算法轉折點減少了41.8%.在24鄰域搜索方式下，改進后的算法相較于原算法轉折點減少了21.7%.從以上實驗結果中可以看出，改進后的算法在3種搜索方式下均取得了一定的效果.

表1 3種不同搜索方式下算法在6張地圖中的生成的路線

3.2 曲線優(yōu)化仿真實驗

為驗證四階三次均勻B樣條曲線對改進A*算法規(guī)劃路徑的優(yōu)化效果，在改進A*算法規(guī)劃出的路徑基礎上進行了實驗，仿真實驗結果見圖4.

如圖4所示，當規(guī)劃路徑出現(xiàn)轉彎時，使用四階三次均勻B樣條曲線優(yōu)化可以生成一條平滑的曲線進行轉彎，甚至規(guī)劃路徑出現(xiàn)兩次或者3次連續(xù)轉彎時，經(jīng)曲線優(yōu)化后的路徑仍可以保證路徑是連續(xù)且光滑的.

3.3 路徑跟隨仿真實驗

如圖5所示，其中左側路徑為改進A*算法生成的路徑，右側路徑為移動機器人實際走過的路徑.從中可以看出機器人能夠很好地沿著規(guī)劃好的路徑行進，從而驗證了基于前視點模型所設計的PID控制器路徑跟隨能力的優(yōu)越性.

3.4 綜合實驗

如圖6所示，利用改進A*算法和四階三次均勻B樣條曲線融合的方法規(guī)劃出的一條從起點到終點的較優(yōu)平滑路徑，路徑距離障礙物有很大的安全距離，不僅有利于后續(xù)移動機器人的跟隨，而且提高了路徑的安全系數(shù).如圖7所示，左側路徑為提前規(guī)劃好的路徑，右側路徑為移動機器人的實際行走路徑，從實驗結果來看，無論是在直線路徑還是在連續(xù)的轉彎路徑上，移動機器人都能對提前規(guī)劃的路徑做到較好的跟隨.

4 結 論

根據(jù)以往的研究可知，本文是第一個提出復雜環(huán)境下的移動機器人路徑規(guī)劃和跟隨的綜合研究.在MATLAB中分項測試了提出的所有理論研究，并通過一個仿真的兩輪差速機器人完成了復雜迷宮環(huán)境下的路徑規(guī)劃和跟隨.基于改進A*算法的路徑規(guī)劃使得生成的路徑轉折點更少，經(jīng)過四階三次均勻B樣條曲線優(yōu)化后的路徑更加平滑，更合適移動機器人進行路徑跟隨.仿真的兩輪差速機器人通過前視點建模的方法，采用PID控制能夠對預期路徑做到完美跟隨，并且從迷宮入口無碰撞到達迷宮出口，驗證了本文所提出的方法可以實現(xiàn)復雜環(huán)境下的移動機器人路徑規(guī)劃和跟隨.