船闸禁停区域无重叠视野激光雷达的外参标定方法与流程

本发明涉及船舶监控，具体涉及一种船闸禁停区域无重叠视野激光雷达的外参标定方法。

背景技术：

1、船舶在闸室内停泊时若越过禁停线进入禁停区域，可能在人字门开门运行时造成撞门事故。做好该区域的船舶监控，确保禁停区无船舶越线，能够有效降低船舶因停靠位置不当给船闸运行带来的安全隐患。

2、激光雷达的稳定性和可靠性与激光雷达位姿关系密切，而船闸闸室是一个室外大规模开放场景，激光雷达位姿受环境因素(如大风大雪)影响剧烈，因此需要周期性进行标定和位姿调整。

3、目前通用的标定方法可以分为基于外观和基于运动两种。基于外观的方法通常是利用多传感器提取共同特征进行标定，需要传感器之间有重叠视野；基于运动的方法利用运动信息进行外参标定，可通过多个传感器的相对位姿进行标定，需要传感器间有重叠视野，也可通过旋转激光雷达，创建共同特征，不需要额外传感器和重叠视野。然而，上述方法无法直接应用于船闸闸室环境下无重叠视野的激光雷达标定。

技术实现思路

1、为解决上述技术问题，本发明提供一种船闸禁停区域场景下，无重叠视野激光雷达的外参标定方法，该方法能够实现在船闸闸室环境下，单个或多个无重叠视野的激光雷达自标定；无需多传感器联合标定，具有节约资源成本且高效高精度的优点。

2、本发明采取的技术方案为：

3、船闸禁停区域无重叠视野激光雷达的外参标定方法，包括以下步骤：

4、步骤s1：使用基准雷达采集船闸闸室禁停区域的环境数据；

5、步骤s2：对步骤s1采集的环境数据使用slam算法建图，得到点云地图；

6、步骤s3：基于平面匹配方法将步骤s2获得的点云地图坐标系转换到船闸绝对坐标系；

7、步骤s4：将激光雷达标定到点云地图，使用ndt算法进行匹配标定，计算出激光雷达外参数。

8、所述步骤s1中，采集的船闸闸室禁停区域的环境数据包括点云数据和imu数据。点云数据和imu数据均是雷达装置从闸室一侧移动到闸室另一侧的整个运动过程中记录的序列数据。具体来说，运动过程中的单帧点云如图3所示，它是由一系列稀疏且无序的点组成，每个点包含三维空间坐标与反射率属性。所使用的imu为6轴，运动过程中的单帧imu数据包括三轴(xyz)加速度和三轴(xyz)角速度。

9、所述步骤s2中，环境数据使用slam算法建图。采用fast-lio2算法，这是一种基于迭代扩展卡尔曼滤波的激光雷达与imu紧耦合算法，包括以下步骤。

10、s2.1：基于迭代扩展卡尔曼滤波的激光里程计：第一帧点云不处理，并初始化第一帧的位姿包括位置和姿态，位置向量用零向量初始化，姿态向量用单位四元数初始化。输入上一帧位姿和上一帧点云当前帧点云pk，当前帧imu测量值am和ωm，通过迭代扩展卡尔曼滤波算法，对当前帧点云的位姿进行估计，并基于匀速运动假设对当前帧点云进行运动畸变矫正，得到矫正后的点云

11、s2.2：激光里程计建图与全局地图维护：从所有矫正后的点云中均匀采样选取出关键帧，利用关键帧点云的位姿可以对关键帧点云进行坐标变换，变换到第一帧位姿所表示的坐标系下，从而实现点云的拼接建图。使用ikd-tree的数据结构来维护点云地图，它支持增量更新和树的动态重平衡，有效地减少了计算开销。最终得到的点云地图如图4所示。

12、所述步骤s3中，点云地图坐标系以采集数据中第一帧点云的坐标系为基准，而船闸绝对坐标系是人为规定的，这里我们采用面向上游、以左侧激光雷达为原点、平行于闸壁从下游往上游方向为y轴正方向，垂直于闸墙向右侧方向为x轴正方向，垂直于闸面向上方向为z轴正方向。绝对坐标系和点云地图坐标系的示意图如图5所示，其中三角形为待标定的激光雷达，即船闸绝对坐标系原点，圆形为基准雷达，即点云地图坐标系原点。

13、所述步骤s3中，基于平面匹配方法的坐标系转换，包括以下步骤：

14、s3.1:设点云地图坐标系为s1，船闸绝对坐标系为s2，x1，x2是s1和s2中的一组对应点，则相对位姿r、t有如下关系

15、x2＝r·x1+t      (1)；

16、其中，r为3×3的旋转矩阵，t为3×1的平移向量；将相对位姿的计算分解为两步进行，先求解出r，再利用式(1)求解t；

17、s3.2:选择点云地图中两个面积最大的水平闸面和垂直闸壁面，求解旋转关系r：

18、选取平面是通过在点云处理软件cloudcompare中裁剪点云地图实现，记所述水平闸面和垂直闸壁面分别为p1、p2，船闸绝对坐标系中对应的平面分别为p′1、p′2，记距离函数fi2(xk)表示平面pi上的任意一点xk到平面p′i的距离平方，构建一个目标函数f(p)表示经p变换后平面p1、p2上的点到对应平面的距离平方和：

