一种不确定性驱动的视觉SLAM信息筛选与感知规划方法

本发明涉及一种不确定性驱动的视觉slam信息筛选与感知规划方法。

背景技术：

1、复杂城市环境下，搜救无人机的飞行高度主要集中在几百米以内的低空空域，在室外飞行时，其飞行空间内不仅存在高大建筑、电线杆等障碍物，还存在大气紊流、阵风等复杂气流干扰，而用于搜救的无人机尺寸小、质量轻，飞行速度低，其受到外界扰动的数量级相对较高，经常导致无人机状态信息剧烈变化，迫切需要实时准确和可靠获取飞行器的飞行运动状态信息，以确保飞行安全。此外，由于复杂城市环境下搜救任务的特殊需要，无人机还需要在民宅、停车场等建筑物内部飞行，而建筑物内部空间狭小、环境未知、全球定位系统、北斗等卫星导航方式使用受限等因素，也对无人机的自主定位和感知规划提出了新的挑战。另一方面，随着微型化高效能源技术的发展，搜救无人机飞行工作时间也会逐步提高，相应地对更长留空时间下的导航精度、可靠性和连续性提出了更高要求。由此可见，搜救无人机在执行任务时需要具备较强的自主探索、感知与运动规划能力，并且保持较高的自主性和可靠性，以满足复杂城市环境下搜救无人机自主探索与可靠飞行的需要。因此在自主导航和探索过程中，筛选出贡献度大的信息，减小不确定性，同时规划一条有利于定位性能提升的轨迹，具有很高的必要性和工程应用价值。

技术实现思路

1、本发明提供一种不确定性驱动的视觉slam信息筛选与感知规划方法。本发明解决了微小型无人机自主探索过程中随着时间的累积，slam系统的不确定性将逐渐增大，尤其是在没有形成回环的情况下，不确定性对系统的影响更加显著的问题，进行了信息筛选与运动规划，减小了无人机自主探索过程中的不确定性，提高了微小型无人机自主探索过程中的定位精度，适合于工程应用。

2、本发明所采用的技术方案有：

3、一种不确定性驱动的视觉slam信息筛选与感知规划方法，包括以下步骤：

4、s1)：推导双目视觉slam重投影误差相对于位姿和地图特征点的jacobian矩阵，进而借助fisher信息矩阵推导出双目视觉slam不确定性的cramér-rao下界；

5、s2)：根据步骤s1)的推导，在前端视觉里程计中，设计基于fisher信息的里程计观测信息筛选方法，筛选出贡献度大的观测信息用于前端里程计的跟踪；

6、s3)：根据步骤s1)的推导和步骤s2)得到的前端跟踪结果，在后端优化中设计基于fisher信息的局部ba优化观测信息筛选方法，筛选出不确定性大于一定阈值的观测信息进行剔除；

7、s4)：根据步骤s3)的筛选结果，在局部ba优化中利用fisher信息和度量准则对不确定性的表征与量化，设计了基于局部状态不确定性的主动回环规划方法，进一步将slam算法和规划算法相结合。

8、进一步地，所述步骤s1)具体为：

9、记世界坐标系下空间点的坐标为pw(xw,yw,zw)，将该空间点变换到相机坐标系下，坐标为pc(xc,yc,zc)，建立双目相机投影模型，如下：

10、

11、

12、

13、式中，u、v为双目相机中左目相机对应的像素点横纵坐标、ur表示双目相机中右目相机对应的像素点横坐标，fx、fy、cx、cy为相机内参，bf为相机基线长度。

14、记(u,v,ur)的重投影误差为e，并对ξ∧左乘一个扰动δξ，考虑误差关于扰动的偏导数，利用链式法则，可以得到：

15、

16、上式即为位姿优化的jacobian矩阵，其中，

17、

18、

19、若要优化地图上的特征点，则用误差项对地图上的特征点求偏导，即

20、

21、上式即为地图特征点优化的jacobian矩阵，其中，

22、

23、至此，推导出了对双目视觉slam位姿和地图特征点优化极为重要的两个jacobian矩阵，上述jacobian矩阵对双目视觉slam不确定性的cramér-rao下界推导同样十分关键。设双目相机的像素重投影误差的协方差矩阵为σuvr，则关于位姿优化的fisher信息矩阵为：

24、

25、关于地图上特征点优化的fisher信息矩阵为：

26、

27、根据cramér-rao下界的定义，位姿不确定性的cramér-rao下界为i-1(ξ)，地图特征点不确定性的cramér-rao下界为i-1(pw)。

28、利用cramér-rao下界与fisher信息矩阵之间的关系，在slam不确定性评估过程中，可根据观测与待优化变量之间的关系，计算fisher信息矩阵，并利用d-opt度量准则，评估当前状态下的不确定性。

29、进一步地，所述步骤s2)具体为：

