一种GNSS增强的多源传感器在线时空标定系统及方法

本发明属于导航定位，尤其涉及一种gnss增强的多源传感器在线时空标定系统及方法。

背景技术：

1、自动驾驶和机器人技术的不断发展对导航和感知提出了更高的要求。传统的单一传感器已经难以满足复杂场景下的高精度导航感知的需求。在这种情况下，多源传感器的信息融合可以有效地解决单一传感器的局限性，保证载体在复杂多变的环境中长时间自主运行。其中常用的传感器包括激光雷达(light detection and ranging，lidar)、相机、惯性测量单元(inertial measurement unit，imu)和全球卫星导航系统(global navigationsatellite system,gnss)等。这些传感器的作用是感知周围环境或自身运动信息，并以此来估计载体状态。其中，激光雷达和相机是感知型传感器，可以测量外部场景的信息来实时计算载体在场景中的位置；而imu则是一种自主无源传感器，它可以通过感知载体的运动来估计状态；gnss则能通过接收卫星发射的无线电测距信号求解载体的全局位置。

2、不同类型的传感器有不同的优势，考虑这些传感器特性互补性，融合这些传感器的信息可以利用各自的优势克服其他传感器的缺陷来提高算法在不同场景中的适用性。而传感器之间的标定是实现信息融合的基本要求。传感器标定可估算出不同传感器之间的刚性位姿变换关系以及时间转换关系，从而使得它们采集的信息能够被转换到统一的空间和时间基准坐标系下使用。标定的精度直接影响到多个传感器信息的冗余和错误关联，进而影响融合后的观测信息的准确性和精度。不精确的标定结果可能会导致错误的状态估计，因此标定的精度对于状态估计的准确性至关重要。

技术实现思路

1、为了解决上述技术问题，本发明提出了一种gnss增强的多源传感器在线时空标定系统及方法。

2、本发明系统的技术方案为一种gnss增强的多源传感器在线时空标定系统，包括：

3、车载终端、激光雷达、惯性测量单元、相机、gnss模块；

4、所述车载终端分别与所述的激光雷达传感器、惯性测量单元、相机传感器、gnss模块依次连接；

5、车载终端部署与车辆座舱内、激光雷达部署于车顶正中上方、惯性测量单元部署于车辆后轴中点的正上方、相机传感器部署于车顶前方向前观测、gnss模块部署于车顶后方开阔位置并减少与激光雷达的相互遮挡；

6、车载终端中卡尔曼滤波器模块获取其上一时刻的状态向量以及协方差矩阵；

7、通过卡尔曼滤波器进行状态预测，得到卡尔曼滤波器的当前时刻的预测状态向量、协方差矩阵；

8、通过点云曲率方法提取得到当前时刻的平面特征点集，进行点云跟踪得到帧间的点云数据关联，通过扩维处理得到卡尔曼滤波器的当前时刻的扩维后预测状态向量，进一步构建点云雅克比矩阵；

9、将卡尔曼滤波器的当前时刻的扩维后预测状态向量进行扩维处理得到卡尔曼滤波器的当前时刻的二次扩维后预测状态向量，结合帧间的特征数据关联、卡尔曼滤波器的当前时刻的二次扩维后预测状态向量构建影像雅克比矩阵；

10、对当前时刻lidar状态扩维完成后的lidar窗口中跟踪丢失的平面特征观测值进行卡尔曼量测更新；

11、对当前时刻相机状态扩维完成后的图像中的特征点观测值进行卡尔曼量测更新；

12、将gnss伪距和相位观测值同基站数据组成rtk双差观测值，进行量测更新；

13、每次进行量测更新后，在经过一段时间的收敛后得到稳定和准确的传感器外参和时间延迟参数。

14、本发明方法的技术方案为一种gnss增强的多源传感器在线时空标定系统及方法，包括以下步骤：

15、步骤1：车载终端中卡尔曼滤波器模块获取其上一时刻的状态向量以及协方差矩阵；

16、步骤2：通过惯性测量单元获取上一时刻到当前时刻之间的imu速度增量、imu姿态增量，结合卡尔曼滤波器的上一时刻的状态向量通过卡尔曼滤波器进行状态预测，得到卡尔曼滤波器的当前时刻的预测状态向量，将卡尔曼滤波器的上一时刻的协方差矩阵通过卡尔曼滤波器进行协方差矩阵转移，得到卡尔曼滤波器的当前时刻的协方差矩阵；

17、步骤3：车载终端中的点云处理模块，通过激光雷达传感器获取当前时刻的雷达点云，将当前时刻的雷达点云通过点云曲率方法提取得到当前时刻的平面特征点集，结合上一时刻的雷达点云进行点云跟踪得到帧间的点云数据关联，将卡尔曼滤波器的当前时刻的预测状态向量进行扩维处理得到卡尔曼滤波器的当前时刻的扩维后预测状态向量，结合帧间的点云数据关联、卡尔曼滤波器的当前时刻的扩维后预测状态向量构建点云雅克比矩阵；

