一种多传感器信息融合的自主导航方法及系统

本发明属于融合导航，具体涉及一种多传感器信息融合的自主导航方法及系统。

背景技术：

1、导航定位技术在人类的社会、经济和军事活动中越来越发挥着重要的支撑作用。随着自动驾驶、无人机和机器人等技术的兴起，高精度、高可靠性、高可用性的低成本导航定位需求得到空前增长。同时定位与建图（simultaneous localization and mapping,slam）所关注的问题是载有传感器的机器人在未知的环境中定位并构建环境地图，是机器人感知自身状态和外部环境的关键技术，通常采用视觉、激光等环境传感器，并结合捷联惯导（sins，strapdown inertial navigation system）进行相对定位与建立地图。在移动机器人实现自动化操作时，先决条件便是准确地估计自身的位置和姿态，并感知所处环境。在准确地感知自身位姿和环境后，机器人才能进行自主决策、规划和控制，完成自动化操作。然而，slam递推不可避免地造成了误差累积，使其无法在大规模、长距离场景下给出可靠的位姿与地图。为解决这一问题，一方面可以检测轨迹闭环，通过回环约束消除部分累积误差；另一方面则通过在开阔环境融合全球导航卫星系统（global navigation satellitesystem, gnss）的绝对观测信息，消除累积误差，提高定位定姿与建图精度，进而提升已建图区域的定位定姿精度。

2、多源融合导航定位与建图方法多基于因子图优化设计，其采用预积分方法提供历元间导航参数的变化约束，并将其余传感器观测因子化，通过因子图优化解算导航参数，利用估计的位姿将点云投影至世界坐标系完成建图。该方法较传统的滤波方案有精度高、可扩展性高等特点，是目前导航定位与建图的主流方法。

3、目前，基于因子图优化的多源融合导航定位方法仍存在以下问题：

4、1）现有方法仅融合视觉或激光单个环境传感器，尚未构建统一的多源融合导航定位因子图模型；

5、2）可分布式部署；或现有的分布式节点方法依赖于主节点，容灾能力差；

6、3）现有方法多采用传统预积分方法，未考虑地球自转、重力变化带来的影响，无法发挥高精度惯性导航器件应有的价值；

7、4）现有方法中激光雷达多采用传统的点到点匹配方式，无法估算协方差，其观测难以融合。

技术实现思路

1、针对以上问题，本发明给出了一种多传感器信息融合的自主导航方法，通过分布式进程设计，融合视觉、激光、惯导、gnss观测信息与已建图区域的匹配定位信息，获取载体高精度位置与姿态，实现根据任务需求灵活部署，且无需人工干预，实现载体自主导航。

2、为达到上述目的，本发明方法的技术方案为：

3、一种多传感器信息融合的自主导航方法，包括以下步骤：

4、步骤s1：接收gnss、sins、视觉和激光雷达原始观测数据并进行初步筛选；

5、步骤s2：接收初步筛选后的gnss、sins、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将sins观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；

6、步骤s3：接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；

7、步骤s4：接收步骤s3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息。

8、进一步地，所述步骤s1中设置4个独立的线程并行处理不同传感器，包括gnss线程、sins线程、视觉线程和激光雷达线程；

9、其中，gnss线程接收gnss原始观测数据，包括伪距、相位和多普勒，并采用实时动态rtk定位模型建模观测值，选取不同卫星导航系统高度角最高的卫星为基准，剔除可视卫星少于4的卫星导航系统；并通过卫星prn号匹配基准站和流动站之间的共视卫星；

10、sins线程根据视觉/激光相邻两次观测选取积分区间，匹配区间内的所有sins观测值存入队列；

11、视觉线程通过orb角点提取算法提取影像中的特征信息，通过光流法匹配方法匹配不同影像间的同名特征点，当可用特征点少于阈值时，重新提取图像中orb角点；

12、激光雷达线程从原始激光雷达点云中提取平面和边缘点，其中原始点云中低曲率和高曲率的点分别对应于数据环境中的平面和线，随后将其参数化；通过计算不同帧间的线-线距离与面-面距离匹配同名线/面特征；上述激光点云曲率表示如下：

13、

