CN113627473A - 基于多模态传感器的水面无人艇环境信息融合感知方法 - Google Patents

Abstract

本发明针对水面无人艇复杂、多变的工作环境，提供了一种基于多模态传感器的水面无人艇环境信息融合感知方法。本发明克服了单一传感器采集的信息只能从某一角度或侧重点获取环境信息、对目标或场景的描述能力具有一定的局限性、抵抗外界的干扰能力较弱的缺点，发挥了各个传感器的优点并加以结合，构成了一种探测距离远、精度高、可靠性强的感知系统，能对不同海面目标判别其类别并获取绝对经纬度位置，对目标进行不同等级威胁度评估，在场景理解的误差、运动引起的图像噪声、弱光或恶劣海况下具有较高的鲁棒性，保证无人艇在执行任务时能提供更为完善的海洋环境感知模型，安全高效的执行各项任务。

Description

基于多模态传感器的水面无人艇环境信息融合感知方法

技术领域

本发明属于水面无人艇环境感知技术领域，具体涉及一种基于多模态传感器的水面无人艇环境信息融合感知方法。

背景技术

水面无人艇作为一个海上的高智能化载体，其复杂、多变的工作环境，对环境感知能力有较高的需求。目前国内外研究者一直在对单一传感器的性能进行完善，但海面环境复杂，其信息具有片面性，特别是在恶劣海况下，单一传感器采集的信息只能从某一角度或侧重点获取环境信息，因而对目标或场景的描述能力具有一定的局限性，抵抗外界的干扰能力较弱，为准确地识别环境目标信息带来了较大的挑战。

多传感器信息融合技术能够弥补各自传感器的缺陷，结合它们的优点，准确地、快速地感知无人艇周围目标信息。例如，激光雷达具有分辨率高、精度高等优点，航海雷达探测范围远能提供较远目标的方位信息，但近距离检测精度较差，正好与激光雷达优劣互补，提高系统鲁棒性。同时光学传感器弥补了雷达在识别物体方面的劣势，它可以捕获物体的细节，如亮度，纹理等特征，依靠深度学习目标检测算法为不同的海面目标划分不同等级度危险标签，但是可见光摄像机又受天气状况影响，只能在天气良好的情况下检测和识别目标，红外摄像机成像是将目标与环境的辐射差异按照固定的映射机制映射成为灰度图像，其成像不受环境条件的限制，而且能突出显示隐藏的目标，因此将可见光图像与红外图像相融合，尽可能多的保留海面目标的特征，与雷达所提供的三维信息相结合，构建更为完善的海面环境感知模型。目前，多种传感器信息融合在国内应用于水面无人艇环境感知的研究相对较少，存在许多待解决的难题，需要开展深入的理论和技术研究。

发明内容

本发明的目的在于提供一种基于多模态传感器的水面无人艇环境信息融合感知方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：将激光雷达、航海雷达、可见光摄像机和红外摄像机在时间空间上配准；将激光雷达与航海雷达坐标系相互转换对齐后，对激光雷达和可见光摄像机进行标定，建立将激光雷达捕获的三维点映射到可见光摄像机捕获的图像数据中的二维点的转化矩阵；

步骤2：利用可见光摄像机和红外摄像机收集海面环境图像数据，对不同的目标海面物体采集足够的样本后进行标注，得到海面目标数据集；利用海面目标数据集在卷积神经网络中进行训练，得到目标检测模型；

步骤3：获取激光雷达捕获的三维点阵云，通过滤波器过滤点云中无效数据与水面无人艇自身航迹所产生的杂乱点云；根据过滤后的点云聚类获取水面无人艇周围不同目标的点云簇并提取质心作为海面目标的特征点；

步骤4：根据不同海况调整航海雷达参数并将所返还的目标数据进行坐标转换，构建海面目标CTRV运动模型，利用卡尔曼滤波和扩展卡尔曼滤波将激光雷达数据与航海雷达数据相融合，得到初步海洋环境感知模型；

步骤5：将红外摄像机与可见光摄像机联合标定配准，将配准后的红外图像与可见光图像进行特征融合，重构融合后的图像，得到海洋环境目标感知光学样本；

f(x,y)＝w₁f_v(x,y)+w₂f_i(x,y)

其中，f_v为可见光图像的特征图，f_v(x,y)为特征图f_v在(x,y)处的特征值；f_i为红外图像的特征图，f_i(x,y)为特征图f_i在(x,y)处的特征值；w₁、w₂为融合权重；

步骤6：根据步骤4获取的初步海洋环境感知模型，针对海面不同目标方位引导可见光摄像机追踪目标，控制云台姿态角转动至目标所在方位角，在可见光摄像机的视场范围内对环境模型内雷达数据进行筛选，将雷达数据对应的目标限制在可见光摄像机的视场范围，将筛选后的雷达数据投影至二维图像坐标系中；

