点线特征融合的移动机器人视觉惯导里程计研究

多位姿信息融合的双目视觉惯性里程计研究一、本文概述随着机器人技术、无人驾驶、增强现实等领域的快速发展，精确且鲁棒的定位技术成为这些领域中的关键要素。

视觉惯性里程计（Visual-Inertial Odometry，VIO）作为一种重要的定位技术，通过融合视觉和惯性传感器信息，能够在复杂和动态环境中实现高精度的位姿估计。

然而，传统的单目或单姿态的VIO系统在某些情况下可能受到尺度模糊、光照变化、动态物体干扰等问题的影响，导致定位精度下降。

为了解决这些问题，本文提出了一种多位姿信息融合的双目视觉惯性里程计方法。

该方法通过同时利用双目视觉提供的深度信息和惯性传感器提供的角速度和加速度信息，实现对机器人或相机位姿的高精度估计。

通过引入多位姿信息融合策略，本方法能够有效地提升VIO系统在复杂和动态环境中的鲁棒性和精度。

本文首先介绍了视觉惯性里程计的基本原理和关键技术，然后详细阐述了所提出的多位姿信息融合的双目视觉惯性里程计方法的理论框架和实现细节。

在此基础上，通过仿真实验和实际场景测试，对所提出的方法进行了性能评估，并与其他相关方法进行了对比分析。

本文总结了所取得的研究成果，并展望了未来的研究方向和应用前景。

本文的研究成果对于推动视觉惯性里程计技术的发展，提高机器人在复杂和动态环境中的定位精度和鲁棒性具有重要的理论意义和实际应用价值。

本文所提出的多位姿信息融合策略也为其他相关领域的研究提供了有益的参考和启示。

二、视觉惯性里程计基本原理视觉惯性里程计（Visual-Inertial Odometry, VIO）是一种融合视觉和惯性传感器信息的导航方法，其核心在于利用视觉信息提供环境结构特征和惯性信息提供高频动态响应的特点，实现精确且鲁棒性的位姿估计。

视觉传感器，如相机，能够捕捉环境中的图像信息，通过特征提取和匹配技术得到帧间的相对运动关系；而惯性传感器，如加速度计和陀螺仪，能够测量载体自身的加速度和角速度，进而推算出载体在短时间内的位移和姿态变化。

近年来流行的一种SLAM方法是使用点线特征进行联合姿势估计。

这种方法将角和边缘等点特征的信息与墙壁和走廊等线面特征的信息结合起来，以提高机器人在未知环境中估计的姿势的准确性。

在这个方法中，机器人的传感器数据，一般来自相机和LIDAR的组合，用于从环境中提取点和线的特征。

这些特征随后用于绘制周边地图，并估计机器人相对于此地图的姿势。

通过将点和线的特征结合起来，该方法能够利用这两类特征的互补性质，从而导致更准确和稳健的姿势估计。

在使用SLAM的点线特性时面临的主要挑战之一是数据关联问题，即确定当前传感器数据中哪些特性对应地图中哪些特性。

在结构重复的环境中，例如许多特征可能相似的长走廊，这一点尤其具有挑战性。

为了解决这个问题，开发了高级数据关联算法，如联合兼容分支和绑定（JCBB）算法，以高效和准确地连接点线特征。

以点线特征为基础的SLAM的另一个重要方面是传感器数据与机器人运动模型的融合。

机器人的运动，按其Odometry或其他运动传感器的估计，提供了其姿势的宝贵信息，可用于根据传感器数据更新姿势估计。

通过安装传感器数据和运动模型，SLAM系统能够不断完善其对机器人在穿越环境时的姿势的估计。

为了说明以点线特征为基础的SAMM的有效性，让我们考虑一个现实世界的例子。

想象一个机器人通过一个大型的工业仓库进行导航，在那里它需要准确估计自己的姿势，以便取出和运送物品。

在这种复杂的环境中，有许多点和线的特征，例如角、架子和过道，可以用来进行姿势估计。

通过使用以点线特征为基础的SLAM方法，机器人能够构建仓库的详细地图并准确估计其姿势，使其能够高效完成任务。

在SLAM中采用点线特征进行联合预测，为在未知环境中提高预测的准确性和稳健性提供了一个很有希望的办法。

通过利用点和线的特性，以及将传感器数据与机器人的运动模型相结合，这种方法为在复杂和具有挑战性的环境中自主导航提供了强大的解决方案。

随着机器人技术的不断进步，我们可以期望看到以点线特性为基础的SLAM的进一步发展，从而导致更有能力和更可靠的机器人系统。

视觉里程计/IMU辅助GPS融合定位算法研究不同的传感器用于导航定位各有其优势与不足,需要依据不同的应用场景对各传感器进行组合以获取最优导航定位性能。

在城市、桥梁等遮挡环境下,GPS 信号容易发生中断或卫星信号质量不佳,导致室外定位精度不够。

为此,本文将主要研究视觉里程计/IMU辅助GPS融合定位算法,提高室外受限环境下导航定位精度与可靠性。

本文的主要研究内容有:(1)分别研究了视觉里程计和IMU的关键技术。

