基于數字地圖和慣性導航的車輛自主導航

文/湯爽 楊逸菲 張巧可 鄂旭

車輛自主導航系統是利用內置傳感器來確定車輛自身所處的相對位置以及行駛方向，采用數學分析的方法來確定車輛行駛路徑，并通過將該車輛行駛路徑與內存電子地圖上的道路進行對比，來確定車輛在地圖上所處的位置及所要到達目的地的方向和所剩距離等信息，并在顯示器上顯示出來，從而起到導航和引導的功能。車輛自主導航是降低車輛突發事件概率及減少人類乘車危險的一項核心技術，將廣泛應用于道路行駛。定位的精確度和控制的穩定性是實現車輛自主導航的關鍵，隨著 GPS（Global position system）、GLONASS（Global navigation satellite system）、伽利略衛星系統和北斗衛星系統的逐步完善，車輛自主導航技術的實現已經成為研究重點。國內外對于自主導航的研究主要集中在如何提高定位控制精度，精度方面如采用 RTK-GPS（Real time kinematic-GPS）差分定位技術、多傳感器信息融合技術、視覺導航技術等；控制方面如采用PID（Proportion integration differentiation） 控制方法、模糊控制、最優控制、純追蹤模型等參考文獻[1]。嚴恭敏[2]等人采用慣導/里程儀的組合算法，可大大減小慣性導航隨時間的積累誤差逐漸變大這一缺點，來為車輛提供精準的定位信息。陳靚影[3]等人采用GPS與慣性導航系統融合的精確連續定位算法，克服獨立的慣性導航系統無限制的位置誤差。由于車輛行駛道路的多樣性，算法的適應性和多樣性都有很大的改善空間。慣性導航系統的誤差還會隨著時間的增長而不斷積累增大,所以本文主要研究的是在數字地圖和慣性導航二者的基礎上提出的一種車輛自主導航算法。應用地圖匹配技術將慣性導航系統所得到的精確的定位信息與數字地圖進行對比和修正，從而提高定位的精度和導航的實時控制性，并根據行駛過程中各種情況的不同需要，建立了直線搜索路徑與曲線搜索路徑的方法，來實現車輛的自主導航行駛功能。

1 平臺體系架構

1.1 車輛自主導航系統平臺搭建

車輛自主導航系統主要由一輛小汽車、加速度計、陀螺儀、方向盤控制裝置及前輪轉角檢測裝置等構成，其中小汽車為試驗平臺，將加速度計和陀螺儀這兩種慣性元件組成慣性導航系統設備，安置到試驗平臺上，去分別測出車輛相對于慣性空間的線運動信息，并在給定的初始條件下，由計算機推測出車輛的姿態、行駛方向、速度、位置等，系統組成如圖1所示。

1.2 路徑搜索與轉向控制

車輛在自主導航的行駛中，系統會根據車輛狀況設置出直線預設路線和曲線預設路線，然后用直線、曲線路徑去定位當前車輛的所處位置，根據車輛當前位置與預設路徑之間的橫向偏差，控制車輛在道路上行駛。轉彎路徑作為一種轉彎角度大的曲線路徑，預設路徑的生成與曲線路徑生成相似。 在用戶選擇轉彎方式后，系統根據車輛的行駛幅度和最小轉彎半徑生成預設路徑，下文則分別介紹直線路徑搜索方法、曲線路徑搜索方法和車輛轉向方式。

1.2.1 直線路徑搜索方法

本文參考借鑒文獻[1]，選用簡化小車為運動模型，不考慮輪胎與地面摩擦和側滑、以及空氣阻力等因素，在直線導航路徑上設一點A，在距A點十米處在設置個B點，連接A、B并延長，為導航的直線預測路線。如圖2a所示，P點為車輛當前所處位置，通過慣性導航可以實時獲取P點的位置坐標，將車輛位置和行駛方向信息與直線預測路線對比，便會得到橫向偏差，通過P向AB做垂線，垂足為P0，PP0的距離d（cm）為車輛在行駛過程中的橫向偏差，直線MN為AB的平行線。

圖1

圖2：直線、曲線搜索圖

圖3：轉向路徑搜索圖

1.2.2 曲線路徑搜索方法

曲線路徑搜索是將一條曲線分成n個小段，再按照直線路徑搜索方法進行橫向偏差的計算，如圖2b所示，在曲線上設置A1, B1, C1,D1四個導航點，P1點為車輛當前所處位置，通過P1點向曲線A1B1段的切線做垂線，垂足為P1`，M1N1為曲線A1D1的切線的平行線，P1P1`長為d（cm），然后求出車輛的當前位置點P分別與每個導航點距離，取最小值，得到與車輛最近的導航點，假設A1點為M，B1點為（M+1），那么就可以按照直線路徑的搜索方法進行處理，同理可得，橫向偏差為d（cm）。