步骤7：对步骤5中获取的海洋环境目标感知光学样本进行目标检测，输出所检测到目标的属性、置信度、目标中心像素的位置、目标检测框的左上角坐标、目标检测框的右下角坐标；

步骤8：根据投影至二维图像坐标系的雷达坐标点，计算雷达坐标点与目标检测框的重叠度，即雷达数据映射至图像坐标系的中点；若映射点在目标检测框内或框外所允许的误差范围内，则将目标检测的结果赋予该雷达坐标点，即将雷达坐标点贴上该目标类别与置信度的属性标签；否则，判定雷达坐标点代表暂无目标属性类别，等待后续光学传感器验证识别；

步骤9：对贴有目标类别与置信度属性标签的雷达坐标点对应的初步海洋环境感知模型中的目标点进行坐标转换，将初步海洋环境感知模型内目标点的相对坐标转换为经纬度信息，将相对速度与航向转换为绝对速度与航向；

步骤10：同时利用目标检测信息、目标雷达信息与水面无人艇自身姿态信息构建加权融合模型，对海面目标进行危险等级度评估，构建完整的海洋环境感知模型。

本发明还可以包括：

所述的步骤4具体包括以下步骤：

步骤4.1：将激光雷达采集的数据集Z_lidar＝{z_l1,z_l2,...,z_lM}和航海雷达采集的数据集Z_radar＝{z_r1,z_r2,...,z_rN}整合为集合Z_lr，并将集合Z_lr中各组数据按照时间升序排列；

其中，集合Z_lidar和Z_radar中的数据均为按时间升序排列；激光雷达采集的数据为z_lm＝(p_mx,p_my)，p_mx表示第m组数据中目标在笛卡尔坐标系x轴的位置，p_my表示第m组数据中目标在笛卡尔坐标系y轴的位置，m＝1,2,...,M，M为激光雷达采集数据的总次数；航海雷达采集的数据为

ρ_n表示第n组数据中目标与水面无人艇的距离，

表示第n组数据中目标在极坐标系下与极轴的角度，

表示第n组数据中目标的速度，n＝1,2,...,N，N为航海雷达采集数据的总次数；

步骤4.2：从集合Z_lr中取第一组数据z₁，初始化目标的CTRV运动状态x₁和状态误差协方差矩阵P₁；初始化k＝2；构造目标的CTRV运动状态集合X，初始化X＝{x₁}；

目标的CTRV运动状态x_k为：

其中，b_kx表示目标在笛卡尔坐标系x轴的位置；b_ky表示目标在笛卡尔坐标系y轴的位置；v_k表示目标的速度，

ψ_k表示目标的偏航角，

表示目标的偏航角速度

初始化协方差矩阵P₁＝E，E为单位阵；

若第一组数据z₁为激光雷达采集的数据z_l1，则令b_1x＝p_1x，b_1y＝p_1y，v₁＝0，ψ₁＝0，

即x₁＝[p_1x,p_1y,0,0,0]^T；若第一组数据z₁为航海雷达采集的数据z_r1，则令

ψ₁＝0，

即

步骤4.3：从集合Z_lr中取第k组数据z_k，计算耗时Δt_k＝t(z_k)-t(z_k-1)，计算运动模型f(x_k-1)与其Jacobian矩阵F_k和协方差矩阵Q_k；

运动模型f(x_k-1)与其Jacobian矩阵F_k的计算方法为：

若

则：

若

则：

协方差矩阵Q_k的计算方法为：

其中，

为设定的常数；

为目标的历史偏航角速度变换倍率的方差，根据集合X中的数据计算；

步骤4.4：计算目标的CTRV运动状态的预测值

及状态误差协方差矩阵的预测值

其中，υ_k为噪声；

其中，υ_a,k为纵向加速度噪声，其均值为0，方差为

为角加速度噪声，其均值为0，方差为

步骤4.5：若第k组数据z_k为激光雷达采集的数据，则执行步骤4.6；若第k组数据z_k为航海雷达采集的数据

则执行步骤4.7；

步骤4.6：根据激光雷达变换矩阵H_lidar与噪声测量协方差矩阵R_lidar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中：

其中，I为单位阵；

为笛卡尔坐标系x轴方向的噪声标准差；

为笛卡尔坐标系y轴方向的噪声标准差；

步骤4.7：根据航海雷达非线性映射线性化后的Jacobian矩阵H_radar与噪声矩阵R_radar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中，

其中，

为不同参数的噪声标准差；

