科学技术与工程

判据	$V -$ /(m·s^-1)	ΔV/(m·s^-1)	T/s	S/m
车辆	3.0	2.5	1.5	10.0
行人	1.0	1.0	1.5	2.0

判据

V -

/(m·s^-1)

ΔV/(m·s^-1)

T/s

S/m

车辆

3.0

2.5

1.5

10.0

行人

1.0

1.5

2.0

判据	$V -$ /(m·s^-1)	ΔV/(m·s^-1)	T/s	S/m
车辆	3.0	2.5	1.5	10.0
行人	1.0	1.0	1.5	2.0

判据

V -

/(m·s^-1)

ΔV/(m·s^-1)

T/s

S/m

车辆

3.0

2.5

1.5

10.0

行人

1.0

1.5

2.0

多传感器融合动目标检测算法
INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)
OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)
过程: 1.接收新一帧传感器数据T_L、T_I、T_R; 2.计算与输出缓存和候选缓存中的现有目标之间的距离S_L、S_I、S_R; 3.If S_L<Thre_L $\\|$S_I<Thre_I $\\|$S_R<Thre_R then 4.更新容器中传感器目标T'_sensor(X,Y,V_x,V_y,Class); 5.else 6.创建新的候选目标T_temp并放入候选缓存; 7.end if 8.传感器目标T'_sensor融合计算位置(X,Y)、速度(V_x,V_y)、目标类别Class并赋予目标ID,得到实际目标T; 9.计算前后两帧目标之间的距离差ΔS和时间差Δt进行帧间匹配。

多传感器融合动目标检测算法

INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)

OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)

过程:
1.接收新一帧传感器数据T_L、T_I、T_R;
2.计算与输出缓存和候选缓存中的现有目标之间的距离S_L、S_I、S_R;
3.If S_L<Thre_L $\|$S_I<Thre_I $\|$S_R<Thre_R then
4.更新容器中传感器目标T'_sensor(X,Y,V_x,V_y,Class);
5.else
6.创建新的候选目标T_temp并放入候选缓存;
7.end if
8.传感器目标T'_sensor融合计算位置(X,Y)、速度(V_x,V_y)、目标类别Class并赋予目标ID,得到实际目标T;
9.计算前后两帧目标之间的距离差ΔS和时间差Δt进行帧间匹配。

多传感器融合动目标检测算法
INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)
OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)
过程: 1.接收新一帧传感器数据T_L、T_I、T_R; 2.计算与输出缓存和候选缓存中的现有目标之间的距离S_L、S_I、S_R; 3.If S_L<Thre_L $\\|$S_I<Thre_I $\\|$S_R<Thre_R then 4.更新容器中传感器目标T'_sensor(X,Y,V_x,V_y,Class); 5.else 6.创建新的候选目标T_temp并放入候选缓存; 7.end if 8.传感器目标T'_sensor融合计算位置(X,Y)、速度(V_x,V_y)、目标类别Class并赋予目标ID,得到实际目标T; 9.计算前后两帧目标之间的距离差ΔS和时间差Δt进行帧间匹配。

多传感器融合动目标检测算法

INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)

OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)

类型	平均精度(AP50)
车辆	0.803	0.857	0.655	0.956
行人	0.787	0.868	0.669	0.934

类型

平均精度(AP50)

激光雷达

相机

毫米波雷达

融合算法

车辆

0.803

0.857

0.655

0.956

行人

0.787

0.868

0.669

0.934

类型	平均精度(AP50)
车辆	0.803	0.857	0.655	0.956
行人	0.787	0.868	0.669	0.934

类型

平均精度(AP50)

激光雷达

相机

毫米波雷达

融合算法

车辆

0.803

0.857

0.655

0.956

行人

0.787

0.868

0.669

0.934

参数名称	数值
整车质量m/kg	8 573
质心到前轴距离l_f/m	4.5
质心到后轴距离l_r/m	5.5
前轮侧偏刚度C_f/[(N·m)·rad^-1]	40 000
后轮侧偏刚度C_r/[(N·m)·rad^-1]	40 000
横摆转动惯量I_z/(kg·m^-2)	8 873
前轮转角δ/rad	-0.733~0.733

参数名称

数值

整车质量m/kg

8 573

质心到前轴距离l_f/m

4.5

质心到后轴距离l_r/m

5.5

前轮侧偏刚度C_f/[(N·m)·rad^-1]

40 000

后轮侧偏刚度C_r/[(N·m)·rad^-1]

40 000

横摆转动惯量I_z/(kg·m^-2)

8 873

前轮转角δ/rad

-0.733~0.733

参数名称	数值
整车质量m/kg	8 573
质心到前轴距离l_f/m	4.5
质心到后轴距离l_r/m	5.5
前轮侧偏刚度C_f/[(N·m)·rad^-1]	40 000
后轮侧偏刚度C_r/[(N·m)·rad^-1]	40 000
横摆转动惯量I_z/(kg·m^-2)	8 873
前轮转角δ/rad	-0.733~0.733

参数名称

数值

整车质量m/kg

8 573

质心到前轴距离l_f/m

4.5

质心到后轴距离l_r/m

5.5

前轮侧偏刚度C_f/[(N·m)·rad^-1]

40 000

后轮侧偏刚度C_r/[(N·m)·rad^-1]

40 000

横摆转动惯量I_z/(kg·m^-2)

8 873

前轮转角δ/rad

-0.733~0.733

面向有无人多车编队的车辆识别跟踪控制系统

PDF下载

庞明喜 , 代昌华 , 王志航 , 肖文山 , 石德乾 , 王鼎衡 ^*

科学技术与工程 | 论文·自动化技术、计算机技术 2025,25(10): 4206-4215

收起

科学技术与工程 | 论文·自动化技术、计算机技术 2025, 25(10): 4206-4215

面向有无人多车编队的车辆识别跟踪控制系统

