改进的多项式曲线拟合轨迹预测算法

doi:10.12305/j.issn.1001-506X.2024.01.32

摘要/Abstract

摘要：

针对传统多项式曲线拟合轨迹预测算法对复杂多变的轨迹预测准确率不高问题, 提出改进的多项式曲线拟合轨迹预测算法。首先, 获得轨迹的曲率、挠率阈值; 然后, 通过该阈值识别预测误差可能较大的轨迹部位, 并采用插值滚动预测算法进行预测; 最后, 采用双误差预测值更新算法, 对预测值进行更新。仿真结果表明, 相较于传统多项式曲线拟合轨迹预测算法, 所提算法的平均位移误差(average displacement error, ADE)下降了42.77%, 最终位移误差(final displacement error, FDE)下降了36.62%, 从而验证了所提算法的可行性和有效性。

关键词: 多项式曲线拟合, 轨迹预测, 曲率, 挠率, 误差

Abstract:

Aiming at the problem that the traditional polynomial curve fitting trajectory prediction algorithm is not accurate enough for complex and changeable trajectory prediction, an improved polynomial curve fitting trajectory prediction algorithm is proposed. Firstly, the curvature and torsion thresholds of the trajectory are obtained. Secondly, the trajectory parts with larger prediction errors are identified by the thresholds above, and the interpolation rolling prediction algorithm is used for prediction. Finally, the double errors predictive value update algorithm is used to update the predicted value. Simulation results show that compared with the traditional polynomial curve fitting trajectory prediction algorithm, the average displacement error (ADE) of the proposed algorithm decreases by 42.77%. The final displacement error (FDE) reduces by 36.62%, which verifies the feasibility and the effectiveness of the proposed algorithm.

Key words: polynomial curve fitting, trajectory prediction, curvature, torsion, error

中图分类号:

黄万炎, 杜万和, 杨淑珍, 俞涛. 改进的多项式曲线拟合轨迹预测算法[J]. 系统工程与电子技术, 2023, 46(1): 280-289.

Wanyan HUANG, Wanhe DU, Shuzhen YANG, Tao YU. Trajectory prediction algorithm based on improved polynomial curve fitting[J]. Systems Engineering and Electronics, 2023, 46(1): 280-289.

图/表 20

图1

图2

图3

图4

图5

图6

图7

图8

表1

图9

图10

图11

图12

图13

图14

图15

表2

表3

表4

表5

参考文献 33

1	HAO X , HUYNH D Q , REYNOLDS M . PoPPL: pedestrian trajectory prediction by LSTM with automatic route class clustering[J]. IEEE Trans. on Neural Networks and Learning Systems, 2021, 32 (1): 77- 90. doi: 10.1109/TNNLS.2020.2975837
2	YOON Y , KIM T , LEE H , et al. Road-aware trajectory prediction for autonomous driving on highways[J]. Sensors, 2020, 20 (17): 4703. doi: 10.3390/s20174703
3	WU Z Y, CAO Z Q, YU Y Y, et al. A multi-robot cooperative hunting approach based on the dynamic prediction of target motion[C]//Proc. of the IEEE International Conference on Robo-tics and Biomimetics, 2017: 587-592.
4	DUAN Y, HUANG X, YU X. Multi-robot dynamic virtual potential point hunting strategy based on FIS[C]//Proc. of the IEEE Chinese Guidance, Navigation and Control Conference, 2016: 332-335.
5	刘文, 胡琨林, 李岩, 等. 移动目标轨迹预测方法研究综述[J]. 智能科学与技术学报, 2021, 3 (2): 149- 160.
	LIU W , HU K L , LI Y , et al. A review of prediction methods for moving target trajectories[J]. Chinese Journal of Intelligent Science and Technology, 2021, 3 (2): 149- 160.
6	LEON F , GAVRILESCU M . A review of tracking and trajectory prediction methods for autonomous driving[J]. Mathematics, 2021, 9 (6): 660. doi: 10.3390/math9060660
7	胡俊, 朱庆保. 基于动态预测目标轨迹和围捕点的多机器人围捕算法[J]. 电子学报, 2011, 39 (11): 2480- 2485.
	HU J , ZHU Q B . A multi-robot hunting algorithm based on dynamic prediction for trajectory of the moving target and hunting points[J]. Acta Electronica Sinica, 2011, 39 (11): 2480- 2485.
