基于轨迹对齐的GNSS-LiDAR外参在线标定方法

本发明涉及移动机器人定位和建图，尤其涉及一种基于轨迹对齐的gnss-lidar外参在线标定方法。

背景技术：

1、近年来自动驾驶技术受到了人们的广泛关注，该技术在一定程度上降低了人们的劳动强度并且提高了生产效率。激光雷达(light detection and ranging,lidar)作为高精度的距离传感器，相比更常见的相机，其有对光照不敏感、能直接获取周围环境三维信息的优点，因此被大规模应用于自动驾驶和移动机器人领域。全球导航卫星系统(globalnavigation satellite system,gnss)可以提供设备在地球上的绝对位姿信息，因此将lidar和gnss数据融合可以提高移动机器人建图和定位的鲁棒性。gnss和lidar之间的精确外参标定矩阵可以将不同传感器的数据转换到统一坐标系下，是实现多传感器数据融合的前提条件。

2、目前已有的gnss-lidar标定方法大致可分为：人工标定物辅助标定、点云优化标定等。文献(c.yan,f.ye,g.xue,w.bai and y.gong,"extrinsic calibration method forgnss/imu and lidar with the custom calibration pattern,"2023ieeeinternational conference on electrical,automation and computer engineering(iceace),changchun,china,2023.)提出了一种基于人工标定物的gnss-lidar外参标定方法，通过人工设计的标定板计算gnss和lidar之间的旋转矩阵和平移向量。但该方法在标定过程中需要使用全站仪测量标定板中每个圆孔的中心位置，在实际操作过程中较为复杂并且需要额外设备，无法在移动机器人上直接使用。文献(秦晓辉，刘硕，常灯祥等，基于激光点云特征的lidar-gnss/imu联合自动标定方法，湖南大学学报，2023年10期)通过构建线特征和面特征的点云地图，并用每一帧激光点云与地图匹配，再结合运动约束求解外参。但该方法需要较为丰富的环境特征，对特定环境依赖较大。

技术实现思路

1、本发明提供一种基于轨迹对齐的gnss-lidar外参在线标定方法，以克服上述技术问题。

2、为了实现上述目的，本发明的技术方案是：

3、一种基于轨迹对齐的gnss-lidar外参在线标定方法，包括如下步骤：

4、s1：初始化gnss与lidar之间的位姿变换矩阵，以初始化gnss-lidar的外参；获取gnss数据和lidar里程计数据；所述gnss数据包括gnss设备的经度、纬度和高度；

5、s2：根据gnss-lidar的外参，获取lidar里程计数据在东北天坐标系下的坐标；以获取lidar里程计在东北天坐标系下的位置；

6、s3：根据gnss数据中的起始参考点数据，获取gnss数据在东北天坐标系下的坐标，以获取gnss设备在东北天坐标系下的位置；

7、s4：根据lidar里程计在东北天坐标系下的位置和gnss设备在东北天坐标系下的位置，获取基于gnss数据与lidar里程计数据的在东北天坐标系下的位置误差的目标函数，以获取lidar里程计数据在东北天坐标系下的位姿变换矩阵；

8、s5：根据所述lidar里程计数据在东北天坐标系下的位姿变换矩阵，对lidar坐标系到gnss坐标系之间的位姿变换矩阵进行更新，获取更新后的lidar到gnss之间的位姿变换矩阵，以实现对gnss-lidar的在线标定。

9、进一步的，所述s5中，获取更新后的lidar坐标系到gnss坐标系之间的位姿变换矩阵方法如下：

10、

11、式中：表示更新后的lidar坐标系到gnss坐标系之间的位姿变换矩阵；表示i时刻lidar里程计数据在东北天坐标系下的位姿变换矩阵。

12、进一步的，所述s3中，获取gnss数据在东北天坐标系下的坐标的方法如下：

13、s31：根据所述起始参考点数据，基于坐标转换公式，获取起始参考点在地心地固坐标系下的坐标值；