针对视觉里程计中特征点匹配计算量大及误匹配的现象,集成图像处理领域中基于欧式距离阈值设定的预处理算法,提出视觉里程计前端特征点匹配优化算法。

通过实验证明,提升了特征点提取的质量。

(2)讨论视觉里程计/IMU组合导航算法理论基础,建立视觉里程计/IMU紧耦合模型。

针对实验中的多传感器时间同步问题,使用软件算法估计硬件设备之间的时间偏差。

(3)提出基于抗差自适应卡尔曼滤波的GPS/视觉里程计/IMU组合算法。

通过手持接收设备采集GPS数据,进一步融合视觉里程计/IMU信息解算载体位置。

在遮挡环境下,单GPS的精度E、N、U三个方向的RMS值分别为:9.7653m、31.8248m、20.8644m,而基于抗差的多系统融合位置精度E、N、U三个方向的RMS 值分别为4.48m、7.55m、5.62m,分别提高了54%、76%和73%。

收稿日期：２０２０ ０８ ０５；修回日期：２０２０ ０９ １８ 基金项目：国家自然科学基金资助项目；海军工程大学自主立项基金资助项目作者简介：潘林豪（１９９１ ），男（通信作者），浙江温州人，博士研究生，主要研究方向为计算机视觉、深度学习（ｊａｙｐａｎｃｏｏｌ＠ｑｑ．ｃｏｍ）；田福庆（１９６２ ），男，教授，博士，主要研究方向为自动控制原理；应文健（１９７９ ），男，讲师，博士，主要研究方向为计算机视觉、机器人控制；佘博（１９８９ ），男，讲师，博士，主要研究方向为故障诊断、机器学习．融合双目视觉与惯导信息的高效视觉里程计算法潘林豪 ，田福庆，应文健，佘　博（海军工程大学兵器院，武汉４３００３３）摘　要：为提高视觉里程计（ＶＯ）在大尺度环境下运行的实时性，提出一种融合双目视觉与惯导信息的视觉里程计算法，主要由前端位姿跟踪和后端局部地图优化两个线程组成。

位姿跟踪线程首先使用惯导信息辅助光流法进行帧间特征点跟踪并估计相机初始位姿；接着通过最小化图像光度误差获取当前帧像素点与局部地图点的对应关系；而后最小化当前帧上局部地图点的重投影误差和惯性测量单元（ＩＭＵ）预积分误差，得到当前帧准确的位姿估计。

后端局部地图优化线程对滑动窗口内的关键帧提取特征点并三角化新地图点，使用光束平差法（ＢＡ）对逆深度参数化表示的地图点位置、关键帧位姿、速度以及陀螺仪和加速度计零偏进行滑窗优化，为前端提供更加精确的局部地图相机位姿和环境信息。

在ＥｕＲｏＣ数据集上的实验表明，相比于ＯＲＢ ＳＬＡＭ２、ＯＲＢ ＳＬＡＭ３算法，该融合双目视觉与惯导信息的视觉里程计算法的定位精度略有下降，但可以较大程度地提高位姿跟踪的实时性。