步骤4.8：若k＜M+N，则令k＝k+1，并返回步骤4.3；否则，输出目标的CTRV运动状态集合X＝{x₁,x₂,...,x_M+N}，与激光雷达感知范围外航海雷达远距离目标数据集合并，得到初步海洋环境感知模型。

本发明的有益效果在于：

本发明针对水面无人艇复杂、多变的工作环境，克服了单一传感器采集的信息只能从某一角度或侧重点获取环境信息、对目标或场景的描述能力具有一定的局限性、抵抗外界的干扰能力较弱的缺点，发挥了各个传感器的优点并加以结合，构成了一种探测距离远、精度高、可靠性强的感知系统，能对不同海面目标判别其类别并获取绝对经纬度位置，对目标进行不同等级威胁度评估，在场景理解的误差、运动引起的图像噪声、弱光或恶劣海况下具有较高的鲁棒性，保证无人艇在执行任务时能提供更为完善的海洋环境感知模型，安全高效的执行各项任务。

附图说明

图1为本发明的整体框架图。

图2为本发明中雷达数据融合示意图。

图3为本发明中硬件系统组成结构图。

具体实施方式

下面结合附图对本发明做进一步描述。

由于海面环境复杂，特别是在恶劣海况下，单一传感器采集的信息只能从某一角度或侧重点获取环境信息，因而对目标或场景的描述能力具有一定的局限性，抵抗外界的干扰能力较弱，为准确地识别目标信息带来了较大的挑战。本发明针对水面无人艇复杂、多变的工作环境，提供了一种基于多模态传感器的水面无人艇环境信息融合感知方法，使无人艇及时根据环境的变化调整自身姿态及状态，在执行任务时能具备更好的环境感知能力，在保障安全的前提下自主地完成各项任务。本发明利用激光雷达、航海雷达、高清摄像机、红外热成像仪多种传感器相结合获得更为准确的环境模型。首先，将四个传感器在时间空间上配准，将激光雷达与可见光摄像机联合标定、将可见光摄像机与红外摄像机标定；将激光雷达点云数据进行坐标转换与自身航迹消除处理、聚类处理得到较为稳定的目标信息，然后将航海雷达所返还目标数据用扩展卡尔曼滤波相融合，得到初步的环境模型；利用不同目标的方位信息引导光学相机自动追踪目标，将所得高清图像与红外图像相融合，依靠目标检测算法为目标贴上类别属性标签，同时将目标检测信息与雷达信息、自身艇姿态信息相融合，对海面目标进行危险等级度评估，不断构建完善海上环境感知模型，提高水面无人艇自身的智能化水平与执行任务的能力。

一种基于多模态传感器的水面无人艇环境信息融合感知方法，包括以下步骤：

步骤1：将激光雷达、航海雷达、可见光摄像机和红外摄像机在时间空间上配准；将激光雷达与航海雷达坐标系相互转换对齐后，对激光雷达和可见光摄像机进行标定，建立将激光雷达捕获的三维点映射到可见光摄像机捕获的图像数据中的二维点的转化矩阵；

步骤2：利用可见光摄像机和红外摄像机收集海面环境图像数据，对不同的目标海面物体采集足够的样本后进行标注，得到海面目标数据集；利用海面目标数据集在卷积神经网络中进行训练，得到目标检测模型；

步骤3：获取激光雷达捕获的三维点阵云，通过滤波器过滤点云中无效数据与水面无人艇自身航迹所产生的杂乱点云；根据过滤后的点云聚类获取水面无人艇周围不同目标的点云簇并提取质心作为海面目标的特征点；

步骤4：根据不同海况调整航海雷达参数并将所返还的目标数据进行坐标转换，构建海面目标CTRV运动模型，利用卡尔曼滤波和扩展卡尔曼滤波将激光雷达数据与航海雷达数据相融合，得到初步海洋环境感知模型；

步骤4.1：将激光雷达采集的数据集Z_lidar＝{z_l1,z_l2,...,z_lM}和航海雷达采集的数据集Z_radar＝{z_r1,z_r2,...,z_rN}整合为集合Z_lr，并将集合Z_lr中各组数据按照时间升序排列；

其中，集合Z_lidar和Z_radar中的数据均为按时间升序排列；激光雷达采集的数据为z_lm＝(p_mx,p_my)，p_mx表示第m组数据中目标在笛卡尔坐标系x轴的位置，p_my表示第m组数据中目标在笛卡尔坐标系y轴的位置，m＝1,2,...,M，M为激光雷达采集数据的总次数；航海雷达采集的数据为

ρ_n表示第n组数据中目标与水面无人艇的距离，

表示第n组数据中目标在极坐标系下与极轴的角度，

表示第n组数据中目标的速度，n＝1,2,...,N，N为航海雷达采集数据的总次数；

