一种基于岸基激光雷达点云的船舶六自由度位姿估计方法

摘要: 本发明涉及港口智能感知、船舶靠泊辅助和三维激光雷达点云处理,尤其涉及一种基于岸基激光雷达点云的船舶六自由度位姿估计方法。背景技术:1、现有激光雷达船舶靠泊感知研...
一种基于岸基激光雷达点云的船舶六自由度位姿估计方法

本发明涉及港口智能感知、船舶靠泊辅助和三维激光雷达点云处理,尤其涉及一种基于岸基激光雷达点云的船舶六自由度位姿估计方法。

背景技术:

1、现有激光雷达船舶靠泊感知研究已从单纯测距逐步发展到三维点云态势分析。相关技术利用三维激光视觉捕捉并构建目标船舶三维几何形状,实现了船舶靠泊距离、靠泊角和靠泊速度测量;以及采用三维激光雷达数据,通过主成分分析计算船舶航向和法向量,并结合船首、船尾特征点对船舶相对于船坞的距离和角速度进行分析;以及针对船舶进出升船机过程,提出基于激光雷达的船舶位姿感知方法,通过点云预处理、k-d树、包围盒和自适应体素滤波等方法提高船舶位置、速度和艏向角感知的鲁棒性。此外有相关技术利用三维激光技术提取泊位几何特征以估计船舶相对泊位的位置和朝向,在无人艇仿真环境中利用单线激光雷达测量船舶与泊位距离。上述方法为船舶靠泊感知提供了重要基础,但多数方法主要关注靠泊距离、靠泊角、速度、位置或航向等局部参数,对船体横摇、纵摇、垂荡等姿态波动考虑不足,难以仅依靠岸基点云连续输出船舶完整六自由度位姿。此外,部分船载式方案依赖船端传感器安装和标定,不便于港口侧对不同船舶进行统一、非接触式监测;部分岸基激光雷达方案主要关注靠泊距离、靠泊速度或靠泊角等局部参数,通常只能提供二维平面位姿或三自由度状态,难以同时输出船舶的空间位置、横摇、纵摇和艏摇等六自由度位姿;部分多传感器融合方案虽然信息丰富,但系统结构复杂,对同步、标定和工程部署要求较高。此外,靠泊过程中水面反射、码头固定设施、背景点云、船体遮挡及点云稀疏等因素会影响船体边缘提取和姿态估计精度,导致估计结果存在跳变或不稳定。因此,有必要提出一种面向岸基部署的、基于激光雷达点云的船舶六自由度位姿估计方法,通过背景点云差分去除静态环境干扰,利用船体候选点云的bev投影和几何边缘特征估计船舶平面位置与艏向角,同时结合三维点云高度分布、局部投影和线段/轮廓匹配估计垂向位置、横摇角和纵摇角,从而实现对靠泊船舶纵荡、横荡、垂荡、横摇角、纵摇角、艏向角的连续、非接触、高精度估计,为智能靠泊辅助、港口岸基监测和无人船靠泊控制提供可靠感知基础。

技术实现思路

1、本发明公开了一种基于岸基激光雷达点云的船舶六自由度位姿估计方法,以克服上述技术问题。

2、为了实现上述目的,本发明的技术方案是:一种基于岸基激光雷达点云的船舶六自由度位姿估计方法,包括如下步骤:

3、s1:通过岸基激光雷达获取目标船舶点云和对应的空场景点云,以获取船体候选点云,进而采用离群点滤波,获取稳定船体点云;

4、s2:根据稳定船体点云,将稳定船体点云沿激光雷达坐标系下z轴方向压缩到固定高度平面,获取固定高度bev投影点云;

5、s3:根据固定高度bev投影点云,从固定高度bev投影点云提取锚点,以获取远端轮廓参考点;

6、s4:根据船舶长度、船宽、锚点,构造l-shape虚拟点云模板,并根据远端轮廓参考点,获取l-shape虚拟点云模板局部坐标系相对于bev全局坐标系的候选旋转角综合得分,以确定最优l-shape虚拟点云模板方向,进而获取船舶横荡、纵荡和艏向角;

7、s5:根据船舶水平中心位置和艏向角,获取船体局部坐标系,将稳定船体点云转换到船体局部坐标系下,以获取稳定船体点云的横向投影轮廓点云和纵向投影轮廓点云;

8、s6:根据纵向投影轮廓点云,在纵向投影轮廓点云中提取船艏特征点、中心最高点,以获取虚拟点云线段;

9、s7:根据虚拟点云线段和横向投影轮廓点云,采用icp三维点云配准方法,获取估计横摇角、估计纵摇角、估计垂荡,并根据所述横荡、纵荡、艏向角,形成完整六自由度位姿,完成基于岸基激光雷达点云的船舶六自由度位姿估计。

10、进一步的,所述固定高度bev投影点云表示为:

11、

