CN111708038A - 基于姿态传感器和gnss的无人船激光雷达点云数据校正方法 - Google Patents

Abstract

本发明公开了一种基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，包括如下步骤：S1：获取激光雷达点云的原始数据；S2：对激光雷达点云数据进行角度插值；S3：激光雷达实时获取GNSS数据，计算每个点云数据的发射时间，获取每个点云数据的无人船位置信息；S4：获取姿态传感器测得的无人船的姿态信息；S5：根据姿态传感器测得的无人船的姿态信息，对激光雷达补充完整的点云数据进行校正。本发明通过给激光雷达进行角度插值处理提高扫描分辨率；接入GNSS，对激光雷达的每个激光加时间戳并记录每个点云数据的位置信息；姿态传感器对激光雷达点云数据进行实时校正，减小激光雷达目标识别跟踪的误差。

Description

基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法

技术领域

本发明涉及一种激光雷达点云数据校正方法，尤其涉及一种基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法。

背景技术

水面无人艇(Unmanned Surface Vessel,USV)具备高度自动化、智能化特点，是未来船舶发展和水上交通运输的一个重要发展方向，在军事、运输、测量等领域都有着光明的应用前景。

激光雷达因其具备测距精度高、实时性好等优点，且相对于摄像头而言，受雨雾环境影响较小，激光雷达适用于近距离的环境感知，能够满足无人船在复杂环境下，尤其是内河和近海复杂环境下自主航行中对周围航行环境进行快速、准确感知的需求。

在复杂环境情况，尤其是近海海域应用场景中，水流、浪等会引起船体的横滚和纵倾，这会导致原有的激光雷达测距、旋转角等信息出现误差，严重影响目标识别的准确率。尤其是在恶劣海况下，激光雷达识别障碍物目标的信息可靠性会大大降低。

目前在无人船领域，对于激光雷达的研究，更多的是目标检测、特征提取和目标识别的相关研究，这些研究都需要精度高、误差小的激光雷达点云数据作为前提。所以，如何校正外部环境和内部因素引起的误差，使误差尽量减小，是提高目标识别的概率和目标信息精度，提高自动避障成功率以及无人船智能航行的重要保障。因此，采用姿态传感器和GNSS(Global Navigation Satellite System)全球卫星导航系统对无人船激光雷达点云数据校正是非常有必要的。

发明内容

本发明要解决的技术问题是提供一种基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，通过给激光雷达进行角度插值处理提高扫描分辨率；接入GNSS，对激光雷达的每个激光加时间戳并记录每个点云数据的位置信息；姿态传感器对激光雷达点云数据进行实时校正，减小激光雷达目标识别跟踪的误差。

本发明为解决上述技术问题而采用的技术方案是提供一种基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，包括如下步骤：S1：获取激光雷达点云的原始数据；S2：对激光雷达点云数据进行角度插值，补充角度值数据，得到完整的点云数据；S3：激光雷达实时获取GNSS数据，计算每个点云数据的激光发射时间，对应于发射时间获取每个点云数据的无人船位置信息并存储；S4：实时获取姿态传感器测得的无人船的姿态信息；S5：根据姿态传感器测得的无人船的姿态信息，对应于点云数据的激光发射时间对激光雷达补充完整的点云数据进行校正。

进一步地，所述步骤S3计算每个点云数据的发射时间具体包括：设激光雷达为N线的激光雷达，激光线的序号S_i(1≤i≤N)，激光发射轮数R_j(1≤j≤M)，两线激光之间的发射时间间隔是Δt，完成一轮激光发射需要时间t，累计M轮激光数据组成一包雷达数据包；设第一个激光发射时间t₀，则精确到每个激光的发射时间为：t_ij＝t₀+t×R_j+Δt×S_i。

进一步地，对应于激光的发射时间，获取姿态传感器测得的无人船的姿态信息，所述姿态传感器T时刻测得的无人船的姿态信息，包括方位角α、俯仰角β和横滚角χ。

进一步地，通过角度插值T时刻补充完整的点云数据包括水平旋转角度φ、垂直角度ω和实测距离R，所述步骤S5中对t时刻的点云数据的校正具体包括：

S51：通过方位角α校正激光的水平旋转角度φ，校正值为φ+α；校正后的水平旋转角为θ，如果φ+α＞360，则θ＝φ+α-360,否则θ＝φ+α；

S52：进行投影变换，将激光雷达输出的极坐标下的水平旋转角度、垂直角度和距离转化为笛卡尔坐标系下的x,y,z坐标，转换关系如下：

x＝R cos(ω)sin(θ)

y＝R cos(ω)sin(θ)

