阅读说明：本技术 一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法 (Method for correcting laser radar point cloud data motion distortion based on integrated navigation system ) 是由 *** 叶扬青 王在满 何杰 方龙羽 梁展豪 何思禹 许�鹏 刘顺财 于 2019-12-03 设计创作，主要内容包括：本发明公开一种基于组合导航系统矫正激光雷达点云运动畸变的方法,利用时间同步器,将组合导航系统的经纬度、高程、姿态角、速度、加速度、角速度、角加速度和时间戳与激光雷达点云数据、时间戳进行融合,利用旋转和平移,将一帧激光雷达点云数据矫正至相同时间点,以矫正激光雷达点云数据运动畸变,稳定、有效地减小由无人船运动导致的激光雷达点云数据误差。本发明方法能为后续的无人船障碍物检测、避障等提供更加精确的激光雷达点云数据。(The invention discloses a method for correcting laser radar point cloud motion distortion based on a combined navigation system, which fuses longitude and latitude, elevation, attitude angle, speed, acceleration, angular velocity, angular acceleration and time stamp of the combined navigation system with laser radar point cloud data and time stamp by using a time synchronizer, corrects a frame of laser radar point cloud data to the same time point by using rotation and translation so as to correct laser radar point cloud data motion distortion, and stably and effectively reduces laser radar point cloud data errors caused by unmanned ship motion. The method can provide more accurate laser radar point cloud data for subsequent unmanned ship obstacle detection, obstacle avoidance and the like.)

一种基于组合导航系统矫正激光雷达点云数据运动畸变的 方法

技术领域

本发明涉及无人船自动驾驶技术领域，具体涉及激光雷达点云数据运动畸变的矫正，更具体涉及基于组合导航系统矫正激光雷达点云数据运动畸变的方法。

背景技术

无人船中实现环境感知的传感器有很多，激光雷达、红外传感器、超声波传感器、声呐、单目相机、双目相机等等。其中，激光雷达擅长近距离障碍物检测，深度分辨能力和精度高，不收外界光照等影响。在各种水域环境中表现的更加稳定精确。因此，激光雷达在无人船自动驾驶领域，作为检测障碍物的主要手段备受关注。激光雷达通过发射激光至障碍物，利用障碍物反射回来的信号，计算距离和反射强度，生成点云地图。

但是，激光雷达是利用内部的旋转电机，带动激光发射器。生成激光雷达点云数据存在着一个过程，而不是同一时刻内的激光点。比如中国专利申请CN109975792A公开一种基于融合多种传感器矫正多线激光雷达点云运动畸变的方法，其是根据全局坐标系和激光雷达某帧起始点坐标系之间的旋转关系将位移畸变变换至起始点坐标系，再将激光雷达局部坐标系下的点变换至起始点坐标系，得到了在某帧起始点坐标系下的点云全部的点和相应的位移畸变。通过在三维点坐标上补偿位移畸变来矫正点云数据。为后续的目标跟踪、路径规划、地图构建、物体识别等算法提供更为精确的多线激光雷达的点云数据。该文献虽然涉及矫正多线激光雷达点云运动畸变的方法，但主要针对无人驾驶汽车，需要计算四个车轮的速度等。而无人船与激光雷达为刚性连接，导致激光雷达会随着无人船旋转和平移，使激光雷达接受到的数据不为相同位置和姿态下的数据，不能统一到一个坐标系下。目前，面对这个问题通常不对激光雷达点云数据进行矫正或只认为是匀速运动。这在无人车自动驾驶领域的影响相对小点，但是在无人船自动驾驶领域会产生较大测量误差。无人车自动驾驶领域，无人车的横滚角和俯仰角很小，而在无人船自动驾驶领域，无人船的姿态角的变化幅度很大，对激光雷达的测量误差很有影响。

因此，研究出可以稳定、有效地减小由无人船运动导致的激光雷达点云数据误差。为后续的无人船障碍物检测、避障等提供更加精确的激光雷达点云数据。

