机器人智能导航全套课件

第一章绪论机器人智能导航是一种深度融合脑科学、认知科学、人工智能等多领域理论和技术来解决机器人在复杂任务场景下的导航、感知、认知及规划问题的前沿导航技术机器人智能导航涉及环境测量感知与理解、导航决策等。其中，环境测量感知与理解是机器人实现自主决策和高效执行任务的基础。环境测量感知通过多源数据空间配准、融合与存储，实现对运动环境要素的统一表征；环境理解是对感知的信息进行深度加工和解读的过程，构建度量与语义的统一模型。导航决策技术是机器人的核心能力之一，基于对环境感知与理解的结果，制定出最优的决策方案。微惯性传感器：基于微机电系统（MicroElectroMechanicalSystem，MEMS）加工技术制作的惯性传感器，包括测量线运动的微机电加速度计和测量角运动的微机电陀螺。1.2.1导航测量感知与理解技术发展石英挠性摆式加速度计石英振梁式加速度计微机电陀螺(基于哥氏效应)随着传感器技术、计算机视觉技术、深度学习技术的不断发展，环境测量感知与理解关键技术不断成熟和完善，在测量层、认知层以及协同层三个技术层级上呈现出并行发展趋势1.2.1导航测量感知与理解技术发展1.2.1导航测量感知与理解技术发展视觉传感器：通过捕捉图像或视频信号，并利用图像处理技术进行分析和识别，以实现目标检测、跟踪、识别等功能。激光雷达：通过发射激光束并接收其反射回来的信号来测量目标物体的距离、速度和方向等信息。新的传感器非重复扫描雷达FMCW激光雷达1.2.1导航测量感知与理解技术发展仿生光罗盘是一种模仿动物器官结构和感知大气偏振光的机理，将大气偏振光信息转化为载体航向信息的导航传感器，主要分为两类，一类是基于光电阵列探测器的面阵型偏振光罗盘，另一类是基于光电二极管的点源型偏振光罗盘。对于单体而言，面向几何、语义、拓扑等信息的认知发展历程经历了从滤波到学习、从单模到多模的过程。具体来说，如图1-2所示，语义分割技术最初依赖于特征分类，后来逐渐发展为采用深度学习；关键目标识别也从模板匹配演进到深度学习阶段。同样，单体SLAM技术也经历了从卡尔曼滤波到深度学习的进步。随着传感器数量增加，多传感器融合技术也不断涌现，进一步提高数据处理的精度和可靠性。1.2.1导航测量感知与理解技术发展1.2.1导航测量感知与理解技术发展语义分割：对机器人而言，语义认知信息包括环境中物体的类别、尺寸、位置、朝向以及度量地图中不同区域的性质等真实属性。1.2.1导航测量感知与理解技术发展目标识别：在深度学习普及之前，目标识别主要依赖于精心设计的特征描述符。2012年后，主要为基于深度学习的方法。1.2.1导航测量感知与理解技术发展SLAM技术：SLAM技术是指机器人利用自身搭载的传感器构建环境地图，同时利用环境信息进行自主定位。1.2.1导航测量感知与理解技术发展激光雷达SLAM技术：激光SLAM主要通过激光雷达直接获得载体相对于环境的距离和方位信息，实现环境的建图和载体自身相对位置的确定。1.2.1导航测量感知与理解技术发展视觉SLAM主要依靠视觉传感器，其价格适中，使用方便。经验场景：科大北门唤起经验知识定位：科大北门已知环境下导航定位未知环境下导航定位视觉同时定位与建图1.2.1导航测量感知与理解技术发展协同语义认知：随着深度学习技术的发展，对图像、点云等原始测量感知信息的语义认知技术在智能导航中发挥了重要作用。1.2.1导航测量感知与理解技术发展协同SLAM：SLAM技术在单一平台场景中已经日益成熟，但随着SLAM任务的不断发展，场景规模呈现扩大化与复杂化的趋势，单平台SLAM技术往往难以满足任务需求，因此集群协作同步定位与建图技术成为研究热点。1.2.1动态路径规划与决策关键技术发展机器人导航规划与决策技术是指结合导航定位与智能决策技术，通过实时获取机器人的位置、速度、姿态等参数，结合环境信息（如路况、障碍物、天气、交通规则等），运用先进的算法与模型，为机器人规划出最优或可行的行驶路径，并做出运动过程中的智能决策，以实现安全、高效、准确地引导机器人从起始点到达目的地的技术。机器人导航规划与决策包含单体决策和群体决策，1.2.1动态路径规划与决策关键技术发展1.2.1动态路径规划与决策关键技术发展单平台导航决策：依靠自身感知信息进行决策。1.2.1动态路径规划与决策关键技术发展集群导航决策技术相较于单平台导航决策技术，主要区别在于其强调多机器人间的协同合作、实时通信与信息共享，以及对复杂性管理的高要求本书作为面向机器人领域的导航技术教材，内容选择上坚持经典与前沿结合、理论与实践结合。因此，本书内容涵盖了机器人导航的数学基础、基于模型的自主导航以及非自主导航、基于学习的机器人智能导航、多机器人协同导航、机器人导航规划等知识，同时设计了机器人导航实践。本书章节逻辑关系如图1-7所示。提交简答题1分此题未设置答案，请点击右侧设置按钮什么是智能导航技术？发展智能导航技术，需要解决哪些关键问题？谢谢机器人智能导航本章内容第二章自主移动机器人研究常用的数学基础2.1贝叶斯滤波框架及算法2.2三维空间刚体运动的描述2.3李群和李代数2.4非线性优化机器人定位建图等感知问题本质上为状态估计问题机器人状态估计中面临大量的不确定性，如控制输入产生的作用，传感器噪声等目前处理不确定性的最有效工具：probabilitytheoryProbabilisticrobotics：representstherobotconfigurationasaprobabilitydistributioncalledbelief.（1）概率知识回顾—随机变量离散随机变量：X可在有限集{x1,x2,…,xn}中取值

P(X=xi)或

P(xi)表示随机变量

X

值为xi的概率连续随机变量：

X可取连续值

p(X=x)或p(x)表示概率密度函数P(X=xandY=y)

=P(x,y)如果X

和

Y独立，则

P(x,y)=P(x)P(y)P(x|y)

是Y

取y

时

X

取为x

的概率

P(x|y)=P(x,y)

/P(y)

P(x,y)=P(x|y)P(y)如果X和Y

独立，则

P(x|y)=P(x)（1）概率知识回顾—联合与条件概率离散的情况连续的情况（1）概率知识回顾—全概率公式（1）概率知识回顾—Bayes公式 equivalentto

and（1）概率知识回顾—条件独立性（2）Bayes滤波框架输入:感知输入

和运动输入：感知模型运动模型系统状态估计的先验概率输出:动态系统状态X的估计该状态的后验概率（亦称Belief）:（2）Bayes滤波框架运动模型：感知模型:Makov假设（2）Bayes滤波框架Makov链z=observationu=actionx=state（2）Bayes滤波框架或（2）Bayes滤波框架KalmanfiltersParticlefiltersHiddenMarkovmodelsDynamicBayesiannetworksPartiallyObservableMarkovDecisionProcesses(POMDPs)Bayesruleallowsustocomputeprobabilitiesthatarehardtoassessotherwise.（Bayes规则可用于计算难以评估的概率）UndertheMarkovassumption,recursiveBayesianupdatingcanbeusedtoefficientlycombineevidence.（满足Markov假设的情况下，递推Bayes更新可有效地结合证据信息）Bayesfiltersareaprobabilistictoolforestimatingthestateofdynamicsystems.（Bayes滤波是一种估计动态系统状态的概率工具）（2）Bayes滤波框架Prediction（运动模型如何建模）Correction（感知模型如何建模）（2）Bayes滤波框架输入前一时刻状态的均值