z＝R sin(ω)

其中R为激光雷达实测距离，ω为激光的垂直角度，即瞬时激光束在YOZ平面内的投影与Y轴的夹角；θ为校正后的激光水平旋转角度，即瞬时激光束在XOY平面内的投影与Y轴的夹角；激光扫描的数据点定义为D(x,y,z)，是数据点的极坐标投影到X、Y、Z轴上的坐标；

S53：通过俯仰角β和横滚角χ校正无人艇平台倾斜，使其投影在大地水平面上，校正后的坐标(x_d,y_d,z_d)，转换关系如下：

x_d＝x cos(β)+z sin(β)

y_d＝x sin(β)sin(χ)+y cos(χ)-zcos(β)sin(χ)

z_d＝-x sin(β)cos(χ)+y sin(χ)+z cos(β)cos(χ)；

S54：通过GNSS安装平台与激光雷达安装平台偏移进行误差修正，修正后的坐标如下：

x_e＝x_d+Δx

y_e＝y_d+Δy

z_e＝z_d+Δz+H

其中Δx、Δy、Δz是激光雷达平台相对于GNSS安装平台在X,Y,Z方向的偏移；H是两个安装平台之间的高度差；

S55：通过激光云校正后的坐标对应于激光发射时间，确定在T时刻，校正后的点云数据，即点云投影在大地平面上的参数：

距离

点云高度H_T＝Z_e。

进一步地，对应于点云数据的激光发射时间，获取校正后的点云数据，同时根据激光发射时间对应的无人船位置信息，对无人船周围目标和环境的三维信息进行复现和回放。

进一步地，所述激光雷达点云的原始数据包括激光测距数据和激光角度数据，激光雷达点云的原始数据是每间隔一组激光测距数据对应输出一次激光角度数据，通过角度插值计算给没有输出激光角度数据的激光测距数据补充角度数据。

进一步地，每组激光测距的时间间隔固定，且激光测距的时间间隔内雷达是匀速旋转的，根据第j组激光测距数据的角度a和第j+2组激光测距数据的角度值c计算平均值，得到第j+1组激光测距数据的角度值b，同时进行过零点判断，具体包括：S21：角度插值计算的公式为b＝a+((c-a)/2)，若c小于a，说明c过了零点，计算时c加上360度，b＝a+((c+360-a)/2)，若c大于a，说明c未过零点b＝a+((c-a)/2)；S22：若计算得到b大于360度，说明b过了零点，计算时b减去360度。

本发明对比现有技术有如下的有益效果：本发明提供的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，通过给激光雷达进行角度插值处理提高扫描分辨率；激光雷达的点云数据结合GNSS实时提供的时间和空间信息，实时描绘和回放无人船周围目标和环境的三维信息，用于自主航行的数据支撑和航行水域三维模型构建；通过姿态传感器和GNSS，校正后的实时校正激光雷达的点云数据和计算障碍物目标信息，适用于水上高速航行姿态动态变化的应用场景；通过实时校正大地平面下的无人艇平台倾斜坐标，提高激光雷达识别目标信息的准确率。

附图说明

图1为本发明实施例中基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法流程图；

图2为本发明实施例中激光雷达点云数据角度插值流程图；

图3为本发明实施例中激光雷达点云数据坐标转换示意图；

图4为本发明实施例中激光雷达点云数据校正应用示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的描述。

图1为本发明实施例中基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法流程图。

请参见图1，本发明实施例的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，包括如下步骤：

S1：获取激光雷达点云的原始数据；

S2：对激光雷达点云数据进行角度插值，补充角度值数据，得到完整的点云数据；

S3：激光雷达实时获取GNSS数据，计算每个点云数据的激光发射时间，对应于发射时间获取每个点云数据的无人船位置信息并存储；

S4：实时获取姿态传感器测得的无人船的姿态信息；

S5：根据姿态传感器测得的无人船的姿态信息，对应于点云数据的激光发射时间对激光雷达补充完整的点云数据进行校正。

具体地，激光雷达点云的原始数据包括激光测距数据和激光角度数据，激光雷达点云的原始数据是每间隔一组激光测距数据对应输出一次激光角度数据，通过角度插值计算给没有输出激光角度数据的激光测距数据补充角度数据。

请参见图2，本发明实施例的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，由于每组激光测距的时间间隔固定，且激光测距的时间间隔内雷达是匀速旋转的，根据第j组激光测距数据的角度a和第j+2组激光测距数据的角度值c计算平均值，得到第j+1组激光测距数据的角度值b，同时进行过零点判断，具体包括：

