一种基于双目图像分析与紧耦合SLAM融合的导航定位系统

本发明涉及定位导航，具体涉及一种基于双目图像分析与紧耦合slam融合的导航定位系统。

背景技术：

1、lbs由室外定位系统和室内定位系统两类组成。室外定位系统研究较为成熟，常用的包括gps和我国自主研发的北斗卫星导航系统，这类系统在户外精度高、信号稳定性强，配合相应设备通常可达到亚米级。然而当处于室内、隧道等卫星信号衰弱甚至屏蔽的地点，定位将产生剧烈漂移，因此室内导航定位已经成为当前的一大研究热点。

2、面对当前公共建筑愈发复杂庞大，居民对陌生环境下室内定位导航的需求日益高涨。在室内导航定位系统的研究中，目前主流研究思想是搭建无线传感器网络的实现方案，常用的方法包括无线网络、蓝牙和超宽带等，但是这些方法通常受制于设备网络系统的搭建。未解决上述问题，我们将图像分析应用于室内惯性导航系统中，采用多传感融合的思想提高室内导航定位的精度，以实现非特征环境下的精准感知与导航定位技术关键技术和应用。

技术实现思路

1、本发明的目的是提供一种基于双目图像分析与紧耦合slam融合的导航定位系统，以解决现有技术中的上述不足之处。

2、为了实现上述目的，本发明提供如下技术方案：优选的，

3、一个实时同步定位、建图和着色框架，一种基于双目图像分析与紧耦合slam融合的导航定位系统，用于弱gnss信号复杂环境下。该系统包括数据采集层、数据处理层、应用层：

4、数据采集层使用基于激光-惯导-视觉-imu-gps融合slam方法，将激光点云特征与imu数据和相机提取并优化后的特征信息进行一同运算，提高整个系统在非结构化环境中的定位精度，通过关键帧的匹配降低整个系统的计算量，使得精度与计算量之间取得平衡，保证了系统的准确率和实时性。

5、数据处理层将下层产生的数据处理整合，通过双目视觉图像分析方法进行灰度化后实景分析和消失点检测，估算出的地面平面位置及相应宽度深度，并通过最小光度误差与特征匹配渲染地图的纹理，构建地图几何结构，实现精准定位与建图。

6、应用层将已提前训练完成的规划路径上传至自主决策系统，为实现最终的自主定位导航系统。

7、数据采集层中包含以下步骤：

8、1、使用双相机、imu和激光雷达提取特征信息

9、2、对于传入的lidar扫描，由于帧内连续移动而导致的运动失真由imu反向传播补偿。

10、3、将全局地图中的一定数量的点(即跟踪点)投影到当前图像。订阅当前激光帧运动畸变校正后的点云信息。

11、在数据处理层中，包含以下步骤：

12、(1)、首先将下层产生的数据整理，进行特征提取与imu预积分。

13、常用的imu必须必须包含用于测量角速度的三轴陀螺仪和用于测量加速度的加速度仪，再通过运算求得运动目标的速度、空间位置等信息。

14、三轴轴陀螺仪是测定角速度的装置，其由两相同质量的块构成，两质量块速度相反，其基本工作原理是科里奥利力，角速度公式为：

15、ωm(t)＝ω′(t)+bg(t)ng(t)   (1)

16、式中,ω(t)为角速度真值；ω′(t)为测量值；bg(t)为零点漂移；ng(t)为测量噪声。

17、三轴加速度计时惯性传感器，其实测定目标相对于载体坐标系的重力加速度和绝对加速度之和的作用，其测量值基本公式如下：

18、am(t)＝r′(t)(am(t)-gw)+ba(t)+na(t)   (2)

19、式中，r′(t)表示世界坐标系转换到体坐标系的旋转；am(t)为相对世界坐标系下的加速度矢量；gw为相对世界坐标系下的重力矢量；na(t)是测量噪声；ba(t)表示加速度计的漂移误差。

20、并将完整状态向量x∈r29定义为：

21、x＝[grit,gpit,gvt,bgt,bat,ggt,irct,ipct,itc,φt]t   (3)

22、其中gg∈r3是在全局帧(即第一个lidar帧)中表示的重力矢量，itc是imu和相机之间的时间偏移，同时假设lidar已经与imu同步，φ是相机内参矩阵。

23、p＝[gpx,gpy,gpz,cr,cg,cb]t＝[gpt,ct]t   (4)

24、(2)、其双目视觉图像分析包括以下步骤：

25、1)、先将每个相机的固有参数和两个相机之间的非固有变换进行离线校准。

26、2)、采用两个图像采集设备获取的图片后对每一张图片提取角点，对相应角点进行匹配和光流法跟踪。

27、3)、对结果进行紧耦合优化，在紧耦合优化滑动窗口时，需要进行视觉残差计算，其是指在当前帧上某一特征点的投影误差。对于特征点i，在第b帧进行观察时视觉残差为：

28、

29、式中,和分别指代当第b帧进行观察时系统估算出的特征点坐标和图片采集设备在成像平面上实际坐标；rc值准确度受两方面影响,一方面是成像本身制约因素，另一方面是图像观察结果,最主要影响因素是图片采集设备在进行离线校准标定结果是否准确无误。本发明在实际实验时采用自标定相机,内参非常精准,视觉残差可低至1像素左右。

30、4)、引入em算法进行正交约束，使用高斯球体进行基于em的方向检测估计，设三个主消失方向分别为vdi，i＝1，2，3，通过em对目标检索获取的线段li聚类，被检索线段对应的法线为nl，设z是图像潜在变量的簇，那么相应的高斯分布为：

31、

32、式中,为高斯方差，先验概率p(zj＝i)＝pi。