及其协方差矩阵

，和运动输入

、观测

：返回当前时刻状态均值

及其协方差矩阵

。（3）卡尔曼滤波算法：输入前一时刻状态的均值

及其协方差矩阵

，和运动输入

、观测

：返回当前时刻状态均值

及其协方差矩阵

。（4）扩展卡尔曼滤波算法：表示机器人位姿在状态空间的分布，即描述机器人自定位的后验概率密度，包括位姿

和权重，(5)粒子滤波定位(MonteCarloLocalization)运动模型(5)粒子滤波定位(MonteCarloLocalization)感知模型假设特征之间的观测互相独立表示机器人位于时检测到特征的可能性(5)粒子滤波定位(MonteCarloLocalization)应用导航定位（自定位/SLAM）单/多目标跟踪多传感器信息融合信号处理、故障诊断……参考书S.

Thrun,W.

Burgard,D.

Fox.ProbabilisticRobotics.MITPress,Cambridge,MA,2005.2.1贝叶斯滤波框架及算法2.2三维空间刚体运动的描述2.3李群和李代数2.4非线性优化由固定的、不变形的部件构成各个部件在运动过程中保持形状和尺寸不变刚体运动的描述是机器人定位、导航和进行复杂任务规划的基础刚体机器人如何描述平移？（x,y,z），（经,纬,高），……如何描述旋转？（偏航,俯仰,滚转），……刚体的一般运动=平移+旋转（位置+姿态）根据欧拉角转动定理可知：任意两个空间直角坐标系，最多只需绕坐标轴的三次顺序定轴转动，即可使二者重合，这三个转动角称为欧拉角。1707-1783（1）欧拉角两类常用的欧拉角经典欧拉角（ProperEulerAngle）：按(z-x-z,x-y-x,y-z-y,z-y-z,x-z-x,y-x-y)轴序列旋转，即第一个旋转轴和最后一个旋转轴相同。YXZZ-X-Z顺序旋转（1）欧拉角两类常用的欧拉角泰特布莱恩角（Tait–Bryanangles）：按(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)轴序列旋转，即三个不同的轴。

Z-Y-X顺序旋转（1）欧拉角欧拉角的性质：不满足交换性有限转动的不可交换性：三个欧拉角的值与转动次序有关。（1）欧拉角欧拉角的性质：不满足叠加性欧拉角的性质：存在周期性周期为2π（1）欧拉角欧拉角的性质：存在奇异点正常情况万向锁当第二次旋转为±90度时，第一次旋转与第三次旋转将使用同一个轴，导致系统失去了一个自由度。这就是万向锁问题。（1）欧拉角欧拉角虽然直观，但是不方便数值计算。如何通过欧拉角计算两个坐标系之间的旋转矩阵？（1）欧拉角对于二维情况o（2）方向余弦矩阵（DCM）则对于任意一点P坐标系1下：坐标系2下：o（2）方向余弦矩阵（DCM）oo旋转矩阵的两种含义是等价的:运动前后的点在同一坐标系的表示2：运动前1：运动后同一个点在两个坐标系的表示1：变换前2：变换后（2）方向余弦矩阵（DCM）对于三维情况：两个坐标系P和QxPyPzPxqyqzq（2）方向余弦矩阵（DCM）矩阵的9个元素均为两坐标系夹角的余弦，故称为方向余弦矩阵。（2）方向余弦矩阵（DCM）我们也可以直接写出绕X、Y、Z轴旋转一定角度的方向余弦矩阵。欧拉角—方向余弦矩阵（2）方向余弦矩阵（DCM）

（2）方向余弦矩阵（DCM）欧拉角—方向余弦矩阵思考题虽然不同转动顺序的欧拉角不同，但两个坐标系之间的方向余弦矩阵是唯一的。（2）方向余弦矩阵（DCM）正交性：传递性：不具有交换性：方向余弦矩阵的性质（2）方向余弦矩阵（DCM）欧拉角VS方向余弦矩阵欧拉角方向余弦矩阵变量的数量39变量的物理意义易于理解不易理解奇异点有无周期性有无有没有一种坐标系旋转变换表示方法，只用3个变量表示，又没有奇异点或周期性呢？（2）方向余弦矩阵（DCM）在刚体定点转动理论中，根据欧拉旋转定理，一个坐标系到参考坐标系的变换，总可以通过绕一个定义在参考坐标系中的矢量的单次转动来实现。如果用表示等效转轴方向的单位向量，用θ表示转角的大小，则动坐标系的角位置完全由和θ确定，用和θ可以组成一个轴角（axis-angle）来表示：（3）四元数四元数理论是数学中一个古老的分支，它的目的是用代数学的方法实现三维空间中的矢量运算。四元数是由英国数学家哈密尔顿于1843年发明的。威廉.哈密尔顿1805-1865（3）四元数四元数（quaternion）是三维空间中的超复数其中：四元数的范数：四元数的共轭：（3）四元数将旋转变换表示成四元数：3维空间中的一个矢量r绕轴转动角度θ得到r’则以四元数描述矢量之间的变换关系为：（3）四元数单位性：传递性：四元数的性质旋转变换表示成四元数（3）四元数Matlab函数：dcm2eul()eul2dcm()eul2qua()qua2dcm()Eigen中的C++变量：Eigen::Vector3dEigen::Matrix3dEigen::Quaterniond（通过构造函数和“=”运算符的重载实现自动转换）不同姿态描述之间的相互转换：Python库：fromtransforms3d.quaternionsimport*fromtransforms3d.eulerimport*（使用成员函数进行转换）2.1贝叶斯滤波框架及算法2.2三维空间刚体运动的描述2.3李群和李代数2.4非线性优化为什么要使用李代数表示旋转变换？最优姿态估计线性代数方法非线性优化方法旋转矩阵李代数对位姿求导、增量、迭代对于所有旋转矩阵构成的集合G，有如下性质：封闭性：结合律：单位元素：逆运算：所以全体旋转矩阵构成了一个群，称为特殊正交群SO(3)它在实数空间上是连续的，所以SO(3)是李群。李群可以求导，导数与李代数有关。（1）李群SO(3)下面尝试求解R的导数形式：已知R具有正交性：两边同时求导（假设对时间t求导）：整理得：于是可以看出，是一个反对称矩阵。（2）李代数so(3)反对称矩阵一定具有以下形式：定义符号^：所以可以用一个三维向量来表示，即：（2）李代数so(3)于是我们得到了SO(3)的导数形式：即为SO(3)对应的李代数，记为so(3)。（2）李代数so(3)另外，SO(3)和so(3)也可以扩展到六维的位姿（位置+姿态），得到类似的SE(3)和se(3)：特殊欧氏群SE(3)SE(3)对应的李代数se(3)（2）李代数so(3)那么是否具有其他某种特殊含义呢？我们对进行积分（零初值），得：若将写成模值和方向的形式：则有：这就是著名的罗德里格斯（Rodrigues）公式，也就是轴角到旋转矩阵之间的变换公式。（2）李代数so(3)小结优势劣势主要应用欧拉角变量意义直观不便于数值计算、存在万向锁用户输入输出方向余弦矩阵（或SO(3)）便于数值计算变量维数冗余、变量意义不直观程序内部数值计算四元数较便于数值计算需要频繁单位化数据保存、程序内部数值计算李代数so(3)便于求导较不便于数值计算位姿的非线性优化欧拉角四元数方向余弦矩阵轴角李代数非线性优化2.1贝叶斯滤波框架及算法2.2三维空间刚体运动的描述2.3李群和李代数2.4非线性优化有m个相机，n个三维特征点，相机的位姿为，三维点世界坐标表示为将j特征点投影到i相机上，得到其投影像素坐标为三者之间构成了一组共视约束