步骤4.2：从集合Z_lr中取第一组数据z₁，初始化目标的CTRV运动状态x₁和状态误差协方差矩阵P₁；初始化k＝2；构造目标的CTRV运动状态集合X，初始化X＝{x₁}；

目标的CTRV运动状态x_k为：

其中，b_kx表示目标在笛卡尔坐标系x轴的位置；b_ky表示目标在笛卡尔坐标系y轴的位置；v_k表示目标的速度，

ψ_k表示目标的偏航角，

表示目标的偏航角速度

初始化协方差矩阵P₁＝E，E为单位阵；

若第一组数据z₁为激光雷达采集的数据z_l1，则令b_1x＝p_1x，b_1y＝p_1y，v₁＝0，ψ₁＝0，

即x₁＝[p_1x,p_1y,0,0,0]^T；若第一组数据z₁为航海雷达采集的数据z_r1，则令

ψ₁＝0，

即

步骤4.3：从集合Z_lr中取第k组数据z_k，计算耗时Δt_k＝t(z_k)-t(z_k-1)，计算运动模型f(x_k-1)与其Jacobian矩阵F_k和协方差矩阵Q_k；

运动模型f(x_k-1)与其Jacobian矩阵F_k的计算方法为：

若

则：

若

则：

协方差矩阵Q_k的计算方法为：

其中，

为设定的常数；

为目标的历史偏航角速度变换倍率的方差，根据集合X中的数据计算；

步骤4.4：计算目标的CTRV运动状态的预测值

及状态误差协方差矩阵的预测值

其中，υ_k为噪声；

其中，υ_a,k为纵向加速度噪声，其均值为0，方差为

为角加速度噪声，其均值为0，方差为

步骤4.5：若第k组数据z_k为激光雷达采集的数据，则执行步骤4.6；若第k组数据z_k为航海雷达采集的数据

则执行步骤4.7；

步骤4.6：根据激光雷达变换矩阵H_lidar与噪声测量协方差矩阵R_lidar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中：

其中，I为单位阵；

为笛卡尔坐标系x轴方向的噪声标准差；

为笛卡尔坐标系y轴方向的噪声标准差；

步骤4.7：根据航海雷达非线性映射线性化后的Jacobian矩阵H_radar与噪声矩阵R_radar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中，

其中，

为不同参数的噪声标准差；

步骤4.8：若k＜M+N，则令k＝k+1，并返回步骤4.3；否则，输出目标的CTRV运动状态集合X＝{x₁,x₂,...,x_M+N}，与激光雷达感知范围外航海雷达远距离目标数据集合并，得到初步海洋环境感知模型；

步骤5：将红外摄像机与可见光摄像机联合标定配准，将配准后的红外图像与可见光图像进行特征融合，重构融合后的图像，得到海洋环境目标感知光学样本；

f(x,y)＝w₁f_v(x,y)+w₂f_i(x,y)

其中，f_v为可见光图像的特征图，f_v(x,y)为特征图f_v在(x,y)处的特征值；f_i为红外图像的特征图，f_i(x,y)为特征图f_i在(x,y)处的特征值；w₁、w₂为融合权重；

步骤6：根据步骤4获取的初步海洋环境感知模型，针对海面不同目标方位引导可见光摄像机追踪目标，控制云台姿态角转动至目标所在方位角，在可见光摄像机的视场范围内对环境模型内雷达数据进行筛选，将雷达数据对应的目标限制在可见光摄像机的视场范围，将筛选后的雷达数据投影至二维图像坐标系中；

步骤7：对步骤5中获取的海洋环境目标感知光学样本进行目标检测，输出所检测到目标的属性、置信度、目标中心像素的位置、目标检测框的左上角坐标、目标检测框的右下角坐标；

步骤8：根据投影至二维图像坐标系的雷达坐标点，计算雷达坐标点与目标检测框的重叠度，即雷达数据映射至图像坐标系的中点；若映射点在目标检测框内或框外所允许的误差范围内，则将目标检测的结果赋予该雷达坐标点，即将雷达坐标点贴上该目标类别与置信度的属性标签；否则，判定雷达坐标点代表暂无目标属性类别，等待后续光学传感器验证识别；

步骤9：对贴有目标类别与置信度属性标签的雷达坐标点对应的初步海洋环境感知模型中的目标点进行坐标转换，将初步海洋环境感知模型内目标点的相对坐标转换为经纬度信息，将相对速度与航向转换为绝对速度与航向；

步骤10：同时利用目标检测信息、目标雷达信息与水面无人艇自身姿态信息构建加权融合模型，对海面目标进行危险等级度评估，构建完整的海洋环境感知模型。

