CN111915677B - 一种基于三维点云特征的船舶位姿估计方法 - Google Patents

Abstract

本发明提供一种基于三维点云特征的船舶位姿估计方法，首先通过从6个方向获取目标船的三维点云数据，采用拼接方式获得目标船的完整点云库。构建目标船的点云模板库。计算待配准目标船舶点云的ISS3D特征点及其特征直方图，得到模板点云库中的特征点与待配准点云库中的对应特征点重合的初始坐标变换矩阵，实现点云在全局范围内的快速粗配准。采用ICP点云配准算法，在上步粗配准的基础上进行点云的精准配准，获得实现点云精准配准的坐标变换矩阵。最后利用精准坐标变换矩阵与点云模板库中船舶的初始位姿信息，完成对目标船进行六自由度的位姿估计。本发明提出基于点云特征的船舶位姿估计算法实验，通过实验数据对比，验证本发明的有效性。

Description

一种基于三维点云特征的船舶位姿估计方法

技术领域

本发明涉及一种基于三维点云特征的船舶位姿估计方法，属于信号处理技术方法。

背景技术

三维激光雷达是用于获取周边信息的非常重要的传感器，具有测量精度高、获得的点云密度高、速度快、成本低等优势，在机器人控制、汽车自动驾驶、无人控制流水线等领域发挥重要的作用。尤其对于船载激光雷达，在开阔海域、狭窄水道、进出港航行过程中获得目标船的三维点云信息，并在此基础上利用点云信息对目标船的位置和姿态开展精准的估计。如果对目标船点云的位姿估计误差较大，不仅带来后续处理的误差较大，甚至会造成撞船事故，为此需要一种可根据三维点云特征来准确估计目标船位置和姿态的方法。

目前典型的三维点云目标位姿估计方法主要基于深度学习和基于视点特征，深度学习具有强大的训练和提取点云特征的能力，基于视点特征的方法可根据视点状态来估计扫描后点云的位置和姿态。如肖仕华在文献《基于深度学习的三维点云头部姿态估计》中，提出一种新的深度学习框架HPENet用于对人体头部姿态的估计算法，并在公共数据集BiwiKinect Head Pose上进行测试，验证了该方法在准确度方面具有更好的性能；再比如张彪在文献《基于视点特征直方图的激光点云模型的位姿估计》中，提出一种目标位姿估计算法，利用激光雷达扫描构成三维点云，建立特征数据库计算视点特征直方图，通过实验证明该方法具有很强的鲁棒性和准确性。以上文献在三维点云目标位姿估计过程中存在如下问题：一是基于深度学习的估计方法需要大量的样本对网络进行训练，收集足量的船舶训练样本具有操作难度大且成本高的问题；二是基于视点特征的方法需要有视点特征图组成的特征数据库作为基础，当特征数据库中待测船舶的视点特征图不完整时，不能提供精确的位姿估计；三是传统的基于视点特征的点云配准过程计算量大，运行时间长。

本发明利用激光雷达获取目标船的三维点云数据，通过对基础点云数据处理得到船舶完整点云数据，通过计算求解点云特征点和特征直方图来建立目标船的完整点云模板库，先后通过将待处理点云与模板库中点云的粗匹配(SAC-IA方法)和精匹配(ICP方法)方式，实现对目标船的位置和姿态的快速精确估计。

发明内容

本发明的目的是提供一种利用目标船三维点云来准确估计其位置和姿态的方法，可以根据船载激光雷达扫描得到的三维点云数据，通过粗匹配和精匹配方式，快速有效判定周围目标船的位置和姿态信息。

本发明的目的是这样实现的：步骤如下：

步骤一：获取目标船点云数据，拼接完整点云并计算ISS3D特征点；

步骤二：计算ISS3D特征点的快速点特征直方图(FPFH)特征描述子，建立目标船点云模板库；

步骤三：基于SAC-IA的点云粗配准；

步骤四：基于ICP的点云精细配准及位姿估计；

利用精准坐标变换矩阵与点云模板库中船舶的初始位姿信息，完成对目标船进行六自由度的位姿估计。

本发明还包括这样一些结构特征：

1.步骤一具体包括：采用点云拼接方法获取船舶的完整点云，从前、后、左、右、上、下6个方向获得船舶点云数据，再将点云拼接得到船舶完整点云数据，对每一种船舶的点云提取ISS3D特征点p_f＝[x,y,z]，具体提取方法如下所示：

步骤1.1：对点云中每个点p_i都设置一个局部坐标系，并设置搜索半径r；

步骤1.2：获取点云每个点p_i在半径r内的所有点，权重计算公式如下所示：

步骤1.3：计算各点p_i的协方差矩阵，如下所示：

步骤1.4：对各点p_i的协方差矩阵进行特征值求解，并从大到小排序，得到

步骤1.5：通过特征值判断是否为关键点，选择阈值ε₁,ε₂，当且/>时，将这个点判断为角点关键点。

2.步骤二具体包括：

步骤2.1：对每一个特征点p_q，寻找与它距离小于r的邻域点p_ki，计算特征点与邻域点之间的三元组(α,φ,θ)；

步骤2.2：对于所有与特征点p_q距离小于r的邻域点p_ki，都按照上一步骤的方法，以这些邻域点p_ki为中心搜索距离小于r的邻域点，计算中心点与邻域点的三元组(α,φ,θ)；

步骤2.3：将特征点p_q的三元组(α,φ,θ)与加权的邻域点p_ki的三元组(α,φ,θ)进行统计，得到最终的快速点特征直方图描述子为：

上式中ω_ki为权重值，其与关键点p_q与邻域点p_ki之间距离有关；

将ISS3D特征点p_f＝[x,y,z]与快速点特征直方图特征描述子V_FPFH合并为一个特征向量E_i＝[x,y,z,V_FPFH]，以此来描述一个特征点，每种类型的船舶点云用这些特征向量的集合进行描述M_j＝[E₁ E₂ … E_n]，并将它们存在点云模板数据库中；最终获得所有类型船舶的点云特征模板库[M₁ M₂ … M_n]。

3.步骤三包括：

步骤3.1：从待处理点云中提取ISS3D特征点，计算这些特征点对应的FPFH特征描述子，从所有特征点中选择两两之间距离大于预定阈值d的n个特征点，以此作为采样点；

步骤3.2：在已建立的模板库中，逐个查找与上一步骤中获得的采样点有相似FPFH特征的关键点；

步骤3.3：将两片点云之间位置关系表示为如下所示的坐标变换形式：

上式中(x_i,y_i,z_i)为已构建的模板库中的关键点，(x′_i,y′_i,z′_i)为待处理点云数据中的采样点，M为变换矩阵；

步骤3.4：给出函数对上一步骤中变换矩阵M的准确度进行参数化表示，其中H(l_i)如下式所示：

上式中：ml为设定的阈值，l_i为经坐标变换后的第i组对应点对的距离，越小，则表示M的准确度越高；

步骤3.5：重复执行k次步骤3.3和步骤3.4，每次随机选择不同的对应点云计算变换矩阵M，并加以评估，选择k次选择中准确度最好的变换矩阵M，作为最终的变换矩阵。

4.步骤四具体是：

采用改进型ICP算法，在点云特征点粗配准的基础上，实现待处理点云与模板库点云数据的特征点精配准，并获得点云之间变换矩阵M，坐标变换矩阵M由旋转矩阵R^*与位移矩阵T^*构成，公式为：

通过这两个矩阵，并结合点云模板库中目标船的位置和姿态信息，得到当前时刻激光雷达扫描到的目标船的位置和姿态的估计信息，当坐标按照x、y、z顺序进行旋转时，旋转矩阵与平移矩阵的表达式可用下式所示的R_xyz(α,β,γ)和T_xyz(x,y,z)表示：

