本发明属于机器人技术领域，具体涉及一种机器人定位与地图构建系统。
背景技术：
同时定位与地图构建(slam)的技术是机器人导航领域一个重要的问题，slam问题可以描述为：机器人能够对探索过的环境建立一个全局的地图，同时能够在任何时候，利用这个地图去推测出自身的位置。机器人通过带有传感器设备在环境中自由的移动，通过采集到的信息对自身位置进行定位，同时在定位的基础上进行地图构建，实现机器人的同时定位和建图。有两个主要的因素影响着slam问题的求解，分别是传感器的数据特征和观测数据相关性，如果能提高数据关联的鲁棒性和准确性并且能够提高传感器数据的利用率，将会提高机器人的定位和地图构建的精度。
目前移动设备上最常见的传感器有相机和imu传感器等，如何设计算法去利用这些常见的传感器设备实现高精度的同时定位和制图是目前的一个研究的热点问题。
本发明创新性的利用imu信息融合双目视觉信息，弥补双目视觉slam中存在的问题，并提高了slam算法的鲁棒性。在此基础之上，构建出具备真实尺度信息的地图。
同时定位与地图创建(slam)的技术是机器人领域比较经典的问题，slam问题可以描述为:机器人从某一未知位置开始在未知环境中运动,在移动过程中估计自身位姿和建立环境地图，实现机器人的自主定位和建图。影响slam系统的两个因素有观测数据的相关性和环境噪声，周围环境观测的正确性依赖于好的的数据相关性，进而影响到环境地图的构建。
技术实现要素：
为了克服已有机器人定位与地图构建系统的鲁棒性较差、准确性较低、适应性较差的不足，本发明提供一种鲁棒性好、准确性高、适应性强的基于双目视觉特征和imu信息的机器人定位与地图构建系统。
本发明解决其技术问题所采用的技术方案是：
一种基于双目视觉特征和imu信息的机器人定位与地图构建系统，所述系统包括双目信息采集、特征提取与匹配模块，改进的imu初始化及其运动模块，视觉slam算法初始化和跟踪模块，局部建图模块，回环检测和优化模块；所述双目信息采集、特征提取与匹配模块包括双目orb特征提取子模块、双目特征匹配子模块和imu信息采集子模块；所述改进的imu初始化及其运动模块包括imu角速率偏差估计子模块、重力加速度预估子模块、imu加速度偏差估计子模块和imu预积分子模块；所述视觉slam算法初始化和跟踪模块包括跟踪帧间运动子模块和关键帧产生子模块；所述局部建图模块包括新关键帧插入子模块、局部ba优化子模块和剔除冗余关键帧模块；所述回环检测及优化模块包括回环检测子模块和全局优化子模块。
进一步，所述双目信息采集、特征提取和匹配模块中，双目orb特征提取子模块通过接受机器人移动过程中双目相机拍摄的图片，使用orb特征提取算法提取左右图像中的orb特征，将提取的结果存储；双目特征匹配模块对左右图片提取的特征点进行匹配，得到特征点的深度信息；imu信息采集模块对机器人内部imu信息进行收集。
再进一步，所述改进的imu初始化及其运动模块中，imu角速率偏差估计子模块利用视觉slam结果，采用优化方法对imu角速率偏差估计；重力加速度及imu加速度偏差预估子模块，利用imu角速率偏差估计子模块的结果，对连续n个关键帧，利用其之间关系获得线性方程，求解得到重力加速度；对加速度偏差估计是将重力加速度对imu加速度的影响排除，在重新对连续当前帧状态的优化个关键帧两两结合，求解方程得到重力加速度的重估计，同时得到imu加速度偏差；imu加速度偏差估计子模块，利用重力加速度及imu加速度偏差预估子模块的结果，将重力加速度对imu加速度的影响排除，重新对连续n个关键帧求解方程得到重力加速度的重估计，同时得到imu加速度偏差；imu预积分子模块将连续两个图像帧之间的所有imu数据通过imu预积分模型集合成一个单独的观测值，得到机器人的运动模型与机器人当前的位姿估计。
更进一步，所述视觉slam算法初始化及跟踪模块中，跟踪帧间运动子模块计算连续两个图像之间的orb特征匹配，计算出本征矩阵，再利用集束优化对估算记过进行进一步估计；在成功返回后，系统开始以纯视觉的方式追踪，关键帧产生子模块监控追踪中间结果，当当前帧满足关键帧的条件则将当前帧加入关键帧集合当中，并把当前的关键帧作为追踪参考帧。
所述局部建图模块中，新关键帧插入子模块)根据上一子模块生成的关键帧，插入关键帧队列中；局部ba优化子模块)，在关键帧插入后，对当前局部视图的关键帧与地图点云作集束优化；剔除冗余的关键帧子模块，根据插入的关键帧，利用筛选策略删除窗口中多余的关键帧，保证地图的大小。
所述回环检测与优化模块中，回环检测子模块将当前帧在数据库中关键帧集合进行查询，检测回环；在成功返回后，对回环上所有关键帧的位姿进行估计，全局优化子模块对回环中的所有关键帧进行全局的图优化，误差因子包括视觉投影因子和imu因子。
本发明的技术构思为：本发明提出的机器人slam系统，是基于双目视觉和惯导信息融合slam系统的一种解决方案。本发明首先利用纯视觉位姿信息对imu偏差模型以及重力加速度方向等进行估计；在视觉位姿中，使用高效率的特征提取算法，对图像提取丰富的描述子，然后利用提取的特征点进行匹配，获取其深度信息；利用基于预积分的imu运动学模型建立相机的运动模型，采用运动模型对相机位置进行实时初步估计；在初步估计的基础上对前后两个图像帧之间位姿进行估计，在利用视觉几何知识，对机器人当前位置进行更加精确的估计；利用关键帧选择策略，实现局部地图点的稀疏构建；在融合imu信息的视觉信息匹配的基础之上，采用基于图优化的后端优化算法，将空间地图点投影因子与imu因子作为因子图中的因子，实时的对地图位置进行精确和实时的估计。本发明实现了基于双目视觉和惯导信息的机器人同步定位于地图创建算法，能够对机器人运动和周围环境信息进行精确的估计。
本发明的有益效果主要表现在：采用了视觉信息和imu信息融合的方法，能够很好的解决视觉slam中的问题，并提高slam系统的鲁棒性；准确性高，适应性强。
附图说明
图1是本发明系统结构图示。
图2是本发明系统流程图示。
具体实施方式
下面结合附图对本发明作进一步描述。
参照图1和图2，一种基于双目视觉特征和imu信息的机器人定位与地图构建系统，包括：双目信息采集、特征提取与匹配模块(1)、改进的imu初始化及其运动模块(2)、视觉slam算法初始化和跟踪模块(3)、局部建图模块(4)、回环检测和优化模块(5)；所述双目信息采集、特征提取与匹配模块(1)包括：双目orb特征提取子模块(1.1)、双目特征匹配子模块(1.2)、imu信息采集子模块(1.3)；所述改进的imu初始化及其运动模块(2)包括：imu角速率偏差估计子模块(2.1)、重力加速度预估子模块(2.2)、imu加速度偏差估计子模块(2.3)、imu预积分子模块(2.4)；所述视觉slam算法初始化及跟踪模块(3)包括：跟踪帧间运动子模块(3.1)、关键帧产生子模块(3.2)；所述局部建图模块(4)包括：新关键帧插入子模块(4.1)、局部ba优化子模块(4.2)、剔除冗余关键帧模块(4.3)；所述回环检测及优化模块(5)包括：回环检测子模块(5.1)、全局优化子模块(5.2)。
所述双目信息采集、特征提取和匹配模块(1)中，双目orb特征提取子模块(1.1)通过接受机器人移动过程中双目相机拍摄的图片，使用orb特征提取算法提取左右图像中的orb特征，将提取的结果存储；双目特征匹配模块(1.2)对左右图片提取的特征点进行匹配，得到特征点的深度信息；imu信息采集模块(1.3)对机器人内部imu信息进行收集；
所述改进的imu初始化及其运动模块(2)中，imu角速率偏差估计子模块(2.1)利用视觉slam结果，采用优化方法对imu角速率偏差估计；重力加速度及imu加速度偏差预估子模块(2.2)，利用子模块(2.1)结果，对连续n个关键帧，利用其之间关系获得线性方程，求解得到重力加速度；对加速度偏差估计是将重力加速度对imu加速度的影响排除，在重新对连续当前帧状态的优化个关键帧两两结合，求解方程得到重力加速度的重估计，同时得到imu加速度偏差；imu加速度偏差估计子模块(2.3)，利用子模块(2.2)的结果，将重力加速度对imu加速度的影响排除，重新对连续n个关键帧求解方程得到重力加速度的重估计，同时得到imu加速度偏差；imu预积分子模块(2.4)将连续两个图像帧之间的所有imu数据通过imu预积分模型集合成一个单独的观测值，得到机器人的运动模型与机器人当前的位姿估计；
所述视觉slam算法初始化及跟踪模块(3)中，视觉slam算法初始化及跟踪模块(3.1)计算连续两个图像之间的orb特征匹配，计算出本征矩阵，再利用集束优化对估算记过进行进一步估计；在子模块(3.1)成功返回后，系统开始以纯视觉的方式追踪，关键帧产生子模块(3.2)监控追踪中间结果，当当前帧满足关键帧的条件则将当前帧加入关键帧集合当中，并把当前的关键帧作为追踪参考帧，；
所述局部建图模块(4)中，新关键帧插入子模块(4.1)根据上一子模块生成的关键帧，插入关键帧队列中；局部ba优化子模块(4.2)，在关键帧插入后，对当前局部视图的关键帧与地图点云作集束优化；剔除冗余的关键帧子模块(4.3)，根据插入的关键帧，利用筛选策略删除窗口中多余的关键帧，保证地图的大小。
所述回环检测与优化模块(5)中，回环检测子模块(5.1)将当前帧在数据库中关键帧集合进行查询，检测回环；在子模块(5.2)成功返回后，对回环上所有关键帧的位姿进行估计，全局优化子模块(5.3)对回环中的所有关键帧进行全局的图优化，误差因子包括视觉投影因子和imu因子。
下面进一步具体介绍对模块的执行情况。
所述双目信息采集、特征提取和匹配模块(1)中：
双目orb特征提取子模块(1.1)，在每次机器人采集到左右图像后，用于对图像进行orb特征的提取，选取orb特征作为图像特征的原因在于orb特征具有较好的视角光照不变性，并且orb特征作为一种二进制的视觉特征，具有提取速度快和匹配速度快的特点，很适合要求实时性的slam系统。提取后orb特征后，将提取的结果存储。
双目特征匹配子模块(1.2)，由于左右图像经过极线校正，搜索匹配点时只需在同一行进行匹配，在每次机器人采集到图像提取特征点后，利用特征点的描述子匹配，利用双目视觉的三角法可计算特征点的绝对尺度，计算公式如下：
在上述公式中，f为相机的焦距，b为两摄像机光心之间的距离；d为视差，即左右图片同一特征点的距离差值，z为特征点的深度；
imu信息采集子模块(1.3)，用于对机器人内部的imu数据进行实时采集，并送至imu预积分子模块(4.1)中进行处理，这是因为imu信息采集模块所采集到的imu数据频率远高于视觉图像采集频率，利用imu预积分可以减少基于因子图的回环检测与优化模块(5)的计算压力。
所述改进的imu初始化及其运动模块(2)中，imu角速率偏差估计子模块(2.1)，利用双目视觉slam的结果，将imu角速率偏差变量加入到机器人状态向量之中，对imu角速率偏差估计。优化公式为：
其中，n是当前关键帧数量,
重力加速度预估子模块(3.2)，利用imu角速率偏差估计子模块(2.1)的结果，对于两个相连接的关键帧之间的关系，利用预积分测量值δv，得到方程：
在上述公式中，rwc代表摄像机中心c相对于世界坐标w的旋转矩阵，rwb代表imu中心b相对于世界坐标的旋转矩阵，pcb代表imu中心b在摄像机坐标下的位置，δt为帧间的时间差，
imu加速度偏差估计子模块(2.3)，利用重力加速度预估子模块(2.2)的结果，利用连续三个关键帧之间的线性关系：
用1,2,3代表i,i+1,i+2三个连续的关键帧，对上式中的各项计算方程如下：
在上式中，[](；1:2)指使用矩阵的前两列，rwc代表摄像机中心c相对于世界坐标w的旋转矩阵，rwb代表imu中心b相对于世界坐标的旋转矩阵，rwi代表当前坐标相对于世界坐标的旋转矩，pb代表imu中心b在世界坐标下的位置，i为单位矩阵δt为帧间的时间差，δv代表帧间的相机速度，δθxy是需要估计的重力加速度方向角，ba是加速度偏差。将所有的三连续关键帧等式堆积成一个线性方程，记作：
a3(n-2)×5x5×1＝b3(n-2)×1
通过上面的线程方程进行奇异值分解，得到重力加速度方向δθxy以及imu加速度偏差ba。
imu预积分子模块，将连续两个图像帧之间的所有imu数据通过b预积分模型集合成一个单独的观测值，得到机器人的运动模型与机器人当前位估计。将imu标记为b，imu输出的加速度与角速率表示为αb与ωb，则在两个imu输出i与i+1之间机器人的运动方程为：
在k与k+1两个关键帧直接的imu预积分结果记作δr、δv、δp，则得到两个关键帧之间的运动模型:
在上式中rwb代表imu中心b相对于世界坐标的旋转，vb代表imu的速度，pb代表imu中心b在世界坐标下的位置，bg、ba分别代表角速率与加速度的偏差。雅可比矩阵
所述视觉slam算法初始化及跟踪模块(3)中，跟踪帧间运动子模块(3.1)，利用orb特征提取子模块(1.1)提取特征后，对连续两个初始化图像fc,fr上的特征进行特征匹配，利用pnp算法求解出两者之间的位姿关系。此后可以利用slam系统追踪不断的对机器人位置进行估计。利用集束优化对估算进一步估计的原因在于集束优化可以修正位姿矩阵，使之更符合orb特征匹配结果。
关键帧产生子模块(3.2)，根据规则，决定是否将当前帧选择为关键帧。其规则如下：
1)至少离上一次重定位距离20帧；
2)局部建图空闲或离上一次关键帧插入相差已有20帧；
3)当前帧中至少含有50个特征点。
所述局部建图模块(4)中，新关键帧插入子模块(4.1)，利用子模块(3.2)生成的关键帧插入到队列中，更新内容有关视图和与关键帧具有相同地图点云产生的边缘，同时更新节点与其他关键帧的链接。然后，剔除局部视图不合格的云点，重新生成新的云点。
局部ba优化子模块(4.2)，局部ba优化当前的关键帧,在交互视图集中所有连接到它的关键帧，和所有被这些关键帧观测到的地图云点。所有其他能够观测到这些云点的关键帧,但没有连接到当前关键帧的关键帧会被优化；其中投影误差为：
eproj(i)＝ρ((x-π(xc))t∑i(x-π(xc)))
其中π(xc)为投影公式，x代表空间点在当前帧上的投影坐标，从世界坐标系坐标值xw到相机坐标系坐标值xc的转换为：
在上式中rcb代表imu中心b相对于当前相机中心c坐标的旋转矩阵，rbw代表世界坐标w对于imu中心b的旋转矩阵。pcb代表imu中心b相对于当前当前相机中心c坐标的平移向量，pwb代表imu中心b相对于当前世界坐标w的平移向量；
imu因子可以表示为:
其中各个误差项分布表示为：
上式中
剔除冗余关键帧模块(4.3),由于imu短时的特性，如果局部ba的本地窗口中的两个连续关键帧的差异不超过0.5秒，允许跟踪线程丢弃多余的关键帧。但是为了能够执行全局优化，在循环关闭之后或者在任何时候细化地图，我们不允许任何两个连续的关键帧差异超过3秒。如果我们使用imu约束关闭完整的ba，我们只需要限制本地窗口中关键帧之间的时间偏移。
所述回环检测与优化模块(5)中，回环检测子模块(5.1)，计算当前关键帧的词袋向量，与其视图相关的邻近图像计算相似度，保留最低分值。然后，我们检索图像识别数据库，丢掉那些分值低于最低分值的关键帧，将当前帧在数据库中关键帧集合进行查询。
全局优化子模块(5.2)，对回环上的所有关键帧进行估计，此时仅考虑视觉投影因子，投影因子公式如当前帧状态的优化子模块(4.2)中所示。
以在配备双目相机和imu的机器人的设备为例，详细介绍本实施例在此设备上的执行过程。
首先在双目与imu信息采集和深度信息获取模块(1)中，对双目视觉信息进行特征提取及特征匹配，并对imu信息进行采集，这两部分将获取的传感器信息传入后续模块，因为机器人在空间中运动，这两个部分将不断的对传感器新得到的数据进行采集和提取。利用双目相机采集图像，将图像送至特征提取算法，将提取到的图像特征的位置与描述子保存，同时利用提取到左右图片的特征点，进行匹配计算特征点的深度并保存。同时对imu传感器的信号进行采集并送至imu预积分算法进行处理。
当双目视觉slam执行一段时间后，改进的imu初始化及其运动模块(2)开始运行。首先是imu角速率偏差估计，将得到imu的角速率偏差估计结果。其次重力加速度预估计对重力加速度进行初步的估计，利用加速度偏差估计结果与尺度可得到重力加速度，可以对imu加速度偏差进行更为精确的估计。imu加速度偏差估计和重力加速度重估计是对imu加速度偏差进行估计，同时对重力加速度再次估计。利用双目与imu信息采集和深度信息获取模块(1)得到的imu信息数据，建立基于imu预计分的运动模型。
利用提取到的特征点信息，视觉slam算法初始化及跟踪模块(3)对视觉slam部分进行初始化操作。依次对追踪模块，关键帧产生模块和地图点进行初始化。初始化后的系统保存了初始化得到的地图点，以及最初的位置信息。视觉初始化完毕后，系统将执行跟踪算法部分，对帧间位姿进行估计。同时利用关键帧选择策略，生成关键帧。利用imu的运动学模型与双目视觉位姿信息，对每一个关键帧的速度进行估计，完成融合imu信息的算法的初始化工作。此部分能够使得双目视觉slam的结果更加鲁棒，减少跟踪失败的情况。
跟踪算法解决了关联的数据问题，之后将进行局部地图的构建。根据上一模块生成的关键帧，在局部视图里，对当前关键帧及其相关视图所看到的地图点云，进行局部ba优化，优化包括对imu及视觉位姿的同时优化。优化后，可以删除队列中一定时间间隔的关键帧，以避免在区域内增加地图的大小。
利用基于图优化算法的回环检测与优化模块(5)对构建的地图进行全局优化，获得有关机器人位置和地图点更精确的估计。这一模块分为两个部分分别进行。首先检测回环子模块(5.1)计算当前关键帧的词袋向量，与其视图相关的邻近图像计算相似度，保留最低分值。然后，我们检索图像识别数据库，丢掉那些分值低于最低分值的关键帧，将当前帧在数据库中关键帧集合进行查询，当检测到回环，则对回环上所有关键帧的位姿进行估计。全局位姿优化子模块(5.2)在每个关键帧插入的时候进行，为了对关键帧和地图点的位置进一步估计，并且对当前所有关键帧的位姿在当前情况下进行进一步的优化，这一步利用了回环检测的结果，能够消除累计的误差照成的影响。