范晶晶，廖培沖

（1.北方工業(yè)大學(xué)城市道路交通智能控制技術(shù)北京重點實驗室，北京 100144；2.北京捷升通達信息技術(shù)有限公司，北京 102300）

1 引言

近年來，隨著人工智能和無人車技術(shù)的不斷發(fā)展，輪式無人平臺獲得了很大的發(fā)展，逐步應(yīng)用于實戰(zhàn)。

無人平臺主要依靠相機、激光雷達、慣導(dǎo)等設(shè)備進行環(huán)境感知，激光雷達等硬件設(shè)備綜合運用到智能車上的成本高，為無人駕駛裝甲車的大批量裝備帶來了很大的阻礙。因此，探索低成本的傳感器方案，并將其運用于無人平臺勢在必行。

為了有效解決無人平臺載廣闊地域的軌跡跟隨問題，并且有效降低傳感器成本，本文提出了一種基于輪速和方向盤轉(zhuǎn)角的無人平臺軌跡估計方法。將實時的差分北斗和軌跡地圖提出的坐標(biāo)轉(zhuǎn)換模型轉(zhuǎn)換到平面坐標(biāo)系內(nèi)，使用標(biāo)準(zhǔn)的卡爾曼濾波器對受干擾的北斗位置點進行濾波，從而得到精準(zhǔn)的軌跡。

2 坐標(biāo)變換原理

本文使用了全局坐標(biāo)系到車輛后軸中心局部坐標(biāo)系的轉(zhuǎn)換，并且在對北斗接收的經(jīng)度等其他信息處理時，完成了大地經(jīng)緯度坐標(biāo)系到二維平面坐標(biāo)的轉(zhuǎn)換。

2.1 平面直角坐標(biāo)轉(zhuǎn)換原理

在全局坐標(biāo)系XOY下A 點的坐標(biāo)為（x，y），在局部坐標(biāo)系X′O′Y′下A 點的坐標(biāo)為（x′，y′），局部坐標(biāo)系原點在全局坐標(biāo)系中的坐標(biāo)為（x0，y0），局部坐標(biāo)系和全局坐標(biāo)系逆時針旋轉(zhuǎn)的角度為θ，可以用下式將A 點坐標(biāo)在兩坐標(biāo)系下轉(zhuǎn)換：

將式（1）中（x-x0）和（y-y0）用xe和ye代替即可得到全局坐標(biāo)誤差向局部坐標(biāo)轉(zhuǎn)換：

2.2 高斯投影轉(zhuǎn)換原理

為了將北斗輸出的大地坐標(biāo)系下的經(jīng)度等信息轉(zhuǎn)化為二維平面坐標(biāo)系值，以便于進行軌跡誤差的計算，本文采用了高斯-克呂格投影的方法，該方法的原理如圖1所示。用一個圓柱筒體把地球包圍起來，圓柱筒和地球相切于子午線，在相切的子午線兩側(cè)分別取經(jīng)線1.5°或3°，再將此區(qū)域內(nèi)投影到圓柱筒壁之上，最后將圓柱筒沿著母線剪開并鋪平，即可得到圖1中右側(cè)的平面二維坐標(biāo)系，需要注意的一點是此坐標(biāo)系的橫縱坐標(biāo)和標(biāo)準(zhǔn)的坐標(biāo)相反。

圖1 高斯投影原理

3 基于卡爾曼濾波的軌跡估計模型

北斗定位狀態(tài)在受干擾時會在固定解、浮動解之間變化，易出現(xiàn)波動值，波動值的偏差有時可達3 m。不能準(zhǔn)確反映無人車的當(dāng)前位置，這對軌跡跟蹤控制的影響極大?？紤]到面對的是線性系統(tǒng)，所以沒有采用無跡卡爾曼濾波、擴展卡爾曼濾波等算法。分別采用標(biāo)準(zhǔn)卡爾曼濾波、平方根卡爾曼濾波（SRKF）以及改進的Sage-Husa 自適應(yīng)卡爾曼濾波（ISHAKF）對北斗數(shù)據(jù)進行了濾波，發(fā)現(xiàn)標(biāo)準(zhǔn)卡爾曼濾波的效果比較理想。

3.1 基于標(biāo)準(zhǔn)卡爾曼濾波的模型建立

將無人車的運動視為平面內(nèi)的二維運動，將軌跡地圖的第一個北斗點作為平面坐標(biāo)系的原點，無人車運動時可近似認(rèn)為勻速直線運動。

狀態(tài)變量選?。?/p>

式（2）中：x，y為高斯坐標(biāo)系下的坐標(biāo)；v為由輪速計算得到的車速。

假設(shè)k時刻的狀態(tài)變量為xk=[xk，yk，vk]T，則k+1 時刻的值為：

進一步計算：

式（3）中，θk可以表示為：

式（4）中：ψ為車輛的航向角；α為方向盤轉(zhuǎn)角。

則有：

式（5）中：Φk+1，k為狀態(tài)轉(zhuǎn)移矩陣；Vk為系統(tǒng)噪聲。

觀測變量Z=[x，y，v]T。

觀測方程為Zk=HkXk+wk，其中為觀測矩陣。

根據(jù)式到式可得卡爾曼濾波方程，狀態(tài)變量一步預(yù)測為：

狀態(tài)估計：

濾波增益矩陣：

一步預(yù)測誤差方差矩陣：

估計誤差方差矩陣：

3.2 仿真分析

采集車輛原始方向盤轉(zhuǎn)角、北斗位置、輪速信息，利用matlab 進行仿真，嚴(yán)重算法的可行性。仿真結(jié)果如下。北斗原始軌跡如圖2所示。估計軌跡如圖3所示。

圖2 北斗原始軌跡

圖3 估計軌跡

如圖2所示，沒有加輪速信息和方向盤轉(zhuǎn)角信息時，北斗定位點出現(xiàn)跳變。圖3中，加入了輪速信息和方向盤轉(zhuǎn)角信息后，原北斗跳變點被糾正過來，軌跡變得平滑。

4 結(jié)論

針對無人裝甲車軌跡跟隨問題，本文提出了一種基于輪速和方向盤轉(zhuǎn)角的無人裝甲車估計算法。通過經(jīng)緯度轉(zhuǎn)平面坐標(biāo)模型將軌跡地圖和北斗經(jīng)緯度值轉(zhuǎn)換到平面坐標(biāo)系，利用卡爾曼濾波算法融合輪速信號、方向盤轉(zhuǎn)角信息和北斗定位信息，從而修正了北斗定位的跳變點，得到了平滑可用的定位軌跡。