基于改進(jìn)A*算法的POL-Robot路徑規(guī)劃*

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

（大連理工大學(xué)機械工程學(xué)院，遼寧 大連 116023）

0 引言

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

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

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

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

1 偏振光傳感器定向原理

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

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

圖1 偏振光定向原理

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

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

太陽視線在導(dǎo)航坐標(biāo)系下投影坐標(biāo)為：

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

2 基于偏振光的智能導(dǎo)航系統(tǒng)實現(xiàn)

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

圖2 基于偏振光的智能導(dǎo)航系統(tǒng)框架

2.1 改進(jìn)A＊算法

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

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

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

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

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

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

圖3 不同啟發(fā)函數(shù)生成的路徑圖對比

表1 實驗數(shù)據(jù)對比

2.2 路徑優(yōu)化處理

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

圖4 不同啟發(fā)函數(shù)生成的路徑圖對比

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

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

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

因此，θ的更新公式為：

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

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

代入公式，獲得下一個路徑點坐標(biāo)值：

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

圖5 路徑優(yōu)化處理流程

3 室外智能導(dǎo)航實驗

3.1 實驗平臺

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

圖6 POL-Robot平臺

3.2 路徑規(guī)劃實驗與結(jié)果

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

圖7 實際環(huán)境

分別以標(biāo)準(zhǔn)A*算法和本文改進(jìn)A*算法在兩種不同的地圖環(huán)境下驗證，如圖8所示。表2所示為兩種算法規(guī)劃路徑的對比。

表2 實驗數(shù)據(jù)對比

圖8 標(biāo)準(zhǔn)A*算法與改進(jìn)A*算法在不同地圖下生成的路徑對比圖

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

4 結(jié)束語

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