基于无迹卡尔曼滤波的定位融合与校验算法研究

doi:10.19562/j.chinasae.qcgc.2021.06.005

摘要/Abstract

摘要：

针对传感器定位精度受环境影响较大及其对自动驾驶车辆行驶安全影响较大的问题，提出一种定位融合及校验算法。通过建立短时短距航迹推算模型进行定位预测，使用无迹卡尔曼滤波实现预测定位结果与实际定位结果的非线性融合，从而提高定位精度。通过定位校验算法，判断实际定位数据是否发生偏移，并研究主动避险控制算法使车辆能够保持定位失效前的轨迹继续行驶直至停车，进而提高车辆行驶安全性。仿真和实车试验结果表明，该算法既能够提高定位精度，也能够有效判断定位是否发生偏移，同时在定位失效发生后，可以控制车辆按参考轨迹安全行驶。

关键词: 自动驾驶, 航迹推算, 定位预测, 定位校验, 主动避险控制

Abstract:

For the problem that the positioning accuracy of the sensor is greatly affected by the environment and it has great impact on the driving safety of autonomous vehicles， a positioning fusion and verification algorithm is proposed. The short?term short?range track calculation model is established for positioning prediction. And the UKF （Unscented Kalman Filter） is used to realize the nonlinear fusion between the predicted positioning result and the actual positioning result so as to improve the positioning accuracy. Whether the actual positioning has shifted can be judged by the positioning verification algorithm. And an active risk avoidance algorithm is studied to enable the vehicle to maintain the trajectory before positioning failure and continue to run until it stops， so as to improve the driving safety of the vehicle. Simulation and experimental results show that the algorithm can not only improve the positioning accuracy， but also effectively judge whether the positioning has offset， and ensure the vehicle to continue to run safely according to the saved reference trajectory after the positioning failure occurs at the same time.

Key words: autonomous driving, track calculation, positioning prediction, positioning verification, active collision avoidance control

李兴佳,李建芬,朱敏,彭能岭,左帅. 基于无迹卡尔曼滤波的定位融合与校验算法研究[J]. 汽车工程, 2021, 43(6): 825-832.

Xingjia Li,Jianfen Li,Min Zhu,Nengling Peng,Shuai Zuo. Research on Positioning Fusion and Verification Algorithm Based on UKF[J]. Automotive Engineering, 2021, 43(6): 825-832.

图/表 13

图 1

表 1

传感器定位数据状态判断表"

传感器定位数据状态

判断条件

正常