本发明针对水面无人艇复杂、多变的工作环境，克服了单一传感器采集的信息只能从某一角度或侧重点获取环境信息、对目标或场景的描述能力具有一定的局限性、抵抗外界的干扰能力较弱的缺点，发挥了各个传感器的优点并加以结合，构成了一种探测距离远、精度高、可靠性强的感知系统，能对不同海面目标判别其类别并获取绝对经纬度位置，对目标进行不同等级威胁度评估，在场景理解的误差、运动引起的图像噪声、弱光或恶劣海况下具有较高的鲁棒性，保证无人艇在执行任务时能提供更为完善的海洋环境感知模型，安全高效的执行各项任务。

实施例1：

本发明提供了一种基于多模态传感器的水面无人艇环境信息融合感知方法，针对水面无人艇复杂、多变的工作环境，在执行任务时能具备更好的环境感知能力，使无人艇及时根据环境的变化调整自身姿态及状态，在保障安全的前提下自主地完成各项任务。由于海面环境复杂，特别是在恶劣海况下，单一传感器采集的信息只能从某一角度或侧重点获取环境信息，因而对目标或场景的描述能力具有一定的局限性，抵抗外界的干扰能力较弱，为准确地识别目标信息带来了较大的挑战。本发明利用激光雷达、航海雷达、高清摄像机、红外热成像仪多种传感器相结合获得更为准确的环境模型。首先，将四个传感器在时间空间上配准，利用张正友标定法建立的3D-2D转换关系将激光雷达与可见光摄像机联合标定，可见光与红外双摄像机标定。激光雷达点云数据进行坐标转换与自身航迹消除处理，然后利用GB-DBSCAN聚类处理得到较为稳定的目标信息，然后将航海雷达所返还目标数据用扩展卡尔曼滤波相融合，得到初步的环境模型。后利用不同目标的方位信息引导光学相机自动追踪目标，将所得高清图像与红外图像相融合，依靠目标检测算法为目标贴上类别属性标签，同时将目标检测信息与雷达信息、自身艇姿态信息构建Bayes模型相融合，对海面目标进行危险等级度评估，不断构建完善海上环境感知模型，提高水面无人艇自身的智能化水平与执行任务的能力。

本发明运用多种传感器，设计一种新的适用于无人艇的信息融合环境感知技术策略。多艇上装载的激光雷达与航海雷达不间断获取目标信息构建周围初步环境模型，得到目标方位信息引导光学传感器感知目标，红外热成像仪与可见光摄像机进行特征层面融合，融合后的图像可应用于目标检测算法，得到目标属性类别。同时将目标检测信息与雷达信息、自身艇姿态信息构建Bayes模型相融合，对海面目标进行危险等级度评估，不断构建完善海上环境感知模型，提高水面无人艇自身的智能化水平与执行任务的能力。其具体的实施方法如下。

步骤1，将四个传感器在时间空间上配准，激光雷达与航海雷达坐标系相互转换对齐，后对激光雷达和相机进行标定，利用张正友标定法求出可见光相机内参矩阵，根据光学传感器和激光雷达固定装配位置的相对关系解算出转换矩阵，其公式为S₀m＝K[R,T]M，光学传感器捕获的图像数据用(u,v)表示，激光雷达捕获的3维点阵云用(x,y,z)表示，目标是建立一个转化矩阵M，将3维点(x,y,z)映射到2维点(u,v)，坐标转移公式为S₀m＝K[R,T]M。

其中利用张氏标定法得到相机内参

f_u以及f_v是水平方向和垂直方向的有效焦距，u₀以及v₀是图像平面(image plane)的中心点，又称主点坐标。然后根据光电吊舱和激光雷达固定装配位置的相对关系得到外参矩阵[R,T]，其中S₀为尺度因子，K为摄像机内参,R为旋转矩阵，T为平移向量。

步骤2，利用光学传感器收集海面环境图像数据，用无人艇所载相机对海面不同目标拍摄足够的视频资料，后处理为图片数据，用LabelImage工具对海面数据集进行标注，后处理达到神经网络训练要求输入格式。

步骤3，利用所采集到的海面目标数据集在卷积神经网络中进行训练，对训练数据集进行Mosaic数据增强等操作后，使用轻量化卷积神经网络，以达到实时性要求与嵌入式设备运行需求。经过训练测试，筛选得到最优的目标检测模型。

步骤4，利用Conditional Removal滤波器删除激光雷达点云中无效数据与自身航迹所产生的杂乱点云。由于海面环境的特殊性以及无人艇航行时航迹的影响，激光雷达扫描得到的点云密度分布不均匀，产生散乱的干扰点。这会给点云特征识别以及点云聚类带来干扰，造成目标漏检或错检。Conditional Removal滤波器以每一个点为圆心生成临域，计算临域内点至圆心之间的欧氏距离，以此为判断条件并且将临域内点数目过少的点识别为离群干扰点，同时设定艇自身螺旋桨后适当的区域(1m～2m)为航迹区域并将其一并从数据集中去除。由此去除水面上的杂乱数据点与艇自身航迹所产生的点云。

