CN117890903A - 一种基于雷视匹配的无人艇航迹修正方法 - Google Patents
一种基于雷视匹配的无人艇航迹修正方法 Download PDFInfo
- Publication number
- CN117890903A CN117890903A CN202410296160.9A CN202410296160A CN117890903A CN 117890903 A CN117890903 A CN 117890903A CN 202410296160 A CN202410296160 A CN 202410296160A CN 117890903 A CN117890903 A CN 117890903A
- Authority
- CN
- China
- Prior art keywords
- target
- state
- radar
- coordinate system
- matching
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000012937 correction Methods 0.000 title claims abstract description 36
- 239000013598 vector Substances 0.000 claims abstract description 131
- 230000000007 visual effect Effects 0.000 claims abstract description 82
- 238000005259 measurement Methods 0.000 claims abstract description 72
- 238000001514 detection method Methods 0.000 claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 27
- 230000008569 process Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000012790 confirmation Methods 0.000 claims description 30
- 238000001228 spectrum Methods 0.000 claims description 21
- 230000014509 gene expression Effects 0.000 claims description 16
- 230000001131 transforming effect Effects 0.000 claims description 12
- 238000013507 mapping Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 5
- 238000013519 translation Methods 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 3
- 230000003595 spectral effect Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 abstract description 9
- 230000003287 optical effect Effects 0.000 abstract description 7
- 230000007246 mechanism Effects 0.000 abstract description 4
- 241001391944 Commicarpus scandens Species 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 9
- 230000000694 effects Effects 0.000 description 8
- 230000006870 function Effects 0.000 description 6
- 238000006243 chemical reaction Methods 0.000 description 4
- 238000012549 training Methods 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 241001522296 Erithacus rubecula Species 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
- 238000002592 echocardiography Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 239000011810 insulating material Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000007903 penetration ability Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Landscapes
- Radar Systems Or Details Thereof (AREA)
Abstract
一种基于雷视匹配的无人艇航迹修正方法，涉及对海观测技术领域。本发明是为了解决现有对海目标跟踪的过程中，当多目标航迹发生交叉时易出现目标数据丢失、航迹断裂的问题。本发明所述的一种基于雷视匹配的无人艇航迹修正方法，使用FMCW雷达进行目标检测与跟踪，使用光学摄像头进行视觉辅助，使用Yolov5方法获取多目标的视觉信息，通过坐标系转换获取目标在真实世界的运动信息，使用多源信息融合的方法实现传感器目标匹配。使用卡尔曼滤波方法，实现多目标运动轨迹的预测和更新。对所有状态量测值与卡尔曼滤波后的预测状态向量使用匈牙利算法进行数据匹配，使用两级匹配机制，实现轨迹修正。
Description
技术领域
本发明属于对海观测技术领域，尤其涉及对海目标跟踪过程中的多源信息融合技术。
背景技术
无人艇凭借着更高的安全性、更低的人力成本、更广泛的应用场景，已经成为了对海观测与感知的重要媒介之一。出于对无人艇安全航行的考虑，在复杂环境下对周围海面目标的检测与跟踪尤为重要。毫米波雷达凭借较短的波长、对雨雾较强的穿透能力、较远的探测距离和较高的分辨率，已经成为无人艇对海感知的有效传感器之一。但是，艇载雷达难免会因为海浪起伏而丢失目标回波，进而影响目标检测与跟踪效果；点云信息也不利于目标的识别和特征的描绘。单目视觉相机作为传统传感器，通过图像可以获取目标更丰富的特征并具有更宽的探测范围，完美地弥补了毫米波雷达的不足。雷达视觉融合的感知手段可以将多源信息进行整合，避免了单传感器随机性误检、漏检的问题，提高了感知系统的鲁棒性，增强了感知效果。
杨宇君等在海上船只目标跟踪的研究中，建立了多目标的运动方程，使用卡尔曼滤波对目标运动轨迹进行滤波更新，完成对海面多目标的跟踪。赵玉蓉等改进了YOLOv4的网络模型，并使用K-means++聚类算法对数据集进行预处理，实现对海面多目标的实时检测。
目前对海观测相关研究大多只关注对多目标检测效果的提升，忽略了当目标航迹重叠、目标遮挡时出现的目标航迹丢失的情况；目前的对海观测相关研究大多是基于单传感器对目标进行检测、跟踪，忽略了多源信息融合对检测效果的提升。
当雷达在无人艇上对周围目标进行跟踪时，由于海浪起伏导致的角度变化，雷达可能会随机性的丢失目标的回波，从而在实时跟踪过程中丢失数据，造成目标航迹断裂。当多目标航迹发生交叉时，由于目标被遮挡，雷达会误将两个目标认定为一个，从而丢失单目标一段时间内的回波，对海感知系统易出现目标航迹标注错误、航迹断裂等问题，从而影响目标检测和跟踪效果。
发明内容
本发明是为了解决现有对海目标跟踪的过程中，当多目标航迹发生交叉时易出现目标数据丢失、航迹断裂的问题，现提供一种基于雷视匹配的无人艇航迹修正方法。
一种基于雷视匹配的无人艇航迹修正方法，包括：
通过艇载雷达信息获取各目标的雷测状态向量；
利用Yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息；
将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值；
对各目标的状态量测值和预测状态向量进行匹配，实现对无人艇航迹的修正。
进一步的，上述通过艇载雷达信息获取各目标的雷测状态向量，包括：
对艇载雷达工作过程中产生的个啁啾信号分别进行/>个时间点采样，获得二维复中频信号矩阵；
分别对所述二维复中频信号矩阵的各行进行快速傅里叶变换，获得各行频率谱，利用各行频率谱谱峰处的频率值分别计算各目标至艇载雷达的雷测距离：
；
其中，为线性调频时间，/>为艇载雷达的扫频带宽，/>为光速；
分别对所述二维复中频信号矩阵的各列进行快速傅里叶变换，获得各列频率谱，利用各行频率谱与各列频率谱构成的二维结果图中每个交点处的相位差分别计算各目标的雷测速度和相对艇载雷达的雷测角度/>：
；
；
其中，为二维结果图中第/>个交点处行频率谱与列频率谱的相位差，/>为载波信号频率，/>为啁啾信号的索引值，/>为一个啁啾信号的周期，/>为艇载雷达波长，/>为艇载雷达相邻两个接收天线之间的间距；
利用所述各目标的雷测距离和相对艇载雷达的雷测角度/>分别计算各目标的雷测三维坐标/>，/>，/>，/>；
各目标的雷测速度和雷测三维坐标构成各目标的雷测状态向量。
进一步的，上述利用Yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息，包括：
利用艇载相机采集含有各目标的视频，并将视频中每一帧视觉图像输入至Yolov5网络进行检测，获得每一帧视觉图像中目标所在检测框的像素信息，所述像素信息包括检测框的宽度、高度和中心坐标。
进一步的，上述将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值，包括：
将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标；
利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，利用各目标在视觉图像中的视觉坐标以及视觉速度构成各目标的视觉状态向量；
将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，进而将同一目标的雷测状态向量和视觉状态向量进行融合，获得目标的状态量测值：
，
其中，，/>，/>，/>，
和/>分别为目标相对艇载雷达的切向距离和径向距离，/>和/>分别为目标相对艇载雷达的切向速度和径向速度，/>为目标相对艇载雷达的雷测角度，/>为目标至艇载雷达的雷测距离，/>，/>为目标的雷测速度，/>为目标的视觉速度。
进一步的，上述将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标，包括：
将目标在世界坐标系下的雷测三维坐标变换至相机坐标系下，获得目标在相机坐标系下的坐标；将所述目标在相机坐标系下的坐标变换至图像坐标系下，获得目标在图像坐标系下的坐标；将所述目标在图像坐标系下的坐标变换至像素坐标系下，获得目标在像素坐标系下的像素坐标；构建出世界坐标系与像素坐标系之间的映射关系，并根据该映射关系将所述各目标的像素信息变换至世界坐标系下，获得目标在世界坐标系下的雷测三维坐标，作为各目标在视觉图像中的视觉坐标；
所述世界坐标系与像素坐标系之间的映射关系表达式如下：
；
其中，为目标在视觉图像中的像素坐标，/>为目标在世界坐标系下的雷测三维坐标，/>为图像坐标系的原点在像素坐标系中的坐标，/>和/>分别为世界坐标系到相机坐标系的旋转矩阵和平移矩阵，中间变量/>、/>，/>为艇载相机拍摄视频的帧率，/>是像素坐标系中横轴的1个像素值在图像坐标系中横轴对应的值，/>表示像素坐标系中纵轴的1个像素值在图像坐标系中纵轴对应的值。
进一步的，上述利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，包括：
根据下式计算第帧视觉图像中目标的视觉速度/>：
；
和/>分别为第/>帧和第/>帧视觉图像中目标的视觉坐标，中间变量/>，/>为艇载相机拍摄视频的帧率。
进一步的，上述将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，包括：
计算艇载雷达测得的目标与艇载相机测得的目标/>之间的匹配度/>，若该匹配度/>大于预设匹配度阈值，则目标/>与目标/>为同一目标，否则目标/>与目标/>不为同一目标；
所述匹配度的表达式为：
；
其中，中间变量，/>和/>为目标/>的视觉横向距离和纵向距离，/>和/>为目标/>的雷测横向距离和纵向距离，/>为目标/>的视觉速度，/>为目标/>的雷测速度，/>和/>分别为距离系数和速度系数。
进一步的，上述预测状态向量包括确认态预测状态向量和暂定态预测状态向量；
所述对各目标的状态量测值和预测状态向量进行匹配，包括：
S1：将时刻各目标的状态量测值分别与/>时刻各目标的确认态预测状态向量进行级联匹配，相互匹配的状态量测值和确认态预测状态向量执行S2，未实现匹配的状态量测值执行S3，未实现匹配的确认态预测状态向量执行S7；
S2：对匹配到状态量测值的时刻确认态预测状态向量进行卡尔曼滤波更新，获得/>时刻确认态输出状态向量，将该/>时刻确认态输出状态向量添加至轨迹链中，完成航迹修正；
S3：将未实现匹配的状态量测值分别与时刻各目标的暂定态预测状态向量进行IoU匹配，相互匹配的状态量测值和暂定态预测状态向量执行S4，未实现匹配的状态量测值执行S6，将未实现匹配的暂定态预测状态向量删除；
S4：判断与匹配成功的时刻暂定态预测状态向量相邻的/>时刻及/>时刻暂定态预测状态向量是否也匹配到状态量测值，是则将/>时刻暂定态预测状态向量转换为时刻确定态预测状态向量，然后执行S2，否则执行S5；
S5：对匹配到状态量测值的时刻暂定态预测状态向量进行卡尔曼滤波更新，获得/>时刻暂定态输出状态向量，将该/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；
S6：将未实现匹配的状态量测值对应目标作为新目标，将该新目标的时刻状态量测值作为新目标的/>时刻暂定态预测状态向量并进行卡尔曼滤波更新，获得该新目标的时刻暂定态输出状态向量，将该新目标的/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；
S7：判断未实现匹配的确认态预测状态向量匹配失败的次数是否超出容错阈值，是则将该确认态预测状态向量删除，否则将该确认态预测状态向量保留，完成航迹修正。
进一步的，上述卡尔曼滤波更新，包括：
利用时刻的预测状态向量计算/>时刻的残差/>及残差和协方差：
；
利用计算/>时刻的滤波增益/>：
；
利用计算/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差/>：
；
所述时刻的预测状态向量包括/>时刻的状态预测值/>和协方差预测值/>，所述/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差构成/>时刻的输出状态向量；
为状态转移矩阵，/>为/>时刻的卡尔曼滤波输出值，/>为/>时刻的卡尔曼滤波输出协方差，/>为/>时刻的状态量测值，/>为量测增益矩阵，/>为/>时刻量测噪声的协方差矩阵，/>为单位矩阵。
进一步的，利用时刻目标的输出状态向量计算/>时刻目标的状态预测值和协方差预测值/>：
。
本发明所述的一种基于雷视匹配的无人艇航迹修正方法，使用FMCW雷达进行目标检测与跟踪，使用光学摄像头进行视觉辅助，使用Yolov5方法获取多目标的视觉信息，通过坐标系转换获取目标在真实世界的运动信息，使用多源信息融合的方法实现传感器目标匹配。使用卡尔曼滤波方法，实现多目标运动轨迹的预测和更新。对所有状态量测值与卡尔曼滤波后的预测状态向量使用匈牙利算法进行数据匹配，使用两级匹配机制，对于遮挡而丢失数据状态量测值的目标，设置一定的容错阈值。本发明能够实现雷达、视觉信息的深度融合，提高目标跟踪效果，并且有效解决了跟踪过程中数据丢失、航迹交叉的问题。
附图说明
图1为基于雷视匹配的无人艇航迹修正方法的流程图；
图2为距离速度参数联合估计示意图；
图3为Yolov5网络结构示意图；
图4为目标检测框与真实框示意图；
图5为世界坐标系与相机坐标系转换示意图；
图6为相机坐标系与图像坐标系转换示意图；
图7为图像坐标系与像素坐标系转换示意图；
图8为匈牙利算法示意图；
图9为两级匹配示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其它实施例，都属于本发明保护的范围。需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
参照图1至图9具体说明本实施方式，本实施方式所述的一种基于雷视匹配的无人艇航迹修正方法，包括以下步骤：
步骤一：通过艇载雷达信息获取各目标的雷测状态向量。
对艇载雷达工作过程中产生的个啁啾信号分别进行/>个时间点采样，获得二维复中频信号矩阵，如图2所示。在该二维复中频信号矩阵中按行对复中频信号进行快速傅里叶变换得到每行信号的频率谱，利用恒虚警率检测技术对每行信号的频率谱进行去噪，并利用去噪后谱峰处的频率值/>计算/>时刻目标至艇载雷达的雷测距离/>：
；
其中，为/>时刻线性调频时间，/>为/>时刻艇载雷达的扫频带宽，/>为光速。
在二维复中频信号矩阵中按列对复中频信号进行快速傅里叶变换得到每列信号的频率谱，每行信号的频率谱与每列信号的频率谱构成二维结果图，如图2所示。利用该二维结果图中每个交点处的相位差计算时刻各目标的雷测速度/>和各目标相对艇载雷达的雷测角度/>：
；
；
其中，为二维结果图中第/>个交点处行频率谱与列频率谱的相位差，/>为载波信号频率，/>为啁啾信号的索引值，/>为一个啁啾信号的周期，/>为雷达波长，/>为调频连续波雷达相邻两个接收天线之间的间距。
由此，利用和/>获取/>时刻目标在真实世界的雷测三维坐标：
。
本步骤中，时刻各目标的雷测速度/>以及在雷测三维坐标/>共同构成目标的雷测状态向量。
步骤二：通过Yolov5算法对艇载相机采集的每帧视觉图像进行检测，获取多目标的像素信息。
使用标注好的海面多目标运动图像构建数据集。采用数据增强技术，通过随机剪裁、随机排列、随机缩放等方式形成新的图像，从而对数据集的数量进行扩充，提高模型的鲁棒性，并提高网络模型的泛化能力。随后将数据集按4：3：3的比例划分为训练集、验证集、测试集，设定迭代次数为1000次，用于训练适用于检测、识别海上机动目标的Yolov5网络模型。
本实施方式使用卷积神经网络中相对成熟、效果较好的Yolov5网络模型对视觉图像中的多目标进行实时检测。如图3所示，Yolov5网络模型包括四个部分，分别为：输入端、主干网络（Backbone）、颈部网络（Neck）和输出端（Head），四个部分分别完成数据输入、特征提取、特征融合和结果输出。在输出端定义损失函数，从而衡量网络模型对多目标的检测输出值与实际值的差异程度。出于模型鲁棒性考虑，使用三类损失函数，通过加权的方式共同对模型的损失函数进行刻画，表达式如下：
；
为检测框总数，/>，/>、/>和/>分别表示/>、/>和/>的权重。
衡量了所有检测框中目标的分类结果与实际情况的差异程度，由二值交叉熵函数构成：
；
刻画了检测框内是否真实存在目标，由二值交叉熵函数构成：
；
刻画了检测框的定位损失，在传统的检测框损失函数的基础上添加了对长宽比的匹配程度，表达式如下：
；
表示第/>个检测框分类结果的真实与否，如果分类结果与真实情况符合，则为正样本，取值为1，反之则为负样本，取值为0。/>则表示模型第/>个检测框将样本认定为某类的概率，在0-1之间取值。/>表示第/>个检测框的真实程度，如果第/>个检测框内存在目标，则取值1，反之，则取值0。/>表示第/>个检测框的置信度，在0-1之间取值。如图4所示，/>和/>分别为真实框和检测框，/>为检测框与真实框并集区域的对角线长度，/>为检测框与真实框中心点之间的距离。检测框/>和真实框/>的交互比/>的表达式为：
。
中间变量和/>的表达式分别为：
，/>。
和/>分别为真实框和检测框的宽度，/>和/>分别为真实框和检测框的高度。
通过衡量检测框与真实框的长宽比、中心点的距离、重叠面积的差异程度，更加完备地反映了模型对多目标的检测能力，也决定了网络模型的迭代方向。
将艇载相机采集的实时视频数据逐帧输入到训练好的Yolov5网络模型中，实现对每帧视觉图像的检测，获得每一帧视觉图像中目标所在检测框的像素信息，所述像素信息包括检测框的宽度、高度和中心坐标。
步骤三：对各目标的像素信息进行坐标系转换，构建各目标的视觉状态向量。将雷测状态向量与视觉状态向量进行多源信息融合，获取多目标的状态量测值。
首先，得到多目标检测框的像素信息后，为了与毫米波雷达测得的真实位置相融合，需要将像素信息进行坐标系转换到世界坐标系下，具体包括：世界坐标系到相机坐标系、相机坐标系到图像坐标系、图像坐标系到像素坐标系。
1、世界坐标系到相机坐标系的转换。
如图5所示，世界坐标系是反映目标在真实世界中实际位置的坐标系，由表示。相机坐标系由/>表示，/>是相机光心，/>为相机光轴。两坐标系通过旋转平移完成转换，表达式如下：
；
其中，和/>分别为世界坐标系到相机坐标系的旋转矩阵和平移矩阵。
2、相机坐标系到图像坐标系的转换。
通过透视成像将3D数据转换为2D数据，如图6所示，其中表示图像坐标系，/>为相机坐标系。二者之间转换方程的表达式如下：
；
其中，为比例系数，/>为相机的光心到图像坐标系/>所在平面的距离。由此可得图像坐标系中待求点/>的坐标，表达式如下：
。
3、图像坐标系到像素坐标系的转换。
通过物体的实际位置坐标（mm）计算相应的像素值（pixel），如图7所示，表示图像坐标系原点/>在像素坐标系中的坐标。两坐标系之间的转换方程表达式如下：
；
该表达式的矩阵形式如下：
；
其中，是像素坐标系中横轴的1个像素值/>在图像坐标系中横轴对应的值，/>表示像素坐标系中纵轴的1个像素值/>在图像坐标系中纵轴对应的值。
综上能够得到世界坐标系与像素坐标系之间的映射关系，实现目标真实位置到图像像素信息的映射，表达式如下：
，/>。
至此，实现了将视觉图像的像素信息转换为真实世界的位置信息。
由于视频是由连续帧的图像构成，因此可以完成多目标速度的计算。假设某目标在第帧图像中的像素位置为/>，在第/>帧图像中的像素位置为，则根据坐标系转换，该目标在第/>帧图像中的视觉坐标为，在第/>帧图像中的视觉坐标为/>。由此可知，该目标在第/>帧图像中的视觉速度为/>，具体表达式如下：
；
；
其中，为相机拍摄视频的帧率。
根据雷达和相机测得目标的速度和距离相似度来度量二者是否是同一目标。视觉目标和雷达目标/>之间的匹配度/>的表达式如下：
；
其中，和/>为视觉目标/>的横向距离和纵向距离，/>和/>为雷达目标/>的横向距离和纵向距离，/>为视觉目标/>的视觉速度，/>为雷达目标/>的雷测速度，/>和分别为距离系数和速度系数。
在目标匹配的过程中，轮循计算视觉目标和雷达目标/>的匹配度，并与预设匹配度阈值/>相比较，如果满足/>，则认为视觉目标/>和雷达目标/>为同一物体。轮循迭代，直到完成所有目标的匹配。完成匹配后，得到/>时刻下目标的状态量测值/>：
，
，
为了减少误差，采用算术平均的方法来确定：
。
其中，和/>分别为目标相对艇载雷达的切向距离和径向距离，/>和/>分别为目标相对艇载雷达的切向速度和径向速度。
步骤四：如图9所示，使用两级匹配机制对所有状态量测值与预测状态向量进行两级匹配，进而实现无人艇的航迹修正。
S1：将时刻各目标的状态量测值分别与/>时刻各目标的确认态预测状态向量进行一级匹配，相互匹配的状态量测值和确认态预测状态向量执行S2。未实现匹配的状态量测值执行S3。未实现匹配的确认态预测状态向量，表示目标当前丢失，但是目标之前非常活跃，即为目标被遮挡，假如直接删除该轨迹，而之后目标又出现并形成新的航迹，那么它被遮挡之前的航迹就会丢失，使得轨迹失去连贯性。所以这种情况下不能直接删除，而是执行S7。/>
一级匹配为级联匹配，该匹配使用马氏距离与余弦距离/>分别衡量第/>个状态量测值与第/>个确认态预测状态向量的中心和形状的相似程度，并进行加权平均，得到总体相似度/>：
，
，
，
其中，、/>分别为第/>个状态量测值中心点的横纵坐标，/>、/>分别为第个确认态预测状态向量中心点的横纵坐标，/>和/>分别为第/>个状态量测值的宽度和高度，/>和/>分别为第/>个确认态预测状态向量的宽度和高度。
将所有总体相似度组成行/>列的代价矩阵。如图8所示，使用匈牙利算法对代价矩阵进行循环遍历，计算零值并实时更新代价矩阵的元素值，不断迭代得到最优的分配方式，实现/>个状态量测值与/>个确认态预测状态向量的最优配对。
S2：对匹配到状态量测值的时刻确认态预测状态向量进行卡尔曼滤波更新，获得/>时刻确认态输出状态向量，将该/>时刻确认态输出状态向量添加至轨迹链中，完成航迹修正。
进行卡尔曼滤波更新的过程如下：
假设时刻目标的状态方程和量测方程分别为：
；
其中，和/>分别为/>时刻目标的状态转移向量和量测向量，/>和/>分别为轨迹预测模型的状态转移矩阵和量测增益矩阵，/>为/>时刻轨迹预测模型的状态噪声， />为/>时刻轨迹预测模型的量测噪声。
利用时刻的预测状态向量计算/>时刻的残差/>及残差和协方差：
；
利用计算/>时刻的滤波增益/>：
；/>
利用计算/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差/>：
；
所述时刻的预测状态向量包括/>时刻的状态预测值/>和协方差预测值/>，所述/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差构成/>时刻的输出状态向量；
为状态转移矩阵，/>为/>时刻的卡尔曼滤波输出值，/>为/>时刻的卡尔曼滤波输出协方差，/>为/>时刻的状态量测值，/>为量测增益矩阵，/>为/>时刻量测噪声的协方差矩阵，/>为单位矩阵。
之后，利用时刻目标的输出状态向量计算/>时刻目标的状态预测值和协方差预测值/>：
。
S3：将未实现匹配的状态量测值分别与时刻各目标的暂定态预测状态向量进行二级匹配，相互匹配的状态量测值和暂定态预测状态向量执行S4。未实现匹配的状态量测值执行S6。对于二级匹配失败而剩余的暂定态预测状态向量，目标超出测量范围，即目标丢失，则将未实现匹配的暂定态预测状态向量删除。
二级匹配为IoU匹配。IoU匹配使用状态量测值与暂定态预测状态向量的定位损失作为判决依据，通过先验估计的方式设定一个阈值/>，/>大于该阈值则二级匹配成功，否则二级匹配失败。
S4：判断与匹配成功的时刻暂定态预测状态向量相邻的/>时刻及/>时刻暂定态预测状态向量是否也匹配到状态量测值，是则将/>时刻暂定态预测状态向量转换为时刻确定态预测状态向量，然后执行S2，否则执行S5。
S5：对匹配到状态量测值的时刻暂定态预测状态向量进行卡尔曼滤波更新，获得/>时刻暂定态输出状态向量，将该/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正。
S6：将未实现匹配的状态量测值对应目标作为新目标，将该新目标的时刻状态量测值作为新目标的/>时刻暂定态预测状态向量并进行卡尔曼滤波更新，获得该新目标的时刻暂定态输出状态向量，将该新目标的/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；
S7：目标被遮挡，则需要设计容错阈值，容忍它丢失几帧数据，如果它在容错阈值内的几帧后重新能够出现可以匹配的状态量测值，则对其继续跟踪，不删除它的轨迹，保证了目标轨迹的完整性，避免了轨迹断裂的情况。具体的，判断未实现匹配的确认态预测状态向量匹配失败的次数是否超出容错阈值，是则将该确认态预测状态向量删除，否则将该确认态预测状态向量保留，完成航迹修正。
本实施方式所述的一种基于雷视匹配的无人艇航迹修正方法，使用FMCW雷达进行目标检测与跟踪，使用光学摄像头进行视觉辅助，根据雷达回波信息通过快速傅里叶变换获取多目标的距离、速度、角度等信息，根据光学摄像头成像使用Yolov5方法获取多目标的视觉信息。对像素坐标系进行转换，实现目标像素信息到真实运动信息的映射。通过对雷达、视觉的多源信息匹配，提高目标检测效果。对所有状态量测值进行卡尔曼滤波，实现多目标运动轨迹的预测和更新，从而提高多目标跟踪效果。针对目标遮挡导致的数据丢失问题，对每一帧由多源信息融合得到的所有状态量测值与卡尔曼滤波后的预测状态向量进行数据匹配，使用两级匹配机制，对于遮挡而丢失数据状态量测值的目标，设置一定的容错阈值，当目标再次出现时重新配对。本实施方式能够实现雷达、视觉信息的深度融合，提高目标跟踪效果，并且有效解决了跟踪过程中数据丢失、航迹交叉的问题。
Claims (10)
1.一种基于雷视匹配的无人艇航迹修正方法，其特征在于，包括：
通过艇载雷达信息获取各目标的雷测状态向量；
利用Yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息；
将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值；
对各目标的状态量测值和预测状态向量进行匹配，实现对无人艇航迹的修正。
2.根据权利要求1所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述通过艇载雷达信息获取各目标的雷测状态向量，包括：
对艇载雷达工作过程中产生的个啁啾信号分别进行/>个时间点采样，获得二维复中频信号矩阵；
分别对所述二维复中频信号矩阵的各行进行快速傅里叶变换，获得各行频率谱，利用各行频率谱谱峰处的频率值分别计算各目标至艇载雷达的雷测距离：
；
其中，为线性调频时间，/>为艇载雷达的扫频带宽，/>为光速；
分别对所述二维复中频信号矩阵的各列进行快速傅里叶变换，获得各列频率谱，利用各行频率谱与各列频率谱构成的二维结果图中每个交点处的相位差分别计算各目标的雷测速度和相对艇载雷达的雷测角度/>：
；
；
其中，为二维结果图中第/>个交点处行频率谱与列频率谱的相位差，/>为载波信号频率，/>为啁啾信号的索引值，/>为一个啁啾信号的周期，/>为艇载雷达波长，/>为艇载雷达相邻两个接收天线之间的间距；
利用所述各目标的雷测距离和相对艇载雷达的雷测角度/>分别计算各目标的雷测三维坐标/>，/>，/>，/>；
各目标的雷测速度和雷测三维坐标构成各目标的雷测状态向量。
3.根据权利要求1所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述利用Yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息，包括：
利用艇载相机采集含有各目标的视频，并将视频中每一帧视觉图像输入至Yolov5网络进行检测，获得每一帧视觉图像中目标所在检测框的像素信息，所述像素信息包括检测框的宽度、高度和中心坐标。
4.根据权利要求1所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值，包括：
将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标；
利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，利用各目标在视觉图像中的视觉坐标以及视觉速度构成各目标的视觉状态向量；
将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，进而将同一目标的雷测状态向量和视觉状态向量进行融合，获得目标的状态量测值：
；
其中，，/>，/>，/>；
和/>分别为目标相对艇载雷达的切向距离和径向距离，/>和/>分别为目标相对艇载雷达的切向速度和径向速度，/>为目标相对艇载雷达的雷测角度，/>为目标至艇载雷达的雷测距离，/>，/>为目标的雷测速度，/>为目标的视觉速度。
5.根据权利要求4所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标，包括：
将目标在世界坐标系下的雷测三维坐标变换至相机坐标系下，获得目标在相机坐标系下的坐标；将所述目标在相机坐标系下的坐标变换至图像坐标系下，获得目标在图像坐标系下的坐标；将所述目标在图像坐标系下的坐标变换至像素坐标系下，获得目标在像素坐标系下的像素坐标；构建出世界坐标系与像素坐标系之间的映射关系，并根据该映射关系将所述各目标的像素信息变换至世界坐标系下，获得目标在世界坐标系下的雷测三维坐标，作为各目标在视觉图像中的视觉坐标；
所述世界坐标系与像素坐标系之间的映射关系表达式如下：
；
其中，为目标在视觉图像中的像素坐标，/>为目标在世界坐标系下的雷测三维坐标，/>为图像坐标系的原点在像素坐标系中的坐标，/>和/>分别为世界坐标系到相机坐标系的旋转矩阵和平移矩阵，中间变量/>、/>，/>为艇载相机拍摄视频的帧率，/>是像素坐标系中横轴的1个像素值在图像坐标系中横轴对应的值，/>表示像素坐标系中纵轴的1个像素值在图像坐标系中纵轴对应的值。
6.根据权利要求4所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，包括：
根据下式计算第帧视觉图像中目标的视觉速度/>：
；
和/>分别为第/>帧和第/>帧视觉图像中目标的视觉坐标，中间变量/>，/>为艇载相机拍摄视频的帧率。
7.根据权利要求4、5或6所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，包括：
计算艇载雷达测得的目标与艇载相机测得的目标/>之间的匹配度/>，若该匹配度大于预设匹配度阈值，则目标/>与目标/>为同一目标，否则目标/>与目标/>不为同一目标；
所述匹配度的表达式为：
；
其中，中间变量，/>和/>为目标/>的视觉横向距离和纵向距离，/>和/>为目标/>的雷测横向距离和纵向距离，/>为目标/>的视觉速度，为目标/>的雷测速度，/>和/>分别为距离系数和速度系数。
8.根据权利要求1所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述预测状态向量包括确认态预测状态向量和暂定态预测状态向量；
所述对各目标的状态量测值和预测状态向量进行匹配，包括：
S1：将时刻各目标的状态量测值分别与/>时刻各目标的确认态预测状态向量进行级联匹配，相互匹配的状态量测值和确认态预测状态向量执行S2，未实现匹配的状态量测值执行S3，未实现匹配的确认态预测状态向量执行S7；
S2：对匹配到状态量测值的时刻确认态预测状态向量进行卡尔曼滤波更新，获得时刻确认态输出状态向量，将该/>时刻确认态输出状态向量添加至轨迹链中，完成航迹修正；
S3：将未实现匹配的状态量测值分别与时刻各目标的暂定态预测状态向量进行IoU匹配，相互匹配的状态量测值和暂定态预测状态向量执行S4，未实现匹配的状态量测值执行S6，将未实现匹配的暂定态预测状态向量删除；
S4：判断与匹配成功的时刻暂定态预测状态向量相邻的/>时刻及/>时刻暂定态预测状态向量是否也匹配到状态量测值，是则将/>时刻暂定态预测状态向量转换为/>时刻确定态预测状态向量，然后执行S2，否则执行S5；
S5：对匹配到状态量测值的时刻暂定态预测状态向量进行卡尔曼滤波更新，获得时刻暂定态输出状态向量，将该/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；
S6：将未实现匹配的状态量测值对应目标作为新目标，将该新目标的时刻状态量测值作为新目标的/>时刻暂定态预测状态向量并进行卡尔曼滤波更新，获得该新目标的/>时刻暂定态输出状态向量，将该新目标的/>时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；
S7：判断未实现匹配的确认态预测状态向量匹配失败的次数是否超出容错阈值，是则将该确认态预测状态向量删除，否则将该确认态预测状态向量保留，完成航迹修正。
9.根据权利要求8所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，所述卡尔曼滤波更新，包括：
利用时刻的预测状态向量计算/>时刻的残差/>及残差和协方差/>：
；
利用计算/>时刻的滤波增益/>：
；
利用计算/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差：
；
所述时刻的预测状态向量包括/>时刻的状态预测值/>和协方差预测值，所述/>时刻的卡尔曼滤波输出值/>和卡尔曼滤波输出协方差构成/>时刻的输出状态向量；
为状态转移矩阵，/>为/>时刻的卡尔曼滤波输出值，/>为/>时刻的卡尔曼滤波输出协方差，/>为/>时刻的状态量测值，/>为量测增益矩阵，/>为/>时刻量测噪声的协方差矩阵，/>为单位矩阵。
10.根据权利要求9所述的一种基于雷视匹配的无人艇航迹修正方法，其特征在于，
利用时刻目标的输出状态向量计算/>时刻目标的状态预测值/>和协方差预测值/>：
。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410296160.9A CN117890903B (zh) | 2024-03-15 | 2024-03-15 | 一种基于雷视匹配的无人艇航迹修正方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410296160.9A CN117890903B (zh) | 2024-03-15 | 2024-03-15 | 一种基于雷视匹配的无人艇航迹修正方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117890903A true CN117890903A (zh) | 2024-04-16 |
| CN117890903B CN117890903B (zh) | 2024-06-07 |
Family
ID=90644445
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410296160.9A Active CN117890903B (zh) | 2024-03-15 | 2024-03-15 | 一种基于雷视匹配的无人艇航迹修正方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117890903B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104749576A (zh) * | 2015-04-08 | 2015-07-01 | 四川九洲空管科技有限责任公司 | 一种多雷达航迹关联融合方法 |
| US20190120955A1 (en) * | 2017-10-20 | 2019-04-25 | Texas Instruments Incorporated | System and method for camera radar fusion |
| CN114859339A (zh) * | 2022-04-29 | 2022-08-05 | 电子科技大学 | 一种基于毫米波雷达的多目标跟踪方法 |
| CN115372958A (zh) * | 2022-08-17 | 2022-11-22 | 苏州广目汽车科技有限公司 | 一种基于毫米波雷达和单目视觉融合的目标检测和跟踪方法 |
| CN115657008A (zh) * | 2022-12-28 | 2023-01-31 | 北京无线电测量研究所 | 一种机载太赫兹雷达多目标跟踪方法和装置 |
| WO2023066156A1 (zh) * | 2021-10-18 | 2023-04-27 | 长沙中车智驭新能源科技有限公司 | 一种视觉雷达感知融合方法及终端设备 |
| CN116381672A (zh) * | 2023-03-06 | 2023-07-04 | 江南大学 | 基于孪生网络雷达x波段多扩展目标自适应跟踪方法 |
| CN116778290A (zh) * | 2023-06-20 | 2023-09-19 | 上海无线电设备研究所 | 一种基于深度学习算法的雷达视觉数据关联方法 |
| CN117075112A (zh) * | 2023-08-25 | 2023-11-17 | 中国人民解放军国防科技大学 | 一种方位角航迹匹配的无人艇雷达光电融合方法 |
| CN117233745A (zh) * | 2023-11-15 | 2023-12-15 | 哈尔滨工业大学（威海） | 一种在非平稳平台的对海机动目标跟踪方法 |
| CN117630911A (zh) * | 2023-11-25 | 2024-03-01 | 中国人民解放军海军航空大学 | 一种基于Singer模型的无人艇雷达光电融合方法 |
-
2024
- 2024-03-15 CN CN202410296160.9A patent/CN117890903B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104749576A (zh) * | 2015-04-08 | 2015-07-01 | 四川九洲空管科技有限责任公司 | 一种多雷达航迹关联融合方法 |
| US20190120955A1 (en) * | 2017-10-20 | 2019-04-25 | Texas Instruments Incorporated | System and method for camera radar fusion |
| WO2023066156A1 (zh) * | 2021-10-18 | 2023-04-27 | 长沙中车智驭新能源科技有限公司 | 一种视觉雷达感知融合方法及终端设备 |
| CN114859339A (zh) * | 2022-04-29 | 2022-08-05 | 电子科技大学 | 一种基于毫米波雷达的多目标跟踪方法 |
| CN115372958A (zh) * | 2022-08-17 | 2022-11-22 | 苏州广目汽车科技有限公司 | 一种基于毫米波雷达和单目视觉融合的目标检测和跟踪方法 |
| CN115657008A (zh) * | 2022-12-28 | 2023-01-31 | 北京无线电测量研究所 | 一种机载太赫兹雷达多目标跟踪方法和装置 |
| CN116381672A (zh) * | 2023-03-06 | 2023-07-04 | 江南大学 | 基于孪生网络雷达x波段多扩展目标自适应跟踪方法 |
| CN116778290A (zh) * | 2023-06-20 | 2023-09-19 | 上海无线电设备研究所 | 一种基于深度学习算法的雷达视觉数据关联方法 |
| CN117075112A (zh) * | 2023-08-25 | 2023-11-17 | 中国人民解放军国防科技大学 | 一种方位角航迹匹配的无人艇雷达光电融合方法 |
| CN117233745A (zh) * | 2023-11-15 | 2023-12-15 | 哈尔滨工业大学（威海） | 一种在非平稳平台的对海机动目标跟踪方法 |
| CN117630911A (zh) * | 2023-11-25 | 2024-03-01 | 中国人民解放军海军航空大学 | 一种基于Singer模型的无人艇雷达光电融合方法 |
Non-Patent Citations (3)
| Title |
|---|
| 于文才;: "扇扫雷达多目标航迹跟踪算法研究", 电子世界, no. 08, 30 April 2016 (2016-04-30), pages 183 - 184 * |
| 任珈民;宫宁生;韩镇阳;: "基于YOLOv3与卡尔曼滤波的多目标跟踪算法", 计算机应用与软件, no. 05, 12 May 2020 (2020-05-12), pages 169 - 176 * |
| 甘兴旺等: "基于视觉的船舶环境感知数据融合算法研究", 中国造船, vol. 62, no. 2, 30 June 2021 (2021-06-30), pages 201 - 210 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117890903B (zh) | 2024-06-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Major et al. | Vehicle detection with automotive radar using deep learning on range-azimuth-doppler tensors | |
| CN109444911B (zh) | 一种单目相机和激光雷达信息融合的无人艇水面目标检测识别与定位方法 | |
| US20220197281A1 (en) | Intelligent decision-making method and system for unmanned surface vehicle | |
| CN110850403B (zh) | 一种多传感器决策级融合的智能船水面目标感知识别方法 | |
| CN111626290B (zh) | 一种复杂海面环境下的红外舰船目标检测识别方法 | |
| CN111239766B (zh) | 基于激光雷达的水面多目标快速识别跟踪方法 | |
| Muhovič et al. | Obstacle tracking for unmanned surface vessels using 3-D point cloud | |
| CN109085570A (zh) | 基于数据融合的车辆检测跟踪算法 | |
| CN109283538A (zh) | 一种基于视觉和激光传感器数据融合的海上目标大小检测方法 | |
| CN110738121A (zh) | 一种前方车辆检测方法及检测系统 | |
| CN110988818A (zh) | 基于条件生成式对抗网络的欺骗干扰模板生成方法 | |
| CN101840579A (zh) | 一种利用视频分割与粒子滤波实现多目标跟踪方法 | |
| Santos et al. | Underwater place recognition using forward‐looking sonar images: A topological approach | |
| Clunie et al. | Development of a perception system for an autonomous surface vehicle using monocular camera, lidar, and marine radar | |
| Cui et al. | 3D detection and tracking for on-road vehicles with a monovision camera and dual low-cost 4D mmWave radars | |
| CN115876198A (zh) | 基于数据融合的目标检测及预警方法、设备、系统和介质 | |
| CN115272416A (zh) | 基于多源传感器融合的车辆和行人检测跟踪方法及系统 | |
| Helgesen et al. | Sensor combinations in heterogeneous multi-sensor fusion for maritime target tracking | |
| CN117075112A (zh) | 一种方位角航迹匹配的无人艇雷达光电融合方法 | |
| CN117630907B (zh) | 一种红外成像与毫米波雷达融合的海面目标跟踪方法 | |
| Zhang et al. | Rvdet: Feature-level fusion of radar and camera for object detection | |
| Zou et al. | Sonar Image Target Detection for Underwater Communication System Based on Deep Neural Network. | |
| Zhao et al. | Nighttime pedestrian detection based on a fusion of visual information and Millimeter-Wave radar | |
| CN117890903B (zh) | 一种基于雷视匹配的无人艇航迹修正方法 | |
| Ma et al. | Vehicle tracking method in polar coordinate system based on radar and monocular camera |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |