一种基于改进RANSAC的目标船姿态估计方法

本发明属于激光雷达探测的，尤其涉及一种基于改进ransac的目标船姿态估计方法。

背景技术：

1、周围船舶姿态估计非常重要，为避碰规划算法奠定了基础，同时对于执行运河航行、编队巡航、海上补给等任务的舰船也具有重要的应用价值。姿态估计中，随机采样一致算法(ransac)被广泛用于点云的特征提取，但由于随机抽样过程中引入的不确定性，导致算法由于选取点的差异出现错误估算。后续的一系列改进算法包括提取地平面提高有效点云，加入角度聚类提高效果等方法均无法满足海上航行识别舰船的需求。基于激光雷达的舰船特征提取主要存在两个问题：1)海上容易受到环境扰动的影响；2)海上识别距离更远，稀疏点云识别难度大。

技术实现思路

1、本发明克服了现有技术的不足之一，提供了一种基于改进ransac的目标船姿态估计方法，实现海上目标舰船姿态的较高精度估计。

2、根据本公开的一方面，本发明提供一种基于改进ransac的目标船姿态估计方法，其特征在于，包括以下步骤：

3、步骤1：激光雷达原始点云数据预处理；

4、步骤2：对预处理后的点云进行基于点云法向量的滤波器过滤，该滤波器估计每个点的法向量，并根据法向量差值对点云进行划分，保留最大的点集作为主平面点云，其余点集作为无关点集进行剔除；

5、步骤3：对滤波后的点云采用随机采样一致算法ransac获得目标特征平面的法向量；

6、步骤4：利用特征平面的法向量计算目标船的姿态信息。

7、进一步地，作为本发明的一种优选技术方案，所述步骤1包括：点云体素栅格下采样，对下采样后的点云进行坐标转换，从激光雷达坐标系转换至自船坐标系。

8、进一步地，作为本发明的一种优选技术方案，所述步骤2中，所述估计每个点的法向量包括：

9、通过欧拉距离定义点云邻域，邻域的具体大小根据测试获得；

10、利用最小二乘法拟合点云邻域的特征平面，通过特征平面的平面方程计算该平面的法向量，作为离散点的估计法向量。

11、进一步地，作为本发明的一种优选技术方案，所述步骤2中，所述法向量差值的计算，采取公式：

12、

13、其中qi、qj为法向量和δd表示两个法向量之间的差值，中a,b,c分别表示法向量在东北高坐标系中的三维坐标。

14、进一步地，作为本发明的一种优选技术方案，所述步骤2中，所述对点云进行划分包括：

15、在点云中随机选取一个点，如果该点的法向量与现有点集中的其他点相似，则将该点放入该点集中，否则，将创建一个新的点集来包含它；循环该过程，直到所有点云中的点都被划分完毕。

16、进一步地，作为本发明的一种优选技术方案，所述步骤4中，所述目标船的姿态信息包括：目标船偏航角，目标船横摇角，目标船偏航速率和目标船横摇速率。

17、本发明采用上述技术方案，能产生如下的技术效果：

18、本发明的方法，通过改进的ransac方法提取舷侧平面，并计算目标船的横摇角、偏航角、横摇率和偏航率。在标准ransac方法的基础上增加了一个基于法向向量的滤波器，以排除点云中不相关的点，提高特征提取过程的准确性。然后，ransac过程根据几何关系给出最优平面函数，用于计算姿态信息。本发明方法可以在点云质量不高的海上环境下有效提取船只目标的特征平面，并精确计算船只的姿态信息。采用用两种典型船型(即集装箱船和游艇)进行了三次数值模拟，模拟结果表明，估计角度和角速率的平均误差分别为0.039°和0.111°/s，比基于标准ransac的估计方法精度提高了99％。

技术特征：

1.一种基于改进ransac的目标船姿态估计方法，其特征在于，包括以下步骤：

2.根据权利要求1所述基于改进ransac的目标船姿态估计方法，其特征在于，所述步骤1包括：点云体素栅格下采样，对下采样后的点云进行坐标转换，从激光雷达坐标系转换至自船坐标系。

3.根据权利要求1所述基于改进ransac的目标船姿态估计方法，其特征在于，所述步骤2中，所述估计每个点的法向量包括：

4.根据权利要求1所述基于改进ransac的目标船姿态估计方法，其特征在于，所述步骤2中，所述法向量差值的计算，采取公式：

5.根据权利要求1所述基于改进ransac的目标船姿态估计方法，其特征在于，所述步骤2中，所述对点云进行划分包括：

6.根据权利要求1所述基于改进ransac的目标船姿态估计方法，其特征在于，所述步骤4中，所述目标船的姿态信息包括：目标船偏航角，目标船横摇角，目标船偏航速率和目标船横摇速率。

技术总结
本发明公开了一种基于改进RANSAC的目标船姿态估计方法，其特征在于包括以下步骤：步骤1：激光雷达原始点云数据预处理；步骤2：对预处理后的点云进行基于点云法向量的滤波器过滤，保留最大的点集作为主平面点云，其余点集作为无关点集进行剔除；步骤3：对滤波后的点云采用随机采样一致算法RANSAC获得目标特征平面的法向量；步骤4：利用特征平面的法向量计算目标船的姿态信息。本发明在点云质量不高的海上环境下有效提取船只目标的特征平面，并精确计算船只的姿态信息，提高特征提取过程的准确性，估计角度和角速率的平均误差分别为0.039°和0.111°/s，比基于标准RANSAC的估计方法精度提高了99％。

技术研发人员：王鸿东,魏圣哲,李正宇,楼建坤
受保护的技术使用者：上海交通大学
技术研发日：
技术公布日：2024/3/27