吳建民，王民鋼

(西北工業(yè)大學(xué)航天學(xué)院，西安 710072)

1 概述

隨著生產(chǎn)的發(fā)展和科學(xué)技術(shù)的進(jìn)步，移動(dòng)機(jī)器人在工業(yè)、軍事、服務(wù)業(yè)等領(lǐng)域得到了廣泛應(yīng)用，尤其在核電站和外層空間等惡劣環(huán)境下，移動(dòng)機(jī)器人更是具有無(wú)可比擬的優(yōu)勢(shì)［1］。基于視覺(jué)的輪式機(jī)器人由于其系統(tǒng)組成簡(jiǎn)單、環(huán)境感知能力強(qiáng)、承載大、驅(qū)動(dòng)和控制相對(duì)方便、行走速度快等優(yōu)點(diǎn)得到大量應(yīng)用［2］。

為了實(shí)現(xiàn)基于視覺(jué)的輪式移動(dòng)機(jī)器人(Wheeled Mobile Robot，WMR)導(dǎo)航控制，人們提出了許多路徑規(guī)劃和優(yōu)化的方法［3］。典型的路徑規(guī)劃方法有人工勢(shì)場(chǎng)法［4］、改進(jìn)人工勢(shì)場(chǎng)法［5］、虛擬力法［6］、柵格法［7］、改進(jìn)柵格法［8］及蟻群算法［9］等。但由于WMR本身是一種非線性強(qiáng)耦合的對(duì)象且環(huán)境因素存在不確定性，導(dǎo)致了在對(duì)WMR 進(jìn)行控制時(shí)，出現(xiàn)了與障礙物碰撞和路徑不平滑等問(wèn)題。文獻(xiàn)［10］給出了基于圖像的障礙物快速檢測(cè)方法，文獻(xiàn)［11］基于四叉樹(shù)環(huán)境模型進(jìn)行路徑規(guī)劃與機(jī)器人控制，但這種方法算法復(fù)雜，運(yùn)算量大;文獻(xiàn)［12］提出了一種基于危險(xiǎn)斥力場(chǎng)的自動(dòng)駕駛汽車主動(dòng)避撞局部路徑規(guī)劃算法，但僅適用于平直道路。

本文利用障礙物延伸法建立移動(dòng)機(jī)器人路徑規(guī)劃的環(huán)境模型，在人工勢(shì)場(chǎng)法的基礎(chǔ)上，結(jié)合路徑平滑的要求，借鑒文獻(xiàn)［12］中的危險(xiǎn)斥力場(chǎng)函數(shù)，提出一種利用斥力場(chǎng)劃定WMR 安全通道，并在通道內(nèi)對(duì)WMR 的路徑進(jìn)行優(yōu)化的方法。

2 環(huán)境建模

環(huán)境建模是機(jī)器人視覺(jué)導(dǎo)航的首要任務(wù)。對(duì)室內(nèi)環(huán)境的建模主要分為3 個(gè)步驟:首先對(duì)機(jī)器人所采集的視覺(jué)圖像進(jìn)行預(yù)處理，然后利用邊緣檢測(cè)算法確定障礙物邊緣，最后利用延伸原理確定機(jī)器人視場(chǎng)范圍內(nèi)障礙物特征點(diǎn)。

2.1 圖像預(yù)處理

圖像預(yù)處理首先將彩色圖像轉(zhuǎn)換成HSV 圖像，這樣簡(jiǎn)化了運(yùn)算，拋棄了圖像的顏色紋理等信息。然后采用中值濾波對(duì)圖像進(jìn)行平滑，既能有效濾除脈沖噪聲或顆粒噪聲，又能保護(hù)圖像邊緣。

2.2 圖像邊緣檢測(cè)

本文主要利用圖像邊緣界限判斷障礙物，所以邊緣檢測(cè)的準(zhǔn)確度直接影響到路徑規(guī)劃。使用梯度算子是進(jìn)行邊緣檢測(cè)的主要方法，常見(jiàn)的梯度算子有拉普拉斯算子、高斯拉普拉斯算子以及索貝爾算子?？紤]到視覺(jué)導(dǎo)航的實(shí)時(shí)性要求，本文利用索貝爾算子對(duì)邊緣進(jìn)行檢測(cè)。索貝爾算子為一階導(dǎo)數(shù)算子，對(duì)檢測(cè)室內(nèi)環(huán)境下的圖像邊緣具有很好的效果。將索貝爾算子的水平模板和垂直模板分別與區(qū)域內(nèi)以每個(gè)像素點(diǎn)為中心的3 ×3 鄰域進(jìn)行卷積，即可得到區(qū)域內(nèi)的障礙物邊緣曲線。