步骤5，将过滤后的点云使用GB-DBSCAN聚类算法得到艇身周围不同目标的点云簇。与传统的聚类算法如K-Means相比，DBSCAN算法最大的优点是它不需要事先明确具体的簇的数量，且数据集簇的形状可以是非凸的，但当空间聚类的密度不均匀、聚类间距差相差很大时，聚类质量较差，因为这种情况下参数MinPts和Eps选取困难。GB-DBSCAN算法无需固定的参数，利用雷达半径与角率来创建grid网格，无须判断点与点之间的欧氏距离，只需考虑grid与grid之间是否密度相连，极大的减少了计算量，节约了聚类所需的时间。将聚类后的点云簇提取质心作为海面目标的特征点。

步骤6，据不同海况调整航海雷达参数并将所返还的目标数据进行坐标转换并构建海面目标CTRV运动模型，利用卡尔曼滤波和扩展卡尔曼滤波将激光雷达数据与航海雷达数据相融合，得到初步的环境模型。

为海面目标构建CTRV运动模型，其移动向量五个分量为：

其中v为速度，ψ为角度，

为角速度，

为角速度变换倍率。非线性运动方程x_k+1＝f(x_k,υ_k)展开为：

υ_k为噪声，υ_a,k为纵向加速度噪声，其均值为0方差为

为角加速度噪声，均值为0方差为

k为时刻。

则运动模型Jacobian矩阵为：

协方差矩阵Q为：

激光雷达为笛卡尔坐标系，可检测到位置，没有速度信息，其测量值z_lidar＝(p_x,p_y)，航海雷达为极坐标系，可检测到距离、角度、速度，但精度较低，其测量值

对于航海雷达，没有变换矩阵可将状态向量映射至极坐标中，因此需要将笛卡尔坐标系映射到极坐标系，即：

对于Lidar激光雷达测量模型，因为激光雷达仅测量位置信息，故将状态向量中速度与角速度信息舍去。R为传感器测量中的不确定性，每个位置成分P_x，P_y都会受到随机噪声的影响。测量噪声ω是一个均值为零，协方差为R的分布，即ω～N(0,R)。因此R矩阵是均值为零的2×2协方差矩阵，是噪声矢量ω和它的转置乘积的期望值。其变换矩阵H与噪声测量协方差矩阵为

其中

为x，y方向的噪声标准差。

航海雷达非线性映射线性化后的Jacobian矩阵H_j与噪声矩阵为：

其中

为不同参数的噪声标准差。

最终融合步骤分为预测与修正当下，公式如下所示：

预测部分：

修正当下部分：

其中P_k为状态误差协方差矩阵，可用测量噪声方差来初始化矩阵，I为n维的单位矩阵。激光雷达与航海雷达预测部分公式一致，参数更新部分稍有不同，航海雷达需用

替换即可。当收到目标激光雷达或航海雷达的测量值时，依次执行预测与修正部分即可，具体融合流程步骤如图2所示。

步骤7，红外与可见光双摄像机标定，利用单目摄像机标定得到红外和可见光摄像机的外参数R_i，R_v，t_i，t_v，然后利用下式求得红外和可见光摄像机间的空间变换关系R′，t′。

R′为可见光摄像机相对红外热成像仪的旋转矩阵、t′为可见光摄像机相对红外热成像仪的平移向量。可见光摄像机坐标系与红外摄像机坐标系关系为：

由此可得红外与可见光图像配准关系：

将配准后的红外与可见光图像利用深度学习方法进行特征融合，其网络框架与DenseFuse类似，包含编码器，融合层和解码器三个部分。编码器层设置4层3x3卷积以提取多层次深度特征，融合策略上在传统的Spatial Attention Model上用了基于L₁范数和Softmax运算的新方法，公式如下：

其中f_v为可见光图像的特征图，f_i为红外图像的特征图，f_v(x，y)为特征图f_v在(x，y)处的特征值，f_i(x,y)为特征图f_i在(x，y)处的特征值，f(x,y)为特征图f在(x，y)处的特征值。由此最大限度结合红外图像与可见光的图像的优势，再利用解码器将融合后的图像重构，得到新的同时具有二者优势的海洋环境目标感知光学样本。

步骤8，利用所得雷达海洋环境模型，针对海面不同目标方位引导光学传感器追踪环境内目标，即控制云台姿态角转动至目标所在方位角，同时用直通滤波器将雷达数据在光学传感器视场范围内进行筛选，即通过光学传感器的视场角以及云台姿态角，将雷达点限制在相机感受野范围内，节省计算量。

