結合預測和模糊控制的移動機器人路徑規劃

郭 娜，李彩虹，王 迪，張 寧，劉國名

山東理工大學 計算機科學與技術學院，山東 淄博255049

1 引言

由于移動機器人應用廣泛并能適應惡劣環境的特點，受到了國內外研究者的重視。其中機器人的自主導航能力是實現智能化機器人的前提和基礎。局部路徑規劃是在未知場景中根據傳感器探測出的環境信息，實時規劃出一條從起始點到目標點的最優無碰撞路徑[1-2]。為保證機器人安全避障的同時能夠更快地到達目標點、完成任務，需加強對路徑規劃算法的研究。

目前常用的局部路徑規劃算法有人工勢場法、模糊控制方法和神經網絡算法等。人工勢場法由于其方法簡潔易操作且便于底層實現而被廣泛應用于實踐之中，存在的問題是當機器人處于受力平衡狀態時易陷入局部極小值，從而無法到達目標點，人工勢場法適用于簡單的障礙物環境[3-4]。模糊控制具有魯棒性與基于生理學的“感知-動作”行為，避開了傳統算法中存在的對機器人的定位精度敏感、對環境信息依賴性強的缺點，對處理未知環境下的路徑規劃，顯示了很大的優越性。但模糊控制也有不足之處，在復雜的運行環境下存在對稱無法確定現象和U 型障礙物陷阱等[5-6]。針對此類問題許多學者給出了相應的解決辦法。文獻[7]中提出一種改進型虛目標方法，通過目標點相對于機器人的位置方向設定虛目標，雖解決了多U 型障礙的死鎖問題，但計算過程較復雜，不易根據實際情況調整，易產生冗余路段，路徑不平滑。文獻[8]設計了U型槽模糊控制器用來解決U型陷阱問題，但需通過已知的U型障礙物位置來實現通用模糊控制器和U型槽模糊控制器的切換，對于未知環境實用性不強。文獻[9]為基于多行為的機器人路徑規劃，針對U型障礙物死鎖問題提出了沿墻走的方法，方法實現簡單路徑平滑，但在多U 型環境中仍處于死鎖狀態，并且多行為之間轉換判斷復雜。

針對局部路徑規劃中出現的局部死鎖和路徑冗余等問題，本文設計了行為融合模糊控制器，在此基礎上加入了陷阱預測機制、多U型障礙解決策略和基于人工勢場法的有限狀態機，來解決僅使用模糊控制器實現局部路徑規劃時所存在的問題，提高路徑規劃的效率。將上述方法在MATLAB仿真平臺中實現并進行了驗證。

2 機器人運動學模型

本文采用兩輪差分驅動機器人作為仿真測試平臺[10-11]。左右兩輪是由兩個直流伺服電動機獨立驅動，前輪是可以自由轉向的萬向輪，起支撐作用。移動機器人采用全方位激光雷達傳感器。移動機器人的結構和傳感器探測范圍如圖1所示，其中黃色扇形范圍為模糊控制算法所用到的激光雷達傳感器探測區域，探測范圍為90°，每5°返回一個測量值數據，總共測得19 組數據。將測得的19組數據分為三組，每組分別為6、7、6個數據，每組以最小值融合后的數據分別表示機器人左方L、前方M 以及右方R 與障礙物的距離。

圖1 機器人運動模型

建立機器人全局坐標系，k 表示當前時刻，xk、yk為k 時刻機器人的位置坐標，θ 為機器人前進方向與x軸正方向的夾角，則機器人的位姿可表示為：

根據機器人左驅動輪的線速度vl、右驅動輪的線速度vr以及左右兩輪間的距離l 計算機器人整體的線速度v 和角速度ω：

機器人的運動學模型為：

其中t 為采樣周期。機器人在k+1 時刻的位姿根據上一時刻的位姿和當前的運動變化計算得出。當時機器人作直線運動。

3 行為融合模糊控制器的設計

模糊控制器的設計包含確定輸入、輸出變量，設計隸屬度函數和制定模糊控制規則[12]。行為融合模糊控制器的設計借鑒了文獻[9]中沿墻走行為的思想，將普通避障行為與沿墻走行為通過模糊控制規則的設計融合在了一個模糊控制器中。

