黃 川， 胡 平， 連 靜

(大連理工大學 汽車工程學院, 遼寧 大連 116024)

隨著國家對于汽車工業(yè)的排放要求進一步提高，許多研究轉(zhuǎn)向了行駛過程中的優(yōu)化控制[1-5].這些方法均需要未來行駛道路的坡度數(shù)據(jù)，目前國內(nèi)大部分地圖數(shù)據(jù)庫并不包含足夠精確的坡度信息.因此應用這些技術需要建立一個具有地形信息的地圖數(shù)據(jù)庫.針對汽車行駛地形的研究工作大致分為計算機視覺[6-8]、激光探測[9-10]和加速度傳感器[11-12].計算機視覺與激光探測多應用在對道路坡度和兩旁物體進行探測[6-10].加速度傳感器多用于探測路面的平整度，尤其對于柏油路面的疲勞損壞等造成行駛過程中顛簸的因素具有良好檢測效果[11].然而，上述方法對地形的探測精度并不高，并且需要汽車安裝特定設備并由工作人員按照既定的路線行進行數(shù)據(jù)采集.

本文提出一種后臺運行、基于多臺配備GPS汽車的行駛數(shù)據(jù)重建機動車道3D地圖的方法.首先將GPS定位中經(jīng)緯度坐標轉(zhuǎn)換成平面直角坐標系，并通過總線信號推算汽車行駛軌跡.其次利用最小二乘法以及卡爾曼濾波器或遺傳算法對GPS軌跡與總線信號推算軌跡融合，得到更高精度的3D位置信息并將這些信息通過移動網(wǎng)絡實時上傳至服務器.服務器收到來自多臺汽車上傳的數(shù)據(jù)建立數(shù)據(jù)庫并使用K-聚類算法尋找道路中心線，以此建立機動車道3D地圖.該策略可在后臺實時運行并上傳數(shù)據(jù)，在用戶正常的用車過程中完成高精度機動車道3D地圖的建立，制圖成本低，并且當?shù)缆分匦陆ㄔO導致其地形發(fā)生變化時可以根據(jù)最新數(shù)據(jù)更新地圖數(shù)據(jù)庫.

1 坐標轉(zhuǎn)換及行駛軌跡推算

1.1 坐標轉(zhuǎn)換

GPS與總線信號推算行駛軌跡使用的坐標系不同，因此需要將其轉(zhuǎn)換為通用橫軸墨卡托投影，將經(jīng)緯度坐標轉(zhuǎn)化成平面直角坐標.

(1)

式中：γ代表經(jīng)度；φ代表緯度；a，b，e分別代表地球近似橢圓長、短半軸及偏心率.以GPS天線正下方的地面為參考點，忽略由于路面顛簸造成天線高度的微小變化.天線所處位置坐標列為(x1,y1,z1)，天線正下方地面位置列為(x,y,z).

x=x1；y=y1；z=z1-h/cosθ.

(2)

式中：h代表汽車在水平時GPS天線距離地面的高度；θ代表汽車所處地面與水平方向的夾角.

1.2 行駛軌跡推算

實驗發(fā)現(xiàn)，使用橫擺率推算的汽車行駛軌跡準確性要高于前輪轉(zhuǎn)角信號推算的軌跡，因此采用橫擺率推算汽車行駛軌跡.

(3)

2 數(shù)據(jù)處理方法

2.1 最小二乘法

總線信號還原的軌跡坐標僅代表t時刻車身相對初始時刻的相對位置，并存在系統(tǒng)誤差.采用最小二乘法作為數(shù)據(jù)初步擬合算法，將總線信號還原的軌跡坐標與同一時刻的GPS位置建立對應關系.以t0時刻GPS坐標作為起始位置.

(4)

式中：T是方向余弦變換矩陣；β是旋轉(zhuǎn)角度；[x′,y′]與[x″,y″]分別是轉(zhuǎn)換前后的點陣.程序根據(jù)式(5)對β進行搜索，n代表數(shù)據(jù)中點的個數(shù).

(5)

