文／寧予 張智清 劉明春

高精度定位對無人駕駛汽車的重要性不言而喻。而常用的定位方式若單獨使用則不能長期穩(wěn)定地為無人駕駛車輛提供高精度的定位信息。本文詳細介紹了慣導系統(tǒng)的工作原理，把它作為無人駕駛汽車的定位系統(tǒng)，能夠實現較好的定位效果。

慣導系統(tǒng)是車輛定位的核心設備，包括慣性傳感器（IMU）和全球定位系統(tǒng)（GNSS）。前者用于檢測車輛加速度與旋轉運動，實時估計車輛運動狀態(tài)，信息更新頻率高但存在偏差與噪音；后者通過衛(wèi)星定位，可提供三維坐標和速度，但信息更新頻率低且信號易丟失；兩者結合取長補短，能達到較好的定位效果。

一、慣導數據協(xié)議和語句格式

目前GNSS 接收機上使用最廣泛的協(xié)議是NMEA-0183 協(xié)議，大 多 數 常 見 的 GNSS 接 收機、GNSS 數據處理軟件、導航軟件都遵守或者至少兼容該協(xié)議。NMEA-0183 協(xié)議定義的語句格式眾多，常用的語句格式有$XXGGA、$XXGSA、$XXGSV、$XXRMC、$XXVTG 等，隨著各種衛(wèi)星系統(tǒng)增多，每種報文的報頭不一樣，如GPS 的報文頭為GP（即上述語句格式中的XX 為GP，則 $XXGGA 對應為 GPGGA），GLONASS 的報文頭為GL，中國北斗衛(wèi)星導航（BDS）的報文頭為BD，對于多系統(tǒng)聯合定位（雙星或者多星）的報文頭為GN。各語句中包含時間、經緯度、航向角、海拔高度等定位信息，并按照指定數據格式輸出。以$GPGGA 語句格式為例，其語句中的數據內容如下：

$GPGGA,utc,lat,latdir,lon,londir,QF,sat(No.),hdop,alt,aunits,age,stn(ID),★xx,[CR][LF]

上述語句中各數據段的含義如下表所示：

如某時刻慣導系統(tǒng)輸出的一條完整$GPGGA 數據為：

$GPGGA,011716.00,3120.998 2885,N,12117.5452325,E,4,23,1.1,26.4000,M,10.305,M,01,1496★6D

編號 名稱 描述格式1 $GPGGA Log header 2 utc UTC時間 (時/分/秒)hhmmss.ss 3 lat 緯度：-90～90度 緯度ddmm.mmmm（度分） 格式 ddmm.mmm 4 latdir 緯度方向：N：北；S：南N/S 5 lon 經度：-180～180度 緯度ddmm.mmmm（度分） 格式 ddmm.mmmm 6 londir 經度方向：E：東；W：西E/W 7 QF解狀態(tài)0：無效解； 1：單點定位解； 2：偽距差分；4：固定解； 5：浮動解；n 8 sat No.衛(wèi)星數n x.x 10 alt 高程 h.h 11 a-units 高程單位 M 12 age 差分延遲 dd 13 stn ID 基站號：0000-1023，單機時：AAAA xxxx 14 ＊xx Checksum ＊hh 15 [CR][LF] Sentence terminator [CR][LF]9 hdop 水平DOP值

則 可 知 該 時 刻 經 度為 12117.5452325，緯 度 為3120.9982885。 在 車 輛 行 駛 過程中，慣導系統(tǒng)連續(xù)采集車輛位置信息（慣導系統(tǒng)數據采集周期20ms-50ms），數據處理算法根據相應的語句格式，提取其中的經度、緯度和航向角數據，便可得到車輛連續(xù)的位置信息。

二、慣導數據處理

目前GNSS 接收機普遍采用串口輸出方式。使用工控機、PC、嵌入式控制器等計算平臺，通過串口與慣導設備連接，讀取慣導設備輸出的串口數據，根據數據格式協(xié)議進行解析，選取所需的數據（如經緯度、航向角等），通過坐標轉換，得到平面坐標系下的坐標信息（X、Y）。

（一）坐標系

大地坐標系是大地測量中以參考橢球面為基準面建立起來的坐標系。地面點的位置用大地經度、大地緯度和大地高度表示。主要涉及三類常用的大地坐標系統(tǒng)，即參心坐標系統(tǒng)（1980 西安坐標系、1954 北京坐標系）、地心坐標系統(tǒng)（WGS-84）和地方獨立坐標系統(tǒng)。

平面坐標系，UTM（UNIVERSAL TR ANSVERSE MERCARTOR GRID SYSTEM,通用橫墨卡托格網系統(tǒng)）坐標是一種平面直角坐標，通?；赪GS84 橢球。在北緯84°與南緯80°之間共有60 個經度帶，它們是6 度分帶。為了避免邊界的經度變形，使用了相交柱面進行投影。