已知和求→立体视觉（双目）已知和求→透视投影（相机拍照）已知和求→自定位（PnP）已知和，使用非线性优化方法求对任一个的估计值，则根据投影变换，得到对应的投影

与实际的存在偏差：目标：寻找一个最优的使最小

是的一个非线性函数目标函数高斯牛顿迭代：设迭代增量为，则迭代之后误差的一阶近似：

称为雅克比矩阵迭代后的目标函数变为：高斯牛顿迭代：我们希望这一次迭代使得变为最优，也就是使目标函数取极小值：得：高斯牛顿迭代：已知、，给定位姿初值，迭代终止条件

①计算观测误差

②计算雅克比矩阵

③得到迭代增量

④如果，终止迭代，返回

⑤令，返回第①步返回计算雅克比矩阵：第j个分量：设在世界坐标系下坐标为在相机坐标系下坐标为则有：计算雅克比矩阵：第j个分量：①②计算雅克比矩阵：于是：②①需要利用李代数的性质，此处不做推导高斯牛顿迭代：已知、，给定位姿初值，迭代终止条件

①计算观测误差

②计算雅克比矩阵

③得到迭代增量

④如果，终止迭代，返回

⑤令，返回第①步返回g2o（GeneralGraphOptimization）利用图优化求解问题需要自定义图的顶点和边（一些典型的顶点和边已集成）Ceressolver谷歌开发的一款用于非线性优化的库在谷歌的开源激光雷达SLAM项目cartographer中被大量使用GTSAM佐治亚理工的学者开发完成（GeorgiaTechSmoothingandMapping）开源的、使用C++语言编写。使用因子图优化。非线性优化工具图优化（GraphOptimization）顶点（Vertex）表示可优化的状态量；边（Edge）表示节点之间的约束；利用图优化求解问题。BundleAdjustment、光束平差：滑动窗口、边缘化：因子图优化：贝叶斯网络因子图典型应用：闭环轨迹优化视觉里程计三维建图2.1贝叶斯滤波框架及算法2.2三维空间刚体运动的描述2.3李群和李代数2.4非线性优化谢谢机器人智能导航第三章基于模型的机器人自主导航方法3.1惯性导航3.2视觉建图与定位3.3激光建图与定位3.4多传感器组合导航惯性导航以牛顿力学定律为基础，通过陀螺仪测量载体在惯性参考系的角运动和加速度计测量载体在惯性参考系的线运动，然后将这些数据对时间进行积分运算，从而得到运动载体的姿态、速度及位置信息。按照测量方式的不同，惯性导航可分为两类：平台式惯导的核心部分是一个稳定平台，它为加速度计测量导航系下的线运动提供了基础，同时保证加速度计有一个良好的工作环境平台式惯导捷联式惯导没有实体的陀螺稳定平台，而是由导航计算机来实现相应的功能，计算机实时计算三个方向的角速度，进行坐标系转换，形成一个数学意义的平台捷联式惯导优点1工作自主性能强2提供较多的导航参数3抗干扰能力强缺点1误差随时间累积2高精度系统成本高、体积大3需要初始对准误差累积特性惯性导航系统的误差会随时间累积，这是其最主要的缺点。因此，惯性导航系统通常需要与其他导航方式（如GPS、视觉、激光等）进行组合，以抑制误差的累积增长。惯性导航微分方程是描述载体运动状态的基础方程。它通过连续测量载体的加速度和角速度，结合初始条件，建立速度、位置和姿态随时间变化的数学模型。速度微分方程描述加速度计测量的比力矢量与载体运动加速度、引力加速度之间的关系位置微分方程描述载体位置随时间的变化关系，可采用直角坐标或经纬度高程表示姿态微分方程描述载体坐标系与导航参考坐标系的相对姿态关系随转动角速度的动态变化微分方程的作用连续测量建立模型实时计算系统核心3.1.1惯性导航微分方程惯性导航的速度微分方程也称为比力方程。比力是指在惯性系中敏感的作用在单位质量上的非引力外力，与加速度具有相同的量纲。3.1.1惯性导航微分方程速度微分方程比力矢量载体运动加速度矢量引力加速度矢量用表示载体相对地球的速度矢量，即3.1.1惯性导航微分方程速度微分方程地球自转角速度矢量导航参考坐标系相对于地球坐标系旋转矢量重力加速度矢量惯性系或地球系下的直角坐标系位置微分方程可以写为：位置矩阵方法当地水平地理坐标系采用"北东地"坐标系时，使用位置矩阵方法，用当地水平地理坐标系与地球坐标系之间的方向余弦矩阵将位置微分方程表示为：直角坐标系经纬度高程位置矩阵三种形式的表示方法可以相互转换。直角坐标系表示方法物理意义明确，适合进行力学编排；经纬度高程表示法适合进行导航参数的解算。3.1.1惯性导航微分方程位置微分方程位置微分方程的3种形式的表示方法可以相互转换。直角坐标系物理意义明确适合进行力学编排经纬度高程适合导航参数解算符合导航习惯位置矩阵矩阵形式表示便于坐标系转换相互转换关系三种形式的表示方法可以相互转换：直角坐标经纬度高程位置矩阵在实际应用中，根据具体需求选择合适的位置微分方程形式。直角坐标系表示方法物理意义明确，适合进行力学编排；经纬度高程表示法适合进行导航参数的解算。3.1.1惯性导航微分方程姿态微分方程描述了载体坐标系与导航参考坐标系的相对姿态关系随转动角速度的动态变化。通常可以采用方向余弦矩阵、四元数矩阵和等效转动矢量进行表示。方向余弦矩阵DirectionCosineMatrix四元数矩阵QuaternionMatrix等效转动矢量RotationVector方向余弦矩阵直观易懂，无奇异点，但参数较多（9个）四元数参数较少（4个），计算效率高，无奇异性转动矢量参数最少（3个），但存在奇异点3.1.1惯性导航微分方程姿态微分方程由于惯性导航是根据递推求解姿态、速度、位置等导航参数，递推的初值必不可少。初始对准的必要性递推求解需要初值惯性导航根据递推求解导航参数提供计算基准为后续导航计算提供基准关键工作步骤是惯性导航系统工作的关键步骤3.1.2初始对准初始对准的内容初始姿态确定载体的初始姿态（最重要）初始速度由外部测量设备输入初始位置由外部测量设备输入最重要的工作一般情况下，载体的初始位置和初始速度由外部测量设备输入，因此初始对准的过程最重要的工作就是确定载体的初始姿态。3.1.2初始对准初始对准的过程也可以看作确定载体坐标系和导航坐标系之间方向余弦矩阵的过程，因而可以通过矢量叉乘的方法构造出三个向量确定三个矢量在两个坐标系中的投影，从而确定两个坐标系之间的旋转关系。参考矢量选择通常可以选重力矢量和地球自转角速度作为参考。重力矢量方向指向地心，大小约为9.8m/s²地球自转角速度方向沿地球自转轴，大小约为15°/h当惯性传感器精度较低，无法测量到载体系中的地球自转角速度ω_ie时，可以引入外部传感器辅助惯导进行初始对准。3.1.2初始对准当惯性传感器精度较低，无法测量到载体系中的地球自转角速度时，可以引入外部传感器辅助惯导进行初始对准。3.1.2初始对准矢量叉乘构造选取当地水平地理坐标系为导航坐标系，可知：捷联惯性导航解算主要通过陀螺仪和加速度计测量数据，结合初始条件，进行实时积分运算，以解算出载体的速度、位置和姿态。姿态更新根据陀螺仪测量的角速度更新载体姿态速度更新根据加速度计测量的比力更新载体速度位置更新对速度积分更新载体位置解算流程传感器数据积分运算姿态速度位置捷联惯性导航解算是惯性导航系统的核心，通过实时积分运算，确保导航信息的准确性和实时性。3.1.3捷联惯性导航解算惯性导航姿态更新也有三种方法，分别是方向余弦矩阵法、四元数法和欧拉法。方向余弦姿态微分方程根据前文的推导，方向余弦姿态微分方程为：微分方程的解在时间间隔(t_k-1,t_k)内，微分方程的解为：3.1.3捷联惯性导航解算姿态更新算法记旋转矢量为积分项可以展开为泰勒级数形式在计算过程中，通常是对正弦函数和余弦函数继续作泰勒展开，当所取的阶次越高、数据更新频率越快时，计算的精度越高。3.1.3捷联惯性导航解算惯性导航姿态更新也有三种方法，分别是方向余弦矩阵法、四元数法和欧拉法。姿态更新算法速度微分方程可以表示为比力方程的形式。在时间间隔内，微分方程的解包括比例积分增量和重力哥氏加速度积分增量。速度微分方程的解在时间间隔(t_k-1,t_k)内，微分方程的解为：增量计算公式3.1.3捷联惯性导航解算速度更新算法速度更新算法是捷联惯性导航解算的重要组成部分，通过积分计算速度的变化，得到载体的实时速度信息。对速度矢量进行积分即可得到位置矢量，当导航解算频率比较高时，积分方式直接采用梯形积分。位置微分方程根据位置微分方程，在地理坐标系下的经度、纬度和高程更新可以表示为：其中T为采样时间梯形积分方法计算简单高效适用于高频率解算位置更新流程速度积分位置3.1.3捷联惯性导航解算位置更新算法3.1惯性导航3.2视觉建图与定位3.3激光建图与定位3.4多传感器组合导航视觉建图与定位是利用相机作为传感器采集场景的图像信息，结合多视图几何模型预测场景的结构信息（地图）与相机的位姿信息，再经过时间递推构建出整个序列的全局地图。因为其低廉的成本在多种无人平台中得以应用。成本优势视觉传感器相比于激光雷达等其他传感器具有明显的成本优势：价格低廉相机成本远低于激光雷达体积小便于集成到各种机器人平台信息丰富提供丰富的场景纹理和颜色信息后端优化模型透视投影模型多视图几何重投影误差模型优化位姿光度平差法全局优化视觉建图与定位技术因其低廉的成本和丰富的信息获取能力，在多种无人平台中得以广泛应用。视觉定位与建图方法大致可分为"感知"和"优化"两部分，它们分别在算法中代表前端和后端。前端（感知）负责捕捉当前场景中显著局部特征，并使用匹配算法对提取出的特征进行跟踪后端（优化）使用多视图几何模型对前端预估的初始位姿和地图进一步优化，并使用闭环校正模块对前端定位结果的积累误差进行校正按照前端提取与跟踪的局部特征的类型，可以把视觉定位与建图方法分为光流法与特征点法，其中光流法注重特征提取的效率，特征点法则注重特征提取的精度，两者在计算机视觉中各具特色，互为补充。光流法OpticalFlow注重特征提取的效率，通过分析图像中像素点的位移来计算物体运动计算效率高适合实时应用稠密光流提供像素级运动信息对光照敏感光照变化影响精度特征点法Feature-based注重特征提取的精度，通过检测和匹配图像中的显著特征点进行位姿估计精度高特征点匹配准确鲁棒性强对光照变化不敏感计算量大特征提取和匹配耗时光流法通过分析图像中像素点的位移来计算物体运动，适用于运动检测、目标跟踪等场景。光流的定义光流描述短时间内相机运动所拍摄得到的图像中同一物体上的像素在相邻帧中的像素运动向量。该向量分为u和v图像横纵坐标上的位移。光度不变假设假设同一物体上的像素在相邻帧中的灰度值保持不变。该假设称为光度不变假设。3.2.1光流估计光流方程组求解构建光流方程组，使用最小二乘法求解：基于局部光度一致模型的光流提取算法通过光度不变假设和一阶泰勒展开推导光流方程，使用最小二乘法求解光流在图像横纵坐标上的移动速度。3.2.1光流估计基于局部光度一致模型的光流提取算法是一种基于图像低层信息的特征提取与跟踪算法，如果在前后帧运动过程中物体表面光照变化明显则会破坏局部光度一致性模型，从而导致光流估计精度变低。深度学习类方法使用多层神经网络提取图像的深度语义特征，并使用与任务相关的回归器将深度特征转化为具体的估计值。深度学习的优势高鲁棒性在多种计算机视觉任务中得以应用深度语义特征提取图像的高层抽象特征光照鲁棒提升光照变化场景中的估计精度3.2.1光流估计视觉特征提取是计算机视觉中的关键步骤，涉及从图像中检测并提取出具有代表性的局部或全局信息。常见方法包括基于手工设计的特征提取和基于深度学习的特征提取，这些方法有助于计算机更准确地理解和分析图像内容。手工设计特征Hand-craftedFeatures依赖图像特定属性边缘、角点、纹理等计算效率高适合实时应用深度学习特征DeepLearningFeatures高层语义特征深度神经网络提取鲁棒性强适应复杂光照环境主要特征提取算法手工设计SIFT,SURF,ORB,Harris深度学习SuperPoint,LIFT应用场景SLAM,三维重建3.2.2视觉特征的提取基于手工设计的特征点提取算法主要指的是早期计算机视觉领域中，由研究人员根据图像处理和几何学原理手工设计的特征检测和描述方法。角点是指图像中两个边缘相交的点，这些点在图像分析和计算机视觉中非常重要，因为它们提供了图像结构的重要信息。SIFT算法Scale-InvariantFeatureTransform提取图像中的尺度不变特征构建尺度空间，使用高斯差分检测关键点利用关键点邻域的主方向增强旋转不变性基于图像梯度的直方图描述符SURF算法Speeded-UpRobustFeaturesSIFT的变体，计算速度更快使用积分图像加速计算基于Hessian矩阵的近似检测关键点基于Haar小波响应的描述符SIFTvsSURF速度SURF更快精度SIFT更高鲁棒性都较好3.2.2视觉特征的提取ORB（OrientedFASTandRotatedBRIEF）是一种计算机视觉中的特征提取算法，它结合了FAST（FeaturesfromAcceleratedSegmentTest）关键点检测和BRIEF（BinaryRobustIndependentElementaryFeatures）描述符，同时增加了对图像旋转的鲁棒性。FAST特征提取1遍历图像中的每个像素点作为候选角点2确定以该点为中心的16个像素点组成的圈3对比中心点和周围候选点的亮度4通过检查部分像素点加速检测5足够多的像素点亮度差异超过阈值则确定为角点3.2.2视觉特征的提取BRIEF描述符采样模式定义由多个点对组成的采样模式亮度比较比较每个点对的亮度值二进制字符串生成二进制描述符，如256位ORB算法特点高效计算速度快旋转不变增强鲁棒性二进制便于快速匹配SLAM应用ORB-SLAM系列ORB算法因其高效性和良好的鲁棒性，在视觉SLAM中得到了广泛应用，特别是在ORB-SLAM系列中发挥了重要作用。3.2.2视觉特征的提取使用深度学习网络重构局部特征提取算法，借助深度学习神经网络的高鲁棒性解决复杂光照环境下稳定特征跟踪的问题。解决的核心问题传统方法的问题在光照变换明显的路段，传统手工设计的特征点会出现低层信息的巨变，影响特征跟踪，降低视觉定位精度深度学习的优势借助深度学习神经网络的高鲁棒性，解决复杂光照环境下稳定特征跟踪的问题3.2.2视觉特征的提取SuperPoint网络结构输入图像编码器兴趣点检测描述子生成使用深度学习网络重构局部特征提取算法，借助深度学习神经网络的高鲁棒性解决复杂光照环境下稳定特征跟踪的问题。SuperPoint通过深度神经网络学习特征点和描述子，在复杂光照环境下具有更强的鲁棒性，是视觉定位与建图领域的重要进展。3.2.2视觉特征的提取MagicPoint网络结构MagicPoint网络的结构采用传统的编码/解码结构，编码器使用类似VGG模型的卷积神经网络...编码器将单通道灰度图像编码为1/8原边长的图像增加64个通道65个通道包括原图局部非重叠像素区域和无兴趣点通道解码器对所有通道使用Softmax交叉熵函数舍弃概率最小的通道通过Reshape函数整形到原始尺寸的热度图特征检测网络训练方法为了优化网络对真实图像中潜在兴趣点的提取，使用自适应单应性估计的方法：1随机单应性变换2兴趣点检测3反解到原图4联合训练3.2.2视觉特征的提取MagicPoint网络结构MagicPoint网络的结构采用传统的编码/解码结构，编码器使用类似VGG模型的卷积神经网络...SuperPoint通过编码/解码结构实现兴趣点检测，通过单应性自适应方法提高泛化能力，联合训练特征点位置和描述子，在视觉定位任务中表现出色。3.2.2视觉特征的提取光束平差（BundleAdjustment，BA）是一种基于成像光束空间交会的几何模型建立的测量法，通过最小化损失函数优化相机内外参数及三维空间点位置来精确解决几何测量问题。在建筑、测绘、制造及机器人导航等领域有重要应用，该方法操作简单且能明确记录光的传播轨迹和特征值。光束平差法是指从视觉重建中提炼出最优的3D模型和相机参数（内参和外参）。从每个特征点反射出来的几束光线，把相机姿态和特征点的位置做出最优的调整之后，收束到光心的这个过程，简称BA。通过最小化重投影误差优化相机参数和三维点位置应用领域建筑测绘制造机器人导航3.2.3光束平差光束平差的核心思想多视角观测同一特征点被多个相机观测光束交会光线应交于同一点优化调整最小化重投影误差光束平差是视觉定位与建图中的核心优化方法，通过最小化重投影误差来优化相机参数和三维点位置，是实现高精度视觉重建的关键技术。3.2.3光束平差重投影误差最小二乘闭环检测是SLAM系统中的一项关键技术，它允许系统识别机器人是否已经返回到之前访问过的地点。这个过程对于校正长时间累积的定位误差至关重要，可以确保地图一致性并优化运动估计，是实现精准定位与环境感知的核心环节。闭环检测的作用校正累积误差校正长时间累积的定位误差确保地图一致性保持全局地图的连贯性优化运动估计提高位姿估计精度重要性核心环节实现精准定位与环境感知的核心环节误差累积问题视觉里程计存在累积误差，需要闭环检测校正闭环检测是SLAM系统中的关键技术，通过识别机器人是否返回到之前访问过的地点，校正累积误差，确保地图一致性，是实现精准定位的核心环节。3.2.4视觉闭环检测闭环检测过程的关键步骤1.关键帧选择闭环检测首先需要确定哪些关键帧是候选的闭环帧。这些关键帧通常存储在一个队列中，等待闭环检测的处理。2.词袋模型使用词袋模型（BoW）来表示关键帧的特征，将图像特征量化，形成特征向量描述图像内容。3.候选关键帧筛选通过比较当前关键帧的BoW向量与历史关键帧的BoW向量之间的相似度，来初步判断是否存在闭环的可能性。3.2.4视觉闭环检测使用词袋模型（Bag-of-Words，BoW）来表示关键帧的特征。BoW是一种将图像特征量化的方法，它将图像中的特征点与预先训练好的视觉词汇相关联，形成一个特征向量，用于描述图像内容。词袋模型因其计算效率高和实现简单，在SLAM和其他视觉任务中仍然非常流行，是闭环检测的重要工具。闭环检测过程的关键步骤4.连续性检测进一步筛选候选关键帧，确保它们在共视图中具有良好的连续性。这通常意味着候选关键帧需要与多个其他候选关键帧形成共视关系。5.特征匹配对于筛选后的候选关键帧，进行更精确的特征匹配，以确定它们与当前关键帧之间的空间关系。6.闭环验证使用RANSAC或其他鲁棒估计方法来验证闭环假设。如果闭环验证成功，则确认闭环检测成功。常见的相似度度量准则：余弦相似度、欧氏距离、曼哈顿距离、杰卡德相似系数、皮尔逊相关系数闭环检测是一个完整的过程，从关键帧选择到闭环验证，每个步骤都至关重要，确保系统能够准确识别闭环并校正累积误差。3.2.4视觉闭环检测ORB-SLAM2是一个基于特征的SLAM系统，3.2.5典型视觉建图与定位方法它能够利用单目相机、双目相机、RGB-D相机以及IMU等多种传感器输入进行实时的定位和建图。ORB-SLAM2是一个基于特征的SLAM系统，3.2.5典型视觉建图与定位方法VINS-MONO是一个用于单目视觉的惯性SLAM系统VINS-MONO系统采用了紧耦合的方式将视觉和惯性数据融合在一起，解决了单目SLAM中的尺度漂移问题，提高了系统的精度和鲁棒性。它是视觉惯性SLAM领域的一个重要贡献。3.2.5典型视觉建图与定位方法它结合了单目相机的图像信息和惯性测量单元（IMU）的数据来实现精确的状态估计和地图构建。3.1惯性导航3.2视觉建图与定位3.3激光建图与定位3.4多传感器组合导航激光雷达（LiDAR）是机器人定位和建图的常用传感器之一，与相机相比具有更高的定位测量精度。激光雷达可以通过扫描环境获取场景的三维坐标信息，从而实现高精度的自主定位和建图。激光雷达类型2D激光雷达价格低、体积小，提供平面扫描信息3D激光雷达旋转式和固态，提供三维空间信息激光雷达（LiDAR）是机器人定位和建图的常用传感器之一，与相机相比具有更高的定位测量精度。激光雷达可以通过扫描环境获取场景的三维坐标信息，从而实现高精度的自主定位和建图。代表性算法2DSLAMGmapping,Hector-slam3DSLAMLOAM,LeGO-LOAM特征提取曲率特征提取扫描匹配ICP算法激光建图与定位技术因其高精度和可靠性，在室内机器人、自动驾驶等领域得到了广泛的研究和应用。激光雷达对环境扫描获得的数据以扫描点的形式输出，形成二维或三维点云。其中点云每一个点都包含了一个三维空间坐标信息，有些激光雷达还会同步输出该激光点的光强度反射信息。点云特点三维坐标每个点包含x,y,z坐标光强度信息部分雷达输出反射强度离散分布空间离散点集数据获取帧率一般在10Hz左右存储格式按扫描线和扫描点顺序存储SDK支持设备厂家提供软件开发包LAS文件格式文件特点由美国摄影测量与遥感协会发布的标准LiDAR数据格式，二进制文件格式包含信息三维空间坐标、强度、分类、反射率等属性信息3.3.1激光点云数据的获取与处理LAS格式文件读取在Python环境下，可以使用laspy库实现对LAS格式文件的读取和处理：importlaspyin_file=laspy.file.File("input.las",mode='r')ROS话题获取如果激光雷达提供ROS驱动功能包，可通过订阅ROS话题获取数据：2D激光雷达sensor_msgs/LaserScan3D激光雷达sensor_msgs/PointCloud2点云消息类型转换PointCloud和PointCloud2消息类型之间可以互相转换：PointCloud→PointCloud2convertPointCloudToPointCloud2(input,output)PointCloud2→PointCloudconvertPointCloud2ToPointCloud(input,output)3.3.1激光点云数据的获取与处理PCL（PointCloudLibrary）是一个大型的开源函数库，提供对二维或三维图像特别是三维点云数据的处理，是处理三维点云数据的有效工具。PCL发展历程1由WillowGarage公司为PR2机器人开发构建22010年11月从ROS中分离出来3逐渐成为独立的函数库PCL功能模块滤波特征关键点配准kd树八叉树分割采样一致性表面PCL与ROS集成在ROS下使用PCL时，经常需要将点云数据在ROS消息格式和PCL点云格式之间进行转换：PCL→ROStoROSMsg(pcl_cloud,cloud_msg)ROS→PCLfromROSMsg(cloud_msg,pcl_cloud)3.3.1激光点云数据的获取与处理八叉树（octree）是一种数据组织结构。它通过对三维空间中的任意一个立方体栅格进行划分，可以得到2×2×2共8个子立方体。将划分前后的大立方体和小立方体之间的关系称为父子关系，构成树型结构。八叉树结构特点递归划分对三维空间进行从粗到细的划分树型结构父子关系构成树型结构多分辨率支持不同分辨率下的空间划分使用八叉树对三维点云进行表示，最终得到的八叉树有两类叶子节点：一类是表示了空间无实体区域（可通行空间），另一类表示了该空间为实体所占据（不可通行空间）。3.3.1激光点云数据的获取与处理迭代最近点法（IterativeClosestPoint，ICP）是一种用于计算两组三维空间中点集之间最优拟合变换的迭代方法，可以求解不同机器人位姿状态下获取的点云之间的最优变换关系。ICP算法核心思想最近邻搜索寻找对应点变换估计计算最优变换迭代优化不断更新对应关系3.3.2迭代最近点算法ICP算法原理已知机器人当前帧获取的激光点云数据为P，机器人存储的局部或全局点云地图中与P对应的点云集合为Q。通过迭代求解的方式，求取一个坐标变换T，使得P点云和Q点云经坐标变换后可以尽量接近。ICP算法步骤3.3.2迭代最近点算法在激光雷达建图和定位过程中，通过曲率特征提取降低计算量和提高位姿估计鲁棒性。曲率特征提取是一种常用的方法，它通过计算点云中每个点的曲率来识别边缘点和平面点。特征点分类边缘点曲率值较大的点平面点曲率值较小的点曲率特征提取是激光雷达建图和定位中的重要技术，通过识别边缘点和平面点，可以显著提高扫描匹配的效率和精度。3.3.3曲率特征提取与特征匹配曲率计算公式对于扫描线中的点，曲率c的计算公式为：在得到边缘点和平面点之后，可以通过构建非线性最小二乘模型来求解最优位姿。边缘点匹配残差基于点到直线距离，平面点匹配残差基于点到平面距离。平面点匹配非线性最小二乘模型构建非线性最小二乘模型来求解最优位姿：3.3.4扫描匹配与位姿优化边缘点匹配边缘点匹配残差基于点到直线距离：平面点匹配残差基于点到平面距离：3.1惯性导航3.2视觉建图与定位3.3激光建图与定位3.4多传感器组合导航在多传感器系统中，需要实现时间同步和空间位置同步。时间同步确保不同传感器的数据在时间上对齐，空间位置同步确保数据在空间坐标系上对齐。只有实现了精确的时空同步，才能进行有效的数据融合。时间同步TimeSynchronization硬件时间同步依赖统一时钟源，使用GPS或PPS信号软件时间同步通过时间插值实现，适用于异频传感器空间同步SpatialSynchronization自标定单个传感器的内参标定联合标定多个传感器之间的外参标定时空同步的重要性数据对齐确保时序一致性坐标转换统一坐标系有效融合实现数据融合3.4.1多传感器时空同步时间同步是多传感器组合导航的关键环节。硬件时间同步依赖统一时钟源，软件时间同步通过时间插值实现。相机与IMU的时间偏差标定是重要的同步工作。硬件时间同步HardwareSync统一时钟源所有传感器使用相同的时钟信号GPS/PPS信号使用GPS或PPS脉冲信号同步硬件触发通过硬件触发信号同步采集软件时间同步SoftwareSync时间戳对齐为每个传感器数据添加时间戳时间插值在异频传感器间进行插值缓冲机制使用缓冲区处理时间偏差相机与IMU时间偏差标定标定方法通过特征点轨迹与IMU数据的相关性分析时间偏移估计相机和IMU之间的时间偏移量在线标定在系统运行过程中动态估计时间偏差3.4.1多传感器时空同步空间同步与标定是多传感器组合导航的重要环节。通过自标定和联合标定，可以确定传感器之间的坐标转换关系，实现数据在空间上的对齐。IMU标定加速度计标定标定零偏、比例因子、安装误差陀螺仪标定标定零偏、比例因子、交叉耦合相机标定内参标定焦距、主点、畸变参数外参标定相机相对于载体坐标系的位置联合标定方法相机-IMU标定确定相机和IMU之间的相对位姿关系激光雷达-相机标定建立激光雷达和相机之间的坐标转换多传感器联合标定同时标定多个传感器的外参关系3.4.1多传感器时空同步FAST-LIVO框架利用激光雷达、惯性和视觉传感器测量实现稳健且准确的状态估计，包括LIO和VIO两个子系统。基于滤波的融合方法通过状态预测和更新实现多传感器数据的融合。FAST-LIVO框架LIO子系统激光雷达惯性里程计VIO子系统视觉惯性里程计紧耦合融合实现稳健准确的状态估计3.4.2基于滤波的多传感器组合导航算法FAST-LIVO使用状态转换模型来描述系统状态的演化3.4.2基于滤波的多传感器组合导航算法广义加减法定义流形上的广义加减法运算：状态转换模型状态转换方程：IMU状态递推基于IMU运动学模型进行状态递推协方差传播状态FAST-LIVO通过状态转换模型描述系统演化，使用ESIKF进行状态估计，实现了激光雷达、惯性和视觉传感器的紧耦合融合。3.4.2基于滤波的多传感器组合导航算法帧到地图的测量模型LiDAR测量模型稀疏直接视觉对准测量模型误差状态迭代卡尔曼滤波器更新最小平方误差估计3.4.2基于滤波的多传感器组合导航算法LIO-SAM算法通过平滑和映射的紧耦合激光雷达惯性里程计框架，使用因子图进行传感器融合。基于优化的方法通过构建优化问题来融合多传感器数据，具有更高的精度。LIO-SAM系统特点因子图建模使用因子图表示优化问题紧耦合融合激光雷达和IMU紧耦合闭环检测消除长时间累积误差优化方法优势高精度全局优化提高精度鲁棒性强处理异常测量灵活性好易于添加新传感器3.4.3基于优化的多传感器组合导航算法LIO-SAM将激光雷达惯性里程计问题构建为因子图机器人状态表示四种类型的因子IMU预积分因子激光雷达里程计因子GPS因子闭环因子因子图优化通过最小化所有因子的残差和来求解最优状态3.4.3基于优化的多传感器组合导航算法惯性导航基于牛顿力学定律速度、位置、姿态微分方程初始对准与捷联惯导解算视觉建图与定位光流估计与特征提取光束平差与闭环检测ORB-SLAM2与VINS-MONO激光建图与定位点云数据获取与处理ICP算法与曲率特征提取扫描匹配与位姿优化多传感器组合导航时空同步与标定方法基于滤波的融合算法基于优化的融合算法1简述惯性导航的原理和特点。2惯性传感器是指哪些传感器？3请简述加速度计测量的比力与加速度之间的区别。4简述激光建图与定位中为什么要使用特征提取，而不是直接使用原始三维点云。思考题与习题谢谢第四章基于模型的机器人非自主导航方法4.1卫星导航技术4.2无线网络定位技术4.3蓝牙定位技术4.4射频定位技术