则最终估计的船舶旋转姿态角度(α,β,γ)和平移位置信息(x,y,z)计算公式如下式所示：

5.步骤3.3具体为：

在n个采样点中随机选择4个点，对于其中每一个点在与之相似的多个特征点中随机选择一个点作为对应点，通过这4对点解得仿射变换矩阵M，得到两片点云之间的坐标变换关系，并给出在计算仿射变换矩阵前对所有的对应点进行初步筛选的判定条件为：

上式中：d_pn为待处理点云中第n个特征点，d_qn为模板库中与之对应的特征点，δ表示对应点匹配度，δ数值越小表示匹配度越高，当对应点对完全匹配时δ＝0。

与现有技术相比，本发明的有益效果是：本发明避免了获取大量船舶点云样本导致的操作难度大且成本高问题，解决了点云模板不完整导致的估计精度低的问题，同时提高了计算速度，缩短了运行时间。首先，本发明方法不需要像深度学习需要大量的样本数据，而只需要目标船模型及其点云模板即可，节省大量的存储空间；其次，本发明与基于视点特征的方法相比，不需要额外的视点数据，并通过拼接构建完整点云模板库提高了估计精度，更具优越性。再次，在本发明中，粗配准实现两片点云在整体上快速配准，精配准负责提高配准精度，通过粗配准与精细配准相结合的方法并进行优化，提高了点云配准精度和效率。

附图说明

图1本发明船舶三维点云位姿估计流程图

图2(a)-图2(f)分别为前、左、后、右、上、下六个方向的点云数据图像

图3(a)为目标船模型图，图3(b)为目标船完整点云图

图4完整点云与场景点云特征点提取效果图

图5(a)目标船1配准前的图，图5(b)目标船1配准后的图，图5(c)目标船2配准前的图，图5(d)目标船2配准后的图；

图6是目标船位姿估计值与真实值数据表。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1至图6，本发明提出的一种船舶三维点云的位姿估计方法的流程如下图所示。本发明首先通过从6个方向获取目标船的三维点云数据，采用拼接方式获得目标船的完整点云库。利用上步获得的完整点云计算获得ISS3D特征点及其特征直方图，构建目标船的点云模板库。计算待配准目标船舶点云的ISS3D特征点及其特征直方图，采用SAC-IA点云配准算法，借助特征直方图将点云模板库与待处理点云库的特征点一一对应，得到模板点云库中的特征点与待配准点云库中的对应特征点重合的初始坐标变换矩阵，实现点云在全局范围内的快速粗配准。采用ICP点云配准算法，在上步粗配准的基础上进行点云的精准配准，获得实现点云精准配准的坐标变换矩阵。最后利用精准坐标变换矩阵与点云模板库中船舶的初始位姿信息，完成对目标船进行六自由度的位姿估计。本发明提出基于点云特征的船舶位姿估计算法实验，通过实验数据对比，验证本发明方法的有效性。

本发明的步骤如下：

步骤1.获取目标船点云数据，拼接完整点云并计算ISS3D特征点

本发明采用点云拼接方法获取船舶的完整点云，具体实现方法是从前、后、左、右、上、下6个方向获得船舶点云数据，再将点云拼接得到船舶完整点云数据，对每一种船舶的点云提取ISS3D特征点p_f＝[x,y,z]，具体提取方法如下所示：

Step 1：对点云中每个点p_i都设置一个局部坐标系，并设置搜索半径r；

Step 2：获取点云每个点p_i在半径r内的所有点，权重计算公式如下所示：

Step 3：计算各点p_i的协方差矩阵，如下所示：

Step 4：对各点p_i的协方差矩阵进行特征值求解，并从大到小排序，得到

Step 5：通过特征值判断是否为关键点，选择阈值ε₁,ε₂，当且/>时，将这个点判断为角点关键点。

步骤2.计算ISS3D特征点的快速点特征直方图(FPFH)特征描述子，建立目标船点云模板库