8	张强, 张振标. 基于曲线拟合的机动目标轨迹预测算法研究[J]. 信息化研究, 2018, 44 (6): 12-15, 30.
	ZHANG Q , ZHANG Z B . Research of maneuvering target tra-jectory prediction based on curve fitting[J]. Informatization Research, 2018, 44 (6): 12-15, 30.
9	李世杰, 雷虎民, 周池军, 等. 基于控制变量估计的高超声速再入滑翔目标轨迹预测算法[J]. 系统工程与电子技术, 2020, 42 (10): 2320- 2327.
	LI S J , LEI H M , ZHOU C J , et al. Trajectory prediction algorithm for hypersonic reentry gliding target based on control variables estimation[J]. Systems Engineering and Electronics, 2020, 42 (10): 2320- 2327.
10	CAO X , XU X Y . Hunting algorithm for multi-AUV based on dynamic prediction of target trajectory in 3D underwater environment[J]. IEEE Access, 2020, 8, 138529- 138538. doi: 10.1109/ACCESS.2020.3013032
11	WANG Z J, NIE Z Q, SHENG G. Dynamic position predicting of underactuated surface vessel with unscented Kalman filter[C]// Proc. of the 2018 Chinese Automation Congress, 2018: 4030-4033.
12	GUO G , ZHAO S J . 3D multi-object tracking with adaptive cubature Kalman filter for autonomous driving[J]. IEEE Trans. on Intelligent Vehicles, 2023, 8 (1): 512- 519. doi: 10.1109/TIV.2022.3158419
13	MOHAMMAD Z , HOSSEIN G Y , MEHRAN Y . Real-time object tracking based on an adaptive transition model and extended Kalman filter to handle full occlusion[J]. The Visual Computer, 2020, 36 (4): 701- 715. doi: 10.1007/s00371-019-01652-3
14	ABBAS M T , JIBRAN M A , AFAQ M , et al. An adaptive approach to vehicle trajectory prediction using multimodel Kalman filter[J]. IEEE Trans. on Emerging Telecommunications Technologies, 2020, 31 (5): 3734. doi: 10.1002/ett.3734
15	THIPPHAVONG D P , SCHULTZ C A , LEE A G , et al. Adaptive algorithm to improve trajectory prediction accuracy of climbing aircraft[J]. Journal of Guidance, Control, and Dynamics, 2013, 36 (1): 15- 24. doi: 10.2514/1.58508
16	HAN X , TIAN C . Estimation of buoy drifting based on adaptive parameter-varying time scale Kalman filter[J]. Journal of Control and Decision, 2021, 8 (3): 353- 362. doi: 10.1080/23307706.2020.1808863
17	WU Z J , TIAN S , MA L . A 4D trajectory prediction model based on the BP neural network[J]. Journal of Intelligent Systems, 2019, 29 (1): 1545- 1557. doi: 10.1515/jisys-2019-0077
18	CHEN K , SONG X , REN X X . Pedestrian trajectory prediction in heterogeneous traffic using pose keypoints-based convolutional encoder-decoder network[J]. IEEE Trans. on Circuits and Systems for Video Technology, 2021, 31 (5): 1764- 1775. doi: 10.1109/TCSVT.2020.3013254
19	LI S, ZHI Y, MELLADO S, et al. 3DOF pedestrian trajectory prediction learned from long-term autonomous mobile robot deployment data[C]//Proc. of the IEEE International Conference on Robotics & Automation, 2018: 5942-5948.
20	ALTCHE F, FORTELLE A. A LSTM network for highway trajectory prediction[C]//Proc. of the IEEE 20th International Conference on Intelligent Transportation Systems, 2017: 353-359.
21	ALAHI A, GOEL K, RAMANATHAN V, et al. Social LSTM: human trajectory prediction in crowded spaces[C]//Proc. of the IEEE Conference on Computer Vision and Pattern Recognition, 2016: 961-971.
22	ZHANG Y H , WEI W , YU D , et al. A tracking and predicting scheme for ping pong robot[J]. Journal of Zhejiang University-Science C(Computers & Electronics), 2011, 12 (2): 110- 115.
23	AKABANE R , KATO Y . Pedestrian trajectory prediction based on transfer learning for human-following mobile robots[J]. IEEE Access, 2021, 9, 126172- 126185. doi: 10.1109/ACCESS.2021.3111917
24	YANG J , SUN X , WANG R G , et al. PTPGC: pedestrian trajectory prediction by graph attention network with Conv- LSTM[J]. Robotics and Autonomous Systems, 2022, 148, 103931. doi: 10.1016/j.robot.2021.103931
25	SHANNON C . A symbolic analysis of relay and switching circuits[J]. Electrical Engineering, 1938, 57 (12): 713- 723. doi: 10.1109/EE.1938.6431064
26	POSTNIKOV A, GAMAYUNOV A, FERRER G. Transformer based trajectory prediction[C]//Proc. of the 35th Conference on Neural Information Processing Systems, 2021.
27	ZHU Z R , WU J W , YAN R , et al. Cutting force prediction considering tool path curvature and torsion based on screw theory[J]. The International Journal of Advanced Manufacturing Technology, 2021, 114 (5/6): 1601- 1621.
28	PAGANI L , SCOTT P . Curvature based sampling of curves and surfaces[J]. Computer Aided Geometric Design, 2018, 59, 32- 48. doi: 10.1016/j.cagd.2017.11.004
29	LU L Z , ZHAO S Q . High-quality point sampling for B-spline fitting of parametric curves with feature recognition[J]. Journal of Computational and Applied Mathematics, 2019, 345, 286- 294. doi: 10.1016/j.cam.2018.04.008
30	YE X Y , ZHU S P , CHEN S . Research on model predictive trajectory following control of automatic vehicle considering prediction error[J]. International Journal of Wireless and Mobile Computing: IJWMC, 2021, 21 (1): 52- 58. doi: 10.1504/IJWMC.2021.119062
31	VICTOR P , TOMOYUKI M . Smooth curve fitting of mobile robot trajectories using differential evolution[J]. IEEE Access, 2020, 8, 82855- 82866. doi: 10.1109/ACCESS.2020.2991003
32	YAN W, SUNA Y. Three-dimensional curve fitting based on cubic B-spline interpolation curve[C]//Proc. of the 7th International Congress on Image and Signal Processing, 2014: 765-770.
33	ZHENG F, FANG F, MA X D. Trajectory sampling and fitting restoration based on machine vision for robot fast teaching[C]// Proc. of the 15th IEEE Conference on Industrial Electronics and Applications, 2020: 604-609.

