文／寧予 張智清 劉明春

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

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

一、慣導(dǎo)數(shù)據(jù)協(xié)議和語句格式

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

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

上述語句中各數(shù)據(jù)段的含義如下表所示：

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

$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 經(jīng)度：-180～180度 緯度ddmm.mmmm（度分） 格式 ddmm.mmmm 6 londir 經(jīng)度方向：E：東；W：西E/W 7 QF解狀態(tài)0：無效解； 1：單點定位解； 2：偽距差分；4：固定解； 5：浮動解；n 8 sat No.衛(wèi)星數(shù)n x.x 10 alt 高程 h.h 11 a-units 高程單位 M 12 age 差分延遲 dd 13 stn ID 基站號：0000-1023，單機(jī)時：AAAA xxxx 14 ＊xx Checksum ＊hh 15 [CR][LF] Sentence terminator [CR][LF]9 hdop 水平DOP值

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

二、慣導(dǎo)數(shù)據(jù)處理

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

（一）坐標(biāo)系

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

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

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

（二）坐標(biāo)系轉(zhuǎn)換

圖1 慣導(dǎo)系統(tǒng)應(yīng)用框圖

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

利用上述公式并進(jìn)行轉(zhuǎn)換，編寫代碼，其中L 為經(jīng)度，B 為緯度，最后得到X 為平面坐標(biāo)系橫軸，Y為平面坐標(biāo)系縱軸。

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

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

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

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

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

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

3.讀取串口數(shù)據(jù)，根據(jù)慣導(dǎo)數(shù)據(jù)格式，判斷是否為需要處理的數(shù)據(jù)，如對比數(shù)據(jù)標(biāo)識符是否為$GPGGA；

4.提取經(jīng)緯度、航向角等信息，通過坐標(biāo)轉(zhuǎn)換，將經(jīng)緯度位置信息換算為大地坐標(biāo)系的位置信息；

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

圖2 坐標(biāo)系轉(zhuǎn)換公式

圖3 根據(jù)坐標(biāo)系轉(zhuǎn)換公式編寫代碼

（二）軌跡數(shù)據(jù)后處理

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

1.軌跡點等間距化

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

2.彎道曲線擬合

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

（三）測試與結(jié)果分析

1.測試方案

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

2.測試結(jié)果分析

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