19、

20、式中：p＝(r，t)为待求外参；t表示旋转后两个平面对应点的距离；t＝(p，xk)表示对点xk施加变换p，通过最小化式(2)的目标函数得到p，将旋转矩阵r的求解转化为非线性优化问题；

21、s3.3：计算式(2)中r和t初值：

22、记平面p1、p2对应单位法向量为n1、n2，平面p′1、p′2对应单位法向量n′1、n′2；n1、n2向量叉乘得到n3，n′1、n′2向量叉乘得到n′3；根据向量叉乘几何意义和船闸绝对坐标系s2定义可知，n′1、n′2、n′3分别为z轴、x轴、y轴单位向量。

23、计算出旋转矩阵r的初值，具体来说分两步进行，首先根据n1与n′1计算出旋转矩阵r1，对点云p2用r′进行坐标变换，此时，点云p2中n1与n′1两法向量平行，即实现了水平闸面的旋转对齐。

24、在此基础上，同理，根据p2的法向量n2与n′2，计算出旋转矩阵r2，从而累乘可得旋转矩阵r的初值，如式(3)所示，其中，i为下标，θ为旋转角，对应的旋转轴为u＝[ux，uy，uz]。旋转矩阵r的初值可以确保非线性优化过程收敛。

25、

26、非线性优化进一步迭代优化r和t。具体来说，如式(2)所示构造损失函数，优化变量为r和t，使用牛顿法进行迭代，步骤如下：

27、step1.给定初始值r0，t0，精度阈值σ，迭代步数k＝0；

28、step2.计算损失函数梯度gk和hessian矩阵hk；

29、step3.如果||gk||＜σ，即梯度为0，到达极值点，停止迭代；

30、step4.计算搜索方向

31、step5.更新优化变量rk+1＝rk+γdk，tk+1＝tk+γdk；

32、step6.k＝k+1，返回step2迭代；

33、其中，精度阈值σ和学习率γ均为认为设定的超参数。

34、s3.4：在点云地图坐标系和船闸绝对坐标系中找任意一组对应点作为x1、x2，根据公式(1)，计算出平移向量t。

35、所述步骤s4包括以下步骤：

36、s4.1：使用激光雷达采集一段时间内多帧点云数据，统计滤波器过滤每帧点云噪声，对多帧点云数据进行时域积分叠加，再使用体素栅格滤波器进行下采样，得到待配准的雷达点云。待配准的雷达点云就是多帧点云合并，如图6所示。

37、s4.2：对待配准的雷达点云使用ndt算法与地图进行匹配：

38、将点云地图划分为体素栅格，计算每个体素内的点云均值μ和协方差σ，构建高斯分布，如式所示：

39、

40、根据雷达点云在点云地图上的初始姿态p＝(r,t)，对激光雷达点云实施变换，点xi经过t＝(p,xi)变换后得到点x′i，计算得到点x′i的概率密度为：

41、

42、计算出所有待配准雷达点云的概率密度，概率越大表示两个点云重合越好，当概率取最大值时，认为此时p为两个点云间的最优位姿；

43、通过取对数、取反并去掉常数项优化式(4)，构建目标函数f(p)：

44、

45、其中，n表示待配准的雷达点云中点的个数。使用牛顿法对式(5)进行迭代求解，得到最优位姿p。

46、s4.2的初始姿态p＝(r,t)由人工观测得出，具体来说是在cloudcompare中夹杂待配准点云和点云地图，软件内人工旋转平移待配准点云至于点云地图对应场景重合，从软件内导出初始姿态。

47、本发明一种船闸禁停区域无重叠视野激光雷达的外参标定方法，技术效果如下：

48、1)本发明步骤1中，使用基准雷达采集数据，一次采集良好即可永久使用，建图数据不会受到极端天气影响，从而保证后续算法的稳定和可靠。其次数据采集方便，仅需单人携带雷达扫描一圈即可，相比于基于标定物的算法，操作方便节省人力。

49、2)本发明步骤2中，旨在通过激光slam建图，让地图充当各个雷达之间的”共同视野”，新颖地解决了其他标定算法在船闸场景下遇到的问题，无需标定物、无需雷达共同视野。此外，选用的fast-lio2算法极其鲁棒与通用，鲁棒的优点在于，可以让操作人员在执行步骤1时，无需精细扫描，易于操作。通用的优点在于，支持成本低廉的固态机械雷达。

50、3)本发明步骤3中，巧妙的利用船闸场景的特点，来对点云地图进行旋转，操作简单。相对于测绘手段，本方法无需借助额外的测绘设备，且通过非线性优化算法计算的旋转矩阵，精度足够高。

51、4)本发明步骤4中，借助点云地图，实现了多激光雷达标定，相比测绘手段，通过将待标定雷达的激光点云与点云地图进行点云配准的方式，精度极大提升，因为测绘手段只能测量雷达外表面点，无法准确估计出雷达中心点位置。此外，操作简便通用，一次手动标定后，后续即可实现自动标定，实用性强。