33、5)、把滑动窗口中视觉残差和惯导单元中预积分数据约束放在同一框架之下进行优化处理。

34、在本系统设计中，采用紧耦合数据融合，滑动窗口内的状态向量为

35、

36、式中,为视觉与惯导的平移量；为视觉与惯导的旋转量；λn为逆深度信息；xk为惯导的状态变量,包括速度、三维位置坐标、深度等。

37、最终就构成了一个非线性最小二成问题，见下式：

38、

39、式中,至是视觉残差；c是观测点集；是惯导单元预积分数据约束。

40、对于实时方向的估计更新，本发明将从视频帧获得的方向与来自imu的角度测量相结合。

41、x＝[γωλ]   (9)

42、

43、pt-＝apt-1at+q   (11)

44、式中,x为状态向量；γ为来自视频的横滚角；ωγ为来自陀螺仪的横滚角pt-是t时刻先验估计协方差；q是过程激励噪声协方差,有更新方程为：

45、

46、

47、

48、式中，errort+1为误差观测；h为测量矩阵。

49、在对偏航脚和俯仰角的估计中，可以采用从加速计提取的重力分量表示，再用上述方法融合。

50、

51、

52、式中,βg、αg分别为偏航角和俯仰角；xg、yg、zg分别为重力在每个方向上的分量。

53、6)、采用边缘化先验法解决，对于无关状态标量进行消元整合。

54、高斯牛顿法求解非线性最小二乘问题公

55、式为：

56、

57、式中,δxa、δxb为状态变量,通过高斯消元消去ct,求得

58、(b-cta-1c)δxb＝bb-cta-1ba   (18)

59、从而舍弃状态δxa，仅保留系统中的δxb以提高计算效率。

60、(3)、其构建地图几何结构包括以下步骤：

61、1)、对于传入的lidar扫描，由于帧内连续移动而导致的运动失真由imu反向传播补偿。

62、2)、利用误差状态迭代卡尔曼滤波器(esikf)最小化点对平面残差来估计系统的状态。

63、3)、通过双目视觉图像分析系统，进行灰度化后实景分析和消失点检测，估算出的地面平面位置及相应宽度深度。

64、4)、在收敛状态下，该扫描的点被附加到全局地图上，并将相应的体素标记为激活或停用。

65、(4)、其渲染地图的纹理包括以下步骤：

66、1)、利用帧到帧光流来跟踪地图点并通过最小化perspective-n-point pnp)来优化系统状态跟踪地图点的投影误差。

67、假设跟踪了m个地图点ρ＝{p1,...pm}在最后一帧图像ik-1上，投影为{ρ1k-1，…ρmk-1}，它们在当前图像ik中的位置表示为{ρ1k，...ρmk}。

68、perspective-n-point投影误差：以第s个点ps＝[gpst,cts]∈ρ为例，投影误差计算如下:

69、

70、

71、其中在每次esikf迭代中的当前状态估计值,计算方法如下：

72、

73、

74、

75、其中和分别是gps和的真实值。然后，我们得到真零残差的一阶泰勒展开

76、

77、

78、

79、

80、

81、

82、

83、

84、

85、

86、k＝(htr-1h+p-1)-1htr-1 (33)

87、

88、2)、通过最小化跟踪点之间的帧到地图光度误差来进一步细化系统的状态估计。

89、以第s个跟踪点ps∈ρ为例，光度误差计算如下：

90、

91、考虑ys和cs的测量噪声：

92、

93、

94、

95、

96、

97、

98、

99、

100、

101、

102、表示h，r，和p相似：

103、

104、

105、

106、

107、这个帧到地图vio esikf更新被迭代直到收敛。然后将收敛状态估计用于渲染地图的纹理；更新当前跟踪点集p以供下一帧使用；在lio或vio更新的下一帧中作为imu传播的起点。

108、

109、3)、使用收敛状态估计和原始输入图像，我们执行纹理渲染以更新全局地图中点的颜色。

110、首先，在所有激活的体素中的检索所有点。假设总共有n个点，记为ζ＝{p1,...pn}。如果点ps落在当前图像帧中，通过对当前图像帧上相邻像素的rgb颜色值进行线性插值，得到其观测颜色γs和协方差通过贝叶斯更新，图像上新观测到的点的颜色与记录在地图中的现有颜色值cs进行融合，得到更新后的颜色值和cs的协方差：

111、

112、

113、

114、4)、在纹理渲染完成后，我们对所跟踪的点集p进行更新：若点集p的点计算出的pnp重投影误差或者光度误差较大，则从跟踪点的集合中将其删除；若将点投影到当前图像帧上，附近没有其他跟踪点(比如设置半径为50像素)，则将其添加到点集p中。

115、在应用层中，包含以下步骤：

116、应用层将已提前训练完成的模型上传自主定位导航系统。

117、其自主定位导航系统识别接收到的视频信息，进行图像处理。图像处理器基于nvidia jetson tx2平台进行搭建，deep learning中的gan算法对图像进行增强，选用roberts算子模板进行边缘检测。通过dqn网络对路径进行规划，最后通过自主定位导航系统将路径规划推荐给用户。

118、在上述技术方案中，本发明提供的技术效果和优点：

119、1、本发明通过将图像分析应用于室内惯性导航系统中，提高对于陌生环境的适应性，提高室内导航定位的精度。

120、2、本发明通过提出了一个实时同步定位、建图和着色框架，一种基于双目图像分析与紧耦合slam融合的导航定位系统，解决了基于lidar、惯性和视觉测量的紧耦合融合的实时同步定位、3d建图和地图渲染问题，提高系统的鲁棒性和准确性，避免了弱gnss信号下定位不准，误差较大的问题，解决了单一传感器数据捕获和处理精度不高的问题。