计算ISS3D特征点p_f＝[x,y,z]的快速点特征直方图(FPFH)特征描述子V_FPFH，计算步骤如下所示：

Step 1：对每一个特征点p_q，寻找与它距离小于r的邻域点p_ki，然后计算特征点与邻域点之间的三元组(α,φ,θ)。

Step 2：对于所有与特征点p_q距离小于r的邻域点p_ki，都按照上一步骤的方法，以这些邻域点p_ki为中心搜索距离小于r的邻域点，然后计算中心点与邻域点的三元组(α,φ,θ)。

Step 3：将特征点p_q的三元组(α,φ,θ)与加权的邻域点p_ki的三元组(α,φ,θ)进行统计，得到最终的快速点特征直方图(FPFH)描述子，如下式所示：

上式中ω_ki为权重值，其与关键点p_q与邻域点p_ki之间距离有关。

将ISS3D特征点p_f＝[x,y,z]与快速点特征直方图(FPFH)特征描述子V_FPFH合并为一个特征向量E_i＝[x,y,z,V_FPFH]，以此来描述一个特征点，每种类型的船舶点云用这些特征向量的集合进行描述M_j＝[E₁ E₂ … E_n]，并将它们存在点云模板数据库中。最终获得所有类型船舶的点云特征模板库[M₁ M₂ … M_n]。

步骤3.基于SAC-IA的点云粗配准方法

将激光雷达扫描获得的原始点云数据进行滤波、分割后，获得一系列的待处理点云数据，对这些点云数据提取ISS3D特征点和FPFH特征描述子，并进一步处理得到特征向量，利用SAC-IA点云配准方法将获得的特征向量与模板库内特征开展粗配准工作，具体实现方法如下步骤所示：

Step 1：从待处理点云中提取ISS3D特征点，然后计算这些特征点对应的FPFH特征描述子，从所有特征点中选择两两之间距离大于预定阈值d的n个特征点，以此作为采样点。

Step 2：在已建立的模板库中，逐个查找与上一步骤中获得的采样点有相似FPFH特征的关键点，此时模板库中一定会有多个特征点与采样点特征相似，通过下一步骤计算两片点云变换矩阵方式确定寻找关键点。

Step 3：将两片点云之间位置关系表示为如下所示的坐标变换形式：

上式中(x_i,y_i,z_i)为已构建的模板库中的关键点，(x_i′,y_i′,z_i′)为待处理点云数据中的采样点，M为变换矩阵。

在n个采样点中随机选择4个点，对于其中每一个点在与之相似的多个特征点中随机选择一个点作为对应点，通过这4对点解得仿射变换矩阵M，得到两片点云之间的坐标变换关系。为了提高随机选择对应点的准确性，本发明在计算仿射变换矩阵前对所有的对应点进行初步筛选，初步筛选判定条件如下式所示：

上式中：d_pn为待处理点云中第n个特征点，d_qn为模板库中与之对应的特征点，δ表示对应点匹配度，δ数值越小表示匹配度越高，当对应点对完全匹配时δ＝0。本发明设置一个阈值t，当δ小于阈值t时，即表示这组对应点对是符合要求的，可利用它们计算仿射变换矩阵M。

Step 4：本发明设计一个函数对上一步骤中变换矩阵M的准确度进行参数化表示，其中H(l_i)如下式所示：

上式中：ml为设定的阈值，l_i为经坐标变换后的第i组对应点对的距离。在本发明中，越小，则表示M的准确度越高。

Step 5：重复执行k次Step 3和Step 4，每次随机选择不同的对应点云计算变换矩阵M，并加以评估，选择k次选择中准确度最好的变换矩阵M，作为最终的变换矩阵。

本发明通过以上步骤，获得变换矩阵M后，完成了点云的粗配准工作，初步完成了待处理点云特征点与模板库中特征点的匹配工作。

步骤4.基于ICP的点云精细配准及位姿估计方法