18、步骤4：车载终端中的图像处理模块，通过相机传感器获取当前时刻的相机影像，将当前时刻的相机影像通过fast角点探测方法进行特征提取，得到当前时刻的相机影像特征点集，结合上一时刻的相机影像特征点集通过光流方法进行特征跟踪得到帧间的特征数据关联，将卡尔曼滤波器的当前时刻的扩维后预测状态向量进行扩维处理得到卡尔曼滤波器的当前时刻的二次扩维后预测状态向量，结合帧间的特征数据关联、卡尔曼滤波器的当前时刻的二次扩维后预测状态向量构建影像雅克比矩阵；

19、步骤5：当前时刻lidar状态扩维完成后，对此时的lidar窗口中跟踪丢失的平面特征观测值进行卡尔曼量测更新；

20、步骤6：当前时刻相机状态扩维完成后，对此时的图像中的特征点观测值进行卡尔曼量测更新；

21、步骤7：车载终端中的gnss处理模块，通过gnss接收机获取到新的gnss观测数据，将gnss伪距和相位观测值同基站数据组成rtk双差观测值，进行量测更新；

22、步骤8：每次进行量测更新后，多源传感器间的外参和时延就会改变，在经过一段时间的收敛后，就能得到稳定和准确的传感器外参和时间延迟参数。

23、作为优选，步骤1中所述卡尔曼滤波器的状态，定义为：

24、δx＝[δxins δxgnss δxcal-c δxcal-l δxcam δxlidar]t

25、其中，δxins表示ins误差状态向量，δxgnss表示gnss误差状态向量，δxcal-c表示相机与imu间的外参和时延误差状态向量，δxcal-l表示lidar与imu间的外参和时延误差状态向量，δxcam表示滑动窗口内相机的位姿误差状态向量，δxlidar表示滑动窗口内lidar的位姿误差状态向量；

26、

27、δxgnss＝[δng δne δnc]t

28、

29、

30、

31、

32、其中：

33、δxins表示ins误差状态向量，和分别表示载体在e系下的姿态、速度和位置的误差状态向量，δba、δbg的分别表示加速度计零偏和陀螺仪零偏的误差状态向量；

34、δxgnss表示gnss的误差状态向量，其具体的δng、δne、δnc分别代表gps(g)卫星系统的模糊度误差状态向量、galileo(e)卫星系统的模糊度误差状态向量、bds(c)卫星系统的模糊度误差状态向量；

35、δxcal-c表示相机与imu间的外参和时延误差状态向量，和分别代表相机坐标系到imu坐标系之间外参的旋转参数、平移参数的误差状态向量，δtd_c表示相机时延误差状态向量；

36、δxcal-l表示lidar与imu间的外参和时延误差状态向量，和分别代表lidar坐标系到imu坐标系之间外参的旋转参数、平移参数的误差状态向量，δtd_l表示lidar时延误差状态向量；

37、δxcam表示滑动窗口内相机的位姿误差状态向量，分别表示滑动窗口中第m个相机位姿的旋转误差状态向量和平移误差状态向量。

38、δxlidar表示滑动窗口内相机的位姿误差状态向量，分别表示滑动窗口中第n个lidar位姿的旋转误差状态向量和平移误差状态向量。

39、作为优选，步骤3所述构建点云雅克比矩阵，具体定义如下：

40、h＝[hins hgnss hcal_c hcal_l hcam hlidar]

41、其中，hins表示新lidar状态对ins状态的雅克比矩阵，hgnss表示新lidar状态对gnss状态的雅克比矩阵，hcal_c表示对相机外参时延的雅克比矩阵，hcal_l表示对lidar外参时延的雅克比矩阵，hcam表示对滑动窗口内历史相机状态的雅克比矩阵，hlidar表示对滑动窗口内历史lidar状态的雅克比矩阵。

42、步骤3中，每次当lidar数据到来的时刻，系统的状态向量需要进行扩维以便在后续构建点云特征约束；

43、当收到时间戳为tl时刻的lidar位姿时，系统则扩增tl+td_l时刻的lidar位姿状态；

44、新的lidar位姿初值是通过ins的机械编排得到的位姿和传递而得到的：

45、

46、

47、其中，和分别是当前lidar-imu外参的旋转矩阵和平移向量，表示扩增的tc+td_l时刻的lidar位姿状态初值，和表示ins的机械编排得到的imu位姿。

48、作为优选，步骤4所述构建影像雅克比矩阵，具体定义如下：

49、h＝[hins hgnss hcal_c hcal_l hcam hlidar]

50、其中，hins表示新lidar状态对ins状态的雅克比矩阵，hgnss表示新lidar状态对gnss状态的雅克比矩阵，hcal_c表示对相机外参时延的雅克比矩阵，hcal_l表示对lidar外参时延的雅克比矩阵，hcam表示对滑动窗口内历史相机状态的雅克比矩阵，hlidar表示对滑动窗口内历史lidar状态的雅克比矩阵。

51、当获取到新时刻的相片时，对滤波器的状态向量进行扩维，将新时刻的相片位置和姿态加入到滤波器的状态向量之中，得到滤波器的总的状态向量；