模糊控制器設計為四輸入、二輸出的結構，如圖2所示。輸入量為機器人分別在L、M、R 三個方向與障礙物的距離dL、dM和dR，以及機器人前進方向與目標點方向的夾角Angle。輸出量為機器人左右驅動輪的線速度vl和vr。

圖2 模糊控制器結構圖

將輸入、輸出量模糊化，規定模糊語言變量。表1給出了模糊變量的論域和語義對照。

圖3是輸入量Angle 的隸屬度函數，當Angle 值落在L、M、R 上時，模糊規則設置為普通避障，當Angle值落在LB、RB 上時，模糊規則設置為沿墻走行為。所設計的模糊控制規則如表2所示，使用Mamdani法進行模糊推理。

表1 模糊語言變量語義對照表

圖3 輸入量Angle 的隸屬度函數

表2 模糊控制規則表(vl vr)

從表2 中可以看出當Angle=M,[dLdMdR]=FNF or NNN 時有兩種動作可選擇，根據兩種可選動作分為兩個模糊控制器，其他規則保持不變。在以上情況中選擇左轉(SM、SF)的模糊控制器稱為左轉模糊控制器，選擇右轉(MS、FS)的模糊控制器稱為右轉模糊控制器。

通過仿真測試，上述所設計的模糊控制器能夠在一般障礙環境和包含U 型障礙物的環境中規劃出一條平滑無碰撞的較優路徑，但依然存在如下問題：

（1）如何判斷機器人即將進入陷阱區，預防死鎖，減少路徑長度。

（2）在多U型障礙環境中存在局部死鎖現象。

（3）在傳感器的感知范圍內，出現對稱無法確定的障礙物時，使用比較dL和dR切換模糊控制器的方法，可能導致轉彎方向選擇不佳而產生大量冗余路段。

針對以上問題，提出了相應的改進策略。

4 針對模糊控制算法的改進策略

針對行為融合模糊控制器出現的問題，分別提出了建立陷阱預測機制、設計多U 型復雜障礙物解決策略、人工勢場法和有限狀態機等方法，使機器人能夠跳出死鎖狀態，正常行駛。

4.1 陷阱預測機制

移動機器人在未知環境中根據實時探測的環境信息并按照所設計模糊控制器中的控制規則規劃路徑，具有一定的局限性，當機器人進入陷阱區直至前方不可通行后沿墻走出陷阱區，產生了大量冗余路徑，甚至可能造成死鎖。為了減少路徑長度，盡量避免進入陷阱區，預防死鎖，這里設計了一種簡單的陷阱預測機制。

陷阱預測機制根據傳感器探測到的19組數據來判斷前方是否可通行，若無空值（空值代表傳感器探測范圍內沒有障礙物）如圖4（a），則認定前方存在陷阱區域不可通行，更改模糊控制器的輸入量，設置左、中、右三方向與障礙物的距離值為NNN ，提前繞開障礙物，避免路徑冗余；若有空值如圖4（b），則說明前方可通行，不做任何更改，機器人正常行走。

圖4 陷阱預測機制環境示意圖

其中傳感器探測的距離數據是陷阱預測機制的關鍵，獲得的數據越多，區域劃分得越細，對陷阱的預測結果越準確。

4.2 多U型復雜障礙物解決策略

當前進方向與目標點方向的夾角處于RB 或LB時，機器人可以輕松沿墻走出U 型障礙物；但當障礙物為多U 型時，機器人沿墻行走累計轉角超過了180°，前進方向與目標點的夾角不再處于RB 或LB，機器人沒有走出障礙陷阱，繼續朝目標點前進陷入局部死鎖。

假設當機器人向目標點方向行走時，遇到障礙物左轉避障，需經過右轉回歸到正常行駛中。根據上述情景，設置一個變量wc 為累加轉角和，初始值為零，以左轉角度為正值，右轉角度為負值，將每一步的轉角角度累加至wc 中，故機器人在正常行走時wc 的絕對值不應超過180°，若超過180°則說明未能走出陷阱區域，根據公式（4）設置Angle 值，使之繼續沿墻行走，直至走出陷阱。

4.3 基于人工勢場法的有限狀態機設計

當遇到對稱無法確定的障礙物，無論是選擇一個默認動作或是通過比較傳感器測得左右兩方向與障礙物的距離來選擇動作，都無法達到很好的路徑規劃效果。針對這種對稱無法確定的問題，本文提出了一種基于人工勢場法的有限狀態機方法。