全屏

庞明喜, 代昌华, 王志航, 肖文山, 石德乾, 王鼎衡^*

作者信息

西北机电工程研究所智能装备与技术研究室, 咸阳 712099

庞明喜(2000—),男,汉族,陕西安康人,硕士研究生。研究方向:模式识别与智能决策。E-mail:1419657226@qq.com。

通讯作者:

* 王鼎衡(1988—),男,汉族,陕西西安人,博士,助理研究员。研究方向:武器装备智能化。E-mail:wangdai11@163.com。

Vehicle Identification, Tracking and Control System for Unmanned Multi Vehicle Formation

Ming-xi PANG, Chang-hua DAI, Zhi-hang WANG, Wen-shan XIAO, De-qian SHI, Ding-heng WANG^*

Affiliations

Intelligent Equipment and Technology Research Office of Northwest Institute of Mechanical and Electrical Engineering, Xianyang 712099, China

出版时间: 2025-04-08 doi: 10.12404/j.issn.1671-1815.2402872

文章导航

摘要

收起

面向有人车引导的无人多车编队场景,设计并实现无人车在编队行驶中的车辆识别与轨迹跟踪控制系统,提出了一种多传感器后融合动目标检测算法,使用激光雷达、相机和毫米波雷达3种传感器作为数据源,分别使用欧式聚类、深度学习和运动学推理的方法对潜在目标进行检测,进而提出后融合方法将多源检测结果融合以实现对前方车辆的准确检测。基于前车轨迹生成期望路径并设计卡尔曼滤波器对期望路径进行平滑和滤波。构建车辆动力学模型、车辆道路误差模型并设计鲁棒H∞控制器进行车辆轨迹跟踪控制仿真。仿真与实车验证结果表明:在测试路段对前方车辆的平均识别准确率大于95%;实时期望路径相对于真实轨迹的均方差和轨迹平均变化率在滤波前后分别降低17.3%和48.6%;侧向控制位置误差和航向角误差相较于PID(proportional integral derivative)控制分别降低了29%和41%;车辆编队以最高54 km/h的速度实现编队整体的稳定行驶。

关键词

多传感器数据融合 / 多车编队 / 无人驾驶 / 轨迹跟踪 / 鲁棒控制

Abstract

收起

In the context of unmanned multi-vehicle formation guided by manned vehicles, a system for vehicle recognition and trajectory tracking control of unmanned vehicles during formation driving was devised and executed. An algorithm for multi-sensor fusion moving target detection was proposed, leveraging data from lidar, camera, and mmWave radar sensors. The algorithm utilizes Euclidean clustering, deep learning, and kinematic reasoning techniques for target detection. Additionally, a fusion methodology was introduced to integrate detection outcomes from various sources for precise identification of vehicles in the vicinity. Paths were anticipated based on the trajectories of preceding vehicles, and a Kalman filter was developed to smooth and filter these paths. A vehicle dynamic model, vehicle road error model, and the robust H∞ controller was established for vehicle trajectory tracking control simulation. Outcomes from simulation and real vehicle validation show as follows. The average recognition accuracy of preceding vehicles in test scenarios exceeds 95%. The mean squared error and average trajectory deviation rate of real-time anticipated paths decrease by 17.3% and 48.6% respectively pre and post filtering. Lateral control position error and yaw angle error decrease by 29% and 41% correspondingly compared to PID control. Vehicle formations attain stable working at speeds of up to 54 km/h.

Key words

multi sensor data fusion / multi vehicle formation / autonomous driving / trajectory tracking / robust control

引用本文

庞明喜, 代昌华, 王志航, 肖文山, 石德乾, 王鼎衡. 面向有无人多车编队的车辆识别跟踪控制系统. 科学技术与工程, 2025 , 25 (10) : 4206 -4215 . DOI: 10.12404/j.issn.1671-1815.2402872

Ming-xi PANG, Chang-hua DAI, Zhi-hang WANG, Wen-shan XIAO, De-qian SHI, Ding-heng WANG. Vehicle Identification, Tracking and Control System for Unmanned Multi Vehicle Formation[J]. Science Technology and Engineering, 2025 , 25 (10) : 4206 -4215 . DOI: 10.12404/j.issn.1671-1815.2402872

正文

收起

近年来,可自主行驶的无人车辆在多个领域取得了显著的发展和广泛的应用。如民用领域以新能源车企为代表的智能驾驶汽车,工业领域封闭环境下的无人矿产运输车辆,军用领域的无人侦察车、无人装备运输车等^[1]。虽然目前自动驾驶车辆可以实现一定程度上的自主行驶,但在复杂环境和任务下的完全自主还难以脱离人的参与,尤其是工业和军用领域常见的多车编队行驶场景受限于多智能体协同控制尚未解决的诸多问题^[2],导致完全自主化的编队行驶距离实际应用的目标尚远。因此,在有无人协同框架下,以有人车为引导令无人车执行自主编队行驶是一种更为合理的方案。一般地,无人车自动驾驶的技术难点大体上可落实在关键环境信息的识别、自身运动轨迹的规划以及控制车辆对轨迹进行跟踪这3个方面。在有人车引导下的多车编队行驶中,上述3方面工作应分别落实为:①无人车对场景中运动目标尤其是前车目标可靠、稳定、准确的检测;②判别前车目标并计算其稳定的、弱扰动的轨迹;③对前车轨迹进行稳定跟踪。

