《基于ROS的室内移动机器人自主导航技术.docx》由会员分享，可在线阅读，更多相关《基于ROS的室内移动机器人自主导航技术.docx（43页珍藏版）》请在课桌文档上搜索。
1、随着机器人技术的发展，近些年来室内移动机器人的研究热度越来越高。本文主要利用RoS操作系统对室内移动机器人的建图定位与自主导航关键技术进行研究。ROS是目前开发较为完整具有广阔应用前景的开源机器人操作系统，具有分布式、模块化、代码复用等优良特点。同时ROS还提供了许多开源库，在这些库的基础之上研究同时定位与地图构建和机器人自主导航技术可以节省大量时间。近些年来视觉SLAM的研究变得越来越火热，其中ORB-SLAM2是比较优秀的算法之一，它具有计算量比较小、能够实时运行等优点。在介绍ORB-SLAM2的框架之前，首先会介绍视觉SLAM的一些基础知识，在此基础之上简要介绍ORB-SLAM2的前端和
2、后端,最后在ROS环境下对该算法进行验证并和VINS-MONO进行对比。机器人自主导航的核心可以归结为机器人的定位和路径规划。机器人定位解决机器人在环境中的位置问题，然后利用路径规划寻找一条从起点到终点的路线完成机器人的自主移动。本文利用RoS中自适应蒙特卡洛完成机器人定位，结合A*全局路径规划算法验证移动机器人的导航效果并和Dijkstra算法进行对比。关键词：ROS.自主导航、视觉SLAM、路径规划AbstractInrecentyears,withthedevelopmentofrobottechnology,researchonindoormobilerobotshasbecomein
3、creasinglypopular.Thisarticlefocusesonthekeytechnologiesofmapping,localization,andautonomousnavigationofindoormobilerobotsusingtheROSoperatingsystem.ROSiscurrentlythemostcompleteopensourcerobotoperatingsystemwithbroadapplicationprospects.Ithasexcellentfeaturessuchasdistributedcomputing,modularity,an
4、dcodereuse.Atthesametime,ROSalsoprovidesmanyopensourcelibraries,whichcansavealotoftimeinresearchonsimultaneouslocalizationandmapping(SLAM)androbotautonomousnavigationtechnologybasedontheselibraries.Inrecentyears,researchonvisualSLAMhasbecomeincreasinglypopular.Amongthealgorithms,0RB-SLAM2isoneoftheb
5、etterones,asithastheadvantagesoflowcomputationalcomplexityandreal-timeoperation.Beforeintroducingtheframeworkof0RB-SLAM2,somebasicknowledgeofvisualSLAMwillbediscussed.Onthisbasis,thefront-endandback-endof0RB-SLAM2willbebrieflyintroduced.Finally,thisalgorithmwillbevalidatedintheROSenvironmentandcompa
6、redwithVINS-M0N0.Thecoreofrobotautonomousnavigationcanbeattributedtorobotlocalizationandpathplanning.Robotlocalizationsolvestheproblemoftherobotspositionintheenvironment,andthenusespathplanningtofindapathfromthestartingpointtotheendpointtocompletetherobotsautonomousmovement.ThisarticleusesAdaptiveMo
7、nteCarloinROSforrobotlocalization,andcombinesA*globalpathplanningalgorithmtoverifythenavigationeffectofmobilerobotsandcompareitwithDijkstrasalgorithm.Keywords:ROS,autonomousnavigation,VSLAM,pathplanningIll目录第一章绪论11.1 课题研究背景和意义11.2 国内外研究现状11.2.1 1视觉SLAM国内外发展现状11.2.2 室内移动机器人国内外发展现状31.2.3 存在问题与不足41. 3可
8、行性分析51.4本文主要研究内容6第二章ORB-SLAM2原理及框架72. 1视觉SLAM基础72.1.1 刚体运动及其优化72.1.2 相机模型92.1.3 非线性优化122.2跟踪线程141.1.1 2.1引言141.1.2 特征提取151.1.3 特征匹配161.1.4 运动估计172 .3后端算法18第三章基于A*算法的移动机器人路径规划技术213 .1ROS基础213.1.1ROS简介213.1.2ROS通信方式233.2ROS下0RB-SLAM2验证243.3机器人定位273.3.1机器人定位问题分类273.3.2蒙特卡洛定位283.4路径规划与导航293.4.1基于DijkStr
9、a的全局路径规划算法293.4.2基于A*的全局路径规划算法原理及实现293.5ROS下A*算法仿真验证313.5.1基于代价地图的路径规划环境模型313.5.2A*算法及其性能验证33结论35致谢36参考文献37第一章绪论1.1课题研究背景和意义移动机器人已经成为人类社会中重要的一员，它们可以用于许多领域，如工业自动化、医疗护理、环境监测等。室内移动机器人是移动机器人的一种，可以在人类活动的场所自主移动，并且执行任务。它们已经广泛应用于各种环境，如医院、办公室、仓库和家庭等。然而，室内环境的复杂性和不确定性对移动机器人的自主导航构成了挑战。室内环境的变化频繁，例如障碍物的移动、室内物品的更换
10、等，需要机器人能够实时更新环境的信息。同时，室内环境的结构也很复杂，机器人需要具有高精度的定位和导航能力，才能避免碰撞和完成任务。视觉SLAM技术是解决移动机器人室内自主导航问题的一种重要方法。视觉SLAM技术可以通过机器人上搭载的相机等传感器获取环境信息，同时实现机器人的位置定位，从而辅助实现自主导航。作为一个开放式机器人操作系统，ROS提供了完整的机器人软件框架，包括通信、控制、导航等功能，方便机器人的开发和部署。ROS中有许多开源的包，因此利用ROS进行开发可以极大缩短开发时间。本论文的研究目的是探索基于ROS的室内移动机器人自主导航技术，并以视觉SLAM技术为基础，实现机器人对室内环境
11、的建图和定位。主要完成的工作是在RoS环境下对0RB-SLAM2和A*全局路径规划算法进行验证。本文的研究成果可能会为机器人软件系统的开发提供实践经验，为室内移动机器人的自主导航技术提供进一步参考。L2国内外研究现状1.2.1视觉SLAM国内外发展现状(1)国外研究现状视觉SLAM是一种利用相机和图像信息实现实时定位和地图构建的技术。在国外，视觉SLAM领域得到了广泛的研究和发展，涌现出许多创新的方法和算法。特征提取和匹配：特征提取和匹配是视觉SLAM的关键步骤之一。国外的研究者提出了多种高效的特征描述子，例如SlFT、SURF#ORB,用于提取图像特征点。同时，他们还研究了各种特征匹配算法，
12、包括基于描述子相似度、几何约束和局部一致性等方法，以提高匹配的精度和鲁棒性。视觉里程计：视觉里程计是视觉SLAM中的核心任务之一，用于估计相机在连续图像序列中的运动信息。国外的研究者提出了多种视觉里程计算法，包括基于特征点的方法（如C）RB-SLAM、LSD-SLAM和DSO）和直接法（如DVO-SLAM和DTAM）O这些方法利用图像间的特征点或像素级的灰度信息来估计相机的位姿变化，从而实现定位和地图构建。地图构建和优化：国外的研究者提出了多种地图构建和优化算法，用于将相机观测到的特征点或像素映射到三维空间，并优化地图的精度和一致性。常用的方法包括基于滤波器（如扩展卡尔曼滤波器和粒子滤波器）的
13、实时地图构建和基于图优化的离线地图优化。这些方法能够处理环境中的动态物体、误匹配和重投影误差等问题，提高地图的准确性和稳定性。语义SLAM：近年来，国外的研究者开始关注将语义信息与视觉SLAM相结合，以实现更加语义丰富的地图构建和场景理解。他们利用深度学习技术进行图像的语义分割和物体识别，将场景中的语义信息与地图关联起来。这样的语义SLAM方法可以用于智能导航、场景理解和交互式系统等领域。多传感器融合：为了提高定位和地图构建的准确性和鲁棒性，国外的研究者在近些年将视觉传感器和一些其他的惯性传感器进行融合。多传感器融合能够利用各个传感器的优势，提供更准确和稳定的定位和地图信息。（2）国内研究现状
14、在国内，视觉SLAM作为一项重要的研究领域得到了广泛的关注和发展。国内研究者在视觉SLAM算法优化、硬件平台搭建、应用拓展等方面取得了一系列重要进展。算法优化与创新：国内研究者在视觉SLAM算法优化和创新方面进行了大量的工作。他们优化了一系列高效的特征提取和匹配算法以加快特征提取和匹配的速度。实时性和鲁棒性：国内研究者注重提高视觉SLAM系统的实时性和鲁棒性。他们通过算法优化、硬件加速和并行计算等手段，提高了算法的运行效率和实时性能。同时，他们还研究了鲁棒的特征提取和匹配算法，以应对光照变化、动态物体和遮挡等复杂环境条件，提高系统的鲁棒性和稳定性。多传感器融合：国内研究者也将多传感器融合应用于
15、视觉SLAM中，他们将惯性测量单元(IMU).激光雷达、RGB-D摄像头等与视觉传感器相结合，通过融合多源数据来提高定位和地图构建的准确性。同时，国内研究者还探索了基于多传感器融合的SLAM算法在无人车、智能交通等领域的应用。1. 2.2室内移动机器人国内外发展现状(1)国外研究现状移动机器人技术是人工智能领域的重要研究方向之一，在国外的研究中得到了广泛关注和探索。自主导航和路径规划：自主导航是移动机器人的关键技术之一。国外研究者致力于开发高效的自主导航算法，使机器人能够在未知环境中实现自主移动。其中，SLAM(SimultaneousLocalizationandMapping)技术是常用的
16、方法，通过结合传感器数据和地图信息，实现机器人的自我定位和地图构建。此外，路径规划算法也是关注的热点，研究者通过优化算法和学习方法，使机器人能够在复杂环境中规划最优路径，并避免障碍物。环境感知和感知技术：为了使机器人能够感知和理解周围的环境，国外研究者提出了各种先进的感知技术。传感器是机器人感知环境的主要工具，包括激光雷达、视觉传感器、深度相机等。这些传感器可以获取环境中的物体位置、形状、颜色等信息，以支持机器人的决策和行为规划。此外，近年来深度学习技术的兴起，为机器人的感知能力带来了巨大的提升。通过深度学习算法，机器人能够进行物体识别、目标跟踪和场景理解等任务，从而更好地适应复杂多变的环境。
17、协作机器人和人机交互：随着机器人技术的发展，研究者开始关注多个机器人之间的协作和与人类的交互。协作机器人可以在任务执行中相互合作，提高效率和灵活性。另外，人机交互也是一个重要研究方向。研究者致力于开发机器人与人类之间自然和有效的交互方式，如语音识别、姿态识别和情感识别等。这样的研究有助于实现人机共生和智能服务机器人的应用。(2)国内研究现状近些年国内对移动机器人的研究也越来越多，在研究开发新技术的基础之上,国内的学者同时注重应用层面的开发。应用领域的拓展：国内研究者将移动机器人技术广泛应用于各个领域。例如，在物流仓储领域，研究者开发了自动导航的AGV（自动导引车）系统，提高了物流仓储的效率。在
18、医疗领域，研究者利用移动机器人进行康复训练和辅助手术。教育与研究：在教育和研究领域，国内研究者积极探索移动机器人的应用。移动机器人被应用于学校和科研机构中，作为教学辅助工具和研究平台。通过与学生和研究人员的互动，移动机器人不仅能够提供实验和演示的机会，还能促进学生对科学和技术的兴趣培养，并激发创新思维。社会服务与辅助功能：国内研究者也将移动机器人应用于社会服务和辅助功能领域。例如，在老年照护中，机器人可以提供日常生活的帮助，如智能导航、药物提醒和紧急呼叫等。此外，移动机器人还可以在灾害救援和环境监测等领域发挥重要作用，通过携带传感器和执行任务，为人类提供更安全、高效的服务。工业自动化：国内的工
19、业自动化领域也是移动机器人应用的重要领域之一。机器人在工业生产线上的应用，可以提高生产效率、降低人力成本和减少生产事故。例如，自动导航的移动机器人可以在工厂内物料搬运和装配过程中发挥重要作用。同时，国内研究者也致力于开发更智能、灵活的工业机器人，通过与其他自动化设备和系统的集成，实现协作生产和智能制造的目标。总体而言，近些年室内移动机器人在各个领域实现了广泛的应用。随着人工智能和机器人技术的不断发展，移动机器人将继续在未来发挥重要的作用，为人类的生活和工作带来更多的便利和创新。L2.3存在问题与不足在室内移动机器人和视觉SLAM的发展过程中，存在一些问题和不足，这些问题可能限制了其应用范围和性
20、能。环境感知和建模的挑战：在室内环境中，机器人需要准确地感知和建模环境，以便进行路径规划和导航。然而，复杂的室内环境、动态物体和光照变化等因素会对感知和建模造成挑战，导致误差和不完整的地图，影响路径规划的准确性和可靠性。实时性和计算复杂度：移动机器人需要在实时性要求下进行路径规划和导航。然而，视觉SLAM算法通常需要大量的计算资源和时间来处理图像数据和构建地图，这可能限制了其在实时应用中的使用。解决这个问题需要对算法进行优化，或者结合其他传感器和技术来提高实时性和效率。鲁棒性和鲁邦性：在复杂的室内环境中，机器人需要具备良好的鲁棒性和鲁邦性，以应对各种挑战和干扰。例如，光照变化、遮挡物、不可预测
21、的动态物体等可能导致传感器数据的不稳定和误差，进而影响路径规划的性能。改善鲁棒性和鲁邦性是进一步提升室内移动机器人和视觉SLAM系统的关键问题。长期自主导航：长期自主导航是指机器人在长时间的运行中能够持续更新地图和路径规划，以适应环境的变化。然而，由于传感器的漂移、定位误差和地图的更新问题，机器人在长期运行中可能存在累积误差和路径规划失效的情况。解决这个问题需要开发新的方法和技术，如视觉惯性融合、闭环检测和增量式地图更新等。多机器人协同和冲突避免：在多机器人系统中，机器人之间的协同和冲突避免是一个复杂的问题。在室内环境中，多个机器人共享同一空间，需要避免碰撞并有效地协调任务。解决这个问题需要研
22、究多机器人路径规划和协同算法，以实现安全和高效的多机器人协同。1. 3可行性分析(1)经济可行性：成本：研制室内移动机器人需要大量的资金投入，涉及到硬件、软件以及人工成本，例如视觉传感器、SLAM算法、ROS机器人导航技术、框架结构等费用。此外，还要考虑生产制造、销售渠道和服务支持等方面的成本。市场需求：当前市场对室内移动机器人的需求逐渐增加，可以应用于物流仓储、地图绘制、家庭保洁等领域。但是，在推广和普及过程中，需要花费大量的时间和精力打开市场。可持续发展：考虑长远利益，在室内移动机器人设备的研究过程中，需要注重可持续发展，例如采用环保材料、低功耗设计等措施。（2）技术可行性：视觉SLAM技
23、术：这是实现机器人室内定位和导航关键技术，可以在没有GPS信号的情况下自主完成室内定位和导航。但是，其在复杂室内环境中的定位精度和稳定性仍面临挑战。ROS机器人自主导航技术：ROS机器人自主导航技术可以提供机器人路径规划、避障等辅助功能。该技术需要硬件和软件相结合，并能够实时获取机器人传感器信息，保证机器人导航的有效性。在室内移动机器人研究方面，除了经济可行性,还应注重技术可行性。当前，由于技术水平、需求及产业因素等问题，室内移动机器人尚未广泛应用，我们需要在不断完善基础设施，推广市场的同时，不断优化技术，提升其应用价值和竞争力。L4本文主要研究内容本文主要对移动机器人自主导航的关键技术进行研
24、究，主要包括0RB-SLAM2视觉SLAM和移动机器人的自主导航两个方面。首先在第一章绪论部分会对视觉SLAM和移动机器人的国内外发展的现状做一个研究，紧接着提出当前研究存在的问题，并对本文研究的内容可行性的分析。在第二章ORB-SLAM2原理框架这一章中，首先会引出一些关于视觉SLAM的基础知识。包括用于SLAM位姿描述的三维刚体运动和位姿优化的李群与李代数。介绍了相机模型中常见的针孔相机模型和双目以及RGBD相机模型。此外还介绍了SLAM中常见的用于求解非线性最小二乘的高斯-牛顿和列文伯格-马夸尔特法。引出基础知识之后着重ORB-SLAM2的跟踪线程，包括特征提取特征匹配和位姿估计等知识，
25、最后简要介绍了ORB-SLAM2后端的回环检测。在本文的第三章先是对ROS进行了简要的介绍，紧接着在ROS下对0RB-SLAM2进行验证，并和另一主流的视觉SLAM方法VINS-MONo进行对比。然后介绍了移动机器人的自主导航技术，主要包括机器人定位和A*全局路径规划算法。最后搭建RC）S机器人对A*算法进行验证，并和DijkStra算法进行了对比。第二章ORBSLAM2原理及框架1.1 视觉SLAM基础1.1.1 刚体运动及其优化(1)三维刚体运动视觉SLAM中的位姿估计是基于三维刚体运动，三维刚体运动用于描述不同坐标系之间的变换关系。坐标系是相对于某一参考系建立的，通常包括世界坐标系和相机
26、坐标系等多种类型。三维刚体运动描述了一个点在初始坐标系中的位置，并经过旋转、平移等操作后，在新坐标系中的位置发生了哪些改变。因此,通过三维刚体运动学，结合位姿估计算法，可以有效地实现对不同坐标系之间的精确定位。假设世界坐标系为兄，相机坐标系为月，我们假设相机是刚体，进而把研究的对象转为三维的刚体运动。这里所指的刚体不仅仅有位置，也就是相机具体处于空间的什么地方，也有具体的姿态，也就是相机的朝向。通过一次三维的旋转可以使得两个坐标系的姿态是一样的，再通过一次平移使得两个坐标系完全重合，此时两个坐标系的位姿是一样的。在用详细的数学语言进行描述之前首先引入向量，空间中的两个点连成按一个方向连成的一条
27、线就是一个向量，他只是空间中存在的东西，并不具有坐标这一说法，只有在引入了坐标系之后才可以谈某个向量在该坐标系下的坐标。假设三维空间中有一组三维的相互正交的基向量佃勺，%)，对于向量。有：=e1e2e3a2=1e1+a2e2+tz3e3(2-1)儿则称(q,生,心)丁为。在此基下的坐标。假设世界坐标系和相机坐标系的单位正交基分别为色勺修)和(4,4,4)则有:a%ee2e3a2=，e2a2(2-2)对式(2-2)两边左乘一个444了则有:(2-3)定义中间的矩阵为旋转矩阵，旋转矩阵有一些特殊性质，它是正交矩阵，也就是说它的转置等于它的逆，即rr=/,其中/是单位矩阵。另外，旋转矩阵的行列式为1
28、，即IHI=1。我们把任意维的旋转矩阵的集合定义为：(2-4)SO(n)=RRnxf,R,R=/,det(R)=1在完成旋转的变换之后我们在此基础之上加入平移。假设向量。在经过旋转化R后加入了平移变换，则有：ci=Ra+1(2-5)式(2-5)中的/被称作平移向量，它的作用很简单，就是把一个坐标系平移使得其和另一个坐标系完全重合，从而完成一整个坐标系的变换。但是这种表达方式存在一定的缺陷，经历过旋转和平移之后的变换并不是线性变换。假如进行了两次变换：NjI,62：b=Rxa+tc=RJb+12(2-6)根据式(2-6),从。到C的变换可以写成：c=T?2(/?+zl)+r2(2-7)但是这样的
29、描述非常的繁冗，在引入了变换矩阵之后，重写为式(2-8)：dR/呵=T=T(2-8)LdL,山Ld式(3-8)中的T称为变换矩阵，这样通过一个矩阵描述了旋转和平移两个变换。同时这种矩阵又被称为特殊欧式群，即5E(3)=TR4x41T1T=/,det(R)=1(2-9)(2)李群与李代数视觉SLAM中，李群和李代数是非常重要的数学工具，用于描述相机的位姿变换和其它的变换。李群和李代数都是数学上的概念，可以用于表示旋转和平移的变换。李群是一个连续的群，它可以被看作是一个由无穷小变换组成的集合。而李代数是一个与李群相关的切向量空间，它可以用来描述李群的无穷小变换。在视觉SLAM中，相机的位姿变换可以
30、使用李群表示。一般情况下，我们使用特殊欧氏群(SPeeiaIEUClideangroup,SE)来表示相机的位姿变换，因为它包括了旋转和平移两个变换，而且具有良好的代数性质。在三维空间中，SE(3)代表了平移和旋转的变换。李代数则是用于计算李群的无穷小变换的。在二维空间中，SE(2)的李代数为三维向量，包括平移向量和旋转角度。在三维空间中，SE(3)的李代数是一个六维的向量，包括平移向量和旋转矩阵的无穷小表示。利用李群和李代数，我们可以在视觉SLAM中进行位姿估计、优化和滤波等操作。在视觉SLAM中，通常会使用基于李代数的优化算法来优化相机位姿和地图点的位置，以得到更加准确的相机位姿和地图点位
31、置。2. L2相机模型(1)单目相机单目相机的模型有很多种，目前最常见的就是针孔模型，主要原理就是小孔成像，把相机当做是小孔，将三维世界的影像投影到相机平面上变成二维。下面对小孔成像模型进行数学建模。图2T：单目相机模型首先定义两个坐标系，相机坐标系。-印r,和物理成像平面的坐标系。-Xyz,其中相机的光心。可以理解为针孔模型的小孔。现有三维世界点P,其坐标为(X,RZ),经过成像之后落在。-Xyz上点P,其坐标为(X,y,Zj0焦距/为物理成像平面到相机成像平面的距离，根据相似三角形原理有：(2-10)XYZ=Xf由于现实中的三维世界到物理平面的像是倒立的，通常会在比例前加负号来表明这种倒像
32、的关系，但是由于最后相机呈现的相片经过处理后旋转到了正常的角度，并且为了模型建立的方便，把负号去掉，同时把XJ整理到左侧可得：x=/z,y=zQ-II)由于计算机处理图片都是以像素的形式处理，还需要引出像素坐标系-m-v,像素坐标系和物理成像平面的关系是缩放和平移的关系。轴定义为原点向右，U轴定义为原点向下。假设物理成像到像素坐标在轴上缩放了a倍，在y轴上缩放了4倍，原点偏移了(生,分)，则像素坐标为：(2-12)S,XXaX+cx=af-+cx=fx-+cx乙乙Yu=y+cy=f-+cy=f-+cy乙乙将式(2-12)写成矩阵的形式为:(O OU/fxO= K(RPmt) = KTR(2-1
33、3)上式中间的矩阵成为相机的内参矩阵K,一般一个相机生产出来之后其内参就已经是确定的，通过各种手段去确定相机的内参矩阵就叫相机内参的标定。相机内参描述的是相机自身固有的参数和属性，与相机内参对应的是相机的外参数,相机外参数表示是世界坐标系到相机坐标系的变换。设点P在世界坐标系下的坐标为匕,在相机坐标系下的坐标为以,则有：(2-14)(2)双目相机由于单目相机无法确定物体的深度信息，在后来的发展中诞生了双目相机和深度相机。双目相机也不可以直接测量物体的深度，但是可以根据左右目来计算三维空间中点的深度。模型如下图所示。成像平面P=(xtz)图2-2：双目相机模型L为左光圈中心，R为右光圈中心，f为
34、焦距，即和Z为成像平面上像素坐标，根据相似三角形的原理可以得到：z-fh-xl+xr整理(2-14)得到:fbdefZ=丝,d=xx,(2-15)dd被称作是视差，代表的含义是空间中的点在成像平面上的横坐标之差，视差越大则距离越近，反之越远。(3)RGB-D深度相机相比双目计算深度，深度相机的方式更为直接，深度相机可以获得每个像素的深度信息，按照其测量原理目前可以分为两大类：(1)结构光型，通过红外结构光测量像素深度，这种相机是采用结构光产生的编码投射到目标物体上，通过相机接收目标物体反射回来的编码图案，进而计算出目标物体在空间中的深度信息。它具有精度高、测量范围大和速度快等特点。例如：Kin
35、ect、IntelReaISenSe等。(2)TOF型，通过飞行时间测量像素深度，这种相机是采用飞行时间原理，在拍摄目标物体时发射光子，并通过接收器回收反射光信息，计算出物体到相机的距离。它具有精度高、测量范围较大和速度快等优点，但成像质量相对较差。例如：KineCt2和一些TOF传感器等。2.L3非线性优化在视觉SLAM中，往往会遇到优化问题，这些问题可以建模成一个包含高斯噪声的非线性最小二乘问题，解决非线性最小二乘的常用方法有阶梯度法，高斯牛顿法(GN)和列文伯格马夸尔特法(LM),本章介绍使用最为广泛的GN和LM法。(1)高斯牛顿法首先考虑一个最小二乘的问题：minF(x)=H/(x)I
36、l(2-16)x2需要注意的是目标函数是F(X)而不是r(X),对/(X)进行一阶泰勒展开可得：/(xx)(x)+(x)ax(2-17)其中(x)是一个列向量，可以通过求解/(x)关于X的导数得到，于是可以将问题转化为寻找一个增量X,使得II(x+ax)最小，将问题转化为线性最小二乘问题的求解:ax=argmin1/(x)+(x)x2(2-18)2对式(2-18)展开平方项可得：IIl/(X)+J(x)taxll2=(+J(x)x)()+7(x)x)22(2-19)=g(l(x)l2+2(x)7(x)7xx7(x)7(x)7ax)令A1的导数等于0：J(x)(x)+J(x)Jt(x)ax=0(
37、2-20)根据式(220)可以得到如下方程组：J(x)Jt(x)ax=-JMf(X)(2-21)H(x)g(x)上面这个公式称为高斯牛顿方程(GaUSS-NeWtonequation)或者正规方程(Normalequation)定义系数H和g,式(2-21)可以写成：Hn=g(2-21)高斯牛顿法的求解步骤可以总结为：(1)首先给定初始值小。(2)对于第n次迭代，计算J(Z)和误差f(xtl)o(3)求解方程“二XZI=g。(4)如果“足够小则停止，否则，x,l+1=xn+xrt,继续返回第二步。(1)列文伯格-马夸尔特法LM法选择给/添加一个信赖区域，因而这个方法也叫信赖区域方法，只有在信赖
38、区域内才认为二阶的泰勒展开在展开点附近有良好的效果，在区域外近似可能会出现问题。为了描述近似程度的好坏，定义指标。来刻画：/(x+x)-(x)a7JMAX式(2-22)的分母是近似模型的下降值，分子是实际函数的下降值。如果计算得到。接近1,表示近似是良好的，如果太大，说明实际减少的值远大于近似减少的值，应该要增大信赖区域，否则如果较小，则需要缩小信赖区域。LM法的具体实现步骤如下：(1)给定初始值与以及初始的半径。(2)对于第m次迭代，求解：min(Xm)+J()7AXnIl2JLl2xm2(3)按照公式求解仪,如果0.75,则=2，若0.25,则=0.5。(4)如果。大于某个确定的阈值，则认
39、为可行，令+=Xz(5)判断算法是否收敛，收敛则结束，否则继续进行第二个步骤。2.2跟踪线程2.2.1引言ORB-SLAM2跟踪线程主要完成的工作是视觉里程计、重定位，局部地图跟踪和关键帧判断。视觉里程计是比较关键的部分，其核心问题是怎么样根据已知的图形去估计未知的相机运动。但是如果但从整张图片来看，想要恢复相机的运动是十分困难的，因此一般会选择先从图中找出特征点，再从特征点中寻找方法恢复相机的运动。特征点是可以体现图像特征的一个个点的集合(经典SLAM中也叫路标)关于特征点我们希望他可以有许多良好的性质，比如在提取的时候可以不受光照影响，可以不受相机旋转和相机前后平移的影响，也就是可以保证旋
40、转不变性和尺度不变性。为了解决这些问题不少学者提出了解决的方法,比如SlFT、SURF、ORB等等。ORB-SLAM2采用的是ORB提取特征点的方法。2.2.2特征提取一个特征点由关键点和描述子两部分组成。关键点就是能够表征图像一些关键部位的信息点，一般采用角点作为关键点，提取角点的方法有Harris角点、FAST角点、GFTT角点等。ORB-SLAM2采用FAST方法提取关键点，FAST和其他方法相比具有不可替代的优点：计算速度快，实时性强。因而ORB-SAM2也具有较好的实时性。FAST关键点提取的原理十分简单,只需要比较像素大小，找出像素差大的点即可。他的检测过程如下：(1)首先在假设选
41、中的像素点为a点，他的亮度为(2)然后设置一个检测的阈值7；(3)以a点为圆心画一个半径为3的圆，选取圆上的16个点作为比较点(4)假如这十六个点中存在点a,其周围有连续N个亮度大于/+或者小于则a点可以被认为是特征点。N的数量可以为12、9、11。常用的一般为11。为了解决尺度不变性，采取构建图像金字塔解决。图像金字塔是常见的处理尺度不变的手段，主要思想是对图像进行尺度的缩放，构建不同大小的图像进行特征提取和匹配可以保证相机前后平移也能匹配到相同的特征点。为了解决旋转不变性可以计算特征点的图像灰度质心，计算的步骤如下：首先定义区域图像的矩：m%=ZNy(x,y),p,q=0,l(2-23)式
42、子中/(x,y)表示在坐标3,y)处的图像的灰度值，沿着x,y方向的图像的矩分别为：RR网。=x(x,y)二,/(2-24)-O1=W(,y)x=-Ry=-R区域内所有像素灰度总和为:%o=(Q)z9x=-Ry=-R(kZ-Z3)图像的质心为：C=(C).)=(组或)(2-26)700/0关键点的方向为该点指向质心C的方向，记关键点的旋转方向为：=arctan2(cx,cy)=arctan2(w0l,w10)(2-26)2.2.3特征匹配在选取好特征点之后需要进行特征匹配来进行后续的相机位姿的估计。在两个缓慢的连续帧之间，可以得到两帧之间看到一些相同的特征点，特征匹配的任务就是判断哪些特征点属
43、于同一个特征点。首先介绍BRIEF描述子，在得到一个特征点之后可以用BRIEF描述子计算这个特征点周围的一些信息以便进行后续的特征匹配。BRIEF描述字是一组二进制的矩阵或者序列，他在关键点附近随机选取两个点A和B,如果A点的灰度大于B点则取1,反之则为O,ORB-SLAM2采取256位的BRIEF描述子，这样对于每个特征点都可以得到一个横向量，对于N个特征点可以得到N*128的矩阵。得到关键点周围的信息矩阵之后可以计算两个点的相似程度来进行特征匹配，常见的手段是计算两个特征点的汉明距离。对于两个点得到的两个描述子,对比它们相同的位，如果这个位的值一样则不变，否则汉明距离加一，这样下来就能得到
44、256位描述子的汉明距离，一般而言汉明距离越小那么可以认为这两个点越相似。常见的匹配方法有暴力匹配和快速近似最邻法。暴力匹配就是比较该特征点和另一张图中的所有特征点，找到距离较小的。FLANN核心思想是通过构建一个能够快速定位到目标点附近区域的数据结构，比如KD树(K-dimensionalTree)或LSH哈希(LOCaIitySenSitiVeHaShing),来加速最近邻搜索。ORB-SLAM2主要采用FLANN法进行特征匹配。但是由于光线和一些噪声的存在，往往会存在误匹配，可以采用RANSAC等方法来优化，减少误匹配。2.2.4运动估计(1)对极几何假设已经在两张图像中找到了特征点Pl
45、和P2，三维空间中对应的点为P点。假设相机旋转矩阵为及,相机的平移矩阵为f,由相机的内参公式可得：PI=KPWl(2-27)Pi=KPW2由相机的外参公式可得：Pc2=Pe+t(2-28)将式Q-27)带入式Q28)可得：KTP2=RK-p+，(2-29)对式(2-29)左右两边叉乘，可得：Kp2=fRK+(2-30)由于向量叉乘自身结果为0,因而等式右边为0,同时对等式(2-30)左边叉乘(KTP2尸可得:(KP2-2=(KpjrfRKP(2-31)由叉乘的性质式(2-31)可以写成：HKTPI=O(2-32)由此定义本质矩阵E和基础矩阵F：defH=E(2.33)defKTfRKT=F求解
46、出本质矩阵或者基础矩阵就可以得到相机的位姿，常见的求解方法有五点法和八点法。ORB-SLAM2采用八点法求解。(2)三角化即使已经通过对极几何求解得到相机的位姿，但是特征点的深度信息在整个空间当中仍然是未知的。可以利用三角化求解得到关键点的具体深度信息。三角化又叫三角测量，是在不同位置观察同一个路标推测路标深度的算法。图中PI和P2是一对事先匹配好的特征点，他们在空间中对应点P，01和。2是两个位置相机的光心，图像到乙的旋转矩阵和平移矩阵分别为R和t。不考虑误差OlPl和。2P2会相交于空间中的P点，但是由于噪声的存在他们往往不会交于P点。假设*和分别是Pl和Pz对应的归化坐标，则有：wrlx
47、1=mc2Rx2+1(2-34)其中R和t已经在三角化中求得，只需要求解尺度因子眼和加,2,在等式两边同时乘上X；可得：mclxxi=me2xRx2+xiZ=0(3-1)由上式可以求得叫和mc2o2. 3后端算法机器人在移动过程中，由于相机误差等原因，可能会产生位姿估计误差，这些误差会随着时间的推移而积累，最终导致系统的位姿估计不准确。为了解决这个问题，需要对误差进行优化。局部建图的主要任务是把前端获得的关键帧和地图点插入到地图中，然后对临近地图点进行选择和剔除生成新的地图点。除了利用优化方法进行位姿调整外，回环检测也是一种常用的位姿优化方法，特别适用于较大尺度地图的构建。0RB-SLAM2使用的DBoW2(BagsofBinaryWordsforFastPlaceRecognitioninImageSequence)库是
