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

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

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

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

　　0引言

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

　　路徑規(guī)劃的本質(zhì)是在一定約束條件下，找到一條連接起點(diǎn)與終點(diǎn)的無碰撞路徑[4]。常用的智能車路徑規(guī)劃算法包括基于采樣的方法和基于圖搜索的方法。基于采樣的方法包含概率路線圖(PRM)[5]與快速搜索隨機(jī)樹(RRT)[6]等。Karaman等[7]在RRT算法基礎(chǔ)上引人重選父節(jié)點(diǎn)與重布隨機(jī)樹過程，在時(shí)間充沛的情況下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)典的圖搜索算法，但是這些方法并不適用于智能車，因?yàn)槠渎窂讲贿B續(xù)。Dolgov等[14]直接在車輛控制空間內(nèi)米樣，再利用A的思想搜索路徑，解決這一問題。齊堯等[15]考慮安全因素，提出適用于混合的懲罰函數(shù)，得到更為安全、合理的路徑。文中針對(duì)的校園場景，行人較多，道路狹窄，車道線殘缺不全，故智能車的規(guī)劃路徑不考慮車道問題。基于校園場景下的導(dǎo)航規(guī)劃，主要考慮規(guī)劃路徑是否合理，智能車行駛途中能否安全避障等。因此，本文自主導(dǎo)航系統(tǒng)主要解決在車輛運(yùn)動(dòng)學(xué)約束下，定位和路徑規(guī)劃問題，目的是使智能車從當(dāng)前位置安全平穩(wěn)地到達(dá)目標(biāo)位置。

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

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

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

　　2，2估價(jià)函數(shù)

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

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

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

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

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

　　4實(shí)驗(yàn)結(jié)果

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

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

　　4.1靜態(tài)障礙物避障實(shí)驗(yàn)

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

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

　　4.2動(dòng)態(tài)障礙物避障實(shí)驗(yàn)

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

　　4.3校園道路實(shí)驗(yàn)

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

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

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

　　5結(jié)束語

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