发明内容

针对现有技术中存在的技术问题，本发明的目的是：提供一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法，减小激光雷达由于无人船运动导致的激光雷达点云数据误差。

为了达到上述目的，本发明采用如下技术方案：

一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法，所述激光雷达点云数据包括点云坐标和该帧时间戳，其特征在于，按如下步骤进行：

步骤S1，利用时间同步器读取同一时刻的组合导航系统数据与激光雷达点云数据。然后进入步骤S2；

步骤S2，计算激光雷达点云数据帧头激光点水平角度、激光雷达点云数据帧尾激光点水平角度。然后进入步骤S3；

步骤S3，计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后进入步骤S4；

步骤S4，判断上一时刻是否有组合导航系统数据与激光雷达点云数据，若是，进入S5，若否，进入步骤S6；

步骤S5，利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间，并用上一时刻和当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正；

步骤S6，利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间，并用当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正。

作为一种优选，其特征在于步骤S1的组合导航系统数据包括经纬度、高程、姿态角、速度、加速度、角速度、角加速度和时间戳。

作为一种优选，其特征在于步骤S1的时间同步器，利用激光雷达点云数据中的时间戳和组合导航系统数据中的时间戳进行对比配对，得到同一时刻的两组数据。

作为一种优选，其特征在于步骤S2的激光雷达点云数据帧头激光点水平角度为激光雷达点云数据中第一个激光点的x值y值计算的角度

计算公式：

其中：θ为激光雷达点云数据帧头激光点水平角度。x、y为激光雷达点云数据帧头激光点x轴坐标值、y轴坐标值。

激光雷达点云数据帧尾激光点水平角度为帧头水平角角度水平加上角分辨率。水平角分辨率为激光雷达固定参数，并且激光雷达内部旋转电机为顺时针旋转。

作为一种优选，其特征在于步骤S3的计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。由于激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间不能直接测量得到，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点之间的水平夹角及激光雷达内部旋转电机转动频率计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。激光雷达点云数据当前激光点至帧尾激光点所需时间计算方式，按如下步骤进行：

步骤S3.1，计算激光雷达内部旋转电机转动1°所需时间

计算公式：

其中：Δt₁为激光雷达内部旋转电机转动1°所需时间。然后进入步骤S3.2；

步骤S3.2计算激光雷达点云数据当前激光点水平角度。

计算公式：

其中：θ为激光雷达点云数据当前激光点水平角度。x、y为激光雷达点云数据当前激光点x轴坐标值、y轴坐标值。然后进入S3.3；

步骤S3.3计算激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角

计算公式：

Δθ＝360°+θ₁-θ₀

其中：Δθ为激光雷达点云数据的当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角。θ₁为激光雷达点云数据当前激光点水平角度。θ₀为激光雷达点云数据帧尾激光点水平角度。然后进入S3.4；

步骤S3.4，计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间

计算公式：

Δt＝Δt₁*Δθ

其中：Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。Δt₁为激光雷达内部旋转电机转动1°所需时间。Δθ为激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角。

作为一种优选，其特征在于步骤S5用上一时刻和当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正，具体步骤如下：

步骤S5.1，根据前一时刻和当前时刻组合导航系统的加速度和角加速度，计算该时间段内的加加速度(急动速度)和角加加速度

计算公式：

其中：j为无人船加加速度或角加加速度。a₁为当前时刻无人船加速度或角加速度。a₀为前一时刻无人船加速度或角加速度。t₁为当前时刻时间。t₀为前一时刻时间。然后进入步骤S5.2；

步骤S5.2，将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点。然后转到步骤S5.3；

步骤S5.3，根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量。利用已计算得到的无人船加加速度和角加加速度，以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。

作为一种优选，其特征在于步骤S5.3利用已计算得到的无人船加加速度和角加加速度，以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点。具体步骤如下：

步骤S5.3.1，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度和加加速度，计算x轴、y轴、z轴的平移量

