校園智能車定位導(dǎo)航系統(tǒng)設(shè)計與實現(xiàn)

　　摘要：為實現(xiàn)智能車平臺在校園場景下自主定位和導(dǎo)航，提出一種校園智能車導(dǎo)航系統(tǒng)設(shè)計方法。采用慣性導(dǎo)航系統(tǒng)對智能車進行定位，激光雷達創(chuàng)建局部地圖，利用混合A+算法在局部地圖中規(guī)劃出合理的路徑。整個系統(tǒng)中，上位機獲取傳感器采集的數(shù)據(jù)，通過慣性導(dǎo)航系統(tǒng)解算出智能車當(dāng)前位姿，規(guī)劃模塊獲取當(dāng)前智能車位姿和周圍障礙物信息后進行路徑規(guī)劃，并通過串口通信模塊將路徑信息傳遞給下位機控制模塊，實現(xiàn)智能車的自主移動。實驗結(jié)果表明：在校園內(nèi)，智能車能夠從起點出發(fā)，自主規(guī)劃路徑并繞過障礙物到達目標(biāo)地點，驗證了該導(dǎo)航系統(tǒng)的有效性。

　　關(guān)鍵詞：智能車;定位;路徑規(guī)劃;自主導(dǎo)航

　　代成; 程永杰; 蔣濤; 許林成都信息工程大學(xué)學(xué)報2021-12-13

　　0引言

　　隨著自動駕駛技術(shù)的不斷發(fā)展，智能車在日常生活中的應(yīng)用更加廣泛，如校園場景下的人員接駁、路面清潔、外賣快遞派送等。自主導(dǎo)航是智能車實現(xiàn)其智能性和自主性的關(guān)鍵。自主導(dǎo)航主要包括環(huán)境感知、定位和路徑規(guī)劃。無人車的定位導(dǎo)航技術(shù)所使用的傳感器類型主要有GPS、慣性導(dǎo)航系統(tǒng)、WiFi、UWB等，而采用單一傳感器的定位方法大多都具有局限性。如GPS在室內(nèi)、水下等被遮擋情況下定位精度低;慣性導(dǎo)航系統(tǒng)精度受限于傳感器的成本，低成本慣導(dǎo)漂移誤差過大無法單獨使用，高成本慣導(dǎo)無法普及;而基于無線信號的定位方法需要預(yù)先鋪設(shè)設(shè)備，使用場景受限。定位導(dǎo)航技術(shù)按照定位結(jié)果可以分為相對定位和絕對定位兩種方式[1]。絕對定位方式可以直接獲取到目標(biāo)當(dāng)前在全局坐標(biāo)系中的絕對位置，不用通過初始位置進行航跡推算獲取下一時刻的位置，當(dāng)前時刻的位置和下一時刻的位置沒有關(guān)聯(lián)。但是，絕對定位方式容易受外界因素影響，如GPS就是一種絕對定位方式，信號遮擋會影響定位精度。相對定位方式是通過上一時刻的位置來推算下一時刻位置[2]，該定位方式精度受限于傳感器本身測量精度，如慣導(dǎo)系統(tǒng)。因此與傳統(tǒng)的使用單一傳感器的定位方式相比，多傳感器融合的方式有利于提高定位的精度。目前主要采用GPS與慣導(dǎo)系統(tǒng)通過卡爾曼濾波器融合的方式，實現(xiàn)高精度定位。GPS能夠全天獲取相對精確的位置，但是更新頻率較低;慣導(dǎo)系統(tǒng)能夠以高頻率提供短時間內(nèi)精確的位置信息，故二者信息融合能夠優(yōu)勢互補，從而得到更精確的位置信息[3]。