2.3 基于延伸法的障礙物特征點(diǎn)確定

對(duì)于WMR 而言，室內(nèi)環(huán)境下的障礙物多為規(guī)則的幾何體或者是規(guī)則幾何體的集合體。所以假定WMR 位于室內(nèi)平坦地面條件下且初始位置位于安全區(qū)域中，WMR 的安全通道與障礙物之間存在明顯邊界?；谏鲜黾僭O(shè)，本文提出了邊緣延伸原理，即當(dāng)WMR 檢測(cè)到圖像邊緣并且邊緣兩側(cè)的圖像灰度值存在明顯差異時(shí)，沿通道的一側(cè)為安全區(qū)域，另一側(cè)為障礙物。這種方法簡(jiǎn)化了障礙物的判斷步驟，減少了運(yùn)算量，在室內(nèi)簡(jiǎn)單環(huán)境中效果明顯。

假設(shè)機(jī)器人的視場(chǎng)為n×m，障礙物特征點(diǎn)為機(jī)器人視場(chǎng)內(nèi)障礙物邊緣距離圖像中線最近的點(diǎn)，如圖1 所示。當(dāng)?shù)玫秸系K物邊緣曲線后，即很容易確定障礙物特征點(diǎn)。

圖1 障礙物特征點(diǎn)

3 基于逆勢(shì)場(chǎng)法的安全區(qū)域確定

3.1 人工場(chǎng)

人工勢(shì)場(chǎng)法是一種虛擬力法，其基本原理是將機(jī)器人在周圍環(huán)境中的運(yùn)動(dòng)，設(shè)計(jì)成一種抽象的人造引力場(chǎng)中的運(yùn)動(dòng)，目標(biāo)點(diǎn)對(duì)移動(dòng)機(jī)器人產(chǎn)生“引力”，障礙物對(duì)移動(dòng)機(jī)器人產(chǎn)生“斥力”，最后通過(guò)求合力來(lái)控制移動(dòng)機(jī)器人的運(yùn)動(dòng)。它的實(shí)質(zhì)是建立了一種抽象的勢(shì)場(chǎng)，勢(shì)場(chǎng)對(duì)機(jī)器人的作用將決定機(jī)器人的路徑。這種方法結(jié)構(gòu)簡(jiǎn)單，實(shí)時(shí)性強(qiáng)，便于數(shù)學(xué)描述。

在傳統(tǒng)的人工勢(shì)場(chǎng)模型中，引力場(chǎng)通常定義為拋物線函數(shù):

其中，Uatt(q)為引力場(chǎng)函數(shù);ka為引力增益系數(shù);Xi為目標(biāo)在空間的位置;Xg為機(jī)器人在空間的位置;ρ(Xi，Xg)為目標(biāo)點(diǎn)與機(jī)器人之間的歐式距離。則吸引力為引力勢(shì)函數(shù)的負(fù)梯度，即:

對(duì)斥力場(chǎng)函數(shù)，傳統(tǒng)勢(shì)場(chǎng)法采用的是一種FIARS 函數(shù):

其中，Urep(q)為斥力場(chǎng)函數(shù);kr為斥力增益系數(shù);ρ為機(jī)器人到障礙物邊緣的最小距離;ρ0為障礙物的斥力影響范圍。則斥力為斥力場(chǎng)的負(fù)梯度:

因此，機(jī)器人在人工勢(shì)場(chǎng)中所受的合力為:

由于勢(shì)場(chǎng)法本身的原因，該方法存在一定的局限性。對(duì)于復(fù)雜環(huán)境的多障礙物環(huán)境，不合理的勢(shì)場(chǎng)函數(shù)會(huì)導(dǎo)致局部最優(yōu)解，容易產(chǎn)生死鎖現(xiàn)象，因而可能使移動(dòng)機(jī)器人在到達(dá)目標(biāo)點(diǎn)之前就停留在局部最優(yōu)點(diǎn)，局部最優(yōu)解是勢(shì)場(chǎng)法的最大缺陷。

3.2 安全通道確定

