一种基于雷视匹配的无人艇航迹修正方法

本发明属于对海观测，尤其涉及对海目标跟踪过程中的多源信息融合技术。

背景技术：

1、无人艇凭借着更高的安全性、更低的人力成本、更广泛的应用场景，已经成为了对海观测与感知的重要媒介之一。出于对无人艇安全航行的考虑，在复杂环境下对周围海面目标的检测与跟踪尤为重要。毫米波雷达凭借较短的波长、对雨雾较强的穿透能力、较远的探测距离和较高的分辨率，已经成为无人艇对海感知的有效传感器之一。但是，艇载雷达难免会因为海浪起伏而丢失目标回波，进而影响目标检测与跟踪效果；点云信息也不利于目标的识别和特征的描绘。单目视觉相机作为传统传感器，通过图像可以获取目标更丰富的特征并具有更宽的探测范围，完美地弥补了毫米波雷达的不足。雷达视觉融合的感知手段可以将多源信息进行整合，避免了单传感器随机性误检、漏检的问题，提高了感知系统的鲁棒性，增强了感知效果。

2、杨宇君等在海上船只目标跟踪的研究中，建立了多目标的运动方程，使用卡尔曼滤波对目标运动轨迹进行滤波更新，完成对海面多目标的跟踪。赵玉蓉等改进了yolov4的网络模型，并使用k-means++聚类算法对数据集进行预处理，实现对海面多目标的实时检测。

3、目前对海观测相关研究大多只关注对多目标检测效果的提升，忽略了当目标航迹重叠、目标遮挡时出现的目标航迹丢失的情况；目前的对海观测相关研究大多是基于单传感器对目标进行检测、跟踪，忽略了多源信息融合对检测效果的提升。

4、当雷达在无人艇上对周围目标进行跟踪时，由于海浪起伏导致的角度变化，雷达可能会随机性的丢失目标的回波，从而在实时跟踪过程中丢失数据，造成目标航迹断裂。当多目标航迹发生交叉时，由于目标被遮挡，雷达会误将两个目标认定为一个，从而丢失单目标一段时间内的回波，对海感知系统易出现目标航迹标注错误、航迹断裂等问题，从而影响目标检测和跟踪效果。

技术实现思路

1、本发明是为了解决现有对海目标跟踪的过程中，当多目标航迹发生交叉时易出现目标数据丢失、航迹断裂的问题，现提供一种基于雷视匹配的无人艇航迹修正方法。

2、一种基于雷视匹配的无人艇航迹修正方法，包括：

3、通过艇载雷达信息获取各目标的雷测状态向量；

4、利用yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息；

5、将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值；

6、对各目标的状态量测值和预测状态向量进行匹配，实现对无人艇航迹的修正。

7、进一步的，上述通过艇载雷达信息获取各目标的雷测状态向量，包括：

8、对艇载雷达工作过程中产生的个啁啾信号分别进行个时间点采样，获得二维复中频信号矩阵；

9、分别对所述二维复中频信号矩阵的各行进行快速傅里叶变换，获得各行频率谱，利用各行频率谱谱峰处的频率值分别计算各目标至艇载雷达的雷测距离：

10、；

11、其中，为线性调频时间，为艇载雷达的扫频带宽，为光速；

12、分别对所述二维复中频信号矩阵的各列进行快速傅里叶变换，获得各列频率谱，利用各行频率谱与各列频率谱构成的二维结果图中每个交点处的相位差分别计算各目标的雷测速度和相对艇载雷达的雷测角度：

13、；

14、；

15、其中，为二维结果图中第个交点处行频率谱与列频率谱的相位差，为载波信号频率，为啁啾信号的索引值，为一个啁啾信号的周期，为艇载雷达波长，为艇载雷达相邻两个接收天线之间的间距；

16、利用所述各目标的雷测距离和相对艇载雷达的雷测角度分别计算各目标的雷测三维坐标，，，；

17、各目标的雷测速度和雷测三维坐标构成各目标的雷测状态向量。

18、进一步的，上述利用yolov5网络对艇载相机采集的每帧视觉图像进行检测，获取各目标的像素信息，包括：

19、利用艇载相机采集含有各目标的视频，并将视频中每一帧视觉图像输入至yolov5网络进行检测，获得每一帧视觉图像中目标所在检测框的像素信息，所述像素信息包括检测框的宽度、高度和中心坐标。

20、进一步的，上述将同一目标的像素信息与雷测状态向量进行融合，获得目标的状态量测值，包括：

21、将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标；

22、利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，利用各目标在视觉图像中的视觉坐标以及视觉速度构成各目标的视觉状态向量；

23、将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，进而将同一目标的雷测状态向量和视觉状态向量进行融合，获得目标的状态量测值：

24、，

25、其中，，，，，

26、和分别为目标相对艇载雷达的切向距离和径向距离，和分别为目标相对艇载雷达的切向速度和径向速度，为目标相对艇载雷达的雷测角度，为目标至艇载雷达的雷测距离，，为目标的雷测速度，为目标的视觉速度。

27、进一步的，上述将所述各目标的像素信息变换至世界坐标系下，获得各目标在视觉图像中的视觉坐标，包括：

