一种点线特征融合的紧耦合双目视觉惯性SLAM方法与流程
本发明涉及视觉惯性融合的同步定位与地图构建(slam)领域，特别是涉及一种点线特征融合的紧耦合双目视觉惯性slam方法。
背景技术：
同步定位与地图构建(simultaneouslocalizationandmapping,slam)是机器人实现自身位置确定以及感受周围环境的重要手段，也是自主导航领域的基础问题和研究热点。它主要解决的是机器人进入陌生环境后，如何通过传感器(如：双目相机，惯性测量单元等)获取的数据，计算出周围环境的地图表示，并且同时确定自身所处地图中的位置的问题。
双目视觉传感器具有轻量级、易安装、价格低廉以及尺度确定性等优势，使得双目视觉slam成为研究的热点。惯性传感器(inertialmeasurementunit,imu)能够提供自身的运动信息(如：加速度、角速度等)，通过积分进行位置和姿态的计算。惯性传感器具有高速率的数据输出，能够及时响应位置和姿态的变化，但是具有严重的计算累计误差，视觉信息虽然速率较低，但能够提供丰富的信息量，并且通过回环检测可以消除累计误差。视觉惯性融合的slam方法通过对传感器数据的融合提升了整个slam方法的鲁棒性和准确性。
传统视觉slam方法主要依赖环境中的特征点，并对特征点进行跟踪来进行定位和建图。在图像特征点较为稀疏，光照变化较强以及剧烈的运动时，容易影响算法的精度，甚至导致算法失效。视觉惯性融合的slam方法，虽然能通过惯性传感器辅助图像特征数据的关联，同时优化惯性传感器数据，但是当图像特征数据关联出错或者惯性传感器初始化估计出错时，测量的误差会传播至整个算法的求解，从而使得整个视觉惯性融合算法精度严重下降。
技术实现要素：
本发明所要解决的技术问题是提供一种点线特征融合的紧耦合双目视觉惯性slam方法，能够在特征点数目较少以及相机快速运动情况下保证稳定和高精度。
本发明解决其技术问题所采用的技术方案是：提供一种点线特征融合的紧耦合双目视觉惯性slam方法，包括以下步骤：
确定双目相机坐标系与惯性传感器坐标系之间的变换关系，初始化slam系统，建立点线特征加imu跟踪线程、点线特征加imu局部光束法平差线程和点线特征回环检测线程；
所述点线特征加imu跟踪线程计算初始点线特征的三维坐标，并计算前后帧之间的imu预积分信息，imu初始化后，对点线特征进行预测，并进行前后帧和局部地图帧的特征匹配，确定正确的特征数据关联；联合优化点线特征重投影误差和imu增量误差，求解前后帧位姿变换；由点线特征分布及imu测量数据情况确定是否加入新的关键帧，并进行存储；
所述点线特征加imu局部光束法平差线程需要等待所述点线特征加imu跟踪线程增加的关键帧数量达到一定的阈值后才开始执行操作，当关键帧数量达到设定的值时，将启动imu初始化过程，估计出imu数据的加速度和角速度偏差；同时对点线特征三维地图进行处理，删除质量不高的点线特征三维地图点，建立局部关键帧窗口和共视关键帧窗口，对局部关键帧窗口中的关键帧以及对应的局部点线特征三维地图和imu状态量进行局部光束法平差优化，并对关键帧进行冗余剔除，当imu初始化后，将增加imu误差项进行优化，同时求解出关键帧的位姿、点线特征三维坐标以及imu的状态量；
所述点线特征回环检测线程通过点线特征的词袋模型比较当前帧与回环检测关键帧序列中的关键帧，计算当前帧与关键帧的词袋模型得分，检测出回环关键帧后进行全局的带有imu误差项的点线特征光束法平差优化，同时优化imu的状态项、关键帧位姿以及点线特征三维坐标。
所述点线特征加imu跟踪线程接收输入的矫正好的左右立体图像并分别提取点特征和线特征，对左右特征点线进行立体匹配，计算点线特征的三维坐标，创建初始特征地图，同时，计算当前图像到达时的imu预积分；下一帧图像输入时，进行点线特征位置的预测，并在预测位置附近进行特征匹配，通过建立的前后帧特征位置数据关联，优化点线特征的重投影误差，当imu初始化后，将增加imu误差项与重投影误差一起进行优化，求解出前后帧的旋转和平移变换，通过建立局部特征地图，获取更多的特征数据关联，随后通过点线特征分布情况及imu测量数据，确定是否添加关键帧，并进行关键帧的储存。
所述点线特征加imu局部光束法平差线程在建立局部关键帧窗口和共视关键帧窗口时，将前n个关键帧组成局部关键帧窗口，与这n个关键帧存在共视关系的关键帧但不包括这n个关键帧以及第n+1个关键帧组成共视关键帧窗口；在局部ba时，局部关键帧窗口中的点线特征三维坐标，imu状态量，关键帧位姿将全部优化，共视关键帧窗口中的关键帧位姿将固定住加入优化函数中，点线特征三维坐标，imu状态量不加入优化函数中；关键帧的确定由点线特征共同确定。
所述点线特征回环检测线程中的点特征和线特征的词袋模型得分分别为sp和sl，两者的加权作为最终的词袋模型得分。
有益效果
由于采用了上述的技术方案，本发明与现有技术相比，具有以下的优点和积极效果：本发明能够在图像特征点较少处，尤其是在人造建筑环境下，仍然能够保证实时的连续、稳定的高精度的位姿输出，从而能够使得适应的工作环境大幅提升，方法的鲁棒性显著增强。本发明能够提供一种结合imu和点线特征的回环检测方法，能够充分利用点线特征的融合检测出更加精确和鲁棒的回环。
附图说明
图1是本发明的点线特征与imu紧耦合slam示意图；
图2是本发明的点线特征加imu跟踪线程实现过程原理图；
图3是本发明的点线特征加imu局部光束法平差线程实现过程原理图；
图4是本发明的点线特征回环检测实现过程原理图。
具体实施方式
下面结合具体实施例，进一步阐述本发明。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。
本发明的实施方式涉及一种点线特征融合的紧耦合双目视觉惯性slam方法，本方法是在传统slam方法的延伸和改进，如图1所示，其通过提取图像点特征和线特征并进行特征匹配，确定特征数据关联，并通过同时优化imu和视觉的误差项进行位姿的求解和点线特征三维地图构建。
本方法的主要过程通过c++语言实现，并使用g2o库进行误差函数的优化求解，本方法包含以下步骤：
1)相机坐标系与惯性传感器坐标系坐标变换标定，确定两坐标系间的旋转r和平移变换t；双目相机的内外参标定，确定双目相机的内参矩阵k和基线长度b；
2)slam初始化，建立三个线程：点线特征加imu跟踪线程、点线特征加imu局部光束法平差线程和点线特征回环检测线程；
3)如图2所示，点线特征加imu跟踪线程接收硬件同步好的imu数据和矫正好的双目立体图像，并行对左右图中点和线特征进行立体匹配，获取初始点线三维地图；并在每一对新图像帧到来时，计算当前的imu预积分信息，如果imu已经完成初始化，将使用imu进行位姿的预测，通过预测的位姿信息，确定当前特征再下一帧中的大致位置，然后在这个位置附近进行前后帧的点线特征匹配，同时建立局部地图信息，局部地图中的三维点线特征反投影到当前帧，寻找更多的点线特征匹配结果，建立更多的数据关联，之后进行只求解位姿信息的ba，优化函数包括imu误差项，及点线特征重投影误差项，通过非线性优化的方式求解出当前帧的位姿信息。并且要根据点线特征的融合分布情况，确定是否接收当前帧为关键帧。
(4)如图3所示，点线特征加imu局部光束法平差线程在等待跟踪线程选择的关键帧数目达到设定的阈值时，首先开始进行imu的初始化操作，估计出imu的加速度和角速度偏置，并进一步估计重力方向和速度信息，同时对点线特征三维地图进行处理，删除质量不高的点线特征三维地图点，建立局部关键帧窗口和共视关键帧窗口两个优化窗口，其中局部关键帧窗口由前n个关键帧组成，共视关键帧窗口由与这n个关键帧有共视关系但不是这n个关键帧的关键帧和第n+1个关键帧组成。对局部关键帧窗口中的关键帧以及对应的局部点线特征三维地图和imu状态量进行局部光束法平差优化，共视窗口中的关键帧位姿加入优化函数公式(1)中，待优化的函数包括三项，分别是：点特征重投影误差，线特征重投影误差和imu误差项。三项误差项在公式(2)中给出，其中点特征重投影误差为图像平面检测的特征点和三维点反投影点的坐标差，线特征重投影误差为三维线段投影后两个端点到检测出的线段的距离。imu误差项包括旋转增量，速度增量和位移增量以及偏差误差。通过g2o定位定点和边，在根据计算的误差项对待优化变量的雅克比矩阵，确定出更新量，从而使用优化器进行求解。在局部窗口中的关键帧位姿，三维点坐标，三维线坐标，imu速度，偏差将被估计出来。
其中，
其中，ρ为鲁棒性核函数，
(5)如图4所示，所述点线特征回环检测线程通过点线特征的词袋模型比较当前帧与回环检测关键帧序列中的关键帧，计算当前帧与关键帧的词袋模型得分，检测出回环关键帧后进行全局的带有imu误差项的点线特征光束法平差优化，同时优化imu的状态项、关键帧位姿以及点线特征三维坐标。其中，对于点特征和线特征的词袋模型得分分别为sp和sl，需要计算两者的加权作为最终的词袋模型得分：sk＝ωpsp+ωlsl。
其中，
不难发现，本发明能够在图像特征点较少处，尤其是在人造建筑环境下，仍然能够保证实时的连续、稳定的高精度的位姿输出，从而能够使得适应的工作环境大幅提升，方法的鲁棒性显著增强。本发明能够提供一种结合imu和点线特征的回环检测方法，能够充分利用点线特征的融合检测出更加精确和鲁棒的回环。
- 还没有人留言评论。精彩留言会获得点赞！