其中β为云台方位角，ε为相机视场角度，p_x,p_y为雷达目标的xy坐标。将筛选后的雷达数据投影至二维图像坐标系中。

步骤9，利用目标检测算法对融合后的图像进行识别，并输出所检测到目标的属性、置信度、目标中心像素的位置和检测框左上角、右下角的坐标，分别记为(A₁₁..A_n1，B₁₁..B_1n),(C₁₁...C_n1，D₁₁…D_1n)。

步骤10，根据投影至图像平面的雷达坐标，计算点与目标检测框的重叠度，即雷达映射至图像坐标系中点(X，Y)，若点在目标检测框内或框外所允许的误差范围内，即：

A_n1≤X≤C_n1，B_1n≤Y≤D_1n (23)

则将目标检测的结果赋予该雷达点坐标，即雷达点被贴上该目标类别与置信度的属性标签。若目标检测框无映射雷达点坐标，则给予一定的噪声误差允许范围ε，即：

以此来扩大验证范围。剩余无目标检测结果框或未映射至图像上的雷达点，则赋予-1的属性标签，代表暂无目标属性类别，等待后续光学传感器验证识别。

步骤11，对所得海洋环境模型内的点进行坐标转换，依靠无人艇所载惯导得到艇自身的姿态，即航向、位置、速度等，将模型内目标点的相对坐标转换为绝对坐标即经纬度信息，相对速度与航向转换为绝对速度与航向：

其中x₀,y₀,θ,lat₀,lon₀为无人艇雷达所检测到的目标相对坐标与航向及自身经纬度信息，R为地球半径，lat₁,lon₁为转换后经纬度信息。同时利用目标检测信息、目标雷达信息与自身艇姿态信息构建加权模型融合，对海面目标进行危险等级度评估：

T_p,T_t,T_d分别代表着目标属性(Properties)，目标趋势(Tendency)与目标距离(Distance)，可根据不同任务需求分为多个等级，等级可换算为实数1～N，w₁,w₂,w₃为三个威胁度等级因子，依据实际海域情况可进行调整，最后加权获得总的威胁度等级T，构建更为完善的海洋环境智能感知模型。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种基于多模态传感器的水面无人艇环境信息融合感知方法，其特征在于，包括以下步骤：

步骤1：将激光雷达、航海雷达、可见光摄像机和红外摄像机在时间空间上配准；将激光雷达与航海雷达坐标系相互转换对齐后，对激光雷达和可见光摄像机进行标定，建立将激光雷达捕获的三维点映射到可见光摄像机捕获的图像数据中的二维点的转化矩阵；

步骤2：利用可见光摄像机和红外摄像机收集海面环境图像数据，对不同的目标海面物体采集足够的样本后进行标注，得到海面目标数据集；利用海面目标数据集在卷积神经网络中进行训练，得到目标检测模型；

步骤3：获取激光雷达捕获的三维点阵云，通过滤波器过滤点云中无效数据与水面无人艇自身航迹所产生的杂乱点云；根据过滤后的点云聚类获取水面无人艇周围不同目标的点云簇并提取质心作为海面目标的特征点；

步骤4：根据不同海况调整航海雷达参数并将所返还的目标数据进行坐标转换，构建海面目标CTRV运动模型，利用卡尔曼滤波和扩展卡尔曼滤波将激光雷达数据与航海雷达数据相融合，得到初步海洋环境感知模型；

步骤5：将红外摄像机与可见光摄像机联合标定配准，将配准后的红外图像与可见光图像进行特征融合，重构融合后的图像，得到海洋环境目标感知光学样本；

f(x,y)＝w₁f_v(x,y)+w₂f_i(x,y)

其中，f_v为可见光图像的特征图，f_v(x,y)为特征图f_v在(x,y)处的特征值；f_i为红外图像的特征图，f_i(x,y)为特征图f_i在(x,y)处的特征值；w₁、w₂为融合权重；

步骤6：根据步骤4获取的初步海洋环境感知模型，针对海面不同目标方位引导可见光摄像机追踪目标，控制云台姿态角转动至目标所在方位角，在可见光摄像机的视场范围内对环境模型内雷达数据进行筛选，将雷达数据对应的目标限制在可见光摄像机的视场范围，将筛选后的雷达数据投影至二维图像坐标系中；

步骤7：对步骤5中获取的海洋环境目标感知光学样本进行目标检测，输出所检测到目标的属性、置信度、目标中心像素的位置、目标检测框的左上角坐标、目标检测框的右下角坐标；

