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

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

0 引言

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

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

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

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

1 偏振光傳感器定向原理

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

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

圖1 偏振光定向原理

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

表1 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

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

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

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

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

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

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

因此，θ的更新公式為：

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

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

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

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

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

3 室外智能導(dǎo)航實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)

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

圖6 POL-Robot平臺(tái)

3.2 路徑規(guī)劃實(shí)驗(yàn)與結(jié)果

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

圖7 實(shí)際環(huán)境

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

表2 實(shí)驗(yàn)數(shù)據(jù)對(duì)比

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

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

4 結(jié)束語

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