　　路徑規(guī)劃的本質(zhì)是在一定約束條件下，找到一條連接起點與終點的無碰撞路徑[4]。常用的智能車路徑規(guī)劃算法包括基于采樣的方法和基于圖搜索的方法。基于采樣的方法包含概率路線圖(PRM)[5]與快速搜索隨機樹(RRT)[6]等。Karaman等[7]在RRT算法基礎(chǔ)上引人重選父節(jié)點與重布隨機樹過程，在時間充沛的情況下jRT*的路徑最優(yōu)。Gammell等[8]在RRTs找到初始路徑解后，引人橢圓采樣的思想，加快RRT*收斂到最優(yōu)解的速度。Yang等…將RRT與三階貝塞爾曲線相結(jié)合，邊采樣邊平滑，解決路徑曲率不連續(xù)的問題。楊瑤等[1°]結(jié)合Reeds-Sheep曲線、貝塞爾曲線與Bi-RRT*算法，解決了Bi-RRT*轉(zhuǎn)折次數(shù)多、路徑不平滑、過于貼近障礙物和轉(zhuǎn)角過大等問題。A?nn%與jpsM均是經(jīng)典的圖搜索算法，但是這些方法并不適用于智能車，因為其路徑不連續(xù)。Dolgov等[14]直接在車輛控制空間內(nèi)米樣，再利用A的思想搜索路徑，解決這一問題。齊堯等[15]考慮安全因素，提出適用于混合的懲罰函數(shù)，得到更為安全、合理的路徑。文中針對的校園場景，行人較多，道路狹窄，車道線殘缺不全，故智能車的規(guī)劃路徑不考慮車道問題。基于校園場景下的導(dǎo)航規(guī)劃，主要考慮規(guī)劃路徑是否合理，智能車行駛途中能否安全避障等。因此，本文自主導(dǎo)航系統(tǒng)主要解決在車輛運動學(xué)約束下，定位和路徑規(guī)劃問題，目的是使智能車從當(dāng)前位置安全平穩(wěn)地到達目標(biāo)位置。

　　1GPS/慣導(dǎo)組合導(dǎo)航

　　由于GPS和慣導(dǎo)梅工作特性，二者單獨使用性能F.(RN+h)c〇sg>Rjn+h00(5)受限，因此將兩種傳感器信息通過卡爾曼濾波器進行V00U融合是目前常用的定位方式。通過組合導(dǎo)航的方式，式(5)是導(dǎo)航系下坐標(biāo)誤差。￣可以實時地提供載體較翕精度的速度fn、姿態(tài)和傖寶傖0L息g本系統(tǒng)采用的是慣性傳感器為捷II憤導(dǎo)系統(tǒng)F?=-fu0fe(6)(strapdowiiinertialnayigatioiisystem，SlHS)〇SINS主￣fe0J要是由三軸陀螺儀，三軸加速度計和微塑計算機組成，式(6)是導(dǎo)航系下速度誤差。其相較于平臺式慣導(dǎo)擁有緒構(gòu)簡單<童量_、體積小、尺12/?丨3、成本低廉、可靠性高等特點。4=尺21尺22穴23(7)目前.，研究者們對于CPS/慣導(dǎo)系統(tǒng)的組合方式提、況31尺32尺33J出了多種方法，主要可以分為松耦合、緊耦合和超紫耦#3種方式s考慮實際實驗中的輿時性和魯#性，采用松耦合的方式爾個系統(tǒng)各自猶重工作將兩個系統(tǒng)輸出的導(dǎo)■航結(jié)果使用卡爾曼濾波器估計出最優(yōu)導(dǎo)航參數(shù)，其系統(tǒng)框圖如圖1'所示。