步骤8：根据投影至二维图像坐标系的雷达坐标点，计算雷达坐标点与目标检测框的重叠度，即雷达数据映射至图像坐标系的中点；若映射点在目标检测框内或框外所允许的误差范围内，则将目标检测的结果赋予该雷达坐标点，即将雷达坐标点贴上该目标类别与置信度的属性标签；否则，判定雷达坐标点代表暂无目标属性类别，等待后续光学传感器验证识别；

步骤9：对贴有目标类别与置信度属性标签的雷达坐标点对应的初步海洋环境感知模型中的目标点进行坐标转换，将初步海洋环境感知模型内目标点的相对坐标转换为经纬度信息，将相对速度与航向转换为绝对速度与航向；

步骤10：同时利用目标检测信息、目标雷达信息与水面无人艇自身姿态信息构建加权融合模型，对海面目标进行危险等级度评估，构建完整的海洋环境感知模型。

2.根据权利要求1所述的一种基于多模态传感器的水面无人艇环境信息融合感知方法，其特征在于：所述的步骤4具体包括以下步骤：

步骤4.1：将激光雷达采集的数据集Z_lidar＝{z_l1,z_l2,...,z_lM}和航海雷达采集的数据集Z_radar＝{z_r1,z_r2,...,z_rN}整合为集合Z_lr，并将集合Z_lr中各组数据按照时间升序排列；

其中，集合Z_lidar和Z_radar中的数据均为按时间升序排列；激光雷达采集的数据为z_lm＝(p_mx,p_my)，p_mx表示第m组数据中目标在笛卡尔坐标系x轴的位置，p_my表示第m组数据中目标在笛卡尔坐标系y轴的位置，m＝1,2,...,M，M为激光雷达采集数据的总次数；航海雷达采集的数据为

ρ_n表示第n组数据中目标与水面无人艇的距离，

表示第n组数据中目标在极坐标系下与极轴的角度，

表示第n组数据中目标的速度，n＝1,2,...,N，N为航海雷达采集数据的总次数；

步骤4.2：从集合Z_lr中取第一组数据z₁，初始化目标的CTRV运动状态x₁和状态误差协方差矩阵P₁；初始化k＝2；构造目标的CTRV运动状态集合X，初始化X＝{x₁}；

目标的CTRV运动状态x_k为：

其中，b_kx表示目标在笛卡尔坐标系x轴的位置；b_ky表示目标在笛卡尔坐标系y轴的位置；v_k表示目标的速度，

ψ_k表示目标的偏航角，

表示目标的偏航角速度

初始化协方差矩阵P₁＝E，E为单位阵；

若第一组数据z₁为激光雷达采集的数据z_l1，则令b_1x＝p_1x，b_1y＝p_1y，v₁＝0，Ψ₁＝0，

即x₁＝[p_1x,p_1y,0,0,0]^T；若第一组数据z₁为航海雷达采集的数据z_r1，则令

Ψ₁＝0，

即

步骤4.3：从集合Z_lr中取第k组数据z_k，计算耗时Δt_k＝t(z_k)-t(z_k-1)，计算运动模型f(x_k-1)与其Jacobian矩阵F_k和协方差矩阵Q_k；

运动模型f(x_k-1)与其Jacobian矩阵F_k的计算方法为：

若

则：

若

则：

协方差矩阵Q_k的计算方法为：

其中，

为设定的常数；

为目标的历史偏航角速度变换倍率的方差，根据集合X中的数据计算；

步骤4.4：计算目标的CTRV运动状态的预测值

及状态误差协方差矩阵的预测值

其中，υ_k为噪声；

其中，υ_a,k为纵向加速度噪声，其均值为0，方差为

为角加速度噪声，其均值为0，方差为

步骤4.5：若第k组数据z_k为激光雷达采集的数据，则执行步骤4.6；若第k组数据z_k为航海雷达采集的数据

则执行步骤4.7；

步骤4.6：根据激光雷达变换矩阵H_lidar与噪声测量协方差矩阵R_lidar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中：

其中，I为单位阵；

为笛卡尔坐标系x轴方向的噪声标准差；

为笛卡尔坐标系y轴方向的噪声标准差；

步骤4.7：根据航海雷达非线性映射线性化后的Jacobian矩阵H_radar与噪声矩阵R_radar，更新目标的CTRV运动状态x_k及状态误差协方差矩阵P_k，将x_k加入集合X后执行步骤4.8；

其中，

其中，

为不同参数的噪声标准差；

步骤4.8：若k＜M+N，则令k＝k+1，并返回步骤4.3；否则，输出目标的CTRV运动状态集合X＝{x₁,x₂,...,x_M+N}，与激光雷达感知范围外航海雷达远距离目标数据集合并，得到初步海洋环境感知模型。

Images