逆勢(shì)場(chǎng)導(dǎo)向法是在室內(nèi)局部路徑規(guī)劃前提下，舍棄人工場(chǎng)中的引力場(chǎng)模型，只保留斥力場(chǎng)模型，同時(shí)對(duì)于斥力場(chǎng)，限定它只影響WMR 的運(yùn)動(dòng)區(qū)域，不決定WMR 的運(yùn)動(dòng)方向。

改進(jìn)后的斥力場(chǎng)模型為:

其中，Urep(q)為斥力場(chǎng)函數(shù);kr為斥力增益系數(shù);ρ為機(jī)器人到障礙物特征點(diǎn)的距離;ρ0為障礙物的斥力影響范圍。

現(xiàn)假設(shè)WMR 的初始位置位于安全區(qū)域內(nèi)，連接WMR 初始位置與障礙物特征點(diǎn)，在斥力場(chǎng)內(nèi)選擇垂直于此連線且背向障礙物的斥力方向，這個(gè)斥力的終點(diǎn)即為第一個(gè)折線點(diǎn)，依次類推，可以得到安全區(qū)域內(nèi)的第N 個(gè)折線點(diǎn)。用直線連接初始位置和所有切線點(diǎn)，即可形成一個(gè)可供WMR 通過(guò)的安全通道。

逆勢(shì)場(chǎng)導(dǎo)向法解決了人工勢(shì)場(chǎng)法中存在的局部極小值問(wèn)題。通過(guò)調(diào)節(jié)模型參數(shù)可以得到安全通道，這種安全通道既不會(huì)太靠近障礙物，也不會(huì)出現(xiàn)因距離太遠(yuǎn)導(dǎo)致的路徑畸變。逆勢(shì)場(chǎng)導(dǎo)向法也解決了人工勢(shì)場(chǎng)法中存在的目標(biāo)點(diǎn)不可達(dá)的問(wèn)題。只要目標(biāo)點(diǎn)位于安全區(qū)域中，WMR 即可到達(dá)這個(gè)點(diǎn)。利用逆勢(shì)場(chǎng)法確定的安全區(qū)域如圖2 所示。

圖2 利用逆勢(shì)場(chǎng)法確定的安全區(qū)域

逆勢(shì)場(chǎng)模型計(jì)算簡(jiǎn)單，計(jì)算量與環(huán)境空間中的障礙物特征點(diǎn)個(gè)數(shù)成正比;算法魯棒性好，除了固定環(huán)境，還適合進(jìn)行動(dòng)態(tài)環(huán)境下的安全通道規(guī)劃。這種方法得到的安全通道使得WMR 的控制變得更加靈活，為路徑的局部平滑優(yōu)化提供了可能性。

4 路徑平滑問(wèn)題

移動(dòng)機(jī)器人正常工作時(shí)，不僅要保證安全，同時(shí)完成任務(wù)的時(shí)間要足夠短，能量消耗要足夠小，這樣才能保證移動(dòng)機(jī)器人在實(shí)際生產(chǎn)中的正常使用。當(dāng)WMR 在由逆勢(shì)場(chǎng)確定的安全通道中運(yùn)動(dòng)時(shí)，其移動(dòng)軌跡中存在折線點(diǎn)，即數(shù)學(xué)意義上的不連續(xù)點(diǎn)，這樣得到的路徑是折線的、不光滑的。在折線點(diǎn)處，WMR 必定會(huì)因?yàn)轫槙r(shí)轉(zhuǎn)彎而減速，增加了機(jī)器人到達(dá)目的地的時(shí)間。同時(shí)折線點(diǎn)存在造成的減速導(dǎo)致機(jī)器人的功率小于額定功率，增加了機(jī)器人的能量消耗。針對(duì)路徑平滑問(wèn)題，文獻(xiàn)［7］提出了在折線點(diǎn)中間增減虛擬折線點(diǎn)并不斷沿折線切線方向移動(dòng)的方法，這種方法操作簡(jiǎn)便，可以得到非常平滑的路徑，但折線點(diǎn)增加的時(shí)間、位置以及數(shù)量不宜掌握，限制了這種方法的使用。針對(duì)上述問(wèn)題，本文提出利用曲率映射法對(duì)WMR 路徑進(jìn)行平滑優(yōu)化和耗能優(yōu)化。

曲率映射法是將WMR 在安全通道中的運(yùn)動(dòng)軌跡按曲率常數(shù)化，即將WMR 轉(zhuǎn)角的大小映射成WMR 運(yùn)動(dòng)軌跡的曲率，通過(guò)有效地控制轉(zhuǎn)角及其變化率，得到變化緩慢的運(yùn)動(dòng)軌跡，進(jìn)而得到連續(xù)、光滑的WMR 移動(dòng)路徑。曲率映射法的具體步驟如下:

