基于扩展Kalman滤波的无人驾驶车姿态测量研究

张健永，赵忠义，崔永志

基于扩展Kalman滤波的无人驾驶车姿态测量研究

张健永，赵忠义，崔永志

（辽宁工业大学 机械工程与自动化学院，辽宁 锦州 121001）

针对一般传感器精度低而高精度传感器成本高的问题，提出了基于扩展Kalman滤波的无人驾驶车姿态解算的测量算法，该算法产生了姿态测量的线性最小均方误差估计，可以在线对无人驾驶车行驶过程中的噪声进行估计，使得误差降低。Matlab仿真结果说明了该算法的有效性，为无人驾驶车的姿态参数测量提供了研究思路，具有一定的价值。

无人驾驶车；姿态解算；扩展Kalman滤波

无人驾驶车行驶过程中姿态参数的准确且实时地获取是无人驾驶车安全行驶的有力保障，一般情况下利用陀螺仪和加速度计之类的传感器进行测量[1-3]。可是由于传感器的精度、成本以及无人驾驶车行驶在不同路面中的运动学复杂度的缘故，无人驾驶车的姿态参数很多时候并不能直接测量。同时，高精度的传感器成本过高，不利于大面积推广应用；低精度的传感器有一定的漂移误差。这些问题给无人驾驶车的研究带来了很大的难题[4-6]。本文通过扩展Kalman滤波算法对无人驾驶车姿态解算进行仿真验证，结果表明，扩展Kalman滤波算法的姿态参数估计比较精确。

1 姿态解算

1.1 参考坐标系

物体的姿态根据自由度参量来描述，空间中的每一个物体都有6个自由度。物体的运动既是绝对的又是相对的，因此在研究运动的物体姿态变化时，一般要选择坐标系作为参考。不同的情况下所使用的坐标系并不相同，在无人车姿态测量中，选出3个坐标系供使用[7]，如图1所示。

图1 姿态测量坐标系

(1)地理坐标系OXYZ：O为无人车车体质心处，X轴指向北，Y轴指向东，Z轴垂直于当地水平面，即所谓的“东北天坐标系”。

(2)车体坐标系ObXbYbZb：在本文中，Xb轴为车体纵轴向前方向，Yb轴为无人车车体横轴向右方向，Zb轴为垂直于车体向上的方向。

(3)导航坐标系OpXpYpZp：本文选取地理坐标系OXYZ为导航坐标系。

1.2 无人车姿态积分算法

四元数与角速度之间的关系为：

矩阵形式如下：

根据上面3个公式，即可求得3个角度。

2 扩展Kalman滤波算法

扩展Kalman滤波(EKF)是一种适用于非线性系统的无偏、最小方差估计[12-13]。扩展Kalman滤波的基本原理和经典Kalman滤波基本一致[14]；不同的是，扩展Kalman滤波算法是通过对非线性系统的函数进行泰勒展开，舍去高阶量，把非线性模型线性化[15]。对于一个随机系统，具体的估计过程如下所示。

状态方程与测量方程可描述为：

因此，扩展Kalman滤波器基于时间变化的预测方程如下所示：

系统随时间变化的状态更新方程如下所示：

3 仿真研究

为了验证该算法的有效性，设计了基于扩展Kalman滤波的无人驾驶车姿态测量算法的仿真。以航向角30°为初始数据，进行误差分析。

图2为应用本文所述方法得到的航向角误差，从图2可以看到，在没有使用扩展卡尔曼滤波的情况下，角度所受到的干扰噪声较大，误差范围达到3°左右；使用了扩展卡尔曼滤波后，角度的误差范围控制在1°以内。误差精度提高了66%，且航向角趋向于真实值附近，验证了该算法的优点和可行性。

图2 姿态角仿真图

4 结束语