目前车辆目标检测主要依赖激光雷达、相机和毫米波雷达3种传感器。激光雷达在点云上开发的检测算法一类是基于欧式聚类的传统识别算法^[3-4],该方法在保证一定识别率的基础上可以有效降低运算压力;另一类是基于深度学习的方法,如PointPillars^[5]、PointRCNN^[6]和VoxelNet^[7]等,深度学习方法在识别率和准确率上有一定优势,但是存在算力要求高、需要特殊部署条件等问题。基于相机的目标检测主要采用深度学习方法,目前最常用的目标检测方法为YOLO系列^[8-11],在检测速度和准确率上可以满足当前主流任务场景要求。毫米波雷达使用多普勒频移效应检测目标,其有效检测距离较长且对烟尘雨雾等特殊条件适应性强,但是其获取的点云较为稀疏,在目标检测任务上具有一定的局限性。为了克服单传感器检测算法在烟尘、雨雾、弱光、遮挡等特殊场景存在的明显局限性,可通过融合多种传感器信息共同实现检测,相关融合算法一般包括前融合和后融合两种^[12-14]。前融合存在多模态传感器高精度联合标定以及高维多路原始数据并发融合的需求,对传感器安装位置设计和数据处理平台的算力要求较高,且难以避免长时间运行后的标定失效问题。后融合预先实现单传感器的目标识别,再根据识别出的目标特征进行融合,可以有效降低算力负载和算法部署难度,但目前缺乏常用且合理的融合规则和算法。

无人车获取期望轨迹一般是通过规划算法实现,包括基于地图的全局规划算法,如A^*-蚁群算法^[15],以及根据周围环境变化的局部搜索算法,如人工势场法^[16]和深度学习方法^[17]等。然而,编队任务可能包含沙漠、森林等复杂非结构化环境,存在不能获取全局地图和环境信息的情况。实际上,有无人编队场景并不需要每个无人车辆具备强大的自主轨迹规划能力,后续无人车辆仅需通过前方车辆位置信息变化得到本车准确的拟行驶轨迹即可。因此,关键问题在于对前车位置进行实时滤波与平滑处理以及本车期望轨迹规划生成的问题。

传统车辆轨迹跟踪控制方法如PID控制、线性二次型调节器(linear quadratic regulator, LQR)、模型预测控制(model predictive control, MPC)等^[18-21]已经取得一定的成效,但是对路面扰动、车速波动、轮胎柔性等非线性不确定因素的考虑明显不足^[22]。针对控制系统同时面临系统本身不确定性和多类外部干扰的问题,鲁棒控制成为一种重要且合适的解决方法,并成功在车辆跟踪控制领域取得一些成果,如基于H∞的输出反馈控制^[23]、存在系统延时和数据丢失情况下的H∞控制^[24]以及结合H₂/H∞的状态反馈控制^[25]等获得了较好的控制结果。但鲁棒控制在有无人编队场景下的轨迹跟踪适应性尚未得到有效验证。

针对有无人车辆编队在复杂环境下目标检测、基于前车位置的无人车轨迹规划、鲁棒控制在有无人编队场景下的适应性等问题,现提出一种面向有无人多车编队中的车辆识别跟踪控制系统,内含多传感器后融合动目标检测算法、本车期望路径生成与平滑算法、基于H∞控制器的前车轨迹跟踪控制算法等方面的研究内容对相关问题予以解决,该系统经实车验证实验证实其整体运行的有效性和工程应用能力。

1 系统框架设计

收起

基于上述文献的启发和实际应用需求,提出了一种面向有无人多车编队中的车辆识别跟踪控制系统,其结构如图 1所示。系统采用模块化设计思路,包含传感器前端识别、数据后端融合、前方车辆轨迹生成以及目标轨迹跟踪的全链路实现,满足无人车编队控制需求。

2 车辆目标检测实现

收起

2.1 激光雷达点云目标检测

受文献[3,12 -14]的欧式聚类方法所启发,首先对原始点云数据进行VoxelGrid体素降采样,然后通过点云之间的几何关系来滤除地面点云,最后使用欧式聚类方法拟合得到目标矩形框。

本文研究中使用的雷达为速腾聚创M1固态激光雷达,该雷达在单回拨模式下每秒发射787 500个点,为了降低算法运算压力,将感知区域进行体素化分解,使用大小为L×L×L的正方体填充点云区域,每个体素的中心由正方体内所有的点的几何中心(

x -

y -

z -

)确定。体素化降采样后的效果如图2(a)和图2(b)所示。

被检测的目标通常是高于地面的三维物体,所以滤除地面点云可以减小算法计算量。原理是计算降采样后的点云与雷达之间的绝对高度差h=|z_i+s|(其中s为雷达安装高度,z_i为第i个点云的z轴坐标),将绝对高度差与地面距离阈值H_glonal=d_itanθ_global进行比较(其中d_i为第i个点云与雷达的水平距离,θ_global为全局距离角度阈值),当h>H_global时表明该点与车底水平面的高度差大于地面距离阈值,即判定该点为非地面点。滤除地面点后效果如图2(c)所示。

在降采样和滤除地面点云后,分三步对点云进行聚类操作:①对于点云中的一点P,在一帧数据中找出与点P距离小于阈值的k个点,将这k个点加入集合Q;②对集合Q中P点外其他点执行1)中操作,直至Q中元素数量不再增加;③将集合聚类为一个点云簇,然后使用L-shape最小矩形框拟合^[26],并框选出目标车辆。激光雷达聚类检测目标结果如图3所示。

激光雷达以905 nm的激光束作为探测源,有长距离探测、不受环境光的影响、探测精度高的优点,同时也存在成本高、无法穿越烟尘雨雾等特殊场景的缺陷。

2.2 相机目标检测及位置信息获取

综合考虑YOLO系列模型的综合性能和成熟度,本文研究中使用YOLOv5s算法作为2D目标检测器,实车调试车辆检测效果如图4所示。

为保证融合程序输入数据的一致性,需要对图像检测出来的目标赋予位置信息。本文研究中考虑使用激光雷达的聚类结果和图像检测结果进行配准,然后拟合二者目标得到最终的图像目标位置信息。相机和激光雷达数据配准需要考虑空间同步和时间同步问题。

空间同步通过坐标转换实现,二者转换数学模型为