数据	参数方程	描述
1	$\left\{\begin{array}{l}x=\sin t \\y=\cos t \\z=\cos t\end{array}\right.$	t∈[0, 2π]
2	/	3次B样条曲线, 点数230
3	$\left\{\begin{array}{l}x=t \cos t \\y=t \sin t \\z=t\end{array}\right.$	t∈[0, 8π] 点数为300

算法	ADE/mm	FDE/mm	耗时/ms
IPCFTPA	0.004 4	0.015 9	3.902 3
PCFTPA	0.008 2	0.027 0	0.737 3
KF	2.910 6	3.403 9	0.602 4
BPNN	0.057 5	0.181 2	1 130.530 0
LSTM	1.971 4	4.581 6	8 519.360 0

算法	ADE/mm	FDE/mm	耗时/ms
IPCFTPA	0.323 3	1.150 6	1.837 3
PCFTPA	0.558 3	1.827 8	0.754 8
KF	9.896 5	11.63	0.786 9
BPNN	1.107 4	3.312	1 460.910 0
LSTM	6.096 7	12.988	3 656.130 0

算法	ADE/mm	FDE/mm	耗时/ms
IPCFTPA	0.864 7	2.167 3	3.833 2
PCFTPA	1.539 5	4.294 9	0.750 8
KF	3.384 1	3.899 3	0.889 5
BPNN	1.228 1	2.722 1	1 450.820 0
LSTM	4.968 1	10.324	6 919.180 0

数据	K_ADE	K_FDE
仿真数据1	59.70	48.07
仿真数据2	46.34	32.65
仿真数据3	42.09	37.05
仿真数据均值	49.38	39.26
真实数据1	43.83	49.54
真实数据2	32.99	26.78
真实数据3	31.64	25.64
真实数据均值	36.15	33.99
总均值	42.77	36.62