S21：角度插值计算的公式为b＝a+((c-a)/2)，若c小于a，说明c过了零点，计算时c加上360度，b＝a+((c+360-a)/2)，若c大于a，说明c未过零点b＝a+((c-a)/2)；

S22：若计算得到b大于360度，说明b过了零点，计算时b减去360度。

具体地，步骤S3计算每个点云数据的发射时间具体包括：设激光雷达为N线的激光雷达，激光线的序号S_i(1≤i≤N)，激光发射轮数R_j(1≤j≤M)，两线激光之间的发射时间间隔是Δt，完成一轮激光发射需要时间t，累计M轮激光数据组成一包雷达数据包；设第一个激光发射时间t₀，则精确到每个激光的发射时间为：

t_ij＝t₀+t×R_j+Δt×S_i

具体地，对应于激光的发射时间，获取姿态传感器测得的无人船的姿态信息，所述姿态传感器t时刻测得的无人船的姿态信息，包括方位角α、俯仰角β和横滚角χ。

具体地，通过角度插值t时刻补充完整的点云数据包括水平旋转角度φ、垂直角度ω和实测距离R，所述步骤S5中对t时刻的点云数据的校正具体包括：

S51：通过方位角α校正激光的水平旋转角度φ，校正值为φ+α；校正后的水平旋转角为θ，如果φ+α＞360，则θ＝φ+α-360,否则θ＝φ+α；

S52：进行投影变换，将激光雷达输出的极坐标下的水平旋转角度、垂直角度和距离转化为笛卡尔坐标系下的x,y,z坐标，如图3所示，转换关系如下：

x＝R cos(ω)sin(θ)

y＝R cos(ω)sin(θ)

z＝R sin(ω)

其中R为激光雷达实测距离，ω为激光的垂直角度，即瞬时激光束在YOZ平面内的投影与Y轴的夹角；θ为校正后的激光水平旋转角度，即瞬时激光束在XOY平面内的投影与Y轴的夹角；激光扫描的数据点定义为D(x,y,z)，是数据点的极坐标投影到X、Y、Z轴上的坐标；

S53：通过俯仰角β和横滚角χ校正无人艇平台倾斜，使其投影在大地水平面上，校正后的坐标(x_d,y_d,z_d)，转换关系如下：

x_d＝x cos(β)+z sin(β)

y_d＝x sin(β)sin(χ)+y cos(χ)-z cos(β)sin(χ)

z_d＝-x sin(β)cos(χ)+y sin(χ)+z cos(β)cos(χ)；

由于GNSS安装平台与激光雷达安装平台之间存在偏移，因此，需要进行修正；

S54：通过GNSS安装平台与激光雷达安装平台偏移进行误差修正，修正后的坐标如下：

x_e＝x_d+Δx

y_e＝y_d+Δy

z_e＝z_d+Δz+H

其中Δx、Δy、Δz是激光雷达平台相对于GNSS安装平台在X,Y,Z方向的偏移；H是两个安装平台之间的高度差；

S55：通过激光云校正后的坐标对应于激光发射时间，确定在t时刻，校正后的点云数据，即点云投影在大地平面上的参数：

距离

点云高度H_T＝Z_e。

具体地，对应于点云数据的激光发射时间，获取校正后的点云数据，同时根据激光发射时间对应的无人船位置信息，对无人船周围目标和环境的三维信息进行复现和回放。

如图4所示，校正后的点云数据经过无人船控制中心存储并上传实时点云数据至岸基无人船显示控制系统，实时描绘和回放无人船周围目标和环境的三维信息；同时，校正后的点云数据进行激光雷达分析，经由激光雷达图像处理和目标识别处理，并根据高精度GNSS提供的无人船实时位置信息，计算雷达目标精确的位置信息、尺寸、方向和运动状态信息，上传至航行控制单元，为航线规划和航行避障提供数据支撑。

综上所述，本发明实施例的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，通过给激光雷达进行角度插值处理提高扫描分辨率；激光雷达的点云数据结合GNSS实时提供的时间和空间信息，实时描绘和回放无人船周围目标和环境的三维信息，用于自主航行的数据支撑和航行水域三维模型构建；通过姿态传感器和GNSS，校正后的实时校正激光雷达的点云数据和计算障碍物目标信息，适用于水上高速航行姿态动态变化的应用场景；通过实时校正大地平面下的无人艇平台倾斜坐标，提高激光雷达识别目标信息的准确率。

虽然本发明已以较佳实施例揭示如上，然其并非用以限定本发明，任何本领域技术人员，在不脱离本发明的精神和范围内，当可作些许的修改和完善，因此本发明的保护范围当以权利要求书所界定的为准。