(1)U=λK(

R C L

L+

T C L

)

式(1)中:U=(u,v,1)^T为像素坐标;L=(x,y,z)^T为点云坐标;K为相机内参矩阵;

R C L

和

T C L

为激光雷达向相机转换的旋转和平移矩阵;λ为归一化参数。

将点云目标的角点(x_i,y_i,z_i),i=1,2,···,8通过式转换至相机像素坐标系如图5所示,读取相机目标在像素坐标系下的角点坐标(u_i,v_i),i=1,2,3,4,以二者像素坐标的差值即两种目标结果在车辆坐标系下的方位角和高低角来确定二者是否为同一个目标。若为同一目标,则对图像目标赋予位置信息。

时间同步通过外置精确时间同步协议(precision time protocol,PTP)硬件时间同步模块授时实现,同步精度为200 ns。

相机使用可见光探测目标,环境理解能力强、成本低和可视化效果好,但是缺少深度信息、容易受到环境光的影响。

2.3 毫米波雷达目标检测

本文研究中使用的毫米波雷达型号为ARS408,雷达数据以Cluster和Object的形式输出。Cluster是具有位置、速度和信号强度等信息的雷达反射,每一帧数据会重新评估Cluster的可信度,连续的Cluster集群构成具有历史和维度信息的Object目标。将雷达数据中的Object目标作为毫米波雷达检测的结果。

毫米波雷达输出结果缺少目标的类别信息,考虑到车辆和行人的运动往往是有规律可循的,具体表现为行驶的距离S、瞬时速度ΔV、平均速度

V -

等方面的明显差异。设S为目标轨迹点之间的距离和,

V -

为目标轨迹全程的平均速度,ΔV为轨迹后5帧的平均速度,添加行驶时间T滤除异常目标,阈值设置如表 1所示。具体的目标检测结果即根据表1判断获得。

毫米波雷达具有远距测速测距精度高、抗干扰能力强和全天候工作的优点,但是由于波段较长,毫米波雷达的分辨率较低,对目标细节的检测存在不足。

2.4 后融合程序框架设计

综合前文提到的3种传感器,激光雷达具有高精度和高分辨率的特点,可以获取周围环境的深度信息,对物体的形状、位置和大小进行准确测量;毫米波雷达在测距和测速方面表现出色,抗干扰能力强,能够在恶劣天气或复杂环境中稳定工作;相机则擅长捕捉丰富的纹理和颜色信息,能对目标的类别进行准确的识别。所以,融合3种传感器的数据可以综合利用各传感器的优势,优化对远距离目标、恶劣天气条件、复杂纹理信息的检测,实现全天候、全场景的目标稳定识别。

融合程序框架如图 1所示,程序通过设置两段名为“候选缓存”和“输出缓存”的缓存空间来滤波优化单传感器数据结果。融合算法流程如表 2所示,2.1节~2.3节所述的传感器识别结果为T_sensor(X,Y,V_x,V_y,Class),首先计算接收到的传感器数据T_L、T_I、T_R和缓存中现有目标的距离S_L、S_I、S_R,判断距离阈值Thre_L、Thre_I、Thre_R以确定是否更新传感器数据T'_sensor,然后融合计算实际目标,使用T'_sensor计算实际目标T的位置、速度、类别信息并根据场景赋予目标唯一的ID,最后进行目标帧间匹配以提高目标可信度。

算法缓存之间调度关系如图 6所示。融合主要解决的是一帧数据中存在多个传感器检测结果情况下,如何处理可能存在的矛盾信息生成目标,以及前后帧数据中出现的同一目标如何进行匹配的问题。

如图7所示,设T为最终输出的某个实际目标,那么对于一帧数据中的多个传感器检测结果,需要确定将哪些传感器结果(T_L,T_I,T_R)融合为最终的单个目标T。这里以T的历史轨迹为依据,结合车辆运动学模型预测当前帧的车辆预期位置S,基于S设定距离阈值获取包络圆,并认为满足阈值的检测结果数据正是T的可信结果。T的参数获取方式如下:①位置参数(X,Y):将获取到的若干传感器数据进行加权处理,测试发现设置相同的权重为一种较合理的权重策略;②类别参数(Class)前端检测结果中图像检测目标类别的方法相较于激光雷达的点云聚类和毫米波雷达的运动学推理方法更加准确,所以目标最终的类别参数采用图像、激光雷达、毫米波雷达的优先级进行分配;③速度信息(V_x,V_y):使用目标T的前后帧之间位置变化来计算速度及其方向。

对于不同帧数据中出现的同一目标T,下一帧目标位置位于图 7中的包络圆内,即前后两帧之间车辆位移是一个有限值,设定距离阈值S_th和时间阈值T_th,计算前后两帧之间目标的距离差ΔS和时间差Δt,当前后帧两个目标之间存在ΔS<S_th&Δt<T_th关系时,判定为同一个目标。

使用平均精度(AP50)作为识别准确率评价指标,融合算法得到的平均精度对比结果如表 3所示,在测试路段相较于激光雷达、相机和毫米波雷达,融合算法对前车的识别准确率分别提高了19%、12%、46%,对行人的识别率提高了19%、8%、40%,程序运行频率提高至20 Hz,弥补了激光雷达和相机数据因时间同步导致更新频率低的问题。

3 期望路径生成与平滑滤波

收起

在车辆编队行驶过程中,当不存在卫星定位辅助时,主要依据前车行驶过的轨迹来生成期望路径。将前车实时检测的轨迹结果进行存储,并将其作为本车的期望路径发布至局域网内供跟踪模块使用。

在实车调试过程中发现,前车轨迹构成的期望路径存在抖动较大且有异常值情况,所以考虑引入卡尔曼滤波器进行滤波。现阶段考虑车辆运动学模型为点模型,模型为

(2)