本发明采用ICP算法基本原理，在粗配准结果上开展多次迭代计算工作提高配准精度，当对应点的误差小于指定阈值时，完成迭代任务。

本发明结合降采样和K-D树方法，来提高对应点的索引速度，具体实现方式是：将待处理的点云进行降采样以减少点云数量，然后将初始位姿下点云模板库中的点云数据创建为K-D树，即可快速找到与待处理点云特征点距离最近的对应点。

本发明为配准点对增加权重，并设置合理的阈值，该阈值的作用是当点对的距离大于此阈值时，将权重重置为0。本发明采用计算标准差的方法制定阈值，具体方法如下：统计所有点对的距离D，计算这些距离标准差σ，设置标准差的倍数为距离的阈值，具体如下式所示：

Dist_max＝nσ (14)

本发明考虑到点对距离越近则可靠性越高，因此距离越远的点对的权重越小，则权重表达式用下式表示：

在上式中，当点对间距离Dist(p_i,q_i)＝Dist_max时，对应的权重w_i＝0，此时点对可以被视为异值点，并予以剔除。

本发明采用上述改进型ICP算法，在点云特征点粗配准的基础上，实现待处理点云与模板库点云数据的特征点精配准，并获得点云之间变换矩阵M，坐标变换矩阵M由旋转矩阵R^*与位移矩阵T^*构成，公式如下式所示：

通过这两个矩阵，并结合点云模板库中目标船的位置和姿态信息，可以得到当前时刻激光雷达扫描到的目标船的位置和姿态的估计信息，当坐标按照x、y、z顺序进行旋转时，旋转矩阵与平移矩阵的表达式可用下式所示的R_xyz(α,β,γ)和T_xyz(x,y,z)表示：

则最终估计的船舶旋转姿态角度(α,β,γ)和平移位置信息(x,y,z)计算公式如下式所示：

下面结合具体数值给出本发明的基于点云特征的船舶位姿估计算法实验：

本发明开展基于点云特征的目标位姿估计算法实验，具体实现步骤如下所示：

Step 1：获取目标船各方向的点云图像

从前、后、左、右、上、下6个方向，获取目标船的点云数据图像，如图2(a)-2(f)所示。

Step 2：拼接得到目标船的完整点云数据

再将6个方向的点云数据图像进行滤波和拼接，得到最终的完整点云图，如图3(a)-3(b)所示。

Step 3：对点云库的完整点云和分割后的场景点云提取ISS3D特征点，特征点如图4中红点所示，并计算FPFH特征直方图。

Step 4：经SAC-IA与ICP算法进行点云配准，图5(a)-5(d)为2艘目标船的点云配准效果。

Step 5：由三维点云特征计算目标船位置数据，并与真实值对比。

经过点云配准后的变换矩阵，获得目标船的6自由度变换参数，估计参数与真实参数如图6中的表所示。从表中可以看出，虽然估计存在一定误差，但是误差范围不大，本发明方法有效。

Claims (2)

1.一种基于三维点云特征的船舶位姿估计方法，其特征在于：步骤如下：

步骤一：获取目标船点云数据，拼接完整点云并计算ISS3D特征点；

采用点云拼接方法获取船舶的完整点云，从前、后、左、右、上、下6个方向获得船舶点云数据，再将点云拼接得到船舶完整点云数据，对每一种船舶的点云提取ISS3D特征点p_f＝[x,y,z]，具体提取方法如下所示：

步骤1.1：对点云中每个点p_i都设置一个局部坐标系，并设置搜索半径r；

步骤1.2：获取点云每个点p_i在半径r内的所有点，权重计算公式如下所示：

步骤1.3：计算各点p_i的协方差矩阵，如下所示：

步骤1.4：对各点p_i的协方差矩阵进行特征值求解，并从大到小排序，得到

步骤1.5：通过特征值判断是否为关键点，选择阈值ε₁,ε₂，当且/>时，将这个点判断为角点关键点；