计算公式：

其中：

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量。v_x、v_y、v_z为当前时刻无人船x轴方向、y轴方向、z轴方向速度。a_x、a_y、a_z为当前时刻无人船x轴方向、y轴方向、z轴方向加速度。j_x、j_y、j_z为当前时刻无人船x轴方向、y轴方向、z轴方向加加速度。Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后转到步骤S5.3.2；

步骤S5.3.2，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度和角加加速度，计算激光雷达点云数据当前激光点无人船姿态角。

计算公式：

其中：θ为激光雷达点云数据当前激光点无人船姿态角。θ₁为激光雷达点云数据帧尾激光点无人船姿态角。ω为无人船角速度。a为无人船角加速度。j为无人船角加加速度。分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度，计算无人船在激光雷达点云数据当前激光点下无人船姿态角。然后转到步骤S5.3.3；

步骤S5.3.3，利用无人船姿态角，计算无人船x轴、y轴、z轴的旋转量

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_roll为无人船z轴方向旋转量。θ_x、θ_y、θ_z为无人船x轴、y轴、z轴旋转角度。然后转到步骤S5.3.4；

步骤S5.3.4，利用无人船x轴、y轴、z轴的旋转量，将激光雷达点云数据当前激光点旋转至水平姿态下

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_roll为无人船z轴方向旋转量。

为激光雷达点云数据当前激光点。

为水平姿态下的激光点。转到步骤S5.3.5；

步骤S5.3.5，利用x轴、y轴、z轴的平移量，将位置由水平姿态下的激光点平移到全局坐标系下

计算公式：

其中：

为激光雷达点云数据去除运动畸变后的激光点。

为水平姿态下的激光点。

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。

作为一种优选，其特征在于S6用当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正，具体步骤如下：

步骤S6.1，将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点。然后转到S6.2；

步骤S6.2，根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量。利用已知无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。

作为一种优选，其特征在于步骤S6.2利用无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点。具体步骤如下：

步骤S6.2.1，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度，计算x轴、y轴、z轴的平移量

计算公式：

其中：

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量。v_x、v_y、v_z为当前时刻无人船x轴方向、y轴方向、z轴方向速度。a_x、a_y、a_z为当前时刻无人船x轴方向、y轴方向、z轴方向加速度。Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后转到步骤S6.2.2；

步骤S6.2.2，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度，计算激光雷达点云数据当前激光点无人船姿态角

计算公式：

其中：θ为激光雷达点云数据当前激光点无人船姿态角。θ₁为激光雷达点云数据帧尾激光点无人船姿态角。ω为无人船角速度。a为无人船角加速度。j为无人船角加加速度。分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度，计算无人船在激光雷达点云数据当前激光点下无人船姿态角。然后转到S6.2.3。

步骤S6.2.3，利用无人船姿态角，计算无人船x轴、y轴、z轴的旋转量

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_piych为无人船z轴方向旋转量。θ_x、θ_y、θ_z为无人船x轴、y轴、z轴旋转角度。转到S6.2.4；

步骤S6.2.4，利用无人船x轴、y轴、z轴的旋转量，将激光雷达点云数据当前激光点旋转至水平姿态下

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_roll为无人船z轴方向旋转量。

为激光雷达点云数据当前激光点。

为水平姿态下的激光点。转到S6.2.5；

步骤S6.2.5，利用x轴、y轴、z轴的平移量，将位置由水平姿态下的激光点平移到全局坐标系下

计算公式：

其中：

为激光雷达点云数据去除运动畸变后的激光点。为水平姿态下的激光点。

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。

此外，本发明提供一种基于组合导航系统矫正激光雷达点云数据运动畸变矫正的系统，其包括实现上述方法的流程模块，例如可以通过编制计算机程序来实现，达到自动化计算的目的。

总的说来，本发明优点：通过组合导航系统数据与激光雷达点云数据进行融合，利用速度加速度信息，将不同时间记录的点云数据矫正统一到全局坐标系下，得到更加精准有效的激光雷达点云数据。

