基于多传感器融合的无人艇海面目标追踪方法及装置与流程

1.本发明涉及无人艇目标跟踪技术领域，特别是涉及一种基于多传感器融合的无人艇海面目标追踪方法及装置。


背景技术：

2.随着自动驾驶技术的快速发展，继无人机和无人车之后，无人艇也越来越受到关注，在很多领域有着不可替代的作用。其中，无人艇在民用上可用于海上救援、水下考古、海图测量、水污染检测等；无人艇在军用上可用于反潜、探雷、甚至是直接参与作战。
3.无人艇通过传感器设备获得周围环境信息，给决策算法提供数据，最后通过执行机构执行决策，从而实现对无人艇的行为控制。一般无人艇上近距离的感知信息是为了后续的紧急避障，而想要得到性能更好的决策需要更精确的感知信息，但是单传感器设备获得的信息都有各自的优缺点：如激光雷达，虽然测量精度较高但无法用于雨天、雾天，所以一般单靠激光雷达得到的感知信息用来紧急避障时，无法保障安全性；而毫米波雷达可以用于低视觉场景，但测量精度不高。


技术实现要素：

4.本发明的目的是提供一种基于多传感器融合的无人艇海面目标追踪方法及装置，扩展了无人艇感知系统的适应场景，提高了目标信息的精度，增强了无人艇在海上航行的安全性。
5.为实现上述目的，本发明提供了如下方案：
6.一种基于多传感器融合的无人艇海面目标追踪方法，包括：
7.获取激光雷达采集的原始点云数据；
8.获取毫米波雷达采集的原始毫米波数据；其中，所述激光雷达和所述毫米波雷达均位于无人艇上；
9.对所述原始点云数据和所述原始毫米波数据进行融合处理，得到融合后的目标信息；
10.基于所述融合后的目标信息，对海面上的目标进行跟踪。
11.可选的，所述对所述原始点云数据和所述原始毫米波数据进行融合处理，得到融合后的目标信息，具体包括：
12.对所述原始点云数据进行预处理，得到在船体坐标系下第一目标的位置信息；所述第一目标为所述激光雷达检测到的目标；
13.判断所述原始毫米波数据是否含有目标信息；
14.若是，则先对所述原始毫米波数据进行预处理，得到在所述船体坐标系下第二目标的位置信息，接着对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述第二目标为所述毫米波雷达检测到的目标；
15.若否，则将所述第一目标的位置信息确定为融合后的目标信息。
16.可选的，所述对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息，具体包括：
17.当目标距离小于或者等于设定阈值时，对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述目标距离为所述第一目标与所述第二目标之间的距离；
18.当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为雨雾天气时，将所述第二目标的位置信息确定为融合后的目标信息；
19.当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为正常天气时，将所述第一目标的位置信息确定为融合后的目标信息。
20.可选的，所述基于所述融合后的目标信息，对海面上的目标进行跟踪，具体包括：
21.基于卡尔曼滤波算法和所述融合后的目标信息，对海面上的目标进行跟踪。
22.一种基于多传感器融合的无人艇海面目标追踪装置，包括：安装在无人艇上的激光雷达、毫米波雷达和工控机；
23.所述激光雷达，用于采集原始点云数据；
24.所述毫米波雷达，用于采集原始毫米波数据；
25.所述工控机，包括：
26.激光雷达数据检测模块，用于获取激光雷达采集的原始点云数据；
27.毫米波雷达数据检测模块，用于获取毫米波雷达采集的原始毫米波数据；
28.数据融合模块，用于对所述原始点云数据和所述原始毫米波数据进行融合处理，得到融合后的目标信息；
29.跟踪模块，用于基于所述融合后的目标信息，对海面上的目标进行跟踪。
30.可选的，还包括传感器安装平台；
31.其中，在所述传感器安装平台的顶面上安装有所述激光雷达，在所述传感器安装平台的四个侧面上分别安装有所述毫米波雷达，所述传感器安装平台的底面通过支架安装在所述无人艇的尾部。
32.可选的，所述数据融合模块，具体包括：
33.预处理单元，用于对所述原始点云数据进行预处理，得到在船体坐标系下第一目标的位置信息；所述第一目标为所述激光雷达检测到的目标；
34.判断单元，用于判断所述原始毫米波数据是否含有目标信息；
35.第一融合后目标信息确定单元，用于当所述原始毫米波数据含有目标信息时，先对所述原始毫米波数据进行预处理，得到在所述船体坐标系下第二目标的位置信息，接着对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述第二目标为所述毫米波雷达检测到的目标；
36.第二融合后目标信息确定单元，用于当所述原始毫米波数据未含有目标信息时，将所述第一目标的位置信息确定为融合后的目标信息。
37.可选的，在所述对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息方面，所述第一融合后目标信息确定单元，进一步用于：
38.当目标距离小于或者等于设定阈值时，对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述目标距离为所述第一目标与所
述第二目标之间的距离；
39.当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为雨雾天气时，将所述第二目标的位置信息确定为融合后的目标信息；
40.当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为正常天气时，将所述第一目标的位置信息确定为融合后的目标信息；
41.当所述第一目标出现在毫米波范围之外，在激光雷达范围之内时，将所述第一目标的位置信息确定为融合后的目标信息。
42.可选的，所述跟踪模块，具体包括：
43.跟踪单元，用于基于卡尔曼滤波算法和所述融合后的目标信息，对海面上的目标进行跟踪。
44.根据本发明提供的具体实施例，本发明公开了以下技术效果：
45.由于激光雷达测量精度高但是受天气影响较大，毫米波雷达虽然精度低但是具有低成本和适应于恶劣天气的优点，故本发明基于激光雷达采集的原始点云数据和毫米波雷达采集的原始毫米波数据进行目标跟踪，即采用毫米波雷达和激光雷达结合的方式进行目标跟踪，扩展了无人艇感知系统的适应场景，提高了得到目标信息的精度，增强了无人艇在海上航行的安全性。
46.此外，毫米波雷达和激光雷达共同使用，使得重叠的探测区域更大，从而能够更广泛的融合二种传感器的目标数据。
附图说明
47.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
48.图1为本发明为一个激光雷达和四个毫米波雷达的安装位置示意图；(a)俯视图；(b)三维图；(c)侧视图；(d)正视图；
49.图2为本发明激光雷达和毫米波雷达探测区域示意图；其中，1表示一个毫米波雷达的探测区域，2表示四个毫米波雷达结合的探测区域，3表示激光雷达的探测区域；
50.图3为本发明多传感器融合跟踪算法流程图；
51.图4为本发明一种基于多传感器融合的无人艇海面目标追踪方法的流程图；
52.图5为本发明一种基于多传感器融合的无人艇海面目标追踪装置的结构框图。
具体实施方式
53.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
54.鉴于背景技术的介绍，目前阶段需要多传感器融合以适用于低视觉场景和获得更精确的感知信息。由于激光雷达可探测到周围360
°
的目标信息，而毫米波雷达探测的水平
视角只有
±
45
°
，受限于两种传感器水平视角探测范围不一致，目前还没有激光雷达和毫米波雷达融合用于海面目标感知。
55.基于此，本发明提供了一种基于激光雷达和毫米波雷达融合的无人艇海面目标追踪方法及装置，扩展了无人艇感知系统的适应场景，提高了目标信息的精度，增强了无人艇在海上航行的安全性。
56.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
57.实施例一
58.为了解决上述问题，本实施例提供了一种基于激光雷达和毫米波雷达融合的无人艇海面目标追踪方法，主要解决雨雾等恶劣环境对感知结果的影响，并搭建了传感器安装平台，利用四个毫米波雷达和一个激光雷达解决不同传感器水平视角不一致问题。将多个传感器融合后的检测信息作为输入，然后对检测到的目标进行跟踪进而得到更精确的目标信息，从而实现对无人艇周围目标进行稳定准确的检测和跟踪。
59.为实现上述功能，本实施例采用的技术方案如下：
60.步骤一：多传感器安装。
61.按照固定位置安装好一个激光雷达和四个毫米波雷达，并设置好网络通信，使岸基端主机能够接收到激光雷达和毫米波雷达的数据。
62.一个示例为：在无人艇尾部安装一个支架，在支架上按照一个传感器安装平台2，如图1所示，在传感器安装平台2的顶面设置一个激光雷达1，在传感器平台2的四个侧面分别安装一个毫米波雷达3，并且一个激光雷达1和四个毫米波雷达3均通过网线与无人艇上的工控机交换数据。
63.由于每个毫米波雷达的近距离水平视角为
±
45
°
，四个毫米波雷达水平视角叠加可和激光雷达一样得到周围360
°
水平视角的目标信息，从而扩大了两种传感器探测的重叠范围，如图2所示。
64.步骤二：对激光雷达采集的原始点云数据和毫米波雷达采集的原始毫米波数据分别进行预处理。
65.原始点云数据是球坐标系下的数据，为了方便后面坐标转换，首先需要将球坐标系下原始点云数据转换到笛卡尔坐标系下；其次对转换后的点云数据进行预处理，剔除由海浪等环境产生的杂波点、回波数据以及强度值较低的点；然后采用欧氏距离对过滤后的点云数据进行聚类、分割，初步得到包括周围环境的激光雷达目标信息。
66.由于四个毫米波雷达所得的数据的坐标系方向不同，首先需要将四个毫米波雷达得到的原始毫米波数据统一在一个毫米波雷达坐标系下，即根据毫米波雷达安装角度对原始毫米波数据进行坐标变换，得到同一毫米波雷达坐标系下且融合后水平视角为360
°
的毫米波雷达原始目标信息；然后对毫米波雷达原始目标信息进行预处理，剔除由海浪等环境产生的杂波点、回拨数据以及强度值较低的点，得到包括周围环境的毫米波雷达目标信息。
67.步骤三：根据不同情况选择相对应的融合策略，实现多传感器数据的融合。
68.在得到激光雷达目标信息和毫米波雷达目标信息，以及多传感器的时间同步之后，要对多传感器数据进行空间校正，为了简化坐标系转换的步骤，这里将毫米波雷达坐标系和激光雷达坐标系都统一转换至船体坐标系下，即对激光雷达目标信息进行坐标转换，
得到船体坐标系下第一目标的位置信息，对毫米波雷达目标信息进行坐标转换，得到船体坐标系下第二目标的位置信息；所述第一目标为激光雷达检测到的目标，所述第二目标为毫米波雷达检测到的目标。
69.设θ为传感器相对于船体的安装角度，x
boat
、y
boat
、z
boat
分别为目标在船体坐标系下的x、y、z轴坐标；x
sensor
、y
sensor
、z
sensor
分别表示目标在传感器坐标系下的x、y、z轴坐标；t
xyz
为平移向量，则坐标变换公式为：
[0070][0071]
由于激光雷达和毫米波雷达的探测距离不一致，一般情况下激光雷达要比毫米波雷达探测的距离更远。传感器的探测范围也不是恒定的，这也取决于周围环境问题的反射率，且在雨雾等恶劣天气下，激光雷达受影响较大。所以真正需要融合的是探测范围重叠的区域，本发明为了使重叠区域基本覆盖两种传感器的探测范围装置了四个毫米波雷达，从而得到重叠区域内更精确的目标信息。
[0072]
设d
f
为距离阈值或者为设定阈值，l
f
为激光雷达得到的目标数据，即第一目标的位置信息，r
f
为毫米波雷达得到的目标数据，即第二目标的位置信息，σ
l
为激光雷达的量测误差，σ
r
为毫米波雷达的量测误差，t
f
为两种传感器融合后的目标数据。
[0073]
情况一：当激光雷达和毫米波雷达探测到的目标之间距离小于距离阈值d
f
时，当作激光雷达和毫米波雷达探测到的目标为同一目标，则可将两种传感器得到的目标信息按照公式(1)进行融合。
[0074][0075]
情况二：当激光雷达和毫米波雷达探测到的同一方位上的目标之间距离大于距离阈值d
f
且当前天气为雨雾天气时，则判断激光雷达的探测受到了雨雾等恶劣天气的影响，此时则舍弃激光数据，只取毫米波雷达的目标数据。
[0076]
情况三：当激光雷达和毫米波雷达探测到的同一方位上的目标之间距离大于距离阈值d
f
且当前天气正常时，这时由于激光雷达的精度更高，判断为毫米波雷达的探测出现了杂波，此时舍弃毫米波数据，只取激光雷达的目标数据。
[0077]
情况四：当目标出现在毫米波雷达探测距离之外，则只用激光探测的目标数据。
[0078]
步骤四：用融合后的目标信息进行目标跟踪。
[0079]
各传感器得到的目标信息，经上述步骤融合后得到了统一坐标系下的量测数据，又通过运动模型得到各目标状态的预测值，然后利用卡尔曼滤波将目标状态的量测值与预测值融合得到更精确的估计值。
[0080]
设k时刻得到融合后的目标量测集为z
k
，k时刻的目标状态集为x
k
，详细步骤如下：
[0081]
步骤4.1：初始化滤波参数，设系统在k＝0时刻状态向量的初值是x0，一般取第一帧的量测值为其赋值，其协方差矩阵为p
0|0
，一般取经验值。且有如下状态模型和量测模型：
[0082]
x
k 1
＝ax
k
ω(2)；
[0083]
z
k 1
＝hx
k 1
θ(3)；
[0084]
其中，a为状态转移矩阵；h为状态观测矩阵；ω为过程噪声，其协方差矩阵为q；θ为量测噪声，其协方差矩阵为r。
[0085]
步骤4.2：由状态模型以及上一时刻的估计值可得当前时刻的状态预测值即为状态向量的先验；再由量测模型可得测量值的预测值即量测向量的先验；再加上当前时刻的量测值z
k 1
，可得新息
[0086]
步骤4.3：由上一时刻的状态向量协方差矩阵p
k|k
，可得当前时刻的状态向量协方差矩阵的先验从而得到k 1时刻的新息协方差s
k 1
，公式如下：
[0087][0088][0089]
步骤4.4：计算k 1时刻的卡尔曼增益k
k 1
，从而更新k 1时刻的状态和状态协方差，从而得到更精确的目标信息，公式如下：
[0090][0091][0092][0093]
其中，步骤二至步骤四的具体操作可参见图3。
[0094]
实施例二
[0095]
如图4所示，本实施例提供了一种基于多传感器融合的无人艇海面目标追踪方法，包括：
[0096]
步骤100：获取激光雷达采集的原始点云数据。
[0097]
步骤200：获取毫米波雷达采集的原始毫米波数据；其中，所述激光雷达和所述毫米波雷达均位于无人艇上。
[0098]
步骤300：对所述原始点云数据和所述原始毫米波数据进行融合处理，得到融合后的目标信息。
[0099]
步骤400：基于所述融合后的目标信息，对海面上的目标进行跟踪。
[0100]
其中，步骤300具体包括：
[0101]
对所述原始点云数据进行预处理，得到在船体坐标系下第一目标的位置信息；所述第一目标为所述激光雷达检测到的目标。
[0102]
判断所述原始毫米波数据是否含有目标信息；若是，则先对所述原始毫米波数据进行预处理，得到在所述船体坐标系下第二目标的位置信息，接着对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述第二目标为所述毫米波雷达检测到的目标；若否，则将所述第一目标的位置信息确定为融合后的目标信息。
[0103]
进一步地，所述对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息，具体包括：
[0104]
当目标距离小于或者等于设定阈值时，对所述第一目标的位置信息和所述第二目
标的位置信息进行融合处理，得到融合后的目标信息；所述目标距离为所述第一目标与所述第二目标之间的距离。
[0105]
当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为雨雾天气时，将所述第二目标的位置信息确定为融合后的目标信息。
[0106]
当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为正常天气时，将所述第一目标的位置信息确定为融合后的目标信息。
[0107]
步骤400具体包括：基于卡尔曼滤波算法和所述融合后的目标信息，对海面上的目标进行跟踪。
[0108]
实施例三
[0109]
如图5所示，本实施例提供了一种基于多传感器融合的无人艇海面目标追踪装置，包括：安装在无人艇上的激光雷达、毫米波雷达和工控机。
[0110]
所述激光雷达，用于采集原始点云数据。
[0111]
所述毫米波雷达，用于采集原始毫米波数据。
[0112]
所述工控机，包括：
[0113]
激光雷达数据检测模块，用于获取激光雷达采集的原始点云数据。
[0114]
毫米波雷达数据检测模块，用于获取毫米波雷达采集的原始毫米波数据。
[0115]
数据融合模块，用于对所述原始点云数据和所述原始毫米波数据进行融合处理，得到融合后的目标信息。
[0116]
跟踪模块，用于基于所述融合后的目标信息，对海面上的目标进行跟踪。
[0117]
进一步地，本实施例提供的装置还包括传感器安装平台。
[0118]
其中，在所述传感器安装平台的顶面上安装有所述激光雷达，在所述传感器安装平台的四个侧面上分别安装有所述毫米波雷达，所述传感器安装平台的底面通过支架安装在所述无人艇的尾部。
[0119]
所述数据融合模块，具体包括：
[0120]
预处理单元，用于对所述原始点云数据进行预处理，得到在船体坐标系下第一目标的位置信息；所述第一目标为所述激光雷达检测到的目标。
[0121]
判断单元，用于判断所述原始毫米波数据是否含有目标信息。
[0122]
第一融合后目标信息确定单元，用于当所述原始毫米波数据含有目标信息时，先对所述原始毫米波数据进行预处理，得到在所述船体坐标系下第二目标的位置信息，接着对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述第二目标为所述毫米波雷达检测到的目标。
[0123]
第二融合后目标信息确定单元，用于当所述原始毫米波数据未含有目标信息时，将所述第一目标的位置信息确定为融合后的目标信息。
[0124]
进一步地，在所述对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息方面，所述第一融合后目标信息确定单元，进一步用于：
[0125]
当目标距离小于或者等于设定阈值时，对所述第一目标的位置信息和所述第二目标的位置信息进行融合处理，得到融合后的目标信息；所述目标距离为所述第一目标与所述第二目标之间的距离。
[0126]
当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈
值，且当前海面天气为雨雾天气时，将所述第二目标的位置信息确定为融合后的目标信息。
[0127]
当所述第一目标和所述第二目标在同一方位上的所述目标距离大于所述设定阈值，且当前海面天气为正常天气时，将所述第一目标的位置信息确定为融合后的目标信息。
[0128]
当所述第一目标出现在毫米波范围之外，在激光雷达范围之内时，将所述第一目标的位置信息确定为融合后的目标信息。
[0129]
所述跟踪模块，具体包括：
[0130]
跟踪单元，用于基于卡尔曼滤波算法和所述融合后的目标信息，对海面上的目标进行跟踪。
[0131]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
[0132]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。