步骤二：计算ISS3D特征点的快速点特征直方图(FPFH)特征描述子，建立目标船点云模板库；

步骤2.1：对每一个特征点p_q，寻找与它距离小于r的邻域点p_ki，计算特征点与邻域点之间的三元组(α,φ,θ)；

步骤2.2：对于所有与特征点p_q距离小于r的邻域点p_ki，都按照上一步骤的方法，以这些邻域点p_ki为中心搜索距离小于r的邻域点，计算中心点与邻域点的三元组(α,φ,θ)；

步骤2.3：将特征点p_q的三元组(α,φ,θ)与加权的邻域点p_ki的三元组(α,φ,θ)进行统计，得到最终的快速点特征直方图描述子为：

上式中ω_ki为权重值，其与关键点p_q与邻域点p_ki之间距离有关；

将ISS3D特征点p_f＝[x,y,z]与快速点特征直方图特征描述子V_FPFH合并为一个特征向量E_i＝[x,y,z,V_FPFH]，以此来描述一个特征点，每种类型的船舶点云用这些特征向量的集合进行描述M_j＝[E₁ E₂…E_n]，并将它们存在点云模板数据库中；最终获得所有类型船舶的点云特征模板库[M₁ M₂…M_n]；

步骤三：基于SAC-IA的点云粗配准；

步骤3.1：从待处理点云中提取ISS3D特征点，计算这些特征点对应的FPFH特征描述子，从所有特征点中选择两两之间距离大于预定阈值d的n个特征点，以此作为采样点；

步骤3.2：在已建立的模板库中，逐个查找与上一步骤中获得的采样点有相似FPFH特征的关键点；

步骤3.3：将两片点云之间位置关系表示为如下所示的坐标变换形式：

上式中(x_i,y_i,z_i)为已构建的模板库中的关键点，(x_i′,y_i′,z_i′)为待处理点云数据中的采样点，M为变换矩阵；

步骤3.4：给出函数对上一步骤中变换矩阵M的准确度进行参数化表示，其中H(l_i)如下式所示：

上式中：ml为设定的阈值，l_i为经坐标变换后的第i组对应点对的距离，越小，则表示M的准确度越高；

步骤3.5：重复执行k次步骤3.3和步骤3.4，每次随机选择不同的对应点云计算变换矩阵M，并加以评估，选择k次选择中准确度最好的变换矩阵M，作为最终的变换矩阵；

步骤四：基于ICP的点云精细配准及位姿估计；

利用精准坐标变换矩阵与点云模板库中船舶的初始位姿信息，完成对目标船进行六自由度的位姿估计；采用改进型ICP算法，在点云特征点粗配准的基础上，实现待处理点云与模板库点云数据的特征点精配准，并获得点云之间变换矩阵M，坐标变换矩阵M由旋转矩阵R^*与位移矩阵T^*构成，公式为：

通过这两个矩阵，并结合点云模板库中目标船的位置和姿态信息，得到当前时刻激光雷达扫描到的目标船的位置和姿态的估计信息，当坐标按照x、y、z顺序进行旋转时，旋转矩阵与平移矩阵的表达式可用下式所示的R_xyz(α,β,γ)和T_xyz(x,y,z)表示：

则最终估计的船舶旋转姿态角度(α,β,γ)和平移位置信息(x,y,z)计算公式如下式所示：

2.根据权利要求1所述的一种基于三维点云特征的船舶位姿估计方法，其特征在于：步骤3.3具体为：

在n个采样点中随机选择4个点，对于其中每一个点在与之相似的多个特征点中随机选择一个点作为对应点，通过这4对点解得仿射变换矩阵M，得到两片点云之间的坐标变换关系，并给出在计算仿射变换矩阵前对所有的对应点进行初步筛选的判定条件为：

上式中：d_pn为待处理点云中第n个特征点，d_qn为模板库中与之对应的特征点，δ表示对应点匹配度，δ数值越小表示匹配度越高，当对应点对完全匹配时δ＝0。