附图说明

图1是一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法流程图。

图2是计算激光雷达点云数据当前激光点至帧尾激光点所需时间流程图。

图3基于运动信息赋予激光雷达点云数据矫正流程图，其中，图3-1当上一时刻有组合导航系统数据与激光雷达点云数据时的矫正流程图；图3-2当上一时刻没有组合导航系统数据与激光雷达点云数据时的矫正流程图。

图4激光雷达点云数据当前激光点转换到全局坐标系流程图，其中图4-1是对应图3-1时的激光雷达点云数据当前激光点转换到全局坐标系流程图；

图4-2是对应图3-1时的激光雷达点云数据当前激光点转换到全局坐标系流程图。

图5是激光雷达点云数据获得的湖泊图片。

图6是原始激光雷达点云数据。

图7是基于组合导航系统矫正激光雷达点云运动畸变后的点云数据。

具体实施方式

下面将结合具体实施方式来对本发明做进一步详细的说明。

实施例一：基于组合导航系统矫正激光雷达点云数据运动畸变的整体步骤

如图1所示，一种基于组合导航系统矫正激光雷达点云数据运动畸变的方法，其特征在于，按如下步骤进行：

步骤S1，利用时间同步器读取同一时刻的组合导航系统数据与激光雷达点云数据.然后进入步骤S2；

步骤S2，计算激光雷达点云数据帧头激光点水平角度、激光雷达点云数据帧尾激光点水平角度。然后进入步骤S3；

步骤S3，计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后进入S4；

步骤S4，判断上一时刻是否有组合导航系统数据与激光雷达点云数据，若是，进入S5，若否，进入步骤S6；

步骤S5，利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间，并用上一时刻和当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正；

步骤S6，利用已计算得到的激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间，并用当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正。

所述组合导航系统数据包括经纬度、高程、姿态角、速度、加速度、角速度、角加速度和时间戳。激光雷达点云数据包括点云坐标和该帧时间戳。

所述时间同步器，利用激光雷达点云数据中的时间戳和组合导航系统数据中的时间戳进行对比，配对，得到同一时刻的两组数据。

所述激光雷达点云数据帧头激光点水平角度为激光雷达点云数据中第一个激光点的x值y值计算的角度

计算公式：

其中：θ为激光雷达点云数据帧头激光点水平角度。x、y为激光雷达点云数据帧头激光点x轴坐标值、y轴坐标值。

激光雷达点云数据帧尾激光点水平角度为帧头水平角角度水平加上角分辨率。水平角分辨率为激光雷达固定参数。激光雷达内部旋转电机为顺时针旋转。

实施例二：计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间

如图2所示，步骤S3的计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。由于激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间不能直接测量得到，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点之间的水平夹角及激光雷达内部旋转电机转动频率计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。激光雷达点云数据当前激光点至帧尾激光点所需时间计算方式，按如下步骤进行：

步骤S3.1，计算激光雷达内部旋转电机转动1°所需时间

计算公式：

其中：Δt₁为激光雷达内部旋转电机转动1°所需时间。然后进入S3.2；

步骤S3.2计算激光雷达点云数据当前激光点水平角度

计算公式：

其中：θ为激光雷达点云数据当前激光点水平角度。x、y为激光雷达点云数据当前激光点x轴坐标值、y轴坐标值。然后进入S3.3；

步骤S3.3计算激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角

计算公式：

Δθ＝360°+θ₁-θ₀

其中：Δθ为激光雷达点云数据的当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角。θ₁为激光雷达点云数据当前激光点水平角度。θ₀为激光雷达点云数据帧尾激光点水平角度。然后进入步骤S3.4；

步骤S3.4，计算激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。

计算公式：

Δt＝Δt₁*Δθ

其中：Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。Δt₁为激光雷达内部旋转电机转动1°所需时间。Δθ为激光雷达点云数据当前激光点水平角度与激光雷达点云数据帧尾激光点水平角度之间的夹角。