(1)確定運(yùn)動(dòng)軌跡

假設(shè)WMR 位于第N 個(gè)折線點(diǎn)處，需移動(dòng)至第N+1 個(gè)折線點(diǎn)處，第N 個(gè)折線點(diǎn)與第N +1 個(gè)折線點(diǎn)之間的距離為l，車身與折線段之間的夾角為α1，WMR 的轉(zhuǎn)角為α，WMR 運(yùn)動(dòng)軌跡上某一點(diǎn)的曲率為K，則有如下定義:

其中，k 為增益系數(shù);α ＜α1。

對(duì)于特定的路徑，當(dāng)明確了折線點(diǎn)距離與路徑的平滑程度后，就可以確定WMR 的轉(zhuǎn)角，繼而確定WMR 的實(shí)際運(yùn)動(dòng)軌跡。

(2)彎道全局速度修正

假設(shè)WMR 的額定功率為W0，額定功率下所對(duì)應(yīng)的速度為V0，直道速度為額定速度V0，彎道正常速度為V，修正后的速度為V1，則在平滑路徑條件下用曲率K 對(duì)V 進(jìn)行修正，修正公式為:

其中，k*為比例系數(shù);V≤V1≤V0。

(3)彎道局部速度修正

假設(shè)WMR 的轉(zhuǎn)角為α，修正前速度為V1，修正后的速度為V2，定義WMR 前輪偏轉(zhuǎn)時(shí)α ＞0，轉(zhuǎn)角α的變化率為當(dāng)時(shí)，認(rèn)為此時(shí)WMR 位于彎道末端，但仍在彎道內(nèi)且已有出彎的趨勢(shì)，則在這種情況下對(duì)速度進(jìn)行修正，修正公式為:

如圖3 所示，曲率映射法實(shí)現(xiàn)了將折線化的路徑優(yōu)化為曲率較小的平滑路徑，同時(shí)對(duì)彎道速度進(jìn)行全局修正和局部修正，進(jìn)一步優(yōu)化了路徑，尤其是進(jìn)行局部速度修正后，在連續(xù)S 型彎道情況下，極大地提高了WMR 路徑平滑程度。同時(shí)也減少了通過(guò)彎道的時(shí)間和能量消耗，提高了工作效率。

圖3 基于曲率映射法的路徑修正

5 實(shí)驗(yàn)與結(jié)果分析

5.1 仿真對(duì)比

使用Matlab 環(huán)境下的Simulink 模塊進(jìn)行仿真驗(yàn)證，將本文提出的改進(jìn)路徑規(guī)劃及平滑算法和文獻(xiàn)［7］提出的改進(jìn)柵格法進(jìn)行仿真對(duì)比。本文算法仿真參數(shù)如下:kr=1，k=0.2，k*=0.1，ρ0=0.5，初始速度V0=0.5，方向π/4，彎道速度V=0.2;改進(jìn)柵格法以出發(fā)點(diǎn)為極坐標(biāo)原點(diǎn)，使用0°～90°方向的扇形柵格進(jìn)行路徑規(guī)劃，環(huán)道寬度為0.2，柵格規(guī)模為134。仿真結(jié)果如圖4 所示。從中可以看出，2 種算法均順利到達(dá)目標(biāo)點(diǎn)，路徑也很相近。但是對(duì)比算法出現(xiàn)了4 個(gè)極點(diǎn)，而本文算法不僅順利到達(dá)終點(diǎn)，同時(shí)路徑很光滑，適合在輪式機(jī)器人中應(yīng)用。

圖4 2 種算法的對(duì)比結(jié)果

5.2 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)使用自制的輪式移動(dòng)機(jī)器人平臺(tái)，控制核心為ADSP-BF532 浮點(diǎn)DSP，攝像頭采用OV7670，圖像處理使用ADI 公司的圖像處理包實(shí)現(xiàn)。攝像頭高度為45cm，采樣區(qū)域?yàn)檐嚽?0 cm～120 cm，寬度為70 cm 左右。在室內(nèi)環(huán)境下進(jìn)行WMR 導(dǎo)航算法驗(yàn)證，實(shí)驗(yàn)參數(shù)為初始速度V0=0.8 m/s，彎道速度V=0.4 m/s，kr=0.28，k=0.13，k*=1.2，k*1=1.3，ρ0=0.2，實(shí)驗(yàn)參數(shù)與DSP 控制周期等因素相關(guān)。仿真條件為移動(dòng)機(jī)器人長(zhǎng)寬:25 cm ×16 cm，仿真區(qū)域大小:400 cm ×250 cm，機(jī)器人視場(chǎng)為320 ×240 的16位深RGB 圖像。