2.2 卡爾曼濾波器

GPS信號短期誤差較大，但誤差不隨著時間增加.總線信號推算的軌跡在每個測量周期內(nèi)保持較高精度，但每一步的計算都基于前一步結果的積分，誤差隨時間而增大.卡爾曼濾波器[12]通過迭代將兩個測量結果擬合得到一個更接近真實值的結果.假設兩種測量的誤差符合高斯分布，兩種測量結果相互獨立.在不存在控制量的情況下，卡爾曼濾波器的初始化公式為

(6)

式中：xk|k-1和xk-1|k-1分別代表k時刻的預測值和k-1時刻的修正值并始終相等;而pk|k-1和pk-1|k-1分別代表k時刻的誤差協(xié)方差的預測值和k-1時刻的誤差協(xié)方差的修正值；Q是符合高斯分布的過程噪聲協(xié)方差，即行駛軌跡推算中每一步誤差的平方.通過式(6)初始化得到初始值，隨后的迭代為

(7)

式中：Kk代表k時刻的卡爾曼增益；R是符合高斯分布的測量噪聲協(xié)方差，即GPS誤差值的平方；xk|k代表k時刻的輸出修正值；zk代表k時刻系統(tǒng)的觀測值，即GPS測量值.式(6)與式(7)經(jīng)整理為

(8)

2.3 遺傳算法

遺傳算法中，以每一步GPS坐標為圓心，GPS誤差范圍為半徑作圓，隨機分布建立原始種群，以行駛經(jīng)過這些點后與后續(xù)GPS點的偏離程度作為適應度函數(shù)，選擇偏移度小的個體.適應度函數(shù)如下:

(9)

式中：xf,yf分別代表經(jīng)過修正后的未來n個點的橫、縱坐標，并在下一步被用來計算修正后的方差；x1，x2，x3分別代表角度修正參數(shù)及x，y軸修正參數(shù)；T′代表角度修正的變換矩陣；xi，yi分別代表未來n個點經(jīng)過最小二乘法修正后的積分計算結果的橫、縱坐標；xi1，yi1分別代表被修正點的經(jīng)過最小二乘法修正后的積分計算結果的橫、縱坐標；xgn，ygn分別代表GPS結果中未來n個點中每一個點的橫、縱坐標；E是修正結果的最小方差.

2.4 K-聚類

K-聚類[13]將數(shù)據(jù)以K個類心聚成K個聚類.所有數(shù)據(jù)中隨機取K個數(shù)據(jù)作為聚類的初始類心.計算所有數(shù)據(jù)到類心的距離,根據(jù)數(shù)據(jù)間的距離來確定它們之間的相異度.并將其歸類到對應距離最小的那個類心所在的聚類中.根據(jù)聚類結果，重新計算K個聚類各自的中心，計算方法是取聚類中所有元素各自維度的算術平均數(shù).若產(chǎn)生的新類心與之前的類不同，則重復前兩步，直到結果不再變化.算法中K-聚類是分步進行的，只針對點云數(shù)據(jù)庫中的部分數(shù)據(jù)點.選取這些點時首先按照一定距離選取一個點(xt,yt)作為基準點，kt是t時刻相對(t-1)時刻的移動方向的斜率.過(xt,yt)點作一條垂直于該時刻移動方向的輔助線，并在數(shù)據(jù)庫中所有距離參考點距離小于l，并且從行駛方向的斜率與kt相差小于β的點中，找到所有距離直線r的距離小于d的點.

(10)

式中：xt-1，yt-1代表汽車在前一時刻的位置；xr,yr表示直線r的橫、縱坐標；x0，y0代表點云數(shù)據(jù)庫中全部點的坐標.

3 實驗環(huán)境與結果

實驗數(shù)據(jù)采集于具有設計圖紙的內(nèi)部測試場地，準確參數(shù)可以通過查閱設計圖紙獲得.測試場地大部分是海拔高度差在1 m之內(nèi)的平路，同時包含一段上下坡路，坡路的細節(jié)見圖1.測試車采用BMW 530Le，測試汽車的GPS天線安裝在車身頂部，取車身高度作為天線距離地面的高度為1.50 m，信號更新周期為0.10 s.

