基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法

技术特征：
1.基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，具体步骤如下，其特征在于：(1)读取毫米波雷达当前时刻的点云信息，其中第i个点云信息记为p
i
＝[x,y,z,v
x
,v
y
,v
z
]，式中x,y,z表示目标在空间各坐标轴方向的位置分量，v
x
,v
y
,v
z
表示目标在空间各坐标轴方向的速度分量，在获取雷达点云信息后，先使用欧式聚类算法滤除离群噪点，再使用每类点云的质心代表该类点云所对应的物体，质心计算的表达式为：(2)读取单目视觉采集的rgb图像，使用训练好的yolov5n目标检测模型实现感兴趣目标的在线检测与提取，通过目标检测算法得到感兴趣目标的检测框信息为：bbox＝[u,v,w
b
,h
b
]式中，u,v为检测框中心点的像素坐标，w
b
,h
b
为检测框的宽和高；结合感兴趣目标的宽度或高度尺寸信息，利用图像检测框信息和相机投影模型，提出单目深度估计的计算表达式如下：式中，d为目标估计深度，f为相机焦距，w,h为目标实际尺寸，u,v为目标在像素坐标系中的尺寸；(3)对毫米波雷达和单目视觉进行联合标定，实现异源传感器数据的时空对准；(4)对完成时空同步的毫米波雷达和视觉检测数据进行一一匹配，筛选出感兴趣目标的多源传感器信息，采用全局最近邻匹配gnn算法实现数据匹配，两传感器数据之间的距离计算准则选取马氏距离，计算表达式为：式中，e
ij
表示两传感器测量的残差向量，s为对应的协方差。为了衡量数据匹配的风险代价，设置一门限值g来表示两传感器数据之间的最大匹配距离，门限值约束下的数据匹配的风险代价计算公式为：其中，c
ij
为某一数据对匹配的风险代价，d
max
为一远大于d
ij
和g的常数，当把雷达的各个量测元素分配给视觉的各个量测元素的总风险最小时，得到的分配结果为所需的全局最近邻数据匹配结果，实现该过程的优化目标为：邻数据匹配结果，实现该过程的优化目标为：(5)针对匹配成功的雷达数据和视觉数据，将两传感器数据进行融合，选取视觉检测框中心点，使用与之匹配的雷达深度信息对其在载体坐标系中的投影进行修正，计算公式为：
使用雷达测量值的速度信息，对修正视觉测量值投影进行补充，得到每个目标中心点的传感器融合测量值为z
k
＝[x
b
,y
b
,v
xb
,v
yb
]，对于融合测量值，设计了一个卡尔曼滤波器来提高检测鲁棒性，滤波器的预测部分为：式中，状态转移矩阵f
k,k-1
取值为表示匀速运动模型；系统协方差矩阵的初始值p0取单位阵，系统过程噪声矩阵q
k
表示匀速运动模型的扰动量。滤波器的数据更新部分为：式中，z
k
是当前测量值，h
k
是观测矩阵，在此处取值为单位阵，r
k
是观测噪声协方差矩阵。(6)设计一个基于全局最近邻匹配gnn算法和前述卡尔曼滤波器的串级在线多目标跟踪器，输出准确的在线多目标跟踪结果。2.根据权利要求1所述的基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，其特征在于，步骤(2)中需使用tensorrt对yolov5n模型进行推理加速。3.根据权利要求1所述的基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，其特征在于，步骤(3)中所提出的具体传感器时空对准方法为：(3-1)在进行时间对准时，提出采用最近邻时间匹配算法，该算法的原理为：计算雷达数据时间戳和相邻视觉检测数据时间戳的差值，将差值最小且小于设定阈值的数据对进行匹配，认为其对应于同一时刻，实现时间对准；(3-2)完成时间对准后，提出将两传感器数据都转换到载体系，实现数据的空间对准。在所设计的系统中，雷达坐标系、相机坐标系和载体坐标系中点的坐标分别为(x
r
,y
r
,z
r
)、(x
c
,y
c
,z
c
)和(x
b
,y
b
,z
b
)，像素坐标系中点的坐标为(u,v)，根据相机投影模型，将像素坐标系中的点转换到相机坐标系的公式为：
式中，z
c
是通过单目深度估计算法求得的目标距离，f
x
、f
y
、c
x
和c
y
是相机内参，通过棋盘格标定方法获取，根据相机在无人机上的安装位置，得到相机坐标系和载体坐标系之间的转换关系为：毫米波雷达坐标系和相机坐标系之间的转换关系通过直接线性变换法(dlt)求解，结合相机投影模型，存在关系式：式中，(u,v,1)
t
为雷达点云在像素坐标系中的投影，其通过人为观察方法确定，将该式进一步展开为式中，定义p＝(x
r
,y
r
,z
r
,1)，a1＝(a1,a2,a3,a4)
t
，a2＝(a5,a6,a7,a8)
t
，a3＝(a9,a
10
,a
11
,a
12
)
t
，消去z
c
后得如下约束：由该式可得，每对点构建两个方程，当共有n对点时，构建如下矩阵方程：当n≥6时，采用svd分解求解该方程的最小二乘解，对a进行svd分解得：(u
1 σ
1 v1)＝svd(a)进一步考虑尺度因素，得矩阵方程实际解为：
其中，β为比例系数，求得旋转矩阵为为计算带有尺度并具有正交性质的最优旋转矩阵，对进行svd分解得：求得的最优旋转矩阵为：r
cr
＝
±
u
2v2t
求解得到比例系数为：β＝
±
1/(tr(σ2)/3)由于探测点应在雷达和相机的前方，所以有：由该式确定β和r
cr
的正负，进一步求得平移向量为在计算出旋转矩阵r
cr
和平移向量t
cr
后，得到雷达坐标系和相机坐标系之间的转换关系为：再由前述相机坐标系和载体坐标系之间的转换关系，进一步将雷达坐标系中的点转换到载体坐标系中。4.根据权利要求1所述的基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，其特征在于，步骤(4)中，求解全局最近邻数据匹配结果时，采用匈牙利算法。5.根据权利要求1所述的基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，其特征在于，步骤(6)中设计的基于全局最近邻匹配gnn算法和前述卡尔曼滤波器的串级在线多目标跟踪器的原理为：根据轨迹的可信度，将轨迹状态分为确认态和非确认态，跟踪算法的流程如下：(1)使用gnn算法，将融合测量数据和确认态轨迹预测值进行匹配，得到未匹配轨迹、未匹配测量和匹配轨迹三组数据，对于匹配的轨迹，根据与之关联的当前测量值，更新轨迹状态；对于未匹配的轨迹，如果其已连续设定阈值个周期均匹配失败，删除该条轨迹，否则暂时保留该轨迹，但不进行轨迹更新；对于未匹配的测量将作为下一级数据匹配的输入；(2)再次使用gnn算法，将上一级未匹配测量和非确认态轨迹预测值进行匹配，得到新的未匹配轨迹、未匹配测量和匹配轨迹，对于匹配的轨迹，使用关联的测量更新轨迹状态。此时，如果匹配轨迹在连续设定阈值个周期均匹配成功，其状态将转换为确认态；对于未匹配的轨迹，删除该轨迹；对于未匹配的测量，生成新的非确认态轨迹；(3)输出由步骤(1)、(2)生成的确认态轨迹，这些轨迹为在线多目标跟踪的结果。

技术总结
基于视觉/毫米波雷达信息融合的无人机多目标跟踪方法，1、读取毫米波雷达当前时刻的点云信息，计算每类点云的质心信息代表该类点云对应物体的信息；2、读取单目视觉RGB图像，使用深度学习目标检测算法生成目标检测框，通过单目深度估计方法计算目标的深度信息；3、对毫米波雷达和单目视觉进行传感器联合标定；4、对完成时空同步的毫米波雷达和视觉检测数据进行一一匹配；5、根据数据匹配结果，将毫米波雷达和视觉数据融合，并针对融合数据设计了一个卡尔曼滤波器；6、基于全局最近邻匹配算法和前述卡尔曼滤波器，设计了一个多目标跟踪器，得到在线的多目标跟踪结果。本申请能够提高无人机的环境感知能力，进而提高无人机的智能化程度。度。


技术研发人员：王立辉 唐兴邦 陈飞鹏 李勇 任元
受保护的技术使用者：东南大学
技术研发日：2022.11.17
技术公布日：2023/3/3