高斯平面坐標系，以中央子午線與赤道的交點作為坐標原點，以中央子午線的投影為縱坐標軸X，規(guī)定X 軸向北為正，以赤道的投影為橫坐標軸Y，Y 軸向東為正，形成的坐標系。

（二）坐標系轉換

圖1 慣導系統(tǒng)應用框圖

GNSS 接收機輸出的數據通常包含經、緯度信息，為方便且直觀的得到位置信息，利用坐標轉換公式，即可將大地坐標系的經緯度信息轉換為平面坐標系的坐標值。以WGS84 坐標系轉UTM坐標系為例：只針對北半球，中國地區(qū)計算方法，經緯度分別為單位為弧度，UTM 坐 標 為（E,N），單 位 為k m，k0=0.9 9 9 6，E0=5 0 0 k m，e=0.0818192。轉換公式如下：

利用上述公式并進行轉換，編寫代碼，其中L 為經度，B 為緯度，最后得到X 為平面坐標系橫軸，Y為平面坐標系縱軸。

三、基于慣導系統(tǒng)的車輛行駛軌跡采集和處理

智能車輛在行駛過程中需要實時輸出其位置信息，并輸出實時的行駛軌跡，以作為車輛決策、規(guī)劃和控制的依據。車輛的位置信息和行駛軌跡通過對慣導系統(tǒng)采集的數據進行處理得到。

（一）基于ROS 生成實時行駛軌跡

ROS（Robot Operating System）機器人操作系統(tǒng)，是用于編寫機器人軟件程序的一種具有高度靈活性的軟件架構。ROS 提供一些標準操作系統(tǒng)服務，例如硬件抽象，底層設備控制，常用功能實現，進程間消息以及數據包管理。ROS 是基于一種圖狀架構，從而不同節(jié)點的進程能接受、發(fā)布、聚合各種信息（例如傳感，控制，狀態(tài)，規(guī)劃等等）?；赗OS 生成車輛實時行駛軌跡的過程如下：

1.初始化ROS 節(jié)點，發(fā)布話題；

2.設置串口參數，并判斷串口狀態(tài)；

3.讀取串口數據，根據慣導數據格式，判斷是否為需要處理的數據，如對比數據標識符是否為$GPGGA；

4.提取經緯度、航向角等信息，通過坐標轉換，將經緯度位置信息換算為大地坐標系的位置信息；

5.發(fā)布位置坐標到RVIZ（機器人操作系統(tǒng)3D 可視化工具）進行顯示，并保存數據。

圖2 坐標系轉換公式

圖3 根據坐標系轉換公式編寫代碼

（二）軌跡數據后處理

生成行駛軌跡時，由于各種因素（如有行人、障礙物在道路上）導致錄制的軌跡非理想狀態(tài)，此時進行適當的數據處理使得錄制的參考軌跡更加合理、真實。

1.軌跡點等間距化

慣導系統(tǒng)輸出的行駛軌跡，本質上為一連串的位置坐標數據，由于行駛過程中速度變化，導致采集的坐標數據間距不均勻，甚至某一位置存在大量的數據，不僅數據量多，且在自動駕駛時匹配參考軌跡點時不易匹配。因此將錄制的軌跡數據進行等間距化處理，使軌跡點間距相等，得到更加均勻分布的軌跡數據，有利于為自動駕駛系統(tǒng)提供更好的參考軌跡。

2.彎道曲線擬合

慣導系統(tǒng)在采集車輛位置數據，以及后續(xù)生成的路徑軌跡中，車輛在直角轉彎等轉彎半徑較小的情況下，存在行駛軌跡不平滑的問題，導致自動駕駛系統(tǒng)匹配參考軌跡時易偏離。因此需要對轉彎半徑較小的軌跡數據進行曲線擬合（如多項式擬合），擬合為平滑的圓弧，降低參考軌跡匹配偏移，提升行駛平穩(wěn)性。

（三）測試與結果分析

1.測試方案

本文的實車測試在漢騰汽車技術研究院的園區(qū)內進行的，該測試場地空曠，大部分區(qū)域衛(wèi)星信號較好，能保證GPS/IMU 系統(tǒng)具備良好的定位準確性，在測試中，慣導系統(tǒng)的主機安裝于車輛后軸中心處，通過串口與工控機連接實現通訊。

2.測試結果分析

為驗證慣導系統(tǒng)定位的精準性及基于ROS 生成實時行駛軌跡，智能汽車按照規(guī)劃的路線行駛，可在RVIZ 界面觀察到車輛位置以及行駛的軌跡，并將采集的數據進行后處理。經對比可發(fā)現：經處理后的數據所生成軌跡，數據量極大的壓縮，定位點間隔基本一致，且在彎道處的軌跡平滑，可以較合理地反映車輛的行駛軌跡，并作為自動駕駛車輛的參考行駛路徑。