关键词：同时定位与地图构建；特征点法；直接法；传感器融合；光束平差法中图分类号：ＴＰ２４２．６２ 文献标志码：Ａ 文章编号：１００１ ３６９５（２０２１）０６ ０２５ １７３９ ０５ｄｏｉ：１０．１９７３４／ｊ．ｉｓｓｎ．１００１ ３６９５．２０２０．０８．０１８８ＥｆｆｉｃｉｅｎｔｖｉｓｕａｌｏｄｏｍｅｔｒｙａｌｇｏｒｉｔｈｍｃｏｍｂｉｎｉｎｇｓｔｅｒｅｏｖｉｓｉｏｎａｎｄｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｉｎｆｏｒｍａｔｉｏｎＰａｎＬｉｎｈａｏ ，ＴｉａｎＦｕｑｉｎｇ，ＹｉｎｇＷｅｎｊｉａｎ，ＳｈｅＢｏ（ＣｏｌｌｅｇｅｏｆＷｅａｐｏｎ，ＮａｖａｌＵｎｉｖｅｒｓｉｔｙｏｆＥｎｇｉｎｅｅｒｉｎｇ，Ｗｕｈａｎ４３００３３，Ｃｈｉｎａ）Ａｂｓｔｒａｃｔ：Ｉｎｏｒｄｅｒｔｏｉｍｐｒｏｖｅｔｈｅｒｅａｌ ｔｉｍｅｐｅｒｆｏｒｍａｎｃｅｏｆＶＯ（ｖｉｓｕａｌｏｄｏｍｅｔｒｙ）ｉｎｌａｒｇｅ ｓｃａｌｅｅｎｖｉｒｏｎｍｅｎｔ，ｔｈｉｓｐａｐｅｒｐｒｏｐｏｓｅｄａＶＯａｌｇｏｒｉｔｈｍｂａｓｅｄｏｎｓｔｅｒｅｏｖｉｓｉｏｎａｎｄｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｉｎｆｏｒｍａｔｉｏｎ．Ｔｈｅａｌｇｏｒｉｔｈｍｃｏｎｓｉｓｔｅｄｏｆｔｗｏｔｈｒｅａｄｓ：ｔｈｅｆｒｏｎｔ ｅｎｄｔｒａｃｋｉｎｇｔｈｒｅａｄａｎｄｔｈｅｂａｃｋ ｅｎｄｌｏｃａｌｍａｐｐｉｎｇｔｈｒｅａｄ．Ｔｈｅｔｒａｃｋｉｎｇｔｈｒｅａｄｆｉｒｓｔｌｙｕｓｅｄｔｈｅｉｎｅｒｔｉａｌｎａｖｉｇａｔｉｏｎｉｎｆｏｒｍａｔｉｏｎａｓｓｉｓｔｅｄｏｐｔｉｃａｌｆｌｏｗｍｅｔｈｏｄｔｏｔｒａｃｋｔｈｅｆｅａｔｕｒｅｓｂｅｔｗｅｅｎｆｒａｍｅｓａｎｄｅｓｔｉｍａｔｅｔｈｅｉｎｉｔｉａｌｐｏｓｅｏｆｔｈｅｃａｍｅｒａ．Ｔｈｅｎ，ｉｔｏｂ ｔａｉｎｅｄｔｈｅｃｏｒｒｅｓｐｏｎｄｉｎｇｒｅｌａｔｉｏｎｓｈｉｐｂｅｔｗｅｅｎｔｈｅｐｉｘｅｌｓｏｆｔｈｅｃｕｒｒｅｎｔｆｒａｍｅａｎｄｔｈｅｌｏｃａｌｍａｐｐｏｉｎｔｓｂｙｍｉｎｉｍｉｚｉｎｇｔｈｅｐｈｏｔｏ ｍｅｔｒｉｃｅｒｒｏｒ，ａｎｄｏｂｔａｉｎｅｄｔｈｅａｃｃｕｒａｔｅｐｏｓｅｅｓｔｉｍａｔｉｏｎｏｆｔｈｅｃｕｒｒｅｎｔｆｒａｍｅｂｙｍｉｎｉｍｉｚｉｎｇｔｈｅｒｅ ｐｒｏｊｅｃｔｉｏｎｅｒｒｏｒｏｆｔｈｅｌｏｃａｌｍａｐｐｏｉｎｔｓｏｎｔｈｅｃｕｒｒｅｎｔｆｒａｍｅａｎｄｐｒｅ ｉｎｔｅｇｒａｔｉｏｎｅｒｒｏｒｏｆｔｈｅＩＭＵ（ｉｎｅｒｔｉａｌｍｅａｓｕｒｅｍｅｎｔｕｎｉｔ）．Ｔｈｅｂａｃｋ ｅｎｄｌｏｃａｌｍａｐｐｉｎｇｔｈｒｅａｄｅｘｔｒａｃｔｅｄｆｅａｔｕｒｅｓｆｒｏｍｔｈｅｋｅｙｆｒａｍｅｓｉｎｔｈｅｓｌｉｄｉｎｇｗｉｎｄｏｗａｎｄｔｒｉａｎｇｕｌａｔｅｄｎｅｗｍａｐｐｏｉｎｔｓ．ＩｔｕｓｅｄＢＡ（ｂｕｎｄｌｅａｄｊｕｓｔ ｍｅｎｔ）ｔｏｏｐｔｉｍｉｚｅｔｈｅｍａｐｐｏｉｎｔｐｏｓｉｔｉｏｎｓ，ｋｅｙｆｒａｍｅｐｏｓｅｓ，ｖｅｌｏｃｉｔｉｅｓａｎｄｇｙｒｏｓｃｏｐｅａｎｄａｃｃｅｌｅｒｏｍｅｔｅｒｂｉａｓｅｓｉｎｔｈｅｓｌｉｄｉｎｇｗｉｎｄｏｗ，ｗｈｉｃｈｃｏｕｌｄｐｒｏｖｉｄｅｍｏｒｅａｃｃｕｒａｔｅｌｏｃａｌｐｏｓｅｔｒａｊｅｃｔｏｒｙａｎｄｅｎｖｉｒｏｎｍｅｎｔｉｎｆｏｒｍａｔｉｏｎｆｏｒｔｈｅｆｒｏｎｔ ｅｎｄ．ＴｈｅｅｘｐｅｒｉｍｅｎｔｏｎｔｈｅＥｕＲｏＣｄａｔａｓｅｔｓｓｈｏｗｓｔｈａｔ，ｔｈｅｐｏｓｉｔｉｏｎｉｎｇａｃｃｕｒａｃｙｏｆｔｈｅｐｒｏｐｏｓｅｄａｌｇｏｒｉｔｈｍｉｓｓｌｉｇｈｔｌｙｒｅｄｕｃｅｄ，ｈｏｗｅｖｅｒｉｔｃａｎｇｒｅａｔｌｙｉｍｐｒｏｖｅｔｈｅｒｅａｌ ｔｉｍｅｐｅｒｆｏｒｍａｎｃｅｏｆｐｏｓｅｔｒａｃｋｉｎｇｃｏｍｐａｒｉｎｇｗｉｔｈｔｈｅＯＲＢ ＳＬＡＭ２ａｌｇｏｒｉｔｈｍａｎｄｔｈｅＯＲＢ ＳＬＡＭ３ａｌｇｏｒｉｔｈｍ．Ｋｅｙｗｏｒｄｓ：ｓｉｍｕｌｔａｎｅｏｕｓｌｏｃａｌｉｚａｔｉｏｎａｎｄｍａｐｐｉｎｇ（ＳＬＡＭ）；ｆｅａｔｕｒｅｍｅｔｈｏｄ；ｄｉｒｅｃｔｍｅｔｈｏｄ；ｓｅｎｓｏｒｆｕｓｉｏｎ；ｂｕｎｄｌｅａｄｊｕｓｔｍｅｎｔ０　引言作为机器人研究与计算机视觉研究的交叉领域，视觉里程计技术（ｖｉｓｕａｌｏｄｏｍｅｔｒｙ，ＶＯ）［１］已经在无人机［２］、增强现实［３］、实时三维重建［４］等众多领域展现出了广阔的应用前景，是它们执行其他任务的基础。

