基于視覺的輪式移動機器人導航問題研究

吳建民，王民鋼

(西北工業大學航天學院，西安 710072)

1 概述

隨著生產的發展和科學技術的進步，移動機器人在工業、軍事、服務業等領域得到了廣泛應用，尤其在核電站和外層空間等惡劣環境下，移動機器人更是具有無可比擬的優勢［1］?；谝曈X的輪式機器人由于其系統組成簡單、環境感知能力強、承載大、驅動和控制相對方便、行走速度快等優點得到大量應用［2］。

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

本文利用障礙物延伸法建立移動機器人路徑規劃的環境模型，在人工勢場法的基礎上，結合路徑平滑的要求，借鑒文獻［12］中的危險斥力場函數，提出一種利用斥力場劃定WMR 安全通道，并在通道內對WMR 的路徑進行優化的方法。

2 環境建模

環境建模是機器人視覺導航的首要任務。對室內環境的建模主要分為3 個步驟:首先對機器人所采集的視覺圖像進行預處理，然后利用邊緣檢測算法確定障礙物邊緣，最后利用延伸原理確定機器人視場范圍內障礙物特征點。

2.1 圖像預處理

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

2.2 圖像邊緣檢測

本文主要利用圖像邊緣界限判斷障礙物，所以邊緣檢測的準確度直接影響到路徑規劃。使用梯度算子是進行邊緣檢測的主要方法，常見的梯度算子有拉普拉斯算子、高斯拉普拉斯算子以及索貝爾算子。考慮到視覺導航的實時性要求，本文利用索貝爾算子對邊緣進行檢測。索貝爾算子為一階導數算子，對檢測室內環境下的圖像邊緣具有很好的效果。將索貝爾算子的水平模板和垂直模板分別與區域內以每個像素點為中心的3 ×3 鄰域進行卷積，即可得到區域內的障礙物邊緣曲線。

2.3 基于延伸法的障礙物特征點確定

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

假設機器人的視場為n×m，障礙物特征點為機器人視場內障礙物邊緣距離圖像中線最近的點，如圖1 所示。當得到障礙物邊緣曲線后，即很容易確定障礙物特征點。

圖1 障礙物特征點

3 基于逆勢場法的安全區域確定

3.1 人工場

人工勢場法是一種虛擬力法，其基本原理是將機器人在周圍環境中的運動，設計成一種抽象的人造引力場中的運動，目標點對移動機器人產生“引力”，障礙物對移動機器人產生“斥力”，最后通過求合力來控制移動機器人的運動。它的實質是建立了一種抽象的勢場，勢場對機器人的作用將決定機器人的路徑。這種方法結構簡單，實時性強，便于數學描述。

在傳統的人工勢場模型中，引力場通常定義為拋物線函數:

其中，Uatt(q)為引力場函數;ka為引力增益系數;Xi為目標在空間的位置;Xg為機器人在空間的位置;ρ(Xi，Xg)為目標點與機器人之間的歐式距離。則吸引力為引力勢函數的負梯度，即:

對斥力場函數，傳統勢場法采用的是一種FIARS 函數:

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

因此，機器人在人工勢場中所受的合力為:

由于勢場法本身的原因，該方法存在一定的局限性。對于復雜環境的多障礙物環境，不合理的勢場函數會導致局部最優解，容易產生死鎖現象，因而可能使移動機器人在到達目標點之前就停留在局部最優點，局部最優解是勢場法的最大缺陷。

3.2 安全通道確定

逆勢場導向法是在室內局部路徑規劃前提下，舍棄人工場中的引力場模型，只保留斥力場模型，同時對于斥力場，限定它只影響WMR 的運動區域，不決定WMR 的運動方向。

改進后的斥力場模型為:

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

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

逆勢場導向法解決了人工勢場法中存在的局部極小值問題。通過調節模型參數可以得到安全通道，這種安全通道既不會太靠近障礙物，也不會出現因距離太遠導致的路徑畸變。逆勢場導向法也解決了人工勢場法中存在的目標點不可達的問題。只要目標點位于安全區域中，WMR 即可到達這個點。利用逆勢場法確定的安全區域如圖2 所示。

圖2 利用逆勢場法確定的安全區域

逆勢場模型計算簡單，計算量與環境空間中的障礙物特征點個數成正比;算法魯棒性好，除了固定環境，還適合進行動態環境下的安全通道規劃。這種方法得到的安全通道使得WMR 的控制變得更加靈活，為路徑的局部平滑優化提供了可能性。

4 路徑平滑問題