圖1 測試場地上下坡局部側視圖(單位:m)

測試共計7圈，耗時約900 s，行駛里程約17 km，氣溫-10 ℃，天氣晴朗，GPS衛(wèi)星數(shù)量持續(xù)大于7.收集的數(shù)據(jù)包括:車速、橫擺率、車身傾角、GPS經(jīng)緯度及高度.利用坐標轉(zhuǎn)換以及推算的行駛軌跡分別見圖2與圖3.

圖2中，曲線表示的GPS具有長期穩(wěn)定性，但是短期會有較大的偏移，從局部放大圖可見，結果有比較大的波動.與之相反，圖3中曲線代表的總線數(shù)據(jù)推算的行駛軌跡在短期內(nèi)可以準確反映汽車運動軌跡，曲線平滑，并未出現(xiàn)GPS結果中那樣的波動.然而長期來看，誤差會隨著累積逐漸增大.

圖2 GPS測量行駛軌跡

圖3 基于CAN的行駛軌跡

3.1 最小二乘法

選取測試開始的位置作為初始位置，通過最小二乘法將兩種結果初步擬合，結果見圖4，其中GPS軌跡與圖2相比并未發(fā)生變化.CAN相比圖3離散度明顯降低，而且與GPS結果基本重合.高度是一維變量，將GPS初始高度值作為行駛軌跡推算的高度初始值，未作其他修正.

3.2 卡爾曼濾波器

卡爾曼濾波器需要使用的一些參數(shù)見表1.

卡爾曼濾波器的經(jīng)緯度擬合結果見圖4.結果顯示，卡爾曼濾波器保持了GPS與總線信號推算的優(yōu)點，相比單純使用GPS的結果，定位精度大幅提升.

根據(jù)設計圖紙均勻地選取72個點作為標準值，以不同方法繪制的曲線到這些點的最短距離作為誤差值.結果如圖5所示，未經(jīng)過卡爾曼濾波器的GPS平均誤差達到1.65 m，方差0.78.經(jīng)過卡爾曼濾波器融合后，平均誤差降至0.72 m，方差0.12.卡爾曼濾波器將誤差降低了56.4%，描述誤差波動程度的方差降低約84.6%.

圖4 行駛軌跡擬合結果

表1 卡爾曼濾波器設定參數(shù)

圖5 多種算法結果的誤差

測試車GPS高度信號的測量分度值是1 m，遠高于經(jīng)緯度轉(zhuǎn)化為x-y平面直角坐標后的分度值，因此，高度擬合被單獨計算并且僅使用卡爾曼濾波器.測試場地大部分處于水平，僅在一處有上下坡，上下坡融合后的結果與圖1的比較如圖6所示.設計圖中標注出的參考點共12個，以圓點表示，曲線描述了卡爾曼濾波器計算后的結果，結果顯示卡爾曼濾波器的擬合結果與標準值誤差均小于0.20 m.其余72個參考點的計算高度與設計數(shù)據(jù)相比，高度誤差如圖7中的曲線所示，平均誤差為0.12 m.

圖6 卡爾曼濾波器上下坡處高度擬合結果

圖7 卡爾曼濾波器全程高度擬合結果誤差

3.3 遺傳算法

x-y平面上，使用遺傳算法進行信號融合可以得到精度高于卡爾曼濾波器的擬合結果.高度方面，由于GPS高度信號分度值過大，遺傳算法無法持續(xù)穩(wěn)定收斂至最優(yōu)結果附近，因此僅使用遺傳算法對x-y平面數(shù)據(jù)融合.遺傳算法的相關計算由Matlab提供的遺傳算法工具箱完成，誤差取值與具體的參數(shù)設置見表2，表3(未列出系統(tǒng)默認值).