移动机器人视觉里程计综述与研究展望摘要：移动机器人基于激光和视觉SLAM导航可实现环境的智能感知和非固定路径行走，其中视觉SLAM导航是指机器人利用视觉系统实现自主定位与地图创建，其优势为结构简单、成本低，信息量丰富。

而视觉里程计作为移动机器人视觉SLAM导航的前端，能为机器人自主导航提供廉价、可靠的位姿估计。

本文对移动机器人视觉里程计的概念与发展历程进行了简述，总结了实现视觉里程计的不同方法并分别对比了其优缺点，同时也分析了视觉里程计在移动机器人领域的应用以及未来研究展望。

关键词：视觉里程计、位姿估计、移动机器人、导航1、引言在移动机器人系统中，要进行目标探测和定位，对于自身位姿的估计非常重要。

传统的里程计，如轮式里程计因为轮子打滑空转而容易导致漂移；精确的激光传感器价格昂贵；惯性传感器虽然可以测量传感器瞬时精确的角速度和线速度，但是随着时间的推移，测量值有着明显的漂移，使得计算得到的位姿信息不可靠等。

而视觉里程计（Visual Odometry，VO）由于视觉传感器低廉的成本和长距离较为精准的定位在众多传统里程计中脱颖而出。

世界顶级的视觉传感器（工业相机）产品售价约为人民币3000元，是同等级的激光雷达产品售价的10%～20%。

而较低的制造成本意味着较低的产品价格，对提升企业产品竞争力或终端客户投资回报周期都更加利好。

2、视觉里程计发展简述所谓视觉里程计，就是从一系列图像流中恢复出相机的运动位姿，这一思想最早是由Moravec等人[1]提出的，他们首次提出了单独利用视觉输入的方法估计运动，并给出了一种最早期的角点检测算法，将其应用在行星探测车上，这体现了现阶段视觉里程计的雏形，包括特征点检测及匹配、外点排除、位姿估计三大块，使得视觉里程计从提出问题阶段过渡到了构建算法阶段。

Nister等[2]在CVPR上发表的论文中提出了一种利用单目或立体视觉相机来获取图像的视觉里程计系统，宣告VO技术进入了优化算法阶段。

imu和里程计融合原理
IMU（Inertial Measurement Unit）和里程计（Odometer）融合的原理是通过数据融合技术，将IMU采集的姿态信息与里程计采集的位置信息进行融合，以获得更准确、更可靠的姿态和位置信息。

在实际应用中，IMU是一种通过测量载体在惯性参考系下的加速度和角速度来获取其姿态信息的传感器。

它能够实时地获取载体的运动姿态信息，但受到传感器本身的限制，其测量精度和稳定性容易受到多种因素的影响，如非线性效应、噪声、误差等。

里程计是一种通过测量载体在地面上的移动距离和方向来获取其位置信息的传感器。

它通常由轮速传感器和编码器组成，通过测量轮子的转速来计算载体移动的距离和方向。

然而，里程计的测量精度容易受到路面状况、车轮磨损、气压变化等因素的影响。

为了获得更准确、更可靠的姿态和位置信息，可以采用IMU和里程计融合的方法。

具体来说，可以将IMU和里程计采集的数据进行预处理，去除噪声和异常值，然后通过数据融合算法将IMU的姿态信息和里程计的位置信息进行融合。

常用的数据融合算法包括卡尔曼滤波、扩展卡尔曼滤波、粒子滤波等。

在融合过程中，IMU可以提供实时的姿态信息，而里程计可以提供相对稳定的位置信息。

通过数据融合算法，可以对这两种数据进行有效的互补，得到更准确的姿态和位置信息。

同时，还可以通过引入其他传感器和算法，进一步提高数据融合的精度和稳定性。

融合光流跟踪和特征点匹配的单目视觉里程计研究融合光流跟踪和特征点匹配的单目视觉里程计研究摘要：单目视觉里程计（Visual Odometry，VO）是指在没有GPS等定位设备的情况下，通过单目摄像头的输入获取并处理图像信息，实现相机的运动轨迹估计。