移動機器人正常工作時，不僅要保證安全，同時完成任務的時間要足夠短，能量消耗要足夠小，這樣才能保證移動機器人在實際生產中的正常使用。當WMR 在由逆勢場確定的安全通道中運動時，其移動軌跡中存在折線點，即數學意義上的不連續點，這樣得到的路徑是折線的、不光滑的。在折線點處，WMR 必定會因為順時轉彎而減速，增加了機器人到達目的地的時間。同時折線點存在造成的減速導致機器人的功率小于額定功率，增加了機器人的能量消耗。針對路徑平滑問題，文獻［7］提出了在折線點中間增減虛擬折線點并不斷沿折線切線方向移動的方法，這種方法操作簡便，可以得到非常平滑的路徑，但折線點增加的時間、位置以及數量不宜掌握，限制了這種方法的使用。針對上述問題，本文提出利用曲率映射法對WMR 路徑進行平滑優化和耗能優化。

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

(1)確定運動軌跡

假設WMR 位于第N 個折線點處，需移動至第N+1 個折線點處，第N 個折線點與第N +1 個折線點之間的距離為l，車身與折線段之間的夾角為α1，WMR 的轉角為α，WMR 運動軌跡上某一點的曲率為K，則有如下定義:

其中，k 為增益系數;α ＜α1。

對于特定的路徑，當明確了折線點距離與路徑的平滑程度后，就可以確定WMR 的轉角，繼而確定WMR 的實際運動軌跡。

(2)彎道全局速度修正

假設WMR 的額定功率為W0，額定功率下所對應的速度為V0，直道速度為額定速度V0，彎道正常速度為V，修正后的速度為V1，則在平滑路徑條件下用曲率K 對V 進行修正，修正公式為:

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

(3)彎道局部速度修正

假設WMR 的轉角為α，修正前速度為V1，修正后的速度為V2，定義WMR 前輪偏轉時α ＞0，轉角α的變化率為當時，認為此時WMR 位于彎道末端，但仍在彎道內且已有出彎的趨勢，則在這種情況下對速度進行修正，修正公式為:

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

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

5 實驗與結果分析

5.1 仿真對比

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

圖4 2 種算法的對比結果

5.2 實驗驗證

實驗使用自制的輪式移動機器人平臺，控制核心為ADSP-BF532 浮點DSP，攝像頭采用OV7670，圖像處理使用ADI 公司的圖像處理包實現。攝像頭高度為45cm，采樣區域為車前30 cm～120 cm，寬度為70 cm 左右。在室內環境下進行WMR 導航算法驗證，實驗參數為初始速度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，實驗參數與DSP 控制周期等因素相關。仿真條件為移動機器人長寬:25 cm ×16 cm，仿真區域大小:400 cm ×250 cm，機器人視場為320 ×240 的16位深RGB 圖像。

圖5 為實驗狀態圖，圖6 為實際路徑曲線，其中機器人位于(0，0)處。

實驗結果表明，本文研究的算法實用有效，可順利避開障礙物并到達目的地，在連續S 型路徑條件下，可大幅提高路徑的平滑程度。

圖5 實驗驗證示意圖

圖6 實際路徑曲線

6 結束語

移動機器人的路徑規劃問題是機器人研究領域的熱點之一。針對傳統路徑規劃算法存在的不足，本文通過改進現有的人工勢場函數，結合工程應用提出一種可得到平滑路徑的規劃算法。該算法解決了勢場法路徑規劃中的陷阱及路徑震蕩問題，與智能路徑規劃相比，可保證算法實時性，便于嵌入式系統底層實現。所采用的路徑平滑算法適用于非完整約束的輪式機器人，不僅可得到平滑路徑，對于其他路徑規劃算法也同樣適用。但本文僅研究了室內靜態環境下的路徑規劃問題，下一步將研究動態環境下的WMR 避障及路徑規劃問題。

［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］朱大奇，顏明重.移動機器人路徑規劃技術綜述［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］.哈爾濱工業大學學報，2011，43(1):50-55.

［6］江萬里，熊 蓉，褚 健.復雜動態環境下基于側滑力的局部路徑規劃［J］.浙江大學學報，2007，41(10):1609-1614.

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

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

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

［10］趙小川，劉培志，張 敏.一種適用于移動機器人的障礙物快速檢測算法及其實現［J］.機器人，2011，33(1):198-201.

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

［12］肖 浩，宋曉琳，曹昊天.基于危險斥力場的自動駕駛汽車主動避撞局部路徑規劃［J］.工程設計學報，2012，19(5):379-383.