　　GPS/SINS松耦合形式將慣導(dǎo)系統(tǒng)的誤義值作為狀態(tài)囊，將GPS誤差作為量測矂禽，卡爾曼_波器的系統(tǒng)模型為8x=FSx+G〇)(1)式中，&為當(dāng)前時刻先驗狀態(tài)估計，F(xiàn)為動態(tài)系數(shù)矩陣為誤羞狀態(tài)矢量，G為噪聲吳量，W為預(yù)測過程中的噪聲;對應(yīng)狀態(tài)矢鐵中每一個分量的_聲：F(〇3xl〇J%3〇3x3FrF，〇3x：〇，.：Sx=[Srl3xlG=[〇%，丨式(2)中，Svi0Fv〇3x：〇3x：〇3x：del3x3〇3x〇3x3R[及6〇3x3〇3x〇Ff3x3I/，SAxl1y°"f，lx3]⑵(3)⑷

　　式(3)中，5/^是泣置誤差失量;蠢導(dǎo)航系下的速度攝塗矢量;W基載體姿態(tài)誤差矢量;是慣導(dǎo)陀螺儀的誤_矢量;5/鳥慣導(dǎo)加速度計的誤差矢量。為方便編程實現(xiàn)，將連續(xù)系統(tǒng)模型離散化后摒'到離散模型為Szk=HkSxk+rjt(11)屮SINS_滬鄉(xiāng)Szk：rSINS_rGPS^SINS—^GPSyA—A/lSINSGPSh-hILsim，lg?s\vu，sim￣(12)式(11)中，由于內(nèi)包含慣導(dǎo)系統(tǒng)的謨差暈則：羹測矢量屯為GPS觀測值與SINS預(yù)測值之差，如式(12)所示;表示量測噪聲;嘰表示零均值、方差為&的纛測嵊_。

　　2路徑規(guī)劃

　　混合A*募法最早應(yīng)用?于斯坦福自主研發(fā)的智能車Juiiitir上頌過在車_鱗控制中肇樣，梅車輛的擎完整性約束融入其中，：再通過A4的算法流程搜索，其路徑能夠直接用于踉私

　　2.1節(jié)點擴展方式

　　傳統(tǒng)As算法節(jié)點通常不考慮車朝難完：整性約東，_W格屬圍四個或者：八個方向擴據(jù)，如圖2U)所示。由于智能車無法原：地旋轉(zhuǎn)，因此這種擴展方式不符合智能車的運動方式。?昆合A*在智能車的3D狀態(tài)空間中(*>：y，0)離散采樣，生成向一t方位、不同方向擴展的連續(xù)曲線，如圖2(b)所示，遍獅擴展方麟徑能夠滿足動力學(xué)約束6

　　2，2估價函數(shù)

　　與傳統(tǒng)A*算法一：樣，混合A*算法建立估價函數(shù)判斷節(jié)點的優(yōu)先性，眞表達式為f{n)=g{：n)+h{n){B)其中.gU)為起始節(jié)點到當(dāng)前節(jié)點《的代價和，通過遞推累加可得g(n)=g(n^m)+cM(nlli)S!tl{，n){14)，n)=(1+lt.r?，rrs(j+?-8n｝?(15)萁傘表游，點n的父貨點，0喊(的父節(jié)點到節(jié)點所盡的代價，%?為智能車后退行駛的懲罰值，為轉(zhuǎn)角發(fā)生改變的懲罰值，s(Kls?ia，ra〗讓亦從到售貞n的實際行駛距離。懲罰向后行駛與方向改變的行為，能夠使規(guī)劃的路徑更容易跟蹤，盡量滅少對智能車的操作sA(?)表示節(jié)點7T到目標(biāo)節(jié)點的預(yù)估代價值。Mn)由兩種啟發(fā)函數(shù)構(gòu)成，取兩者中的最大值作為節(jié)點77的預(yù)估代價值。如式(16)所汞h{n)=max(ht(n)，/i2(n))(16)笫一種唇發(fā)函數(shù)為忽略環(huán)境障礙物、考慮車輛的非S整性約東，即車輛的最小轉(zhuǎn)彎半徑，該值為當(dāng)前節(jié)蠱(n0，，)到目標(biāo)笮點最低的節(jié)點，作為即格拓展的節(jié)點(nwfcj，同時將n?det實入CI<>siedIjt|中。Step3：若nodey在終點一定范圍內(nèi)，則用Dubim曲線擴展分析，若成功，則返回路徑。&ep4：若n〇dei超出局部地圖，則返M瑪翦路徑+Step5：擴展node，：遍歷其后鐘的每一個'子節(jié)腐(nodea)。(i)計算!Krfe，t到終點的啟發(fā)代價值：UU))與從起點到當(dāng)前點的累計代價值(gU)>，兩者求和得到總代價值即/(w)=n)+/i

　　3系統(tǒng)搭建3.1車載傳感器類型

　　實驗平臺為綠通LT-A627電動汽車，如圖3所示。定位糢塊主要使用SpatialNAV982i|號慣性導(dǎo)航系統(tǒng)，該系統(tǒng)集成GPS接收模塊，可以實現(xiàn)組合導(dǎo)航定位。霄達使用VelodyneVLP-16，可以實時掃描車輛周圍的環(huán)境，從而獲取車輛周圍的轉(zhuǎn)標(biāo)偉眞，實現(xiàn)對周圍目標(biāo)的識別、跟蹤等，傳感器參數(shù)如表U車載計算機侮用net郵ys，Nim)-7000，處理器角Int?srCorei7-8700，主頻為3.20GHz，運行內(nèi)存為16G，底層控制器使用tm32-F407b上位機與下位機俊用理232_：B通射。

　　3.2系統(tǒng)工作流程

　　R0S(ro丨x>top魄tihg夸st伽)[15]是由薇：坦稿大攀人工智能實驗室發(fā)布協(xié)開源的機器人操作系統(tǒng)&該操作系貌可以實現(xiàn)各個硬件模塊的抽象、梢息傳遞、底層的驅(qū)動協(xié)同管理，_時其分布式的特點，可以分模塊迸行開發(fā)，被廣泛運用子機器人行業(yè)，故蘋系統(tǒng)采用基于11〇3/11丨)_：1116.04醜基;本框架1?在11〇3操作系統(tǒng)框架下，系統(tǒng)工作流程如圖4所示。系統(tǒng)開始工作時，慣導(dǎo)模塊進行初始化，通過慣導(dǎo)模塊內(nèi)GPS接收車輛當(dāng)前位置，實現(xiàn)慣導(dǎo)瘼塊的初始對準(zhǔn)a*于慣性器件是逋過積分的方式工作，由上一時刻位餐推算：下一時刻的位寘■此只有初始位姿信息確定之后，慣導(dǎo)系統(tǒng)才能開始正常。同財，雷達初始化生成當(dāng)前焉部代價地圖。系統(tǒng)初始化時間在1分鐘左右，簡初始化完成以后，車輛得到當(dāng)前的位姿信息與周圍環(huán)境信息，在給定車輛要到達的目標(biāo)位置后，規(guī)劃模塊開始規(guī)劃路徑，規(guī)劃好的路徑由跟蹤算法解算出車輛將要行駛的速度和_角槪自、。獲取拿植將要行駛的速度和轉(zhuǎn)角信獻后，通過RS232串□下發(fā)紿底層控制器控制車輛移動。在行駛過程中.，規(guī)劃模塊會根磨霄達有描到的障礙物信息，實時重新規(guī)劃新的路徑，實現(xiàn)自主避障6

　　4實驗結(jié)果

　　為騷證系統(tǒng)的有效性各楱抉調(diào)試完成后，在R0S框架下進行聯(lián)合調(diào)試，并便用Ryk進行界菌可視化，實驗場景為校園場景，全局地圖采用成都信息工程大擧分梆傘為0.2983m/像素的柵格地圖(4846X2816)，黑色方框標(biāo)注為實驗地點，如圖5所示。為驗怔智能車在校園場景下是否能夠?qū)崿F(xiàn)自主導(dǎo)航規(guī)劃，實驗分為三部分^智能車.對靜態(tài)障礙物、動態(tài)障礙物避障和賣際道路實測。本賣驗將在柵格地虜上設(shè)置目標(biāo)位寳，使智能車自主導(dǎo)航規(guī)劃到自標(biāo)位置，通過轉(zhuǎn)折數(shù)量{智能車規(guī)劃路徑

　　40。為轉(zhuǎn)角)和與障礙物的距離來評價規(guī)劃的路徑好壞s

　　4.1靜態(tài)障礙物避障實驗

　　靜態(tài)障礙物避障實驗中，實驗場景設(shè)置在空曠的場地上，障礙物在智能車前方，設(shè)置目標(biāo)點使智能車啟主舞過障礙物到達目標(biāo)點&實驗結(jié)果如M6所示。

　　圖6中，連接車輛的紅色細(xì)線條為局部規(guī)劃路徑，連接車輛的紅色粗線條為行駛過的軌跡，紅色箭頭為目標(biāo)位置以及目標(biāo)位置車頭朝向，白色點集為障礙物，車身，苘圍輝圈為雷達激光掃描地面得到的結(jié)果^根據(jù)圖6可知，車輛在靜態(tài)障礙物的避障實驗中，規(guī)劃路線成功繞過車輛與師點之間的障礙物，購障礙物保持安全距氣

　　4.2動態(tài)障礙物避障實驗

　　動態(tài)避障實驗中，給智能車設(shè)定目標(biāo)俊豈，在車輛對向車輛，。智能車—(a)識別目標(biāo)障礙物規(guī)劃路徑(c)車輛成功避開障礙物向目標(biāo)僮置行進過程中，另外一輛本向反方向駛來，由于平合主要基于校園低速環(huán)境，故動態(tài)障礙物車輛速慶在10ba/h左右，實驗結(jié)巣如圖7所示。如圖7(a)所示，動態(tài)障礙物車輛在較遠(yuǎn)處時，智能車已規(guī)劃出避障路徑，車輛靠近時，局部路徑規(guī)劃出更大轉(zhuǎn)角，使智能車快速繞開動態(tài)障礙物如圖7(b)所示。圖7(c)中可以明顯現(xiàn)察到智能車駛過的軌跡，在對向車輛掌近時出現(xiàn)明顯的大轉(zhuǎn)角，箐能車快速避開動態(tài)障礙物。圖7(d)為智能車完整行駛軌跡。

　　4.3校園道路實驗

　　校0道路實測實驗中，選敢學(xué)校內(nèi)一段路況相對較為復(fù)雜的路徑作為實驗場攀，逋路寬度為7m左右，該段道路一旁■停有部分車輛及自行車，可行駛道路區(qū)域為411去右。途中常著：行人來住?學(xué)：簾目標(biāo)位：置后，智能車需要i人起始佼置自主導(dǎo)航到目標(biāo)位置。實驗結(jié)果如圖8所示。

　　圖8為道路實驗結(jié)果。圖8(a)中道路一旁停有車輛，道路另一旁有行人走過，智能車規(guī)劃出安全路徑行駛。圖8(b)為駛過彎道，彎道外側(cè)和內(nèi)側(cè)均有障礙物，智能車規(guī)劃路徑安全通過。圖8(c)中，車輛到達目標(biāo)位置。圖8(d)為智能車行駛軌跡，全程行駛在道路內(nèi)側(cè)且未貼近路沿。在車輛行駛過程中對路徑質(zhì)量進行評價，其參數(shù)如表2所示。

　　由表2可以看出，無論是靜態(tài)避障、動態(tài)避障還是道路實測實驗，智能車均能與障礙物保持較安全的距離。道路實測時，智能車全程未貼近路沿，安全行駛在道路內(nèi)側(cè)。實驗中最大轉(zhuǎn)角為39°，轉(zhuǎn)折數(shù)量均為0，規(guī)劃路徑平滑符合車輛運動學(xué)約束，智能車跟蹤路徑平穩(wěn)。

　　5結(jié)束語

　　采用慣導(dǎo)和GPS組合導(dǎo)航的方式實現(xiàn)定位，利用激光雷達創(chuàng)建局部地圖，利用混合As算法在當(dāng)前校園地圖進行路徑規(guī)劃，純跟蹤算法對路徑解算獲取速度和轉(zhuǎn)角，通過RS232串口實現(xiàn)與底層的通信，從而控制電機驅(qū)動車輛移動，實現(xiàn)了車輛的自主導(dǎo)航。在校園內(nèi)進行實驗，實驗結(jié)果表明：(1)智能車在靜、動態(tài)障礙物環(huán)境中，能夠自主避障;(2)智能車在校園非結(jié)構(gòu)化道路中，能夠有效地自主規(guī)劃出平穩(wěn)安全的行駛路徑，在遇到無法通行的障礙物前，車輛會采取制動操作等待障礙物消失或離開后重新規(guī)劃路徑。校園實驗表明，系統(tǒng)能夠?qū)崿F(xiàn)在校園內(nèi)進行自主導(dǎo)航，為針對特定應(yīng)用場景的無人接駁車、無人清潔車及快遞派送車等的功能型智能車提供了有效的自主導(dǎo)航解決方案。