VO技术在自动驾驶、机器人导航、无人机等领域有重要应用价值。

但由于单目摄像头只能获得二维图像信息，所以实现稳定和精度较高的VO并不容易。

本文结合光流跟踪和特征点匹配两类方法，提出了一种新的单目VO方法，并进行了实验验证。

实验结果表明，该方法具有较好的准确性和鲁棒性。

关键词：单目视觉里程计；光流跟踪；特征点匹配；估计误差；鲁棒性1. 引言在自动驾驶、机器人导航、无人机等领域，利用单目摄像头获取图像信息来估计相机的运动轨迹是一种十分重要的技术。

单目视觉里程计技术（Visual Odometry，VO）可用于实现相应的目标。

VO在没有GPS等定位设备的情况下，通过处理输入图像信息，估计相机的运动轨迹。

其原理是利用图像中相邻两帧之间的运动信息，结合运动学模型，计算相机相对于前一时刻的运动量。

VO技术的主要应用领域包括自动驾驶、机器人导航、无人机等领域。

虽然技术的应用前景广阔，但是由于单目摄像头只能获得二维图像信息，实现稳定和精度较高的VO 并不容易。

2. 光流跟踪光流跟踪是一种基于像素级别的方法，它通过输入图像中像素点的运动，计算相邻两帧之间的相机运动量。

光流法是通过对图像内像素的偏移量进行计算，实现像素的运动跟踪。

在VO 中，光流法的主要作用在于对图像局部区域内像素的运动进行估计，并通过局部运动结果对整个图像区域内的相对运动进行分析。

然后，基于这些分析结果可以对相对运动量进行计算。

3. 特征点匹配在单目VO中，特征点匹配也是一种常用的方法，它通过提取图像中独特的点，即特征点，并匹配图像中相邻两帧之间的特征点来计算相机的运动轨迹。

特征点通常是图像中一些特定的区域，其特征包括灰度、边缘信息等。

基于视觉点线特征和IMU融合的室内定位技术研究【基于视觉点线特征和IMU融合的室内定位技术研究】摘要：室内定位一直是移动智能设备领域的研究热点之一。

在室内环境中，GPS等全球卫星定位系统的信号受限，不能提供准确的定位服务。

因此，针对室内定位问题，本文提出了一种基于视觉点线特征和IMU（惯性测量单元）融合的定位技术。

该技术利用摄像头获取室内环境的点线特征，并通过IMU获取移动设备的加速度和角速度信息，通过融合这两种信息来提高室内定位的准确性。

关键词：室内定位；视觉点线特征；IMU融合；定位准确性1. 引言室内定位技术在日常生活中具有广泛应用，例如室内导航、智能家居和室内监控等领域。

然而，由于室内环境复杂且信号传输受限，基于全球卫星定位系统（GPS）的定位精度在室内环境中大大降低。

因此，研究开发一种准确可靠的室内定位技术具有重要的意义。

2. 相关工作目前，已经提出了许多室内定位技术，如无线信号定位、惯性导航和视觉定位等。

然而，这些技术都存在一定的局限性，例如无线信号定位容易受到信号干扰，惯性导航在长时间使用后存在积累误差问题。

视觉定位具有许多优势，如较高的精度和不受环境限制的特点，因此本文将重点研究基于视觉点线特征的室内定位技术。

3. 系统设计本文提出的室内定位系统主要包括两部分：视觉特征提取和IMU信息融合。

3.1 视觉特征提取视觉特征提取是基于摄像头获取室内环境的点线特征。

通过在室内环境中设置一系列固定的标志物或者利用室内地图进行特征提取。

在实时定位过程中，利用摄像头采集图像并通过图像处理算法提取出环境中的点线特征。

视觉点线特征可以利用Hough变换、边缘检测和特征匹配等算法进行提取和匹配。

3.2 IMU信息融合IMU是一种集成了加速度计和陀螺仪等传感器的设备，可以测量移动设备的加速度和角速度信息。

IMU信息融合是指将IMU 测量的数据与视觉点线特征进行融合，以提高室内定位的准确度。

IMU测量的数据可以通过卡尔曼滤波器进行融合，使得系统能够更好地估计移动设备的位置和姿态。