12、式中,表示岸基激光雷达实时采集当前帧点云中的第个三维点的横坐标;表示岸基激光雷达实时采集当前帧点云中的第个三维点的纵坐标;表示岸基激光雷达实时采集当前帧点云中的第个三维点的高度坐标;表示稳定的船体候选点云;表示固定高度的bev投影点云;表示固定高度。

13、进一步的,所述远端轮廓参考点表示为:

14、

15、式中,为远端参考点搜索窗口阈值;为真实bev投影点云提取出的远端轮廓参考点;为从bev投影点云中提取的锚点;为二维bev投影点;为各bev投影点到锚点的最大距离;为二维bev投影点集;表示取中位数;表示模长。

16、进一步的,获取候选旋转角综合得分所采用公式如下:

17、

18、式中,为候选旋转角综合得分;表示l-shape虚拟点云模板覆盖率,用于表征l-shape虚拟点云模板栅格被bev投影点云覆盖的程度;为bev投影点匹配比例;表示远端轮廓参考点到bev全局坐标系下的l-shape虚拟点云模板长边的横向误差;表示远端轮廓参考点沿l-shape虚拟点云模板长边方向超出线段范围的误差;表示当前候选旋转角与上一帧滤波l-shape虚拟点云模板方向之间的角度跳变惩罚;至均为权重系数;

19、其中,

20、

21、

22、式中,表示候选旋转角下l-shape虚拟点云模板长边离散化得到的l-shape虚拟点云模板栅格集合;为膨胀后的bev投影点云栅格集合;

23、选择综合得分最高的候选旋转角作为当前帧l-shape虚拟点云模板方向:

24、

25、为抑制单帧点云噪声导致的艏向角跳变,对l-shape虚拟点云模板方向进行低通滤波:

26、

27、式中,表示当前帧综合得分最高的候选旋转角;为当前滤波后的l-shape虚拟点云模板方向;为上一帧滤波后的l-shape虚拟点云模板;为更新系数。

28、进一步的,获取稳定船体点云的横向投影轮廓点云和纵向投影轮廓点云如下:

29、

30、

31、式中,为横向投影轮廓点云;为纵向投影轮廓点云;为船体局部坐标系下的点云集合;为稳定船体点云中第个点在船体局部坐标系下的横向坐标;为稳定船体点云中第个点在船体局部坐标系下的纵向坐标;为稳定船体点云中第个点在船体局部坐标系下的高度坐标。

32、进一步的,所述虚拟点云线段构造如下:

33、首先,连接船艏特征点与中心最高点,构造当前帧虚拟线段:

34、

35、然后,基于当前帧虚拟线段,在上均匀采样,生成虚拟点云线段:

36、

37、式中,为当前帧由船艏特征点和中心最高点连接形成的虚拟线段;表示当前帧虚拟点云线段;为船艏特征点;为中心最高点;为虚拟线段采样点数;为表示虚拟线段上的连续插值参数;表示第个离散采样参数;表示虚拟线段采样点数。

38、进一步的,所述采用icp三维点云配准方法,获取估计横摇角、估计纵摇角、估计垂荡,表示为:

39、icp配准作用于横向投影轮廓时,由提取的旋转角用于估计横摇角:

40、

41、icp配准作用于虚拟点云线段时,由提取的旋转角用于估计纵摇角,由中的高度方向平移量用于估计垂荡:

42、

43、式中,为估计横摇角;为估计纵摇角;为估计垂荡;为横摇角符号补偿系数;为icp配准作用于横向投影轮廓时的旋转角;为纵摇角符号补偿系数;为icp配准作用于虚拟点云线段时的旋转角;为垂荡计算的参考高度或参考平面高度;为垂向平移方向补偿系数;表示由虚拟点云线段icp配准得到的垂向平移量;为最优旋转矩阵,为最优平移向量。

44、有益效果:本发明的一种基于岸基激光雷达点云的船舶六自由度位姿估计方法。通过岸基激光雷达采集船舶点云,结合背景点云差分方法从港口静态环境中提取船舶动态点云,为后续六自由度估计提供稳定输入,能够在岸基部署条件下,仅利用激光雷达点云实现船舶六自由度位姿估计,不依赖船载传感器安装,便于港口侧对靠泊船舶进行统一、非接触式实时监测。采取对船体候选点云进行z向分段筛选和统计滤波,提取甲板附近或船体主体点云,并将其压缩为bev投影;结合已知船长船宽构造l-shape虚拟模板,与船体bev轮廓匹配,解算船舶中心位置和艏向角,提高了复杂码头背景下船体轮廓提取和横荡、纵荡、艏向角平面位姿估计的稳定性,降低三维点云直接处理的复杂度。并在虚拟点云模板获得船体局部坐标系后,基于原始三维船体点云生成横向局部投影与纵向局部投影,通过横向轮廓匹配估计横摇角,通过船艏点和中心最高点构造虚拟线段估计纵摇角和垂荡,补足了传统岸基靠泊感知中对垂荡、横摇角、纵摇角估计不足的问题,实现完整六自由度连续输出,并具有较好的时序稳定性。