X (k) = X (k - 1) + V x d t Y (k) = Y (k - 1) + V y d t

式(2)中:X(k)、Y(k)为在大地坐标系下车辆k时刻的位置;V_x、V_y为车辆大地坐标系下x、y方向上的速度;d_t为步长。

设置转换矩阵和测量矩阵分别为

(3)A=

10 d t 0 010 d t 00100001

(4)H=

10000100

使用式构建卡尔曼滤波器进行实验。计算滤波前真实轨迹点和期望路径距离的均方差(mean square error,MSE)为0.29、滤波后MSE降为0.24,降低了17.3%。计算滤波前后x、y方向上各自偏导数比值的均值$\text { mean }\left(\frac{\Delta y}{\Delta x}\right)$以及轨迹变化率曲线来评估滤波之后轨迹的平滑性。滤波前后变化率绝对值的均值分别为0.70和0.43,降低了48.6%,轨迹变化率曲线如图 8所示,可以看到加入卡尔曼滤波器后有效地消除了目标轨迹抖动较大和存在异常值的情况。

4 前车轨迹跟踪控制实现

收起

4.1 车辆动力学模型构建

在假设车辆行驶在平坦路面并且车辆结构是刚性的条件下,以二自由度单车模型^[25]作为动力学模型的参考。设状态矩阵x=

[y ˙ φ ˙] T

、输出矩阵y=

[y ˙ φ ˙] T

、控制量为前轮转角δ,并构建动力学状态空间模型如式(5)所示,其中,各项变量内容与实验车辆相关参数设置如表 4所示。

(5)

d d t y ˙ φ ˙

- 2 (C f + C r) m v - v - 2 C f l f - 2 C r l r m v - 2 C f l f - 2 C r l r I z v - 2 C f l f 2 + 2 C r l r 2 I z v y ˙ φ ˙

2 C f m 2 C f l f I z

y ˙ φ ˙

1001 y ˙ φ ˙

4.2 车辆道路跟踪误差模型构建

车辆道路跟踪误差模型是轨迹跟踪控制的基础,参考文献[27]可知,e_y为车辆质心与道路参考点的距离偏差,e_φ表示与期望横摆角的偏差。注意这里根据实际情况考虑到航向角和横摆角之间的车辆侧偏角近似为零,故对其予以忽略。

e ˙ y

和

e ˙ φ

分别表示侧向位置和侧向横摆角变化率。误差计算公式为

(6)

e y = Δ Y c o s φ d - Δ X s i n φ d e φ = φ - φ d e ˙ y = y ˙ + v s i n e φ e ˙ φ = φ ˙ - φ ˙ d

式(6)中:ΔX与ΔY分别为参考路径点与当前车辆自身位置在大地坐标系下的横、纵坐标差;

φ ˙ d

=vρ_d;ρ_d和φ_d分别为期望的道路曲率和期望横摆角。

将式(5)代入动力学方程式中,设置状态变量x=

[e d e ˙ d e φ e ˙ φ] T

,控制量为前轮转角u=δ,扰动为参考路径点横摆角w=φ_d,可以得到存在扰动情况下的误差状态方程模型为

(7)

x ˙

0100 0 - 2 C f + 2 C r m v 2 C f + 2 C r m - 2 C f l f - 2 C r l r m v 0001 0 - 2 C f l f - 2 C r l r I z v 2 C f l f - 2 C r l r I z - 2 C f l f 2 + 2 C r l r 2 I z v

0 2 C f m 0 2 C f l f I z

0 - 2 C f l f - 2 C r l r m v - v 0 - 2 C f l f 2 + 2 C r l r 2 I z v

wz=Ix

式中:z为模型的观测量;I为单位矩阵;w为噪声值。

4.3 控制器设计及仿真测试

根据基于线性矩阵不等式(linear matrix inequality,LMI)的H∞控制器存在性和求取方法定理2^[20]可知,对于式所示的模型,设计状态反馈控制器u=Kx后的模型为

(8)

x ˙ = (A + B 2 K) x + B 1 w z = (C 1 + D 12 K) x + D 11 w

式(8)中:A、B₁、B₂、C₁由式(8)可知;D₁₁、D₁₂为0;K为状态反馈增益矩阵。

结合定理2和定理3^[22],设计状态反馈控制率u=Kx=WX^-1x,其中W、X由式(9)所示的线性矩阵不等式求解。

(9)

A X + B 2 W + (A X + B 2 W) T B 1 (C 1 X) T B T 1 - I 0 C 1 X 0 - I < 0 X = X T > 0

将式(5)~式(7)搭建Simulink/TruckSim联合仿真系统进行验证,使用LMI工具箱求解式得到的控制率为

(10)$u=\boldsymbol{W} \boldsymbol{X}^{-1} \boldsymbol{x}=\left[\begin{array}{llll} -2.257 & -1.593 & -8.111 & -0.301 \end{array}\right] \boldsymbol{x}$

在速度5、10、15 m/s的条件下,使用H∞控制和PID控制对车辆在双移线工况下进行仿真,设置PID参数为(0.65,0.001,0),仿真结果如图 9所示,其中pid表示PID控制结果曲线,hinf表示H∞控制结果曲线。

图9(a)显示了3种车速下PID控制和H∞控制的轨迹跟踪对比情况,3种车速H∞都能完成双移线工况下的轨迹跟踪任务,且H∞控制下的轨迹更加平滑和稳定。

图9(b)显示了3种车速下两种控制方法的侧向误差对比情况。3种车速使用H∞控制算法的侧向误差绝对值最大值分别为0.09、0.10、0.25 m,相较于PID控制算法,误差最大值分别减小了40%、45%、29%,表明H∞控制器具有更好的位置跟踪精度。

图9(c)显示了3种车速下两种控制方法的航向角偏差对比情况。3种车速使用H∞控制算法的航向角误差绝对值最大值分别为0.028、0.030、0.051 rad,相较于PID算法分别减小了23%、12%、41%,表明H∞控制器具有更好的航向角跟踪精度。