4.5二维码定位技术4.6磁场定位技术50年代末，美国约翰霍普金斯大学应用物理实验室的研究人员WillianH.Guier博士和GeorgeC.Weiffenbach博士等通过实验表明：已知的地面接收点，精密测量出卫星天线电波中的多普勒频移，可以确定卫星轨道。第一颗人造卫星卫星导航系统的起源卫星运行轨道多普勒频移地面站多普勒效应逆向思维奇妙的想法！用户卫星运行轨道用户设计思想卫星导航系统的起源第一代卫星导航系统1957年前苏联将第一颗人造地球卫星Sputnik送入太空。1950s末，美国约翰.霍普金斯大学应用物理实验室，W.Guier博士和G.Wieffenbach博士发现了多普勒频移。1958年美国启动海军导航卫星系统（NNSS）1964年建成，1967开始民用，1996年结束使命。1965年前苏联建立CICADA卫星导航系统，与NNSS类似。卫星轨道高度低、难以实现精确定轨无法实现实时连续定位，卫星可见时间少(平均1.5h)无三维定位能力信号频率低，难以有效补偿电离层折射影响第二代卫星导航系统1973年，美国国防部制定GPS计划，1978年发射第1颗GPS试验卫星，1995年开始具备运行能力。目前，美国正在积极发展第三代GPS。1976年，前苏联提出GLONASS计划，1982年发射第1颗卫星，1995年建成，2001年俄罗斯恢复优化，2011年运行。1994年“北斗一号”立项，2003年完成建设并投入使用。2004年启动“北斗二号”建设，2012年完成建设。2009年开始“北斗三号”建设，2020完成组网，并提供全球定位服务。1999年欧盟提出建设Galileo系统，2002年项目启动，2016年系统正式启用，2023年启用高精度定位服务。空间部分地面运控部分用户部分卫星信号星历信息等导航信号军用：通信无源：用户仅接收卫星导航信号即可实现定位GNSS系统的三大组成部分虚假位置真实位置用户待定位置r3r2r1三球面交汇测距定位原理卫星发射信号的钟面时间高精度原子钟，30万年1秒实际测量情况石英晶振，几毫秒至几百毫秒误差一天用户接收信号的钟面时间利用钟面时间相减得到的传播时间为：根据钟面时间计算得到的距离：观测距离还受其它因素影响。

