多傳感器融合無人平臺航跡估算方法

范晶晶，廖培沖

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

1 引言

近年來，隨著人工智能和無人車技術的不斷發展，輪式無人平臺獲得了很大的發展，逐步應用于實戰。

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

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

2 坐標變換原理

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

2.1 平面直角坐標轉換原理

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

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

2.2 高斯投影轉換原理

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

圖1 高斯投影原理

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

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

3.1 基于標準卡爾曼濾波的模型建立

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

狀態變量選取：

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

假設k時刻的狀態變量為xk=[xk，yk，vk]T，則k+1 時刻的值為：

進一步計算：

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

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

則有：

式（5）中：Φk+1，k為狀態轉移矩陣；Vk為系統噪聲。

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

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

根據式到式可得卡爾曼濾波方程，狀態變量一步預測為：

狀態估計：

濾波增益矩陣：

一步預測誤差方差矩陣：

估計誤差方差矩陣：

3.2 仿真分析

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

圖2 北斗原始軌跡

圖3 估計軌跡

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

4 結論

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