人工勢場法是一種虛擬力法[13]，它根據當前環境模擬成虛擬勢場，將機器人抽象為一個點，機器人在人工勢場中受到目標點產生的引力和周圍障礙物產生的斥力，機器人沿合力的方向移動，直至到達目標點。有限狀態機是表示有限個狀態以及在這些狀態之間的轉移和動作等行為的數學模型[14-15]。

本文利用有限狀態機實現所設計的兩個模糊控制器的交替執行。將調用左轉模糊控制器的狀態定義為對稱時左轉狀態，調用右轉模糊控制器的狀態定義為對稱時右轉狀態，兩種狀態在非對稱環境中均能正常行走。并采用勢場法中斥力場的構建方法，當遇到對稱無法確定的障礙物時，通過計算出的周圍障礙物對機器人斥力的合力方向做出決策，切換有限狀態機的狀態。

第i 個障礙物對機器人產生的斥力為：

其中，m 為斥力場系數，lmax為傳感器的最大探測范圍，Pr為機器人當前位置坐標，Pi為第i 個障礙物的位置坐標，d(Pr,Pi)為第i 個障礙物與機器人的距離。

所有斥力在x 軸和y 軸上的分量為：

其中，βi為第i 個障礙物與x 軸正方向的夾角。

斥力的合力方向與機器人前進方向的夾角為：

采用人工勢場法中障礙物對機器人產生的斥力方向來切換兩種狀態，構建斥力場，根據公式（5）～（7）計算出傳感器探測范圍內的障礙物對機器人的斥力總和的方向α。若α ≤0 則說明斥力方向在機器人的左側，進入對稱時左轉狀態，機器人左轉直至避開障礙物；若α ＞0 則說明斥力方向在機器人的右側，進入對稱時右轉狀態，機器人右轉直至避開障礙物。圖5為有限狀態機的狀態轉換圖。

圖5 有限狀態機轉換圖

4.4 局部路徑規劃算法流程設計

根據以上設計的模糊控制器以及相應的解決策略，為機器人設計了總的局部路徑規劃算法。首先將傳感器測量的值進行數據融合，啟動預測機制判斷前方是否可通行，若不可通行更新測量值為NNN ；否則執行下一步判斷機器人是否處于多U 型陷阱區。若處于多U型陷阱區執行相應的解決策略，不是則執行下一步使用人工勢場法計算合力方向選擇狀態，調用相應的模糊控制器，根據得到的輸出量計算機器人下一步的位置坐標，若到達目標點程序結束，否則循環迭代此過程直至到達目標點。所設計的局部路徑規劃總的算法流程圖如圖6所示。

圖6 局部路徑規劃算法流程圖

5 仿真實驗測試

在MATLAB R2016 平臺上，對本文提出的算法進行了仿真、驗證。仿真環境是30×30 的二維直角坐標系，“*”代表起始點，“?”代表目標點，黑色區域代表障礙物，綠色虛線代表起始點到目標點的直線路徑，紅色虛線為局部路徑規劃的結果。

5.1 行為融合模糊控制器的仿真研究

如圖7所示，在簡單障礙物環境中或針對某種單一障礙陷阱，僅使用融合了普通避障行為和沿墻走行為的模糊控制器即可規劃出一條較優的路徑。圖7（a）為Angle 處于L、M、R 時的正常避障行走，機器人能夠自如地避開所有障礙物，規劃出一條平滑、較優的路徑。圖7（b）為Angle 處于LB 時的左沿墻行走，機器人能夠沿墻走出此U型陷阱，并在駛離陷阱區域后恢復正常避障行走至目標點。

圖7 模糊控制的仿真結果

5.2 陷阱預測機制的仿真研究

以圖7（b）為例，當機器人進入陷阱后才意識到進入了死區，再沿墻走出死區，就會增加路徑的長度。為了能夠盡早發現死區，提前繞開陷阱區域，加入了預測機制。由于傳感器的測量范圍有限，不同的探測距離會影響預測能力。如圖8所示，（a）為傳感器的探測距離為5的仿真實驗，機器人剛剛進入陷阱區域便發現前方不可通行，右轉繞開陷阱區到達目標點，減少了冗余路徑。（b）為傳感器的探測距離為10的仿真實驗，當機器人到達陷阱區邊緣時已發現前方不可通行，提前右轉繞開陷阱區到達目標點，幾乎避免了路徑冗余。根據實驗驗證，加入陷阱預測機制后能夠有效減少冗余路徑，并且當傳感器的視野范圍更廣、對周圍環境了解得越多時越容易規劃出最優路徑。