$(- 2 σ 1 ≤ x k + 1' - μ 1 ≤ 2 σ 1)$ 且

$(- 2 σ 2 ≤ y k + 1' - μ 2 ≤ 2 σ 2)$

突变偏移

$(4 σ 1 < x k + 1' - μ 1)$ 或 $(- 4 σ 1 > x k + 1' - μ 1)$ 或

$(4 σ 2 < y k + 1' - μ 2)$ 或 $(- 4 σ 2 > y k + 1' - μ 2)$

缓慢偏移

其他不满足正常或突变条件的情况

表 1

图 2

图 3

表 2

图 4

图 5

图6

图 7

图 8

表 3

表 4

图 9

参考文献 14

1	KIM D， JUNG T， YI K S. Lane map⁃based vehicle localization for robust lateral control of an automated vehicle ［J］. Journal of Institute of Control Robotics & Systems， 2015， 21（2）：108-114.
2	GHALLABI FAROUK， et al. Lidar⁃based lane marking detection for vehicle positioning in an HD map ［C］. IEEE International Conference on Intelligent Transportation Systems. Maui， Hawaii， IEEE， 2018.
3	SHARAF R， TAHA M R， TARBOUCHI M， et al. Merits and limitations of using fuzzy inference system for temporal integration of INS/GPS in vehicular navigation ［J］. Soft Computing A Fusion of Foundations Methodologies & Applications， 2007， 11（9）：889-900.
4	EL⁃SHAFIE A， NAJAH A， KARIM O A. Amplified wavelet⁃ANFIS⁃based model for GPS/INS integration to enhance vehicular navigation system ［J］. Neural Computing & Applications， 2014， 24（7-8）：1905-1916.
5	NOURELDIN A， EL⁃SHAFIE A， BAYOUMI M. GPS/INS integration utilizing dynamic neural networks for vehicular navigation ［J］. Information Fusion， 2011， 12（1）：48-57.
6	NEDA N， LANDRY RENÉ JR， JIANHUA C， et al. A new technique for integrating MEMS⁃based low⁃cost IMU and GPS in vehicular navigation ［J］. Journal of Sensors， 2016， 4：1-16.
7	WANG J C， LIU L， LIU Z， et al. Multilayer matching SLAM for large⁃scale and spacious environments ［J］. International Journal of Advanced Robotic Systems， 2015， 12（9）：1-8.
8	高伟健，储江伟，李春雷. 基于CORS网络差分的无人驾驶车定位研究［J］. 森林工程， 2020， 36（2）：97-102.
	GAO W J， CHU J W， LI C L. Localization of unmanned vehicle based on CORS network difference ［J］. Forest engineering， 2020， 36 （2）： 97-102.
9	GRUYER D， BELAROUSSI R， REVILLOUD M. Accurate lateral positioning from map data and road marking detection ［J］. Expert Systems with Applications， 2016， 43（C）：1-8.
10	SON Y S， KIM W， LEE S H， et al. Robust multirate control scheme with predictive virtual lanes for lane⁃keeping system of autonomous highway driving ［J］. IEEE Transactions on Vehicular Technology， 2015， 64（8）：3378-3391.
11	陆孟雄. 基于虚拟行驶环境下的汽车车道保持技术研究［J］. 中国设备工程， 2016（17）：156-157.
	LU M X. Research on vehicle lane keeping technology based on virtual driving environment ［J］. China Equipment Engineering， 2016（17）： 156-157.
12	HUANG C， LIU Y， JIA Y， et al. Position estimation for an unmanned ground car （UGC） by multi⁃sensor fusion under random loss of GPS signals ［C］. International Conference on Cyber Technology in Automation. Shenyang， IEEE， 2015.
13	汪涛. 智能行驶车辆定位技术研究［D］. 长春：吉林大学，2017.
	WANG T. Research on intelligent vehicle positioning technology ［D］. Changchun： Jilin University， 2017.
14	郭孔辉. 驾驶员-汽车闭环系统操纵运动的预瞄最优曲率模型［J］. 汽车工程， 1984， 6（3）：1-16.
	GUO K H. Preview optimal curvature model of driver vehicle closed loop control motion ［J］. Automotive Engineering， 1984， 6（3）： 1-16.

参数	数值	参数	数值
最短固定预瞄距离L_{static_min}	2 m	最长预瞄时间t₂	1.2 s
最长固定预瞄距离L_{static_max}	8 m	周期Δt	0.08 s
预瞄点个数P	6	车辆宽度	2.5 m
最短预瞄时间t₁	0.3 s	车辆长度	10 m
积累帧数FN	5	车道宽度	3.5 m

定位失效时车速/（km·h^-1）	有效轨迹长度/m	安全减速度/（m·s^-2）	给定减速度/（m·s^-2）	最大制动距离/m	最小制动距离/m	平均制动距离/m	横向最大偏差/m	横向最小偏差/m	横向平均偏差/m
10	59.8	-0.06	-0.3	20.1	17.4	18.8	0.14	0.02	0.06
20	59.8	-0.26	-0.4	56.1	50.4	53.4	0.23	0.02	0.07
30	59.8	-0.58	-0.9	55.3	51.1	53.7	0.25	0.05	0.16
40	59.8	-1.03	-1.5	55.2	52.1	53.5	0.23	0.04	0.14
50	59.8	-1.61	-2.2	58.3	47.8	53.8	0.42	0.026	0.20

定位失效时车速/（km·h^-1）	有效轨迹长度/m	安全减速度/（m·s^-2）	给定减速度/（m·s^-2）	最大制动距离/m	最小制动距离/m	平均制动距离/m	横向最大偏差/m	横向最小偏差/m	横向平均偏差/m
10	57.0	-0.06	-0.3	19.3	15.7	17.1	0.49	0.25	0.38
15	59.8	-0.15	-0.45	30.9	24.8	28.1	0.57	0.43	0.51
20	59.8	-0.26	-0.4	59.0	45.4	50.6	0.66	0.49	0.61
25	58.0	-0.42	-0.6	55.7	35.5	43.4	0.79	0.47	0.64
30	44.3	-0.78	-1.0	54.9	38.9	39.7	0.80	0.51	0.69

[1]	付新科,蔡英凤,陈龙,王海,刘擎超. 不确定性环境下的自动驾驶汽车行为决策方法[J]. 汽车工程, 2024, 46(2): 211-221.
[2]	赵晓聪,房世玉,李子睿,孙剑. 社会性驾驶交互关键效用析取与应用[J]. 汽车工程, 2024, 46(2): 230-240.
[3]	马艳丽, 秦钦, 董方琦, 娄艺苧. 基于风险场的不同认知次任务下接管风险评估模型[J]. 汽车工程, 2024, 46(1): 9-17.
[4]	刘卫国,项志宇,刘伟平,齐道新,王子旭. 基于分布式强化学习的车辆控制算法研究[J]. 汽车工程, 2023, 45(9): 1637-1645.
[5]	王明,唐小林,杨凯,李国法,胡晓松. 考虑预测风险的自动驾驶车辆运动规划方法[J]. 汽车工程, 2023, 45(8): 1362-1372.
[6]	朱向雷,吴志新,张宇飞,赵帅,李克秋,孙博华. 基于场景降维及采样方法的场景库优化方法研究[J]. 汽车工程, 2023, 45(8): 1408-1416.
[7]	吴新政,邢星宇,刘力豪,沈勇,陈君毅. 基于错误注入的决策规划系统抗扰性测试与分析[J]. 汽车工程, 2023, 45(8): 1428-1437.
[8]	高锋,冯德福,胡秋霞. 面向NMPC运动规划系统的数值优化加速技术[J]. 汽车工程, 2023, 45(8): 1438-1447.
[9]	芦涛,金馨,廖毅霏,黄圣杰,杨依琳,谢国涛,秦晓辉. 基于雅克比域零空间边缘化的视觉SLAM[J]. 汽车工程, 2023, 45(8): 1457-1467.
[10]	伍文广,田双岳,张志勇,张斌. 非铺装道路凹凸不平特征语义分割方法研究[J]. 汽车工程, 2023, 45(8): 1468-1478.
[11]	林程, 汪博文, 吕沛原, 宫新乐, 于潇. 面向变曲率道路的自动驾驶汽车换道博弈运动规划与协同控制研究[J]. 汽车工程, 2023, 45(7): 1099-1111.
[12]	赵东宇, 赵树恩. 基于级联YOLOv7的自动驾驶三维目标检测[J]. 汽车工程, 2023, 45(7): 1112-1122.
[13]	李军, 周伟, 唐爽. 基于自适应拟合的智能车换道避障轨迹规划[J]. 汽车工程, 2023, 45(7): 1174-1183.
[14]	赵嘉豪,齐志权,齐智峰,王皓,何磊. 基于轮胎特征点的并行大型车辆朝向角计算[J]. 汽车工程, 2023, 45(6): 1031-1039.
[15]	金立生,韩广德,谢宪毅,郭柏苍,刘国峰,朱文涛. 基于强化学习的自动驾驶决策研究综述[J]. 汽车工程, 2023, 45(4): 527-540.