多传感器融合无人平台航迹估算方法

范晶晶，廖培冲

（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 结论

针对无人装甲车轨迹跟随问题，本文提出了一种基于轮速和方向盘转角的无人装甲车估计算法。通过经纬度转平面坐标模型将轨迹地图和北斗经纬度值转换到平面坐标系，利用卡尔曼滤波算法融合轮速信号、方向盘转角信息和北斗定位信息，从而修正了北斗定位的跳变点，得到了平滑可用的定位轨迹。