小型微型计算机系统Journal of Chinese C o m p u t e r Systems 2021年6月第6期 V o l.42 N o.6 2021双目点线特征与惯导融合的机器人SLAM算法研究冯波，刘桂华，曾维林，余东应，张文凯(西南科技大学信息工程学院，四川绵阳621010)E-mail ：fb_941219@163. c o m摘要：针对机器人S L A M系统，在实际场景或低纹理场景中提取的有效特征点数量少，使得系统初始化效果差和定位精度不 高的问题，提出了 一种基于点线特征和I M U信息融合的双目惯导S L A M系统（Stereo Visual-Inertial state estimator based on opti- mized O R B point feature and line f e a t u r e,O O L-V I N S)•首先，对双目视觉进行点线特征的提取与匹配，通过匹配的特征点构建残 差模型，并结合松耦合算法实现系统快速且稳定的初始化.然后，利用点线特征以及三角化算法设计了一种更加鲁棒的方法来 获取路标点的3D信息，以此来实现系统的位姿跟踪.最后，根据位姿跟踪过程中构建的局部三维地图，并结合滑动窗口的非线 性优化对相机位姿进行更新，提高系统的定位精度.实验结果表明.O O L-V I N S在T U M纹理结构类数据集上能获取更多有效的 点线视觉特征，且特征提取耗时为27m s.在E u R o c和T U M-V I数据集上进行初始化实验，实验表明，O O L-V I N S初始化更加快 速稳定.同样地，我们使用以上数据集进行系统性能的实验验证.结果表明，该系统的平均跟踪帧率为25H z,在300m的低纹理 场景中，定位精度可达0.072m.关键词：同时定位与地图构建（S L A M);点线特征融合;稳定初始化;定位精度中图分类号：T P391文献标识码:A文章编号=1000-1220(2021 )06-126749Research on SLAM Algorithm of Fusion Robot Based on Stereo Point-line Feature and Inertial NavigationF E NG B o,L I U G u i-h u a.Z E N G W e i-l i n,Y U D o n g-y i n g,ZH A N G W e n-k a i(School of Information Engineering,Southwest University of Science and Technology ,Mianyang 621010,China)Abstract：For the robot S L A M s ys te m,the n u m b e r of effective feature points extracted in actual scenes or low-quality scenes i s small, resulting in poor system initialization and low positioning accuracy.In th is paper,a binocular in e r t i al navigation system based on the fusion of point-line feature and I M U information(Stereo Visual-Inertial s tate estimator based on optimized O R B point feature and line feature,O O L-V I N S)i s proposed.Firstly,the point and line features are extracted and matched for binocular images.T h e residual m od el i s constructed by matching point and line features,and c om bine d with loosely-coupled algorithm to achieve fast and stable initialization of the system.T h e n,a m o r e robust m e t h o d i s designed based on the point-line features and the triangulation algorithm to obtain the3D information of the landmarks,so as to achieve the system^pose tracking.Finally,according to the local3D m a p constructed during the pose tracking process,and c o m b i ne d with the nonlinear optimization of the sliding w i n d o w to achieve the update of cam er a p o s e,which can significantly improve the accuracy of the system.The experimental results s h o w that O O L-V I N S can obtain a m o r e effective point­line visual features on the T U M texture datasets,and point-line feature extraction takes only27m s.T h e i n it ia li za ti on experiment on the E u R c x:and T U M-V I datasets s h o w that the O O L-V I N S init ia li za ti on i s faster and m o r e stable.Similarly,w e use the above dataset for system performance experiments.T h e results s h o w th at the accuracy of the system in low-texture scenes around 300m can reach0. 072m.In addition,the average frame rate of the system during the tracking process i s 25 H z,which contents the real-time require­men ts.K e y w o r d s：simultaneous localization and mapping(S L A M) ;point-line feature fusion；f ast and stable initialization；the accuracy of the systemi引言近年来，同步定位与建图（Simultaneous Localization andM a p p i n g,S L A M)已经发展成为移动机器人领域的研究热点，其被认为是实现自主导航的核心环节,S L A M包含两个主要任务，即定位和建图.机器人在未知环境中，通过传感器在运 动过程中的检测周围特征来获取机器人的位姿，然后从机器 人角度进行环境的地图构建在激光S L A M与视觉S L A M中，视觉S L A M因其成本低廉，能获取丰富的信息，成 为了目前S L A M方案中的热门方法.收稿日期:2020>06-22收修改稿日期:202(M)7-13基金项目：国防科工局核能开发科研项目（[2016 ]1295 )资助；四川省科技厅重点研发 项目（17Z D Y F0126)资助.作者简介：冯波，男，1992年生,硕士，C C F会员，研究方向为图像处理、三维重建、S L A M;刘桂华，女，1972年生，博士，教授,研究方向为机器学习、三维重建、S L A M;曾维林，男，1993年生，硕士，研究方向为图像处理、三维重建、S L A M;余东应，男，1995年 生，硕士研究生,研究方向为图像处理、三维重建、S L A M;张文凯，男，1996年生，硕士研究生,研究方向为图像处理、三维重建、S L A M.1268小型微型计算机系统2021 年视觉S L A M利用获取的图像以估计相机运动.根据位姿 估计的方式分类，可分为直接法和特征法.直接法如D T A M:3],L S D-S L A M[4]和D S O[5]等根据图像 的像素亮度信息估计相机运动，并通过最小化亮度误差进行 优化求解.然而，它的使用前提条件是基于灰度不变假设，即在连续的图像帧中固定相同空间点的像素灰度级.因此，在照 明情况复杂的应用场景不能满足利用直接法的条件.特征点法，按照提取的特征类型，主要分为点、线（边缘）以及点线结合的3种特征S L A M算法.点特征：主要使用S I F T W ,O R B[7]或S U R F W提取与匹 配点特征.根据特征匹配结果通过增量束调整最小化重投影误 差来估计相机位姿19\线特征：Schenk F a b i a n和Fraundorfer Friedrich提出了一种基于边缘的 R G B D-S L A M，R E S L A M[.通过边缘在不同的光照条件下比其他区域的强度值更稳定这 一特性构建系统.点和线特征:采用点、线特征相结合的方式求 解相机位姿.最具有代表性的是P u m a r o l a等提出的P L-S L A M M]、谢晓佳等人提出的基于点特征与线特征的双目视觉 S L A M%、基于单目点线特征结合[|3_15]以及基于双目的点线 特征系统[16].以上系统定位精度依赖于提取的特征质量，这在某些纹 理较少的场景缺乏鲁棒性，在实际应用中比较困难.同时视觉 S L A M也存在缺点，例如单目视觉无法获取绝对尺度,单目纯 旋转机器人运动无法估计，机器人快速运动时容易跟丢等. Baofu f a n g等人提出的解决由运动等造成的图像模糊匹配难 的点线方案n7],与仅适用视觉传感器的方案相比，具有很好 的实用性.当前多传感器与视觉的融合S L A M系统逐步发 展，尤其是与I M U的融合，因I M U的快速响应、不受成像质 量影响、可估计绝对尺度等特点恰好与视觉互补而成为多传 感器视觉S L A M的一个热点研究.对于相机与I M U融合而言，根据方式视觉惯性里程计(V I O)系统可分为两个主流：松耦合与紧耦合.松耦合为1M U与相机独立进行位姿估计，然后对结果进行融合；紧耦 合为将I M U状态量与相机状态量合并在一起,共同构建运动 方程和观测方程，然后进行位姿估计，且紧耦合通常精度更高、更鲁棒:181，如V I N S-M o n o[l9].在V I N S-M o n o基础上秦通 等人又相继提出了 V I N S-F u Sion[20]，其给出了单目与_、双 目、双目与I M U、双目与G P S几种不同的融合模式，在精度与 稳定性上均有一定程度的提高,但是其为了提高运行速度前 端使用 opencv 的 Shi-T om as 角点（又称 G h l'l,G o o d Feature T o T r a c k)提取与光流跟踪，这在一定程度上损失了系统精度.为了解决特征点过少同时兼顾精度，贺一家等人提出了结 合点线特征的视觉惯性里程计P L-V I O[〜，视觉提取点线特 征的同时与I M U传感器进行紧耦合来估计相机位姿运动，P L-V I O为基于V I N S-M〇n〇w开发，其前端的特征点与V I N S-M o n o—样采用F A S T角点提取与光流跟踪，这在精度 上也有一定的损失.针对室内场景实际应用中有时会存在重复纹理以及光照 变化复杂等问题,本系统前端的特征提取使用点线特征融合 方案,其中点特征提取使用O R B特征点.同时由于一些低成 本I M U传感器测量结果受噪声影响较大，导致使用尺度恢复 的结果不稳定.此时加人双目传感器获取的深度信息进行初始化可以为后端非线性优化的迭代求解提供一个更好的初始 值1221.故本文对双目图像提取O R B点特征与线特征信息融 合双目相机深度信息与I M U传感器进行紧耦合优化估计机 器人位姿,本文将其命名为基于优化的O R B点特征与线特征 的双目惯性视觉里程计（Stereo Visual-Inertial s t at e estimator based on optimized O R B point feature and li ne feature,O O L-V I N S).2 OOL-VINS系统总览如图1所示，整个系统可分为3个部分:1)前端视觉里程 计.执行点线特征的提取与跟踪,另一方面获取频率远髙于视 觉的I M U信息，利用I M U响应快速、不受成像质量影响等性 质，与跟踪匹配的视觉特征融合计算出相机的位姿并创建新 的关键帧;2)系统初始化.初始化采用松耦合的方式获取初 始值.利用立体匹配或S F M求解滑动窗口内所有帧的位姿 (以第一帧作为参考坐标系）和所有路标点的3D位置.然后 将S F M的结果与I M U预积分的结果进行对齐，实现对陀螺 仪偏置的校正，再求解每一帧对应的速度，重力向量方向;3) 后端优化.后端优化使用滑动窗口算法，限制关键帧数量，防 止滑窗内的优化变量个数随时间不断增加.前端里程计初始化后端优化■----=----—--------点特征(FAST)检测匹配跟踪_RGB相机线特征(LSD)检测匹配跟踪跟踪成功特征点双目匹配深度信息RGB相机時征检测与跟踪*-IMU数据IMU预积分相机/IMU位姿________________f_______________视觉-IMU对齐SFM全局图优化J滑动窗口三角化后端优化图1系统框架设计Fig. 1S ys t e m framework design3 OOL-VINS传感器测量与残差模型O O L-V1N S前端将点线特征进行数学建模加上I M U数 据传入后端进行机器人位姿估计以及优化.前端可分为视觉 部分与I M U预积分部分，也可以分为3个部分:O R B点特征 提取与跟踪、线特征的提取与跟踪、I M U预积分，如图2所示.3.1点线特征的提取与匹配基于O p e n C V提取的O R B特征过于集中，会降低S L A M 的精度，对于闭环检测来说，也会降低一幅图像上的信息量. 故本文采用O R B-S L A M1231中的特征点提取策略，通过网格 化图像进行均匀的特征提取.匹配时，为了限制待匹配特征集 大小，而不是对整张图上的每一个特征点都进行匹配计算，从 而减小计算开销，加快匹配速度,对不同阶段采取不同的特征 匹配方式[23].线特征的提取与匹配基于L S D[24]算法与L B D[25]算法, 传统的L S D算法对于图像遮挡、局部模糊等情况，由于区域 增长算法的特性，一条线段会割裂成多段.这使得线特征在 S L A M中的匹配不是一对一,而是多对多的，且分割点在邻近6期冯波等:双目点线特征与惯导融合的机器人S L A M算法研究1269的帧中不同，导致线特征匹配难度增大.故本文中线特征的提取采用谢晓佳等人改进的L S D算法[|2].3.2点线特征的残差模型本小节中，通过前面所述的特征提取与匹配结果，构建点线特征的残差模型.3.2.1点特征的残差模型O R B点特征的残差模型为重投影误差模型如图2所示.假设P, = (X,,)为前一帧第;'个特征点,u，.为R在当前帧的投影，则误差表示为：=u,--7-K e x p(4A)P i(1)其中，&为点P,+为双目匹配求得的深度值,K为相机内参义为前一帧到当前帧变换矩阵的李代数：-0-专3^2'f A =在30-f,(2)-一釜20 _IMU Measurements Pre-integratedIMU Measurements图2视觉重投影误差与I M U预积分Fig.2 Visual reprojection error and I M U pre-integration3.2.2线特征的残差模型线特征的残差模型同样使用重投影误差表示，如图2所 示.即线特征重投影误差表示为:下一帧检测到的匹配线段端 点M',N'e R3到上一帧检测线段的端点M，N E R3投影至 下一帧所得的端点m，n e R3所组成直线的距离之和.=^(M M，i，K) +^(N M,g,K)(3)其中,4(\^,1，€，1^、1^(?4'，丨,^)分别表示\1'、?4'到线段1的距离：d(s，l)=(4)其中，s为端点M’、N'的坐标，1为直线方程，显然，/,、/2为直线方程系数.3.3 IMU测量与残差模型忽略地球旋转，一个6轴I M U(3轴线加速度,3轴角速 度）的测量模型[26]为分别对加速度计测量与陀螺仪测量.假设已知I M U第('时刻的位置、速度以及旋转P_、V「、以及Z时刻至y'时刻的I M U测量值，则通过积分可得到y 时刻的状态为由于在优化求解的时候是随着优化在改变的，为了避免重新传播I M U的测量值，选用 I M U预积分模型[27]，使用转换：q吻=<1岣®*!咕, (5)将积分项从世界坐标系转为I M U坐标系进行求解，则积 分为相对于I M U坐标系前一时刻的状态而不是世界坐标系，如图2所示，以q为例，转换后积分项则为相对于I M U坐标 系的状态,p、v同理.I M U的预积分量是对两个关键帧之间的 测量值进行积分，相当于帧与帧之间的增量[28].则广义的I M U测量值为：^bibjQ i>,w(Pw…；- p^,.- <A?+ y g" A/2j=q^(v；-v r+g M A〇-^ib jb i__(ibjw®tiw b i_(6)同时通过公式(5)转换后求得的积分量是在连续时间模 型基础上，忽略了偏置和噪声，而实际I M U的测量是离散时 间模型.考虑进偏置与高斯白噪声使用基于误差随时间变化 的递推[26](状态转移模型）预测预积分量，即广义的I M U预测值记为P_，则I M U的残差模型为:Tb =r v P f e,*;_ ^bjbj-V1-(7)其中，®表示四元数的乘法运算.[• ]$表示只取四元数的虚部(x,y,z)组成三维向量.4 OOL-VINS初始化算法设计O O L-V I N S视觉里程计部分算法设计基于V I N S-M o n o[19]系统，初始化采用松耦合获取初始值，首先用S F M 求解滑动窗口内所有帧的位姿（以第1帧作为参考坐标系，无尺度信息），同时结合双目对特征点匹配所得的深度信息恢复所有特征点的3D位置（带尺度信息）.然后将S F M的结 果与I M U预积分结果进行对齐,实现对陀螺仪偏置的校正，再求解每一帧对应的速度，求解重力向量方向,恢复单目相机 的尺度因子.V I N S-M o n o对于机器人在特殊运动情况（例如 在局部以恒定加速度运动）以及使用低成本I M U传感器时(I M U测量噪声较大），V I O初始化不能很好的恢复尺度[22],此时使用双目加I M U测量值进行初始化能够解决此时尺度 不可观问题[29].O O L-V I N S在点线跟踪模块和I M U预积分结束及数据 传人初始化模块后，首先对于每个图像帧结合双目相机匹配 获取的深度信息获取特征的3D点•区别于V I N S-M o n o松耦 合时对视觉只使用了 R G B信息而采用5点法（或8点法）求 解相机位姿,O O L-V I N S使用已经带有尺度信息的数据利用 P N P算法进行相机初始化.对于纯视觉的S F M,假设已知相机与I M U之间的外参q^P k,则可以利用外参数以相机坐标系第1帧c。