乘以光速

已知第j颗星的位置，并考虑平流层、电流层延时误差等伪距:伪距观测方程：若同时观测到多颗卫星，则：伪距测量精度=定位精度几何精度因子（GDOP）三维位置精度因子（PDOP）钟差精度因子（TDOP）水平位置精度因子（HDOP）高程位置精度因子（VDOP）PDOP：三维位置精度因子HDOP：水平位置精度因子HorizontalVDOP：高程精度因子TDOP：钟差精度因子由于卫星几何构型的影响，伪距观测误差被加权（放大或缩小）PDOP倍和TDOP倍，即为位置误差、钟差误差。受环境、仪器等因素影响，测量误差不可避免，误差源主要有三类：与卫星有关的误差卫星钟差卫星星历误差卫星硬件延迟卫星天线偏差等与信号传播有关的误差电离层延迟对流层延迟多路径效应与接收机有关的误差接收机钟差接收机天线偏差接收机观测误差接收机噪声多路径效应电离层对流层差分定位（DGNSS）：使用两台以上的卫星接收机同步观测，以确定位置或坐标差。基准站：位置已知，一般处于固定状态移动站：可固定也可移动，可接收基准站信息基线矢量：移动站相对于基准站的三维位置矢量北斗卫星导航系统测距码精度B1:11cmB2:5cmBDS/GPS短基线模糊度固定率≈100%伪距差分定位H:1.5mU:2.0m伪距导航定位H:6mU:10m亚太可用率≈100%相位精度B1:0.5mmB2:0.3mmBDS/GPS