14、式中，为该点在统一扫描线的前后五个点的集合，；、分别为集合中点i和点j的深度，若c大于阈值则为线特征，反之则为面特征。

15、进一步地，提取激光雷达点云中线和面的步骤如下：

16、对于属于第 i条扫描线的点，首先确定在扫描线中的最近点，然后利用和计算一条线，并在条扫描线中找到的最近点，若与线之间的距离小于给定阈值，则使用，和拟合新线，这个过程重复进行，直到不能添加更多的点为止，其中，l为所有线属性点的集合；

17、在提取平面时，从第一条扫描线开始，首先找出在第 i条扫描线中的每个片段 p中的点，其 k个最近的点位于 p中但不在第 i条扫描线中，然后，使用svd算法将这些点拟合到 p中的一个平面上，接着，该平面可以由其方向向量 n表示，该向量是与 a的最小特征值相关联的特征向量，计算所有点到平面的距离之和，并排除大于设定阈值的平面。

18、进一步地，所述步骤s2包括如下子步骤：

19、步骤s2.1：并行接收初步筛选后的gnss、sins、视觉和激光雷达观测数据；

20、步骤s2.2：通过插值方法将sins观测数据对齐至区间起止位置，并通过顾及地球自转和区域重力变化的预积分方法对观测值积分，得到历元间相对位姿，得到sins解算信息；

21、步骤s2.3：利用历元间相对位姿，将前一帧激光点云中的线面投影至当前帧，通过设定阈值判断线到线的距离与面到面的距离，并剔除大于阈值的线或面，得到精化后的激光雷达观测值，即激光雷达解算信息；

22、步骤s2.4：通过随机采样一致方法ransac计算基础矩阵和单应矩阵，以剔除视觉观测中的错误匹配，得到精化后的视觉匹配观测值，即视觉解算信息；

23、步骤s2.5：将高度角低于设置阈值的卫星观测值剔除，得到精化后的共视观测卫星，即gnss解算信息。

24、进一步地，所述步骤s2.2具体实施步骤如下：

25、步骤s2.21：根据低频观测传感器采样时间确定积分间隔，设定低频观测传感器相邻两次观测时间分别为和，在此间隔内有 n个sins量测，即积分所需的观测值，积分区间即为；

26、步骤s2.22：采用考虑地球自转的姿态双子样更新算法积分陀螺输出；进一步计算考虑圆锥效应和划桨效应补偿的速度、位置在积分区间内的增量；

27、步骤s2.223：通过预积分误差模型传播不确定度，为预积分观测值在多传感器融合中提供权值；

28、设定时刻的姿态预积分为，则时刻的姿态预积分为：

29、

30、世界坐标系的姿态增量约束为：

31、

32、式中，表示相对惯性系固定的历史世界系，即历史的 e系；表示imu量测参考系；为相对惯性系固定的历史imu量测参考系；为相对 e系固定的历史imu量测参考系；分别表示imu积分区间起止时刻；表示参考坐标系a至b的旋转矩阵；为地球自转补偿项，与其对应的旋转矢量的解析形式为：

33、

34、其中，为积分时长，为地球自转角速度，若忽略地球旋转补偿项，上式退化为传统的姿态预积分，

35、设定时刻的速度预积分为，则时刻的速度预积分为：

36、

37、上式中，为划桨效应补偿项，其中包含了科里奥利力补偿，

38、设定重力为常值，使用表示整个区间内的重力，则圆锥效应补偿项可表示为：

39、

40、设定时刻的位置预积分为，则时刻的位置预积分为：

41、

42、位置的科里奥利力补偿项近似形式为：

43、

44、对积分区间内的所有imu量测按照如上方式递推，可分别得到区间内顾及地球自转的位置、速度及姿态预积分约束、及。

45、进一步地，所述步骤s3包括如下子步骤：

46、步骤s3.1：判断当前区域是否有可用的地图，若已建图，则仅构建匹配定位因子；

47、步骤s3.2：判断当前轨迹是否回环；若存在轨迹闭环，则将精化后的位姿和点云更新至实时系统；

48、步骤s3.3：接收步骤s2中所述的各传感器解算信息，并将其因子化；同时，将各个观测因子插入因子图中，并设置滑动窗口求解待估计参数，并计算其验后协方差信息；