52、步骤4中，每次当图像数据到来的时刻，系统的状态向量也需要扩增tc+td_c时刻的相机位姿状态。其初值是也是通过ins的机械编排得到的位姿和传递而得到的：

53、

54、

55、其中，和是当前相机-imu外参的旋转矩阵和平移向量，表示扩增的tc+td_c时刻的相机位姿状态初值，和表示ins的机械编排得到的imu位姿。

56、其新的相机状态对旧状态向量的雅克比矩阵与lidar状态的雅克比矩阵类似，不再过多赘述。需要注意的是，在系统不断更新的过程中，相机与lidar的外参和时延都会不断的更新，后续的状态扩增的步骤中，也需要对其变量进行相应的更新。

57、作为优选，步骤5中所述对当前历元中跟踪丢失的lidar特征按照扩展卡尔曼滤波的流程进行lidar量测更新，量测方程为：

58、

59、其中，δrlidar为参与量测更新所有平面点的量测残差向量，hψ为参与量测更新所有平面点对于其对应的平面特征的雅克比，hlidar为lidar位姿状态的雅克比，表示平面特征参数的误差状态，δxlidar表示lidar的位姿误差状态，wlidar是量测噪声。

60、通过hψ的左零空间矩阵aψ，使用零空间操作，使得量测模型与平面特征解耦：

61、

62、其中，δrlidar为参与量测更新所有平面点的量测残差向量，hψ为参与量测更新所有平面点对于其对应的平面特征的雅克比，aψ为hψ的左零空间矩阵，hlidar为lidar位姿状态的雅克比，表示平面特征参数的误差状态，δxlidar表示lidar的位姿误差状态，wlidar是量测噪声。

63、所以，量测模型与平面特征解耦后，可以重写为：

64、

65、其中，和分别为最终的量测残差向量、量测雅克比矩阵和量测噪声，δxlidar表示lidar的位姿误差状态。

66、作为优选，步骤6中所述特征点被连续跟踪三个历元以上且在当前历元跟踪丢失时，或状态向量中的相机位姿数量达到阈值时，相应的特征点按照扩展卡尔曼滤波的流程进行视觉量测更新，观测方程为：

67、

68、其中，δrc为参与量测更新所有视觉特征点的量测残差向量，hf为特征点位置误差状态所对应的雅克比，hc为相机位姿状态对应的雅克比，δxcam表示相机的位姿误差状态，表示视觉特征参数的误差状态，wvision是量测噪声。

69、为了减少状态数量，通过hf的左零空间矩阵af，使用零空间操作，使得量测模型与视觉特征点解耦，就能得到完整视觉量测模型。所以，最后的量测模型可以被写为：

70、

71、其中，和分别为最终的量测残差向量、量测雅克比矩阵和量测噪声，δxcam表示相机的位姿误差状态。

72、作为优选，步骤7中所述gnss量测方程，定义为：

73、

74、其中，zgnss表示gnss量测值，hgnss为量测矩阵，ηgnss为量测噪声，δxins表示ins误差状态向量，δxgnss表示gnss误差状态向量。

75、gnss量测值计算得到的是gnss天线相位中心的位置，而系统状态中的位置误差状态为imu的位置误差。所以，需要建立gnss天线相位中心的位置误差和imu位置误差的关系，再利用该关系建立量测矩阵。其关系可以写为：

76、

77、其中，和分别表示gnss天线相位中心和imu中心的位置误差，为imu的姿态矩阵，lb表示gnss天线相位中心到imu的杆臂，是状态向量中的姿态误差，表示反对称矩阵计算。

78、所以，根据以上的公式，量测残差向量可以写为：

79、

80、其中，δrgnss量测残差向量；p和l分别代表伪距观测值和载波相位观测值；为imu的姿态矩阵；lb表示gnss天线相位中心到imu的杆臂；μ为设计矩阵由各颗卫星的双差方向余弦构成；λ为该载波相位对应的波长；和分别表示载体在e系下的姿态、速度和位置的误差状态向量；δba、δbg的分别表示加速度计零偏和陀螺仪零偏的误差状态向量；δn表示模糊度误差状态向量；ηgnss为量测噪声。

81、本发明中提供的一个或多个技术方案，至少具有如下技术效果或优点：

82、本发明实现了gnss辅助的lidar-imu-camera在线时空标定算法。在基于拓展卡尔曼滤波的框架中加入了多传感器之间的时间和空间转换参数，通过载体的运动激励，使用原始观测值进行融合实现传感器的标定。

83、相比起其他标定技术大多针对两两传感器之间的标定，本发明能够实现多传感器的联合时空标定，在一次标定过程中就能同时得到lidar-imu和camera-imu的外参和时间延迟。而且标定的过程完全在线进行，不需要依赖其他特定的人造标定靶标，其标定的过程不需要繁复的过程和人为的干预。

84、此外，本发明所提出的标定方法在gnss全局信息的辅助下，可以实现更加精准的估计imu零偏和导航状态，降低线性化对滤波器的影响，并为视觉和lidar特征跟踪提供了精确的初始值，从而能够极大地提高标定的精度。