1.2.3 車輛轉向控制

首先通過利用車輛運動學模型的幾何方法。根據車輛行駛速度來確定預設點的位置，得到車輛到達指定位置的圓弧段路徑，并用運動模型來計算車輛的轉角。由圖3c所示，A2E2為車輛行駛轉彎的預設路徑，由一系列點組成，生成方法與曲線路徑生成方法類似，根據車輛行駛速度等間距的取A2, B2, C2, D2, E2五個點為例，開始導航時，車輛當前所處位置P2，搜索落在以P2為圓心，R2為半徑的圓內預設路徑上的點。半徑R2的確定與車輛行駛的速度和曲線的彎曲度有關，選擇車速為0.4M/s，R2為1.6M，恰好A2, B2, C2, D2四個點落在搜索圓內。點B2為距離P2點最近的點，點D2為搜索范圍內的最后一個預測點，故P2D2圓弧為車輛的行駛路徑，點O，為行駛路徑的圓弧的圓心，線段 P2P2`為曲線 A2E2切線的垂線，點 P2`為垂足，線段 P2P2`為橫向偏差（cm）。

在XOY坐標系下，利用慣性導航的定位功能來獲取當前位置坐標P2（X1, Y1），并由車輛行駛弧度和最小轉彎半徑來推算出預設路徑，根據車輛行駛速度和各導航點的間距得出預算點D2的坐標（X2,Y2），將坐標軸旋轉，以P2O`為X軸，P2F2為Y軸，P2（0，0）為車輛當前位置的坐標系F2P2O`的原點，建立直角坐標系，可得新坐標系下F2P2O`預瞄點坐標D2（x,y），XOY坐標系下點P2坐標為(x1,y1)，點 D2坐標為 (x2, y2)，F2P2O,坐標系下D2坐標為(x,y)。

通過D2點向P2O,做垂線，垂足為Q，連接D2O，由圖3c幾何關系建立方程組：

圓弧P2D2的圓心為O`，由幾何關系結合（2）得：

P2O`為圓弧P2D2的半徑，即轉彎半徑。

2 自主導航路徑的搜索方式

2.1 設計思路

如圖4所示，車輛自主導航系統共有四個模塊，由數據收發、數據處理、數據存儲以及顯示界面組成，數據收發模塊負責接收慣性導航系統提供的車輛實時定位坐標，數據處理模塊將INS接到的數據進行坐標系的轉換和參數的設定，根據行駛需求利用直線，曲線路徑搜索算法完成路徑跟蹤，數據存儲模塊負責車輛位置，導航狀態等信息的存儲，顯示界面模塊負責車輛狀態，路徑以及坐標等信息的顯示。

2.2 實驗與測試

本實驗參考借鑒文獻[4]，車輛分別在0.4m/s、0.6m/s、0.8 m/s 的恒定速度下直線行駛，橫向誤差是導致車輛在行駛過程中造成誤差的主要原因，根據實驗數據，建立了數學模型，當車輛分別在 0.4m/s、0.6m/s、0.8 m/s 時，數學模型如（4）（5）（6）所示，由于模型的二次項系數均大于0，故一階導數均為遞增函數，即橫向誤差的變化呈線性遞增。通過計算，線性遞增的斜率分別為 0.0018、0.0034、0.0050，表明車輛行駛速度越快，橫向誤差就越大。采用MATLAB作為實驗環境，CPU采用Intel（R）Core(TM)i7-8550U@1.80GHz 1.99GHz,系統類型為64位操作系統畫出試驗測試圖。

圖4：系統功能結構圖

圖5：實驗效果圖

h為橫向偏差的累積值，t為時間。

試驗結果如圖5（d),（e）,（f）所示：

實驗結果表明：在相同時間內，忽略摩擦力等因素的影響，隨著車輛行駛速度的不斷增大，橫向偏差也會越來越大；隨著車輛行駛速度的不斷減小，橫向偏差也會越來越小。

3 結論

本文通過利用車輛運動學模型的幾何等方法，計算并試驗得出“汽車隨著速度的增大，偏差也會越來越大；隨著車輛行駛速度的不斷減小，橫向偏差也會越來越小”的結論。我們應用地圖匹配技術將慣性導航系統所得到的精確的定位信息與數字地圖的結果進行對比，修正，從而提高定位精度和導航實時控制性，對減小汽車自主行駛的誤差有一定的幫助，并根據行駛過程中的不同環境，參考借鑒文獻[1]建立了直線搜索路徑與曲線搜索路徑的方法。同時在數字地圖和慣性導航結合的基礎上，會更加提高定位精度和導航實時控制性。該方法具有更好的導航效果。