图9(d)显示的是3种车速下方向盘转角对比情况,使用H∞控制相较于PID控制具有更小和更平滑的控制转角,即拥有更好的车辆运行平稳性,与图9(a)中的分析一致。

上述分析综合说明,本文提出的H∞控制器相比目前常用的PID控制器在控制精度、输出稳定性、动态响应和收敛性等方面均有更好的效果,体现了鲁棒控制方法的优势。

5 系统的实车验证效果

收起

依托于某型测试车辆完成系统验证,该车主要参数如表4所示。本章依托于表4所示参数的车辆完成实车验证。编队由四辆该车型构成,头车为有人车、其他3辆车为无人车,在丢失车间通信和GPS信号被干扰的情况下,传感器融合感知开始介入,检测前方车辆并持续跟随实现编队行驶。

测试环境和行驶路线如图10 (a)所示,测试任务包括直线行驶、直角转弯、窄路跟随、定点停车等,道路场景包括平坦公路、高速公路、铁板路段和涉水路面等,实验车速根据有人主车速度而定,属于正常通行范围。本文研究的测试车辆为第2辆从车(编队3车)。

图10(b)显示的是编队车辆行驶至弯道时的运行效果,此时车辆速度为30 km/h。绿色方块即为检测到的目标车辆,方块右上角第1个数字表示车辆ID,第2个数字表示车辆在编队中的位置顺序。弯道处对前方视距内的主车和第1辆从车(编队2车)可以实现准确的检测,没有误检和漏检的情况出现。融合得到的轨迹如绿色方块后部的灰色虚线所示,本车轨迹为白色虚线,该场景下跟踪误差小于0.1 m。

图10(c)显示的是编队车辆直线行驶时的运行效果,此时车辆速度为50 km/h。得益于毫米波雷达的使用,在相机无法观察到前车的前方路况情形下,融合程序可以检测到前前车车头前方物体的前方车辆的存在,为系统提供了一定的安全冗余。同时从前车和本车的轨迹曲线来看,从初始位置开始轨迹在不断收敛,验证了控制器的有效性。

图10(d)显示的是视距内无法观察到车辆情况下对前车的检测跟踪运行效果,此时车辆速度为20 km/h。当相机丢失目标时,车辆前方配备的180°范围雷达点云可以有效检测队伍前方车辆,保证了系统检测目标的完备性。但由于转弯时曲率过大车辆导致跟踪误差也有所增大,此场景下的轨迹跟踪误差小于0.25 m。

综上所述,系统实现了对近距150.0°/60 m,远距14.0°/100 m范围内目标的准确检测和车辆判别,融合得到前车的正确且平滑的轨迹。在城市道路测试场景下,车辆编队以最高54 km/h的速度完成验证实验,测试车辆实现对前车轨迹的稳定跟踪。

6 结论

收起

提出了一种多车编队任务背景下利用多个传感器数据融合检测前方车辆并根据检测结果进行轨迹跟踪控制的系统。首先使用点云聚类、YOLOv5s和运动学推理方法对激光雷达、相机和毫米波雷达等数据进行目标识别。其次提出多传感器后融合方法将3种传感器进行融合推理得到稳定的目标检测结果。然后将检测到的目标轨迹进行平滑滤波生成期望路径,并构建车辆动力学模型、道路误差模型和鲁棒H∞控制器对前车轨迹进行准确跟踪。该系统在目标识别准确率、稳定性、抗干扰性以及实时性上相较于单传感器方法有较大提升,,对前车检测识别准确率高于95%,相对使用单个激光雷达、相机和毫米波雷达传感器时的准确率分别提升了19%、12%和46%;同时,轨迹跟踪控制仿真结果证实了所设计控制器相对传统PID方法具有优势,侧向控制位置误差和航向角误差,相较于传统实车上的PID控制分别降低了29%和41%;系统在实车验证环境下,以54 km/h的速度实现了编队整体稳定行驶,实车验证实验表明了系统整体运行的有效性和工程应用能力。

基金

收起

咸阳市重大科技创新专项(L2023-ZDKJ-JSGG-GY-018)

参考文献

收起

文献

收起

参考文献引证文献

排序方式：

[1]

马志艳, 邵长松, 杨光友, 等. 同步定位与建图技术研究进展[J]. 电光与控制, 2023, 30(3): 78-85, 106.

Zhiyan

, Shao

Changsong

, Yang

Guangyou

, et al. Research progress of SLAM technology[J]. Electronics Optics & Control, 2023, 30(3): 78-85, 106.

[2]

苏治宝, 项燊, 余雪玮, 等. 一种全要素自主车队协同控制仿真系统[J]. 兵工学报, 2023, 44(S2): 35-43.

Zhibao

, Xiang

Shen

, Yu

Xuewei

, et al. A simulation system for cooperative control of autonomous convoy[J]. Acta Armamentarii, 2023, 44(S2): 35-43.

[3]

范晶晶, 王力, 褚文博, 等. 基于KDTree树和欧式聚类的越野环境下行人识别的研究[J]. 汽车工程, 2019, 41(12): 1410-1415.

Fan

Jingjing

, Wang

, Chu

Wenbo

, et al. Research on pedestrian recognition in cross-country environment based on KDTree and euclidean clustering[J]. Automotive Engineering, 2019, 41(12): 1410-1415.

[4]

Qiao

J C

. Component-based 2-/3-dimensional nearest neighbor search based on Elias method to GPU parallel 2D/3D Euclidean Minimum Spanning Tree Problem[J]. Applied Soft Computing, 2021, 100(1). DOI: 10.1016/j.asoc.2020.106928.

[5]

Lang

A H

, Vora

, Caesar

, et al. PointPillars: fast encoders for object detection from point clouds[C]// IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). Long Beach: IEEE, 2019: 12689-12697.

