基于改進A*算法的POL-Robot路徑規劃*

褚金奎，李 晉，李金山，張志超

（大連理工大學機械工程學院，遼寧 大連 116023）

0 引言

隨著科學技術的發展，智能移動機器人不僅在工業、醫療等行業得到廣泛應用，在反恐、排雷、救援和空間等對人有害與危險場合也得到很好的應用[1]。作為智能移動機器人控制系統的重要組成部分，導航技術一直備受關注。偏振光導航是一種新興的仿生導航方法，其仿生靈感來自沙蟻，相比現有衛星、慣性導航方法等，具有無累計誤差、精度高、抗干擾性強等優點，多種偏振光導航方法已被廣泛應用到機器人領域。

LAMBRINOS等[2]模仿沙蟻的偏振導航機理制作導航器件，驗證了移動機器人仿生偏振導航的可行性，并將全景相機與偏振測量單元組合，提高導航性能；褚金奎等[3]制作了六通道偏振光傳感器，設計一種偏振光/GPS/碼盤的組合導航系統，實現機器人的二維導航；DUPEYROUX等[4]制作了基于紫外光的點源式單通道偏振光傳感器，結合光流傳感器提出了一種路徑整合策略并裝配于六足機器人，在不同的天氣、地形下進行了測試補償；武進等[5]設計了一種偏振光/MIMU/GPS組合導航控制系統，并成功應用到四懸翼無人機中，提高其抗磁干擾能力。褚金奎等[6]設計了一種偏振光/雙目視覺仿生組合導航系統，將偏振光傳感器和雙目相機的數據融合，實現地圖構建，可滿足室外機器人自主導航需求，但欠缺路徑規劃功能。本文將基于該系統，融入智能規劃算法，增加路徑規劃功能，實現基于偏振光的移動機器人智能導航應用。

路徑規劃是機器人完成智能導航任務的前提，是指在工作環境下，基于某種評價指標尋找一條從起點到和目標點的無碰撞路徑。現有的路徑規劃算法主要包括：基于搜索的A*算法[7]、Dijkstra[8]算法等；基于采樣Informed RRT*算法[9]等；基于智能方法的遺傳算法[10]、蟻群算法[11]等。A*算法被廣泛應用于自主移動機器人路徑規劃，但該算法規劃的路徑仍存在冗余點、路徑曲率非連續等問題，不利于機器人的控制。王殿君[12]以相鄰兩點為直線的思想，隔項簡化，只保留起點、拐點和終點，有效簡化了路徑點，且能計算出拐點處機器人的旋轉方向和角度，但不具備動態避障能力；段書用等[13]提出了安全距離矩陣和與之配合的啟發函數來改進傳統A*算法，提高了路徑規劃的安全性，但同時增加了路徑長度；有研究提出了通過拓展搜索鄰域增加機器人的移動方向，并減小了路徑長度，但搜索的效率會變低[14-15]。

綜上所述，本文將基于A*算法搜索過程簡便的優點，完善上述改進A*算法的不足，從啟發函數方面改進A*算法，并提出一種梯度插補方法平滑路徑，最后將其應用到自主研發的基于偏振光的移動機器人POL-Robot中，實現POL-Robot的智能導航。仿真與室外實驗表明，本文提出的改進方案減少了路徑冗余點，且路徑平滑，為移動機器人智能導航提供了新思路。

1 偏振光傳感器定向原理

太陽發出的自然光在傳播過程中遇到大氣中的各種粒子等會發生散射現象，在天空中形成穩定的偏振光分布模式，利用瑞利散射理論可建立全天空在晴朗無云條件下的理想大氣偏振模式。在某一時刻天空中任意一點偏振光的最大偏振方向即E矢量總是垂直于由太陽S、觀測點O和被觀測點P組成的平面。倘若處于O點的觀察者可以敏感出天空某點的E矢量方向與自身方向的幾何關系，則可確定出自身的方向。

本文使用東北天坐標系為導航坐標系（n），偏振光傳感器固聯于載體坐標系為右前上坐標系（b），圖1中hs為太陽高度角，fs為太陽方位角。由偏振光傳感器可測得入射光偏振E矢量與傳感器體軸的夾角α，經太陽視位置計算得到絕對航向角。

圖1 偏振光定向原理