采用基于扩展Kalman滤波的无人驾驶车姿态解算测量算法，利用四元数法进行姿态解算，根据测得的数据融合扩展卡尔曼滤波不断估计和校正，从而降低了噪声导致的误差和漂移，提高了姿态测量的精度，实时有效地传输姿态参数，让无人驾驶车的行驶更加安全可靠。对比无滤波的姿态测量值可以看出，该算法可以减少噪声干扰，较为准确地估算出无人驾驶车的姿态角，使整车运行更加稳定可靠。

[1] 王世峰, 戴祥, 徐宁,等. 无人驾驶汽车环境感知技术综述[J]. 长春理工大学学报, 2017, 40(1): 1-6.

[2] 兰京. 无人驾驶汽车发展现状及关键技术分析[J]. 内燃机与配件, 2019(15): 209-210.

[3] 姜允侃. 无人驾驶汽车的发展现状剖析及发展趋势研究[J]. 微型电脑应用, 2019, 35(5): 60-64.

[4] 马天力. 无人驾驶智能车的运动稳定性研究[D]. 西安: 西安工业大学, 2013.

[5] Zhu Qiuming, Stuart L A, Tomas N R. Hierarchical collective agent network (HCAN) for efficient fusion and management of multiple networked sensors[J]. Information Fusion, 2007, 8(3): 266-280.

[6] 陈国峥, 孙建中. 舰载导弹发射装置通用化技术研究[J]. 舰船科学技术, 2003, 25(2): 34-38.

[7] 张小红, 李征航, 乔俊军, 等. 导航学[M]. 武汉: 武汉大学出版社, 2017.

[8] 王焱春, 张新英, 梁芬. 多传感器信息融合的四旋翼姿态解算研究[J]. 单片机与嵌入式系统应用, 2018, 18(8): 39-42.

[9] 姬占礼. 捷联惯导/GPS组合导航在弹箭上的应用研究[D]. 南京: 南京理工大学, 2004.

[10] 宋丽君. 基于MEMS器件的航向姿态测量系统的研究[D]. 西安: 西北工业大学, 2007.

[11] 邓建锋. 面向主动安全的汽车行驶姿态实时采集系统研究[D]. 桂林: 桂林电子科技大学, 2012.

[12] Wang Shuliang, Bi Daping, Li Jianping, et al. Joint detection and tracking algorithm for cognitive radar based on parallel structure of EKF and particle filter[J]. IET Radar, Sonar and Navigation, 2019, 13(11): 1990-1997.

[13] Danijel Pavkovic, Matija Krznar, Ante Komljenovic, et al. Dual EKF-based state and parameter estimator for a LiFePO4 battery cell[J]. Journal of Power Electronics, 2017, 17(2): 398-410.

[14] Sudarshan Swain, Bidyadhar Subudhi. Iterated extended Kalman filter-based grid synchronisation control of a PV system[J]. IET Energy Systems Integration, 2019, 1(4): 219-228.

[15] 吕梦男, 孙涛, 李洁. 基于扩展卡尔曼滤波的车辆状态参数估计[J]. 农业装备与车辆工程, 2018, 56(5): 77-80.

Research on Attitude Measurement of Unmanned Vehicle Based on Extended Kalman Filter

ZHANG Jian-yong, ZHAO Zhong-yi, CUI Yong-zhi

（College of Mechanical Engineering and Automation, Liaoning University of Technology, Jinzhou 121001, China）

Aiming at the problem that the general sensor has low precision and high-precision sensor cost, this paper proposes a measurement algorithm based on extended Kalman filter for the attitude calculation of driverless vehicles. The algorithm generates linear minimum mean square error estimation of attitude measurement, which can be online. The noise during driving the driverless car is estimated, so that the error is reduced. The Matlab simulation results show the effectiveness of the algorithm provides the research on the attitude parameter measurement of the driverless car, which has certain value.

driverless car; attitude solution; extended Kalman filter

TP311

A

1674-3261(2020)04-0216-03

10.15916/j.issn1674-3261.2020.04.002

2019-11-12

张健永(1993-)，男，安徽蚌埠人，硕士生。

赵忠义(1971-)，男，辽宁锦州人，高级工程师，博士。

责任编校：孙 林