30、在前端视觉里程计中，首先需要在当前图像帧与关键图像帧之间进行特征点匹配，得到对应的匹配特征点之后，将两图像帧之间的位姿作为待优化状态，构建最小二乘问题，采用图优化的方法对当前图像帧对应的相机位姿进行优化。在此优化过程中，设定位姿节点为当前图像帧对应的相机位姿，边为多个地图特征点对应的重投影误差构成的一元边。

31、在无人机搜救场景中，当前图像可能匹配到若干个特征，根据每一个特征点的重投影误差构建观测信息的做法在特征点很多的情况下会影响前端视觉里程计的计算效率。因此，本发明设计了基于fisher信息的里程计观测信息筛选方法，具体为：

32、s21)首先根据匹配到的特征点的观测信息构成的边，计算该条边对应的fisher信息矩阵以及不确定性的cramér-rao下界，根据d-opt度量准则量化该观测的不确定性。

33、s22)其次，由于特征点是采用图像金字塔提取到的orb特征，金字塔一共分七层，每层对应的特征点位置的方差不同(考虑每个特征点位置的图像坐标u和v相互独立，特征点位置的不确定性可以用方差阵表示)，在每层中根据fisher信息筛选出不确定性大于一定阈值的观测信息进行剔除，不将其纳入位姿优化的过程中，因为此类观测信息对当前位姿计算的贡献度不大。该过程对应的fisher信息矩阵为：

34、

35、其中，ξ表示相机的位姿，为像素重投影误差的协方差矩阵。

36、通过上述方法，可以在保证精度相当的前提下，提升前端里程计位姿计算的效率。

37、进一步地，所述步骤s3)具体为：

38、前端里程计根据当前图像帧与关键图像帧之间匹配的特征点，以及特征点的重投影误差，估计了两帧图像之间的相对位姿，进而通过航迹推算的方式实现了当前位姿的估计。然而，由于观测噪声的存在，该方式的定位误差将不可避免地随着时间的推移而逐渐累积。因此，在新的关键图像帧出现时，首先采用光束平差法对局部的位姿和地图特征点进行优化，从而获得更精确的位姿和地图信息。该优化过程中同时对地图特征点和相机位姿进行优化。考虑到局部ba中的观测更多，本节在常规的ba优化基础上，提出了基于fisher信息的局部ba优化观测信息筛选方法，在众多观测信息中，根据fisher信息矩阵和不确定性度量，筛选出不确定性大于一定阈值的观测信息进行剔除。与步骤s2)不同的是，由于同时对多个位姿和地图点进行优化，因此需要同时考虑观测信息给位姿和地图点带来的不确定性，此时的fisher信息矩阵为

39、

40、其中，ξj表示相机的位姿，pk表示特征点的空间位置，为像素重投影误差的协方差矩阵。

41、通过上述观测信息筛选方法，可以在局部ba优化的过程中，提升相机位姿和地图点优化的计算的效率。

42、进一步地，所述步骤s4)具体为：

43、在局部ba优化中，由于同时优化了相机位姿与地图特征点，所以连接二者之间的边为二元边。随着关键图像帧的不断加入，局部地图中的位姿节点和地图特征点越来越多，在用fisher信息矩阵表征不确定性时，将它们分别列为状态会使得fisher信息矩阵的维数迅速增大，由此带来的计算负担成倍上升，因此。为了减轻计算负担，本发明将局部ba优化过程中所有的状态(相机位姿、地图特征点)看作一个整体，本发明将其定义为广义节点，连接相机位姿与地图特征点的二元边就变为了连接状态内部的一元边，本发明将其定义为广义一元边。通过上述方式，表征slam不确定性的fisher信息矩阵的维数就固定了下来，等于相机位姿的维数加上地图特征点的维数。

44、根据上述广义一元边的定义，整体的fisher信息矩阵分为以下两个部分：

45、(1)相机位姿的不确定性

46、

47、(2)地图特征点的不确定性

48、

49、由上述两个公式，可以得到完整的表征局部状态不确定性的fisher信息矩阵，用如下公式表示。

50、

51、采用d-opt度量准则，分别对局部ba优化中相机位姿、地图特征点和整个局部状态(即相机位姿与地图特征点作为一个整体)的不确定性进行量化，可以分析出局部地图的不确定性。当某个时刻局部状态的不确定性大于某一阈值时，进行主动回环规划，进一步减小状态的不确定性。

52、本发明具有如下有益效果：

53、1)本发明方法能够在复杂城市环境下执行任务时，如搜索救援、自主探索、结构健康监测环境中，实现基于观测不确定性的信息筛选与优化，并且基于局部状态不确定性进行主动回环规划，满足自主导航与探索过程中的高精度定位需求。

54、2)本发明通过在slam前端视觉里程计和后端优化中，设计基于fisher信息的里程计观测信息筛选方法以及局部光束平差优化观测信息筛选方法，筛选出了不确定性小的观测信息用于搜救过程中的定位与建图，在保证定位精度的前提下，提高了系统的运算效率。此外，根据量化的局部位姿与地图特征点的不确定性，设计了基于局部状态不确定性的主动回环规划方法，有利于辅助搜救无人机的探索与决策。