49、步骤s3.4：序贯更新gnss观测值，首先判断当前历元是否有gnss观测值，若有，则以滑窗解算结果为先验，融合gnss观测值；若当前gnss数据质量差、gnss信号缺失，则不更新；

50、步骤s3.5：窗口滑动，通过边缘化移出窗口内最早一帧观测值，得到该帧的解算结果，及窗口内的视觉及激光点云。

51、进一步地，激光雷达采用参数化线面的因子化方式：

52、对于点到线距离观测，使用普朗克坐标将线特征参数化为，为直线的方向向量，为直线的中心点，则激光线因子表达为：

53、

54、式中，为线特征点，为激光雷达中心在世界坐标系的位置，表示局部坐标系，设定，，，通过协方差传播法则确定的不确定性，根据方差传播定律，表达为：

55、

56、其中：

57、

58、对于平面而言，其观测因子为：

59、

60、式中，为平面法向量；为平面中心；为投影到世界坐标系下的面特征点：

61、

62、式中为激光雷达中心在局部坐标系的位置。同理根据方差传播定律可得点面观测值的不确定度：

63、

64、其中：

65、

66、imu因子如前所述建模为顾及地球自转的预积分因子，gnss因子和视觉观测因子则分别通过rtk和重投影误差建模。

67、进一步地，所述步骤4包括如下子步骤：

68、步骤4.1:初始化位姿序列集，接收步骤s3中解算后的位姿及点云，并将点云投影至视觉图像中，以其作为初值提取特征信息，将位姿插入位姿序列集中；

69、步骤4.2:采用dbow对新插入的图像检测回环；若轨迹闭环，则以闭合差为约束，优化位姿，并将激光点云投影至全局坐标系；

70、步骤4.3:对回环轨迹构建重投影误差并固定位姿，精化视觉点云，最终将视觉点云投影至与激光点云相同的全局坐标系，并将二者融合；

71、步骤4.4:将精化的点云及位姿发送至实时系统，提高实施导航定位的可用性。

72、进一步地，准实时或离线建图系统接收实时系统发送的位姿及点云信息，通过词袋模型（dbow）检测回环，并将该帧位姿加入至位姿序列集中，设定为轨迹上的一帧，在随后的第帧检测到轨迹闭环，此时回环约束表达为：

73、

74、式中，、分别为第帧与第帧的相对位置及姿态变化；残差表达为：

75、

76、而对于在回环轨迹中的所有相邻位姿，均有：

77、

78、式中，、分别为第帧与第帧的相对位置及姿态变化，；残差表示为

79、

80、则回环的总体目标函数为：

81、

82、其中，为回环轨迹序列中所有的位姿，上述方法为位姿网平差，由此得到最优位姿序列，此后，对于位姿序列集中的所有位姿及视觉点云，匹配共同观测后建立重投影误差，进一步精化视觉点云，此时，位姿及视觉、激光点云均为最优解，将视觉及激光点云融合后即可得到高精度点云地图。

83、另一方面，本发明还提供一种多传感器信息融合的自主导航系统，包括：

84、原始观测数据初步筛选模块：其用于接收gnss、sins、视觉和激光雷达原始观测数据并进行初步筛选；

85、解算信息精化模块：其用于接收初步筛选后的gnss、sins、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将sins观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；

86、导航状态解算模块：其用于接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；

87、建图优化模块：其用于接收步骤s3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息；

88、且上述模块间采用低延时通信方法zeromq通信，各个模块可根据不同任务需求采用分布式进程部署。

89、所述多传感器信息融合的自主导航系统用于执行上述多传感器信息融合的自主导航方法中的步骤。

90、与现有技术相比，本发明具有如下有益效果：

91、1)充分发挥了传感器间互相补充的优势，建立了融合四类传感器的通用因子图模型；

92、2)各个进程任务明确，可灵活分布式部署，充分发挥可用的计算资源，提高解算效率；

93、3)采用顾及地球自转和区域重力变化的预积分因子和，充分发挥惯性器件性能，提高历元间约束的精度；

94、4)采用参数化线面的激光雷达观测因子，解决了传统的点到点匹配方法协方差无法估计的难题，降低了激光观测的融合难度。