[6]

Shi

, Wang

, Li

. PointRCNN: 3D object proposal generation and detection from point cloud[C]// IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR). Long Beach: IEEE, 2019: 770-779.

[7]

Zhou

, Tuzel

. VoxelNet: end-to-end learning for point cloud based 3D object detection[C]// IEEE/CVF Conference on Compu-ter Vision and Pattern Recognition. Salt Lake: IEEE, 2018: 4490-4499.

[8]

Redmon

, Divvala

, Girshick

, et al. You only look once: unified, real-time object detection[EB/OL]. (2015-06-08)[2023-10-08]. https://arxiv.org/abs/1506.02640v5. https://arxiv.org/abs/1506.02640v5

[9]

Redmon

, Farhadi

. YOLO9000: better, faster, stronger[EB/OL]. (2016-12-25)[2024-01-28]. https://arxiv.org.libyc.nudt.edu.cn:80/abs/1612.08242v1. https://arxiv.org.libyc.nudt.edu.cn:80/abs/1612.08242v1

[10]

Redmon

, Farhadi

. YOLOv3: an incremental improvement[EB/OL](2018-04-08)[2024-01-28]. https://arxiv.org.libyc.nudt.edu.cn:80/abs/1804.02767v1. https://arxiv.org.libyc.nudt.edu.cn:80/abs/1804.02767v1

[11]

Bochkovskiy

, Wang

C Y

, Liao

H Y M

. YOLOv4: optimal speed and accuracy of object detection[EB/OL]. (2020-04-23)[2024-01-28]. https://arxiv.org.libyc.nudt.edu.cn:80/abs/2004.10934v1. https://arxiv.org.libyc.nudt.edu.cn:80/abs/2004.10934v1

[12]

黄远宪, 周剑, 黄琦, 李必军, 等. 融合相机与激光雷达的目标检测、跟踪与预测[J]. 武汉大学学报(信息科学版), 2024, 49(6): 945-951.

Huang

Yuanxian

, Li

Bijun

, Huang

, et al. Camera-LiDAR fusion for object detection, tracking and prediction[J]. Geomatics and Information Science of Wuhan University, 2024, 96(6): 945-951.

[13]

赵浙栋, 张成涛, 李习刊, 等. 相机与激光雷达融合的道路目标检测研究[J]. 广西科技大学学报, 2024, 35(1): 84-93.

Zhao

Zhedong

, Zhang

Chengtao

, Li

Xikan

, et al. Research on road target detection based on the fusion of camera and LiDAR[J]. Journal of Guangxi University of Science and Technology, 2024, 35(1): 84-93.

[14]

郑少武, 李巍华, 胡坚耀. 基于激光点云与图像信息融合的交通环境车辆检测[J]. 仪器仪表学报, 2019, 40(12): 143-151.

Zheng

Shaowu

, Li

Weihua

, Hu

Jianyao

. Vehicle detection in the traffic environment based on the fusion of laser point cloud and image information[J]. Chinese Journal of Scientific Instrument, 2019, 40(12): 143-151.

[15]

王锋, 李凯璇, 朱子文, 等. 基于改进A^*蚁群融合算法的路径规划研究[J]. 火力与指挥控制, 2024, 49(1): 111-117, 123.

Wang

Feng

, Li

Kaixuan

, Zhu

Ziwen

, et al. Research on path planning based on improved A^* ant colony fusion algorithm[J]. Fire Control & Command Control, 2024, 49(1): 111-117, 123.

[16]

姬鹏, 郭明皓. 基于Frenet坐标下改进人工势场法的无人车局部路径规划[J]. 兵工学报, 2024, 45(7): 2097-2109.

Peng

, Guo

Minghao

. Local path planning for unmanned vehicles based on improved artificial potential field method in frenet coordinates[J]. Acta Armamentarii, 2024, 45(7): 2097-2109.

[17]

董涛涛, 宋宇博. 基于自适应阈值的循环增长地面点云分割算法[J]. 科学技术与工程, 2024, 24(9): 3526-3532.

Dong

Taotao

, Song

Yubo

. Ground point cloud segmentation algorithm with cyclic growth based on adaptive threshold[J]. Science Technology and Engineering, 2024, 24(9): 3526-3532.

[18]

徐明泽, 刘清河. 基于LQR和PID的智能车轨迹跟踪控制算法设计与仿真[J]. 太原理工大学学报, 2022, 53(5): 877-885.

Mingze

, Liu

Qinghe

. Design and simulation of intelligent vehicle trajectory tracking control algorithm based on LQR and PID[J]. Journal of Taiyuan University of Technology, 2022, 53(5): 877-885.

[19]

Altan

, HacioğLu

. Model predictive control of three-axis gimbal system mounted on UAV for real-time target tracking under external disturbances[J]. Mechanical Systems and Signal Processing, 2020, 138. DOI: 10.1016/j.ymssp.2019.106548.

[20]

Rasekhipour

, Khajepour

, Chen

S K

, et al. A potential field-based model predictive path-planning controller for autonomous road vehicles[J]. IEEE Transactions on Intelligent Transportation Systems, 2017, 18(5): 1255-1267.

[21]

余米森, 钱玉宝, 黄华宝, 等. 连续工况下基于PID+LQR算法的自动驾驶车辆横纵向耦合控制[J]. 科学技术与工程, 2022, 22(30): 13490-13496.

Misen

, Qian

Yubao

, Huang

Huabao

, et al. Lateral and longitudinal coupling control of autonomous vehicle based on PID + LQR algorithm under continuous conditions[J]. Science Technology and Engineering, 2022, 22(30): 13490-13496.

[22]

邵嶽. 高压巡检机器人H∞鲁棒控制器设计[J]. 电气技术, 2016(1): 66-71.

Shao

Yue