偏振E矢量在世界坐標系中表示為En=[ex ey ez]T，在載體坐標系中表示為Eb=[cosαsinα0]T，通過方向余弦矩陣Cbn可以實現E在b系和n系的轉換：

將天頂點作為觀測點，傳感器體軸可表示為OPn=[ 001]T，即θ=0、φ=0，則方向余弦可簡化為式（2），實際實驗時，偏振定向傳感器若沒有正對天頂點，會產生誤差，Wei等[16]提出一種補償算法，有效提高偏振光傳感器的輸出精度。

太陽視線在導航坐標系下投影坐標為：

由瑞利散射理論可知En=OSn×OPn，聯立上式可求得航向角為：

2 基于偏振光的智能導航系統實現

智能導航系統框架如圖2所示，本文基于ROS操作系統中構建智能導航系統框架，PC層中，雙目相機、偏振光傳感器、SLAM、路徑規劃等均作為節點分布于ROS操作系統中，節點之間的通信消息始終由機器人中的PC處理和傳輸，其中使用偏振光/雙目視覺組合導航方法，將偏振光傳感器/雙目相機的數據融合，完成SLAM建圖，得到二維地圖中各點的位置和姿態；路徑規劃節點處理改進A*路徑規劃算法與路徑優化，使用動態窗口法，用于實時規劃路徑；以I/O、UART等方式完成Pixhawk處理器與PC的交互通信。因此，通過融合獲取的地圖及各類傳感器信息，可得到導航所需的移動指令，控制移動機器人完成智能導航任務。

圖2 基于偏振光的智能導航系統框架

2.1 改進A＊算法

標準A*算法在遍歷搜索基礎上加入啟發函數引導搜索方向，在搜索空間中對每一個可能節點進行代價值評估，每次從隊列中選取f(n)值最小的節點作為下一個待遍歷的節點，循環遍歷直至找到目標節點。其代價值的計算如下所示：

式中：f(n)為當前點n相對于起始節點和目標節點的總代價值；g(n)為從當前點n到起始節點的真實代價值；h(n)為從當前點n到的目標節點的估計代價值。

A*算法中搜索鄰域表示機器人的轉向范圍，常用鄰域搜索方式的有4鄰域、8鄰域、24鄰域，搜索鄰域數越多時，因需要擴展更多節點，搜索效率會越低，但增加了搜索方向，其最終得到的路徑長度會減小。因此，本文中采用8鄰域搜索方式。

啟發函數h(n)直接影響A*算法的運行效率，設定當前節點n到的目標節點的真實代價值為H(n)，當h(n)＞H(n)時，擴展節點較少，運行效率較高，但通常不會得到最短路徑；當h(n)＜H(n)時，A*算法可規劃出較短路徑，但其擴展的節點更多，運行效率更低；當h(n)=H(n)時為理想情況，能得到最優路徑且不擴展多余節點，運行效率高。二維地圖中常用的計算啟發函數的方法有曼哈頓距離如式（6）所示，歐幾里得距離如式（7）所示。在實際應用中，這兩種啟發函數都不是最佳選擇，曼哈頓距離為兩個坐標點之間橫軸與縱軸的絕對值之和，但忽略了對角線距離；歐幾里得距離為兩點間直線距離，但其忽略了障礙物情況。因此本文結合曼哈頓距離和歐幾里德距離，提出一種新的啟發函數，其計算如公式（8）所示：

式中：n.x、n.y為二維平面下機器人當前位置坐標；goal.x、goal.y為終點坐標。

在12×16的柵格地圖下，對應用不同啟發函數的A*算法進行驗真，實驗結果如圖3和表1所示。由實驗結果可得，本文改進的啟發函數較傳統啟發函數，生成為路徑長度更短，路徑較平滑，總的運行時間也更短。

圖3 不同啟發函數生成的路徑圖對比

表1 實驗數據對比

2.2 路徑優化處理

改進A*算法在路徑回溯過程中，連接各個柵格的中點即可生成路徑，但會造成路徑中轉折點過多、路徑不平滑等問題，如圖4虛線所示，導致機器人出現多次不必要的啟停，影響機器人的壽命及移動效率，需進行路徑優化處理。

圖4 不同啟發函數生成的路徑圖對比