实施例三：运动信息赋予激光雷达点云数据进行矫正流程

当上一时刻有组合导航系统数据与激光雷达点云数据时，如图3-1所示，步骤S5用上一时刻和当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正，具体步骤如下：

步骤S5.1，根据前一时刻和当前时刻组合导航系统的加速度和角加速度，计算该时间段内的加加速度(急动速度)和角加加速度。

计算公式：

其中：j为无人船加加速度或角加加速度。a₁为当前时刻无人船加速度或角加速度。a₀为前一时刻无人船加速度或角加速度。t₁为当前时刻时间。t₀为前一时刻时间。然后进入步骤S5.2；

步骤S5.2，将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点。然后转到步骤S5.3；

步骤S5.3，根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量。利用已计算得到的无人船加加速度和角加加速度，以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。

当上一时刻没有组合导航系统数据与激光雷达点云数据时，如图3-2所示，步骤S6用当前时刻组合导航系统数据，将运动信息赋予激光雷达点云数据进行矫正，具体步骤如下：

步骤S6.1，将激光雷达点云数据帧尾激光点的坐标原点作为全局坐标系坐标原点。然后转到步骤S6.2；

步骤S6.2，根据激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间计算平移量和旋转量。利用已知无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据当前激光点转换到全局坐标系下。

实施例四：激光雷达点云数据当前激光点转换到全局坐标系流程图。

对应于前述当上一时刻有组合导航系统数据与激光雷达点云数据时，如图4-1所示，步骤S5.3利用已计算得到的无人船加加速度和角加加速度，以及无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点。具体步骤如下：

步骤S5.3.1，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度和加加速度，计算x轴、y轴、z轴的平移量

计算公式：

其中：

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量。v_x、v_y、v_z为当前时刻无人船x轴方向、y轴方向、z轴方向速度。a_x、a_y、a_z为当前时刻无人船x轴方向、y轴方向、z轴方向加速度。j_x、j_y、j_z为当前时刻无人船x轴方向、y轴方向、z轴方向加加速度。Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后转到S5.3.2；

步骤S5.3.2，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度和角加加速度，计算激光雷达点云数据当前激光点无人船姿态角

计算公式：

其中：θ为激光雷达点云数据当前激光点无人船姿态角。θ₁为激光雷达点云数据帧尾激光点无人船姿态角。ω为无人船角速度。a为无人船角加速度。j为无人船角加加速度。分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度，计算无人船在激光雷达点云数据当前激光点下无人船姿态角。然后转到步骤S5.3.3；

步骤S5.3.3，利用无人船姿态角，计算无人船x轴、y轴、z轴的旋转量

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_roll为无人船z轴方向旋转量。θ_x、θ_y、θ_z为无人船x轴、y轴、z轴旋转角度。然后转到步骤S5.3.4；

步骤S5.3.4，利用无人船x轴、y轴、z轴的旋转量，将激光雷达点云数据当前激光点旋转至水平姿态下

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_roll为无人船z轴方向旋转量。为激光雷达点云数据当前激光点。

为水平姿态下的激光点。转到步骤S5.3.5；

步骤S5.3.5，利用x轴、y轴、z轴的平移量，将位置由水平姿态下的激光点平移到全局坐标系下

计算公式：

其中：

为激光雷达点云数据去除运动畸变后的激光点。

为水平姿态下的激光点。

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。

对应于前述当上一时刻没有组合导航系统数据与激光雷达点云数据时，如图4-2所示，步骤S6.2利用无人船角速度、速度、角加速度、加速度与相对应的时间将激光雷达点云数据转换到以激光雷达点云数据帧尾激光点的原点作为全局坐标系下的原点。具体步骤如下：

步骤S6.2.1，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船速度、加速度，计算x轴、y轴、z轴的平移量

计算公式：