. Design of high voltage inspection robot H∞ robust controller[J]. Electrical Engineering, 2016(1): 66-71.

[23]

Jing

, Hu

, Yan

, et al. Robust H∞ output-feedback control for path following of autonomous ground vehicles[J]. Mechanical Systems and Signal Processing, 2016(70/71): 414-427.

[24]

Chen

, Shu

, Liu

, et al. Robust H∞ trajectory following of autonomous vehicles with delay and packet dropout-all databases[EB/OL]. [2024-02-01]. https://webofscience-clarivate-cn-s.libyc.nudt.edu.cn/wos/alldb/full-record/WOS:000569985300114. https://webofscience-clarivate-cn-s.libyc.nudt.edu.cn/wos/alldb/full-record/WOS:000569985300114

[25]

Emam

, Fakharian

. Robust path following of a car-like robot in the presence of sliding effect based on LMI formulation[C]// Artificial Intelligence and Robotics (IRANOPEN). Qazvin:IEEE, 2017: 121-126.

[26]

Zhang

, Xu

, Dong

, et al. Efficient L-shape fitting for vehicle detection using laser scanners[C]// IEEE Intelligent Vehicles Symposium (IV). Los Angeles: IEEE, 2017: 54-59.

[27]

代昌华. 基于博弈权限分配的人机横向共享驾驶规划与控制研究[D]. 长春: 吉林大学, 2023.

Dai

Changhua

. Research on planning and control of human-machine lateral shared driving based on game authority allocation[D]. Changchun: Jilin University, 2023.

2025年第25卷第10期

PDF下载

383

166

引用本文

BibTeX

文章信息

doi: 10.12404/j.issn.1671-1815.2402872

接收时间：2024-04-19
首发时间：2025-07-09
出版时间：2025-04-08

补充材料

相关文章

文章信息

作者

出版历史

收稿日期：2024-04-19
修回日期：2024-12-31

基金

咸阳市重大科技创新专项(L2023-ZDKJ-JSGG-GY-018)

作者信息

西北机电工程研究所智能装备与技术研究室, 咸阳 712099

通讯作者:

* 王鼎衡(1988—),男,汉族,陕西西安人,博士,助理研究员。研究方向:武器装备智能化。E-mail:wangdai11@163.com。

参考文献

分享链接

https://castjournals.cast.org.cn/joweb/kxjsygc/CN/10.12404/j.issn.1671-1815.2402872

分享至

全文二维码

扫描看全文

引用本文

BibTeX

本文的引用情况

2种不同金属材料的力学参数

科 Family	属数 Number of genus	种数 Number of species	占总种数比例 Percentage of total species (%)	属 Genus	种数 Number of species	占总种数比例 Percentage of total species (%)
鹅膏菌科Amanitaceae	2	11	5.26	鹅膏菌属 Amanita	10	4.78
小菇科 Mycenaceae	2	12	5.74	丝盖伞属 Inocybe	5	2.39
多孔菌科 Polyporaceae	8	14	6.70	蜡蘑属 Laccaria	5	2.39
红菇科 Russulaceae	3	23	11.00	小皮伞属 Marasmius	6	2.87
				小菇属 Mycena	11	5.26
				光柄菇属 Pluteus	5	2.39
				红菇属 Russula	17	8.13
				栓菌属 Trametes	5	2.39

关闭全屏

BibTeX
EndNote
RefWorks
TxT

判据	$V -$ /(m·s^-1)	ΔV/(m·s^-1)	T/s	S/m
车辆	3.0	2.5	1.5	10.0
行人	1.0	1.0	1.5	2.0

判据

V -

/(m·s^-1)

ΔV/(m·s^-1)

T/s

S/m

车辆

3.0

2.5

1.5

10.0

行人

1.0

1.5

2.0

多传感器融合动目标检测算法
INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)
OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)
过程: 1.接收新一帧传感器数据T_L、T_I、T_R; 2.计算与输出缓存和候选缓存中的现有目标之间的距离S_L、S_I、S_R; 3.If S_L<Thre_L $\\|$S_I<Thre_I $\\|$S_R<Thre_R then 4.更新容器中传感器目标T'_sensor(X,Y,V_x,V_y,Class); 5.else 6.创建新的候选目标T_temp并放入候选缓存; 7.end if 8.传感器目标T'_sensor融合计算位置(X,Y)、速度(V_x,V_y)、目标类别Class并赋予目标ID,得到实际目标T; 9.计算前后两帧目标之间的距离差ΔS和时间差Δt进行帧间匹配。

多传感器融合动目标检测算法

INPUT:激光雷达、相机、毫米波雷达检测结果T_sensor(X,Y,V_x,V_y,Class)

OUTPUT:融合最终目标T(X,Y,V_x,V_y,Class,ID)

类型	平均精度(AP50)
车辆	0.803	0.857	0.655	0.956
行人	0.787	0.868	0.669	0.934

类型

平均精度(AP50)

激光雷达

相机

毫米波雷达

融合算法

车辆

0.803

0.857

0.655

0.956

行人

0.787

0.868

0.669

0.934

参数名称	数值
整车质量m/kg	8 573
质心到前轴距离l_f/m	4.5
质心到后轴距离l_r/m	5.5
前轮侧偏刚度C_f/[(N·m)·rad^-1]	40 000
后轮侧偏刚度C_r/[(N·m)·rad^-1]	40 000
横摆转动惯量I_z/(kg·m^-2)	8 873
前轮转角δ/rad	-0.733~0.733

参数名称

数值

整车质量m/kg

8 573

质心到前轴距离l_f/m

4.5

质心到后轴距离l_r/m

5.5

前轮侧偏刚度C_f/[(N·m)·rad^-1]

40 000

后轮侧偏刚度C_r/[(N·m)·rad^-1]

40 000

横摆转动惯量I_z/(kg·m^-2)

8 873

前轮转角δ/rad

-0.733~0.733