圖5 為實(shí)驗(yàn)狀態(tài)圖，圖6 為實(shí)際路徑曲線，其中機(jī)器人位于(0，0)處。

實(shí)驗(yàn)結(jié)果表明，本文研究的算法實(shí)用有效，可順利避開(kāi)障礙物并到達(dá)目的地，在連續(xù)S 型路徑條件下，可大幅提高路徑的平滑程度。

圖5 實(shí)驗(yàn)驗(yàn)證示意圖

圖6 實(shí)際路徑曲線

6 結(jié)束語(yǔ)

移動(dòng)機(jī)器人的路徑規(guī)劃問(wèn)題是機(jī)器人研究領(lǐng)域的熱點(diǎn)之一。針對(duì)傳統(tǒng)路徑規(guī)劃算法存在的不足，本文通過(guò)改進(jìn)現(xiàn)有的人工勢(shì)場(chǎng)函數(shù)，結(jié)合工程應(yīng)用提出一種可得到平滑路徑的規(guī)劃算法。該算法解決了勢(shì)場(chǎng)法路徑規(guī)劃中的陷阱及路徑震蕩問(wèn)題，與智能路徑規(guī)劃相比，可保證算法實(shí)時(shí)性，便于嵌入式系統(tǒng)底層實(shí)現(xiàn)。所采用的路徑平滑算法適用于非完整約束的輪式機(jī)器人，不僅可得到平滑路徑，對(duì)于其他路徑規(guī)劃算法也同樣適用。但本文僅研究了室內(nèi)靜態(tài)環(huán)境下的路徑規(guī)劃問(wèn)題，下一步將研究動(dòng)態(tài)環(huán)境下的WMR 避障及路徑規(guī)劃問(wèn)題。

［1］James G B，Kamla R.Robotics in Remote and Hostile Environments［J］.Science，2007，318(5853):1098-1102.

［2］Danna V.Nature's Guide to Robot Design［J］.IEEE Intelligent Systems，2002，17(6):4-6.

［3］朱大奇，顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述［J］.控制與決策，2010，25(7):961-965.

［4］Khatib O.Realtime Obstacle Avoidance for Manipulators and Mobile Robots［J］.The International Journal of Robotics Research，1986，5(1):90-98.

［5］于振中，閆繼宏，趙 杰，等.改進(jìn)人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人路徑規(guī)劃［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2011，43(1):50-55.

［6］江萬(wàn)里，熊 蓉，褚 健.復(fù)雜動(dòng)態(tài)環(huán)境下基于側(cè)滑力的局部路徑規(guī)劃［J］.浙江大學(xué)學(xué)報(bào)，2007，41(10):1609-1614.

［7］李天成，孫樹(shù)棟，高 揚(yáng).基于扇形柵格地圖的移動(dòng)機(jī)器人全局路徑規(guī)劃［J］.機(jī)器人，2010，32(4):547-552.

［8］周之平，黎 明，華 路.基于幾何相交測(cè)試的機(jī)器人路徑規(guī)劃算法［J］.控制與決策，2011，26 (11):1695-1698.

［9］任春明，張建勛.基于優(yōu)化蟻群算法的機(jī)器人路徑規(guī)劃［J］.計(jì)算機(jī)工程，2008，34(15):1-3，35.

［10］趙小川，劉培志，張 敏.一種適用于移動(dòng)機(jī)器人的障礙物快速檢測(cè)算法及其實(shí)現(xiàn)［J］.機(jī)器人，2011，33(1):198-201.

［11］李宏超，黃亞樓，闕嘉嵐，等.基于四叉樹(shù)環(huán)境模型的平滑路徑生成方法［J］.機(jī)器人，2001，23(5):426-430.

［12］肖 浩，宋曉琳，曹昊天.基于危險(xiǎn)斥力場(chǎng)的自動(dòng)駕駛汽車主動(dòng)避撞局部路徑規(guī)劃［J］.工程設(shè)計(jì)學(xué)報(bào)，2012，19(5):379-383.