其中：为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需要的平移量。v_x、v_y、v_z为当前时刻无人船x轴方向、y轴方向、z轴方向速度。a_x、a_y、a_z为当前时刻无人船x轴方向、y轴方向、z轴方向加速度。Δt为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间。然后转到S6.2.2；

步骤S6.2.2，利用激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点所需时间和无人船角速度、角加速度，计算激光雷达点云数据当前激光点无人船姿态角

计算公式：

其中：θ为激光雷达点云数据当前激光点无人船姿态角。θ₁为激光雷达点云数据帧尾激光点无人船姿态角。ω为无人船角速度。a为无人船角加速度。j为无人船角加加速度。分别利用无人船x轴、y轴、z轴上的角速度、角加速度、角加加速度，计算无人船在激光雷达点云数据当前激光点下无人船姿态角。然后转到S6.2.3。

步骤S6.2.3，利用无人船姿态角，计算无人船x轴、y轴、z轴的旋转量

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_pitch为无人船z轴方向旋转量。θ_x、θ_y、θ_z为无人船x轴、y轴、z轴旋转角度。转到S6.2.4；

步骤S6.2.4，利用无人船x轴、y轴、z轴的旋转量，将激光雷达点云数据当前激光点旋转至水平姿态下

计算公式：

其中：R_yaw为无人船z轴方向旋转量。R_pitch为无人船y轴方向旋转量。R_pitch为无人船z轴方向旋转量。

为激光雷达点云数据当前激光点。为水平姿态下的激光点。转到S6.2.5；

步骤S6.2.5，利用x轴、y轴、z轴的平移量，将位置由水平姿态下的激光点平移到全局坐标系下

计算公式：

其中：

为激光雷达点云数据去除运动畸变后的激光点。为水平姿态下的激光点。

为激光雷达点云数据当前激光点至激光雷达点云数据帧尾激光点的x轴、y轴、z轴的平移量。

实施例五：应用实施例

下面以实际应用例子阐述本发明矫正方法的适用性。如图5所示为激光雷达点云数据获得的湖泊图片，其获得数据时的无人船相关参数为：当前时刻无人船的速度v_x＝1.5247、v_y＝2.38736、v_z＝0.048667，当前时刻无人船的角速度ω_x＝-1.48657、ω_y＝2.42679、ω_z＝25.0321,当前时刻无人船的加速度a_x＝0.474528、a_y＝-0.3943、a_z＝0.0441112，当前时刻无人船的角加速度a_x＝-4.54753、a_y＝0.39428、a_z＝0.038669,当前时刻无人船的加加速度j_x＝0.416279、j_y＝0.0002、a_z＝0.054422，当前时刻无人船的角加加速度j_x＝-44.1843、j_y＝-15.2908、j_z＝-12.0028；前一时刻无人船的加速度a_x＝0.4329001、a_y＝-0.39432、a_z＝0.038669，前一时刻无人船的角加速度a_x＝-0.1291、a_y＝1.92336、a_z＝1.238949，当前时刻无人船的加加速度j_x＝0.416279、j_y＝0.0002、a_z＝0.054422，当前时刻无人船的角加加速度j_x＝-44.1843、j_y＝-15.2908、j_z＝-12.0028。

图5中白色圆圈中的为图6和图7需要进行比较的部分，其中图6是原始激光雷达点云数据，图7是基于组合导航系统矫正激光雷达点云运动畸变后的点云数据，由于上一时刻的数据，因此步骤S4之后进入的是步骤S5，最终获得的。

由于从整体来看图6原始激光雷达点云数据与图7基于组合导航系统矫正激光雷达点云运动畸变后的点云数据的形状大致相同。因此，采用比较平行度的方式进行性能的比较。比较图5中白色圆圈内的两个建筑物，从图5中可以看出两个建筑物表面相互平行。而图6的两个白色圆圈内的点云数据表示的是图5中的两个表面相互平行的建筑物，这两个建筑物表面之间明显不相互水平。而图7中的两个建筑物表面之间相互平行。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。