14、s32：根据起始参考点在地心地固坐标系下的坐标值，获取gnss数据在东北天坐标系下的坐标。

15、进一步的，所述坐标转换公式为：

16、xe＝(n+alt)cos(lat)cos(lon)

17、ye＝(n+alt)cos(lat)sin(lon)

18、ze＝(n(1-e2)+alt)sin(lat)

19、

20、式中：xe,ye,ze分别为gnss数据在地心地固坐标系下的x轴坐标，y轴坐标和z轴坐标；n表示地球曲率半径；lon,lat,alt分别表示gnss设备中的经度、纬度和高度；e表示地球的椭球体偏心率，a表示地球赤道半径。

21、进一步的，获取gnss数据在东北天坐标系下的坐标所采用公式如下：

22、

23、式中：xj、yj、zj分别表示gnss设备的第j个位置在东北天坐标系下的x轴坐标、y轴坐标和z轴坐标；lon0、lat0、alt0分别为gnss设备中的起始参考点的经度、纬度和高度；分别表示gnss设备的第j个位置在地心地固坐标系坐标系下的x轴坐标、y轴坐标和z轴坐标；分别为gnss设备的起始参考点在ecef坐标系下的x轴坐标、y轴坐标和z轴坐标。

24、进一步的，所述基于gnss数据与lidar里程计数据的在东北天坐标系下的位置误差的目标函数获取如下：

25、

26、

27、

28、式中：e1表示gnss数据与lidar里程计数据的在东北天坐标系下的位置误差；表示gnss设备在东北天坐标系下的位置；表示lidar里程计在东北天坐标系下的位置；e2表示相邻两帧lidar里程计数据的位姿差的误差；表示相邻两帧lidar里程计数据的位姿变换矩阵的逆；表示第i帧lidar里程计在东北天坐标系下的位姿变换矩阵的逆；表示第i+1帧lidar里程计在东北天坐标系下的位姿变换矩阵；表示待优化变量；n表示lidar里程计数据个数；||·||表示范数；ln(·)∨表示李代数算子；表示i时刻lidar里程计相邻帧的位姿差的估计误差协方差；表示i时刻gnss在对应位置上的协方差矩阵：

29、

30、式中：表示gnss在i时刻相对于0时刻的位姿变换矩阵；表示lidar在i时刻相对于0时刻的位姿变换矩阵；为直接观测协方差矩阵由gnss系统直接获取；

31、

32、式中：表示在i时刻lidar里程计数据在东北天坐标系下的位姿；表示lidar坐标系到gnss坐标系之间的位姿变换矩阵；表示lidar里程计数据在i时刻相对于0时刻的位姿变换矩阵；

33、

34、式中：表示lidar里程计数据在i+1时刻相对于i时刻的位姿；表示i时刻lidar在东北天坐标系的位姿；表示i+1时刻lidar在东北天坐标系下的位姿；

35、

36、式中：表示i时刻lidar在东北天坐标系下的位置；表示lidar坐标系到gnss坐标系的旋转矩阵；表示i时刻雷达相对其0时刻的位置；

37、

38、式中：和分别为lidar坐标系到gnss坐标系的旋转矩阵和平移向量。

39、有益效果：本发明的一种基于轨迹对齐的gnss-lidar外参在线标定方法，通过获取lidar里程计在东北天坐标系下的位置和gnss设备在东北天坐标系下的位置，获取基于gnss数据与lidar里程计数据的在东北天坐标系下的位置误差的目标函数，进而得到lidar里程计数据在东北天坐标系下的位姿变换矩阵，最后获取更新后的lidar到gnss之间的位姿变换矩阵，以实现对gnss-lidar的在线标定。本发明以gnss和lidar作为测量装置，无需增加新的设备即能够完成对外参的更新，同时能够以车辆为运动载体，以车载计算机为数据收集和处理平台，既能够完成gnss和lidar之间外参的在线标定操作过程中简单，能够在移动机器人上直接进行使用。