Claims (7)

1.一种基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，包括如下步骤：

S1：获取激光雷达点云的原始数据；

S2：对激光雷达点云数据进行角度插值，补充角度值数据，得到完整的点云数据；

S3：激光雷达实时获取GNSS数据，计算每个点云数据的激光发射时间，对应于发射时间获取每个点云数据的无人船位置信息并存储；

S4：实时获取姿态传感器测得的无人船的姿态信息；

S5：根据姿态传感器测得的无人船的姿态信息，对应于点云数据的激光发射时间对激光雷达补充完整的点云数据进行校正。

2.如权利要求1所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，所述步骤S3计算每个点云数据的发射时间具体包括：

设激光雷达为N线的激光雷达，激光线的序号S_i(1≤i≤N)，激光发射轮数R_j(1≤j≤M)，两线激光之间的发射时间间隔是Δt，完成一轮激光发射需要时间t，累计M轮激光数据组成一包雷达数据包；设第一个激光发射时间t₀，则精确到每个激光的发射时间为：

t_ij＝t₀+t×R_j+Δt×S_i。

3.如权利要求2所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，对应于激光的发射时间，获取姿态传感器测得的无人船的姿态信息，所述姿态传感器T时刻测得的无人船的姿态信息，包括方位角α、俯仰角β和横滚角χ。

4.如权利要求3所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，通过角度插值T时刻补充完整的点云数据包括水平旋转角度φ、垂直角度ω和实测距离R，所述步骤S5中对T时刻的点云数据的校正具体包括：

S51：通过方位角α校正激光的水平旋转角度φ，校正值为φ+α；校正后的水平旋转角为θ，如果φ+α＞360，则θ＝φ+α-360,否则θ＝φ+α；

S52：进行投影变换，将激光雷达输出的极坐标下的水平旋转角度、垂直角度和距离转化为笛卡尔坐标系下的x,y,z坐标，转换关系如下：

x＝Rcos(ω)sin(θ)

y＝Rcos(ω)sin(θ)

z＝Rsin(ω)

其中R为激光雷达实测距离，ω为激光的垂直角度，即瞬时激光束在YOZ平面内的投影与Y轴的夹角；θ为校正后的激光水平旋转角度，即瞬时激光束在XOY平面内的投影与Y轴的夹角；激光扫描的数据点定义为D(x,y,z)，是数据点的极坐标投影到X、Y、Z轴上的坐标；

S53：通过俯仰角β和横滚角χ校正无人艇平台倾斜，使其投影在大地水平面上，校正后的坐标(xd,yd,zd)，转换关系如下：

x_d＝xcos(β)+zsin(β)

y_d＝xsin(β)sin(χ)+ycos(χ)-zcos(β)sin(χ)

z_d＝-xsin(β)cos(χ)+ysin(χ)+zcos(β)cos(χ)；

S54：通过GNSS安装平台与激光雷达安装平台偏移进行误差修正，修正后的坐标如下：

x_e＝x_d+Δx

y_e＝y_d+Δy

z_e＝z_d+Δz+H

其中Δx、Δy、Δz是激光雷达平台相对于GNSS安装平台在X,Y,Z方向的偏移；H是两个安装平台之间的高度差；

S55：通过激光云校正后的坐标对应于激光发射时间，确定在T时刻，校正后的点云数据，即点云投影在大地平面上的参数：

距离

点云高度H_T＝Z_e。

5.如权利要求6所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，对应于点云数据的激光发射时间，获取校正后的点云数据，同时根据激光发射时间对应的无人船位置信息，对无人船周围目标和环境的三维信息进行复现和回放。

6.如权利要求1所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，所述激光雷达点云的原始数据包括激光测距数据和激光角度数据，激光雷达点云的原始数据是每间隔一组激光测距数据对应输出一次激光角度数据，通过角度插值计算给没有输出激光角度数据的激光测距数据补充角度数据。

7.如权利要求6所述的基于姿态传感器和GNSS的无人船激光雷达点云数据校正方法，其特征在于，每组激光测距的时间间隔固定，且激光测距的时间间隔内雷达是匀速旋转的，根据第j组激光测距数据的角度a和第j+2组激光测距数据的角度值c计算平均值，得到第j+1组激光测距数据的角度值b，同时进行过零点判断，具体包括：

S21：角度插值计算的公式为b＝a+((c-a)/2)，若c小于a，说明c过了零点，计算时c加上360度，b＝a+((c+360-a)/2)，若c大于a，说明c未过零点b＝a+((c-a)/2)；

S22：若计算得到b大于360度，说明b过了零点，计算时b减去360度。

Images