72個采樣點使用遺傳算法擬合的結果誤差由圖5中的曲線給出，遺傳算法的平均誤差為0.60 m，方差0.08，分別比卡爾曼濾波器的計算結果降低了16.7%和33%.表明遺傳算法在經(jīng)緯度方面的融合具有比卡爾曼濾波器更小的誤差及更好的穩(wěn)定性.

表2 遺傳算法誤差取值

表3 遺傳算法參數(shù)設置

3.4 K-聚類

K-聚類可以將來自不同車道的數(shù)據(jù)分類并求對應類心.首先根據(jù)式(10)建立運用K-聚類算法所需要的局部數(shù)據(jù)，邊界條件的定義見表4.選用遺傳算法的擬合結果，隨機選取其中一個點進行數(shù)據(jù)挖掘處理，計算過程如圖8所示.

表4 K-聚類邊界條件

圖8 K-聚類計算過程

參考點選自測量數(shù)據(jù)，在圖8中以圓點表示.根據(jù)式(10)，點云數(shù)據(jù)庫中所有距離參考點10 m內(nèi)的點被列出(圖8中全部點).根據(jù)每個點的運動方向以及與建立的輔助線的距離選取K-聚類的數(shù)據(jù)點(圖8中全部非三角形點).參考點上方的三角形點為反向車道行駛時產(chǎn)生的數(shù)據(jù)點，行駛方向與參考點完全相反，因而未被采用.K-聚類算法產(chǎn)生了A，B兩個分類，參考點屬于聚類A.星號表示聚類的類心，作為行車線的中心建立最終機動車道3D地圖.通過以上方法完成對其余參考點的計算.還原的機動車道3D地圖在72個采樣點的橫、縱坐標和高度誤差分別如圖5和圖7中曲線所示.其中橫、縱坐標平均誤差降至0.37 m，方差降為0.03，相比GPS分別降低78%和96%，同時高度平均誤差為0.08 m.

3.5 實 測

取一段真實環(huán)境中的山路進行實車測量，測量地點為沈陽市棋盤山，全程約2 km.由于無法獲得該段道路準確設計參數(shù)，只能通過電子地圖來評估水平面上大數(shù)據(jù)建立地圖方法的準確性，先對該段道路地圖截圖進行預處理， 將公路位置轉(zhuǎn)化成數(shù)據(jù)作為比較對象，評估結果見圖9.

通過大數(shù)據(jù)方法還原的道路表面軌跡與電子地圖數(shù)據(jù)重合，說明二者基本一致.坡度數(shù)據(jù)的采集方式為:駕駛測試車沿路行駛并使用角度測量儀測量一次停車所在位置地面的坡度信息，從起點開始每間隔100 m進行采集，全程總計采集20個數(shù)據(jù)點，一共采集兩次取平均值為測量值，見表5.

圖9 實測結果

表5 坡度數(shù)據(jù)采集結果

測量值接近于計算結果，進一步計算得出:聚類結果與測量坡度值誤差約0.38%.結合通過本文使用大數(shù)據(jù)建立的地圖在經(jīng)緯度上及坡度上的誤差考慮，可以認為大數(shù)據(jù)方法能夠有效建立供新能源汽車使用的包含機動車道地形信息的電子地圖.

4 結 語

通過利用正常行駛汽車的數(shù)據(jù)，還原了高精度的汽車實時位置，這個過程中遺傳算法相比卡爾曼濾波器精度更高.實時位置被上傳至服務器，服務器利用來自多個汽車上傳的數(shù)據(jù)建立數(shù)據(jù)庫，對數(shù)據(jù)進行挖掘，繪制機動車道3D地圖.實驗結果顯示該方法所建立的地圖水平誤差平均為0.37 m，高度誤差平均為0.08 m.地圖的建立過程由軟件自動完成，駕駛員只需正常駕駛汽車，算法可根據(jù)最新的數(shù)據(jù)自動更新3D地圖數(shù)據(jù).建立的3D地圖具有精度高、低成本、實時更新等優(yōu)點，可以為行駛策略優(yōu)化提供相關的地形數(shù)據(jù)支持，降低汽車行駛能耗.