對此，本文提出一種基于g(n)的梯度插補方法，從目標點到起始點過程中，g(n)函數值逐漸減小，通過g(n)函數對影響方向的柵格節點進行加權梯度計算，得到下一個路徑點最終的前進方向，然后以較小的步長分割路徑，得到更多的路徑點，重復此過程即可插值得到平滑的路徑。具體實現如圖4實線所示，Q為起始點，M為目標點即當前點a，a的父節點為c，dx、dy為離目標點a的偏離量，b、d所在柵格為空白可通過區域，原路徑為ac，影響路徑方向的節點有a、b、c、d，考慮路徑可通過當前節點周圍非障礙物的其他空白柵格，通過g(n)函數對a、b、c、d節點進行加權梯度計算，得到新的路徑點a1，重復此過程，即可獲得平滑路徑。

基于g(n)的梯度計算公式為：

式中：θ為路徑回溯過程中的路徑點，第一個路徑點為目標點a；η為步長，設置η為0.5；g(n)為關于坐標x、y的目標函數，即g(θ)=J(x,y)；?θJ()θ為參數θ處的梯度。

因此，θ的更新公式為：

其中，初始偏移量dx、dy計算公式為：

取grad(θ)=?θ+1J(θ)，分別計算柵格節點a、b、c、d的梯度值，加權后得到偏移量dx和dy的更新值。

代入公式，獲得下一個路徑點坐標值：

綜上所述，路徑優化處理具體流程如圖5所示。

圖5 路徑優化處理流程

3 室外智能導航實驗

3.1 實驗平臺

自主搭建移動機器人POL-Robot平臺如圖6所示。本平臺采用模塊化設計，獨立的傳動系統和上位機裝置系統，便于規劃與管理。基于Pixhawk搭建控制系統，實際工作時，Pixhawk以串口形式接受PC生成的控制指令，融合偏振定向傳感器、雙目相機、IMU等信息控制機器人移動，同時將機器人的實時位姿和速度信息發送給PC端。

圖6 POL-Robot平臺

3.2 路徑規劃實驗與結果

為驗證改進A*算法的有效性，基于POL-Robot平臺在某地進行實驗，如圖7所示。使用偏振光/雙目視覺組合導航方法，將偏振光傳感器/雙目相機的數據融合，完成SLAM建圖，得到全局靜態地圖，即一張12×16的柵格地圖，其中，每個柵格的長寬為50 cm。在此基礎上，分別在兩種不同的地圖環境下驗證改進效果。

圖7 實際環境

分別以標準A*算法和本文改進A*算法在兩種不同的地圖環境下驗證，如圖8所示。表2所示為兩種算法規劃路徑的對比。

表2 實驗數據對比

圖8 標準A*算法與改進A*算法在不同地圖下生成的路徑對比圖

實驗結果證明，兩種算法均能準確到達目標點，本文改進A*算法能夠有效完成POL-Robot機器人的路徑規劃，相比標準A*算法，改進A*算法生成的路徑中轉折點個數平均減少53.8%，路徑更加平滑，路徑更加平滑，可減少機器人的啟停及加減速次數，且路徑長度更短，平均減小6.1%，極大增加了基于偏振光的移動機器人在復雜環境下智能導航的效率與穩定性。

4 結束語

偏振光導航及其組合導航已被廣泛應用到機器人領域，具有良好的自主性和抗干擾性。基于偏振光的組合導航系統，可滿足室外復雜干擾環境下機器人導航時的精度和可靠性要求，但欠缺路徑規劃功能。為實現偏振光移動機器人的智能導航應用，本文將基于該系統，融入智能規劃算法，實現路徑規劃功能。針對傳統路徑規劃算法存在搜索效率低、生成路徑曲率非連續等問題，本文提出一種改進A*算法和軌跡優化方法，首先，結合曼哈頓距離和歐幾里德距離改進其啟發函數，然后，利用梯度插補方式平滑路徑。仿真及實驗表明，本文改進A*算法能夠有效完成POL-Robot機器人的路徑規劃，相比標準A*算法，改進A*算法生成的路徑中轉折點個數平均減少53.8%，路徑更加平滑，可減少機器人的啟停及加減速次數，且路徑長度更短，平均減小6.1%，極大增加了基于偏振光的移動機器人在復雜環境下智能導航的效率與穩定性。