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

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

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

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

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

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

1 路徑規(guī)劃

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

1.1 環(huán)境建模

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

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

1.2 算法改進(jìn)

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

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

(1)

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

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

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

(2)

若k=0，說明當(dāng)前路徑進(jìn)行了轉(zhuǎn)折，可能存在非必要的轉(zhuǎn)折點(diǎn)，評價函數(shù)為：

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

(3)

其中，0

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

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

1.3 路徑優(yōu)化

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

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

(4)

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

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

(5)

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

(6)

速度可以表示為：

(7)

加速度可以表示為：

(8)

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

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

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

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

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

(9)

(10)

(11)

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

(12)

(13)

(14)

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

2 路徑跟隨

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

2.1 機(jī)器人運(yùn)動學(xué)建模

(15)

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

(16)

其中，0<|∈R.

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

對p′求導(dǎo)，得到基于前視點(diǎn)的移動機(jī)器人運(yùn)動學(xué)方程：

(17)

2.2 控制器設(shè)計

ex=xr-x-lcosθ，

(18)

Y軸方向的誤差：

ey=yr-y-lsinθ，

(19)

可得位置誤差追蹤方程：

(20)

(21)

(22)

(23)

設(shè)計PID控制器:

(24)

(25)

可得：

(26)

(27)

故可得控制律：

(28)

3 仿真實驗

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

3.1 路徑規(guī)劃實驗

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

(29)

其中，Mi代表第i個轉(zhuǎn)折點(diǎn).

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

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

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

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

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

3.3 路徑跟隨仿真實驗

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

3.4 綜合實驗

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

4 結(jié) 論

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