28、将目标在世界坐标系下的雷测三维坐标变换至相机坐标系下，获得目标在相机坐标系下的坐标；将所述目标在相机坐标系下的坐标变换至图像坐标系下，获得目标在图像坐标系下的坐标；将所述目标在图像坐标系下的坐标变换至像素坐标系下，获得目标在像素坐标系下的像素坐标；构建出世界坐标系与像素坐标系之间的映射关系，并根据该映射关系将所述各目标的像素信息变换至世界坐标系下，获得目标在世界坐标系下的雷测三维坐标，作为各目标在视觉图像中的视觉坐标；

29、所述世界坐标系与像素坐标系之间的映射关系表达式如下：

30、；

31、其中，为目标在视觉图像中的像素坐标，为目标在世界坐标系下的雷测三维坐标，为图像坐标系的原点在像素坐标系中的坐标，和分别为世界坐标系到相机坐标系的旋转矩阵和平移矩阵，中间变量、，为艇载相机拍摄视频的帧率，是像素坐标系中横轴的1个像素值在图像坐标系中横轴对应的值，表示像素坐标系中纵轴的1个像素值在图像坐标系中纵轴对应的值。

32、进一步的，上述利用同一目标在连续两帧视觉图像中的视觉坐标计算目标的视觉速度，包括：

33、根据下式计算第帧视觉图像中目标的视觉速度：

34、；

35、和分别为第帧和第帧视觉图像中目标的视觉坐标，中间变量，为艇载相机拍摄视频的帧率。

36、进一步的，上述将所述各目标的视觉状态向量与所述各目标的雷测状态向量进行匹配，包括：

37、计算艇载雷达测得的目标与艇载相机测得的目标之间的匹配度，若该匹配度大于预设匹配度阈值，则目标与目标为同一目标，否则目标与目标不为同一目标；

38、所述匹配度的表达式为：

39、；

40、其中，中间变量，和为目标的视觉横向距离和纵向距离，和为目标的雷测横向距离和纵向距离，为目标的视觉速度，为目标的雷测速度，和分别为距离系数和速度系数。

41、进一步的，上述预测状态向量包括确认态预测状态向量和暂定态预测状态向量；

42、所述对各目标的状态量测值和预测状态向量进行匹配，包括：

43、s1：将时刻各目标的状态量测值分别与时刻各目标的确认态预测状态向量进行级联匹配，相互匹配的状态量测值和确认态预测状态向量执行s2，未实现匹配的状态量测值执行s3，未实现匹配的确认态预测状态向量执行s7；

44、s2：对匹配到状态量测值的时刻确认态预测状态向量进行卡尔曼滤波更新，获得时刻确认态输出状态向量，将该时刻确认态输出状态向量添加至轨迹链中，完成航迹修正；

45、s3：将未实现匹配的状态量测值分别与时刻各目标的暂定态预测状态向量进行iou匹配，相互匹配的状态量测值和暂定态预测状态向量执行s4，未实现匹配的状态量测值执行s6，将未实现匹配的暂定态预测状态向量删除；

46、s4：判断与匹配成功的时刻暂定态预测状态向量相邻的时刻及时刻暂定态预测状态向量是否也匹配到状态量测值，是则将时刻暂定态预测状态向量转换为时刻确定态预测状态向量，然后执行s2，否则执行s5；

47、s5：对匹配到状态量测值的时刻暂定态预测状态向量进行卡尔曼滤波更新，获得时刻暂定态输出状态向量，将该时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；

48、s6：将未实现匹配的状态量测值对应目标作为新目标，将该新目标的时刻状态量测值作为新目标的时刻暂定态预测状态向量并进行卡尔曼滤波更新，获得该新目标的时刻暂定态输出状态向量，将该新目标的时刻暂定态输出状态向量添加至轨迹链中，完成航迹修正；

49、s7：判断未实现匹配的确认态预测状态向量匹配失败的次数是否超出容错阈值，是则将该确认态预测状态向量删除，否则将该确认态预测状态向量保留，完成航迹修正。

50、进一步的，上述卡尔曼滤波更新，包括：

51、利用时刻的预测状态向量计算时刻的残差及残差和协方差：

52、；

53、利用计算时刻的滤波增益：

54、；

55、利用计算时刻的卡尔曼滤波输出值和卡尔曼滤波输出协方差：

56、；

57、所述时刻的预测状态向量包括时刻的状态预测值和协方差预测值，所述时刻的卡尔曼滤波输出值和卡尔曼滤波输出协方差构成时刻的输出状态向量；

58、为状态转移矩阵，为时刻的卡尔曼滤波输出值，为时刻的卡尔曼滤波输出协方差，为时刻的状态量测值，为量测增益矩阵，为时刻量测噪声的协方差矩阵，为单位矩阵。

59、进一步的，利用时刻目标的输出状态向量计算时刻目标的状态预测值和协方差预测值：

60、。

61、本发明所述的一种基于雷视匹配的无人艇航迹修正方法，使用fmcw雷达进行目标检测与跟踪，使用光学摄像头进行视觉辅助，使用yolov5方法获取多目标的视觉信息，通过坐标系转换获取目标在真实世界的运动信息，使用多源信息融合的方法实现传感器目标匹配。使用卡尔曼滤波方法，实现多目标运动轨迹的预测和更新。对所有状态量测值与卡尔曼滤波后的预测状态向量使用匈牙利算法进行数据匹配，使用两级匹配机制，对于遮挡而丢失数据状态量测值的目标，设置一定的容错阈值。本发明能够实现雷达、视觉信息的深度融合，提高目标跟踪效果，并且有效解决了跟踪过程中数据丢失、航迹交叉的问题。