圖8 加入預測機制后的仿真結果

5.3 多U型復雜障礙物仿真研究

在多U 型的障礙物陷阱中，如圖9（a）所示，由于傳感器測量范圍有限，預測機制無法判斷前方為陷阱區，故沒有提前繞行。進入陷阱區后機器人以左轉沿墻行走，Angle 為LB，當行至A 點時機器人左轉了約360°，此時Angle 為M ，不再沿墻行走，繼續向目標點方向前進造成局部死鎖狀態。圖9（b）為加入了多U 型障礙解決方法的控制策略，機器人在到達A 點后能繼續沿墻行走，直至走出陷阱區域，仿真結果驗證了此方法的有效性。

圖9 加入多U型障礙物解決策略前后結果比較

5.4 基于人工勢場法有限狀態機仿真研究

在封閉復雜的未知環境中，如圖10 所示場景，（a）中黃色扇形區域為機器人位于A 點處傳感器的探測范圍，此時機器人處于對稱無法確定方向的狀態，由于dR略大于dL，因此機器人選擇了右沿墻行走，產生了大量不必要路徑，最終到達目標點。

圖10 加入有限狀態機切換狀態的前后結果比較

機器人在A 點時應能夠判斷左轉或右轉哪一個為最佳選擇，可以看出A 點右邊區域是不可通行的，左邊區域可通行。借鑒人工勢場法中障礙物對機器人的斥力構建斥力場，若斥力的合力在機器人的左邊，則進入對稱時左轉狀態，若在右邊，則進入對稱時右轉狀態。圖10（b）為所設計的基于人工勢場法的有限狀態機的仿真驗證，機器人在A 點所受斥力的合力向左，進入對稱時左轉狀態，順利到達目標點，減少了大量冗余路徑。

5.5 綜合仿真分析

在類似迷宮的障礙物環境中，對本文提出的方法進行了綜合驗證，如圖11所示，（a）中機器人的起始位置位于地圖左上角，通過勢場法判斷進入對稱時左轉狀態，沿墻走出陷阱區域，繼續向目標點方向前進，順利到達目標點。（b）中機器人的起始位置位于地圖中下方，到達陷阱區邊緣時預測前方不可通行，根據勢場法判斷進入對稱時左轉狀態沿墻走出第一個陷阱區，繼續向目標點方向前行，當遇到對稱無法確定方向時根據勢場法判斷進入對稱時右轉狀態，沿墻走出障礙區，成功到達目標點。根據實驗仿真結果表明，本文所設計方法能夠規劃出一條較優路徑，路徑平滑未出現碰撞現象，沒有過多的冗余路徑，達到了預期的規劃要求。

圖11 迷宮環境的仿真結果

6 結論

本文提出了一種基于改進模糊控制的移動機器人局部路徑規劃方法，并針對算法中存在的問題提出了相應的改進策略。設計算法具有以下特點：

（1）在一般障礙物環境中，該算法能夠規劃出一條從起始點到目標點的較優路徑。

（2）在包含陷阱區的障礙物環境中，機器人能夠沿墻走出陷阱區，并有一定的預測能力，優化路徑。

（3）在遇到對稱障礙物時，機器人能夠選擇正確的避障方向，避開障礙物，到達目標點。

改進后的模糊控制算法，適應能力強，能夠在未知、復雜環境下，根據實時傳感信息，規劃出最優或較優路徑。

通常設計模糊控制器，存在當輸入量增多，模糊控制規則表急劇膨脹的問題。本文模糊控制器輸入量的設計采用了數據融合的方式，將傳感器探測到的距離數據固定為三組數據，輸入量不會增加。若對輸入量的模糊語言變量做更細致的劃分也可導致規則數量的增加，由于加入了陷阱預測機制可避免多種情況的產生，并且大量的模糊規則可以融合，因此不會產生規則表急劇膨脹的情況，能夠滿足工程實用性。

未來的研究工作是將神經網絡等算法與模糊控制相結合，增強算法的泛化能力和自學習能力等。