校園智能車定位導航系統設計與實現

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

　　關鍵詞：智能車;定位;路徑規劃;自主導航

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

　　0引言

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

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

　　1GPS/慣導組合導航

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

　　GPS/SINS松耦合形式將慣導系統的誤義值作為狀態囊，將GPS誤差作為量測矂禽，卡爾曼_波器的系統模型為8x=FSx+G〇)(1)式中，&為當前時刻先驗狀態估計，F為動態系數矩陣為誤羞狀態矢量，G為噪聲吳量，W為預測過程中的噪聲;對應狀態矢鐵中每一個分量的_聲：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/^是泣置誤差失量;蠢導航系下的速度攝塗矢量;W基載體姿態誤差矢量;是慣導陀螺儀的誤_矢量;5/鳥慣導加速度計的誤差矢量。為方便編程實現，將連續系統模型離散化后摒'到離散模型為Szk=HkSxk+rjt(11)屮SINS_滬鄉Szk：rSINS_rGPS^SINS—^GPSyA—A/lSINSGPSh-hILsim，lg?s\vu，sim￣(12)式(11)中，由于內包含慣導系統的謨差暈則：羹測矢量屯為GPS觀測值與SINS預測值之差，如式(12)所示;表示量測噪聲;嘰表示零均值、方差為&的纛測嵊_。

　　2路徑規劃

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

　　2.1節點擴展方式

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

　　2，2估價函數

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

　　3系統搭建3.1車載傳感器類型

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

　　3.2系統工作流程

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

　　4實驗結果

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

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

　　4.1靜態障礙物避障實驗

　　靜態障礙物避障實驗中，實驗場景設置在空曠的場地上，障礙物在智能車前方，設置目標點使智能車啟主舞過障礙物到達目標點&實驗結果如M6所示。

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

　　4.2動態障礙物避障實驗

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

　　4.3校園道路實驗

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

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

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

　　5結束語

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