RTKH:0.81cmU:1.08cm相位差分定位H:1.3cmU:2.0cm到达时间法（TOA，TimeofArrival）主要测量无线信号在基站和移动终端之间的单程传播时间或者收发一次的来回传播时间。通过时间将距离测出来。要求收发装置之间时间同步不要求收发装置之间时间同步两种方法均对时钟的精确度有要求到达时间法（TOA，TimeofArrival）主要测量无线信号在基站和移动终端之间的单程传播时间或者收发一次的来回传播时间。通过时间将距离测出来。到达时间差法（TDOA，TimeDifferenceofArrival）通过检测无线信号到达的时间差来确定移动终端的位置。方式一：不同基站的信号到达终端的时间差方式二：同一基站不同类型的信号到达终端的时间差不要求信号源和基站之间时间同步到达角度法（AOA，AngleofArrival）通过检测无线信号到达终端的角度来确定移动终端的位置。要求基站具有角度测量功能通过角度的测量可以确定无线信号到达基站的路径通过两个基站就可以确定移动终端的位置传播规律：无线信号在传播过程中遵循以下规律（基站发射功率一定）：接收到的信号强度取决于发射功率、额外损耗和其他因素的系统常数路径损耗指数发射端和接收端的距离强弱接收到的信号强度与收发双方的距离成反比，距离越近，信号越强；距离越远，信号越弱。不同位置接收到的信号强度可以作为一种特征。利用接收到的信号强度来进行定位的方式称为信号强度法（RSSI，ReceivedSignalStrengthIndication）。位置指纹法传播模型法RSSIRSSI指纹二维码IP指纹013楼教室机房射频标签ID号位置指纹法：对于特定的位置，都有唯一可测的物理量相对应，这种用于标识位置的可测物理量被称为位置指纹。指纹定位是基于无线信号强度的，但相比于基于信号强度的三边测距法具有更高的定位精度且计算方法简单，因而近年来室内无线定位方法研究主要集中在基于指纹的定位算法。基于指纹的定位算法可分成两个阶段：校准阶段与定位阶段，指纹数据库定位技术原理如图4-5所示。基于蓝牙的室内定位技术多种多样，根据是否测距可分为基于测距的定位算法和基于非测距的定位算法。基于测距的定位算法需要通过距离或角度的测算实现定位，而基于非测距的定位算法主要利用信标节点与待定位目标之间的连通性或者指纹信息。其中指纹定位算法与4.2.3节的无线网络指纹定位算法类似，本节主要介绍基于测距的蓝牙定位算法蓝牙（Bluetooth）是用于固定设备、移动设备和个人局域网之间短距离数据交换的无线技术标准。使用2.4-2.485GHz的ISM波段的UHF无线电波。传输距离近，功耗低1、传统蓝牙2、高速蓝牙3、低功耗蓝牙基于低功耗蓝牙（BluetoothLowEnergy,BLE，基于蓝牙4.0）信号强度指示（RSSI）的室内定位技术蓝牙模块作为基站集成了蓝牙设备的移动设备作为终端位置指纹法传播模型法RSSI定位算法数据库信号的传播速度接近于光速，由式(4-14)可以看出，若测量的时间存在微小偏差将会导致巨大的测距误差，从而引起巨大的定位误差蓝牙定位系统主要由定位基站、电子标签、交换机、定位引擎以及后台监控器构成，系统的结构如图4-10所示，在室内将定位基站按照一定的位置关系安装，组成室内定位网络，电子标签安装在移动机器人上并确保正常工作，定位基站与交换机通过网线连接，交换机将定位基站采集到的电子标签的蓝牙数据通无线传输到定位引擎上，定位引擎通过定位算法将采集到的电子标签的测量信息转化为坐标信息，最后将位置信息传输到后台监控器，可以通过监控器实时查看机器人的位置坐标。根据机器人搭载设备的能力，交换机和定位引擎也可能直接部署在机器人平台上。优点缺点蓝牙设备体积小，容易集成在普通的移动终端中定位精度不高，稍高于Wi-Fi定位使用时需要与被定位手机进行密码配对，影响了使用的方便性推广普及基础好，便携式笔记本、手机等移动终端里大多集成有蓝牙模块功耗低，BLE在有一枚内置电池的情况下可使用2-3年短距离定位时容易发现设备且信号传输不受视距的影响相比Wi-Fi，需要重新布设蓝牙基站，蓝牙设备的价格比较昂贵蓝牙系统的稳定性稍差，受噪声干扰大射频识别（RadioFrequencyIdentification，RFID）是一种利用射频信号自动识别目标信号对象并获取相关信息的技术，如图4-11所示，由电子标签、读写器、天线以及控制模块组成。RFID工作机理是通过标签读写器向电子标签发射电磁信号，电子标签受到电磁感应后发出反馈电磁信号，电磁信号再被标签读写器捕获、读取、识别，实现标签读写器和电子标签的双向通信过程。由于RFID技术在定位应用中能够实现厘米级的定位精度，同时具有高保密、可多次重复使用、有一定的穿透能力、可同时识别多个标签、操作快捷方便等特性，所以在很多室内定位技术选择中，RFID定位技术有极大的优势。射频识别（RFID，RadioFrequencyIDentification）是利用射频信号通过空间耦合（交变磁场或电磁场）实现无接触信息传递并通过所传递的信息达到识别的目的。体积小且形状多样耐环境性可重复使用穿透性强数据安全性强有源标签30m无源标签10mNFC（NearFieldCommunication，近场通信）基于RSSI的射频识别室内定位技术待测标签阅读器阅读器阅读器阅读器参考标签参考标签参考标签参考标签参考标签参考标签参考标签优点缺点体积较小，成本低作用距离较近，定位范围较小，一般最长几十米非接触，可非视距穿透能力强，几毫秒内就可以得到较高的定位精度不便于整合到其他系统中需要特定的设备分布和配套设施条形码的发明与应用极大地方便了工业生产与人们的生活。条形码作为一个可以被计算机自动识别的图案，其中存储了生产者预先编码好的数据信息，而计算机只需要通过图像采集设备获取条形码图像，便可完成数据信息的解码与获取。由于一维条码所能存储的信息总量在某些情况下无法满足应用需求，20世纪40年代诞生了二维码。二维码不同于一维条形码，其在二维方向上按一定规律分布黑白相间的图形用以记录信息。与此同时，随着二维码编码技术中引入了纠错码，使得二维码解码成功率大大的增加，污损、缺损、模糊不清的二维码所编码的信息也能够通过解码技术恢复。从表中可以看出，DM码和QR码可全方位识读，在识别速度上，QR码优势明显。识别速度快，有利于机器人的定位快速响应，因此，常用QR码作为机器人定位的二维码。QR码到目前已有多达四十个版本，其规模由21×21个方块到177×177个方块逐代递增，每个版本增加4×4个方块。随着方块规模上升，其信息容量不断增加。为了促进二维码在机器人导航领域的应用，密歇根大学EdwinOlson教授及其实验团队在QR码的基础上，进一步设计了AprilTags二维码[8]，如图4-14所示。它们被设计用于编码更小的数据有效载荷（4到12位之间），从而能够更可靠地从更远的距离进行检测。此外，它们专为高精度定位而设计——可以计算出相机相对于AprilTag的精确三维位姿。二维码的少量磨损等问题可以依靠二维码的容错机制解决。但是除此之外的因噪声干扰、复杂背景、现场光照不均、严重磨损和划痕等问题都需要通过预处理和二维码提取算法来解决，二维码图像识别流程如图4-15所示。预处理主要包括以下几步：图像灰度化，降低图像维度并且减少数据量加快后期运算速度；图像滤波，降低主要噪声对图像造成的不良影响；图像二值化，减少照明不均匀对图像产生的影响同时减少数据量提高后期运算速度。二维码提取算法包括以下两步：二维码定位，减少复杂背景的影响以加快后期运算；二维码畸变校正，对产生畸变的二维码进行校正使其能够被识别。基于Cell-ID的机器人二维码定位原理如图4-16所示。离线阶段，预先在机器人工作空间或者行进路线上规则的铺设不同ID的二维码，并把其对应在场地的坐标记录到数据库中。在线阶段，机器人通过底盘的摄像头采集场地上铺设的二维码图片，识别图像中二维码的ID号，根据ID号查找数据库中对应的位置坐标信息，即可获取机器人当前的位置。该方法成本低，定位精度取决于场地中二维码的铺设精度和密度，常用于室内行进路线比较固定的工业机器人或者仓储机器人等移动平台的导航定位。地磁匹配定位利用室内磁场的强度分布作为先验信号，采用RSSI的方法进行匹配，判断导航对象的室内位置。谢谢第五章基于学习的机器人

智能导航算法5.2基于传统机器学习的导航方法5.1基于学习的智能导航基础5.3基于深度学习的导航方法

5.4基于强化学习的导航方法5.1基于学习的智能导航基础灵瞳L3P机器