【《视觉与惯导融合的机器人室内导航定位算法分析案例》7400字】

PAGE\*Arabic9视觉与惯导融合的机器人室内导航定位算法分析案例目录TOC\o"1-3"\h\u81911.1引言 1246991.2全局路径规划算法A* 257621.3局部路径规划算法DWA 5202031.4视觉与惯性测量单元IMU数据融合 749651.4.1视觉与惯导联合初始化 9177301.4.2双目视觉初始化 1062691.4.3视觉与惯导系统联合初始化 10287421.4.4视觉和惯导信息融合的紧耦合方法 1242701.4.5视觉和惯导信息融合紧耦合模型 131.1引言机器人实现智能化和自主化，导航是必须具备的一项基础功能，导航就是机器人基于地图，实现从起始点到目标点行进的过程，这个过程中要求不发生碰撞并满足自身动力学模型，如不超过速度、加速度等的限制，导航过程示意图如下图所示.图4-1机器人导航示意图将导航的任务进行细分，首先机器人在未知环境中需要使用激光传感器或者是视觉传感器进行地图构建，然后根据构建的地图进行定位，有了地图和定位的基础，就可以根据目标点以及感知的障碍物信息进行路径规划了，因此可以将导航的要素归纳如下。图4-2机器人导航要素本章首先介绍目前主流的全局路径规划算法，即A*算法，此算法目前已成熟应用到各个场景下的SLAM机器人上。在局部路径规划方面，本章介绍了DWA窗口算法，此算法可以让机器人实时监测当前所处路径上的障碍物信息并实现避障功能。但是目前的机器人导航精度仍无法满足复杂场景下的工作需求，传统的单一传感器导航算法仍然存在获取环境信息能力不足的问题，从而导致机器人在导航过程中经过诸如长走廊，无明显纹理特征的特殊环境时，发生较大的定位偏差，降低导航精度，基于此，本章提出了一种惯导与视觉融合的导航定位方法来加强机器人在导航时对复杂环境的适应能力。1.2全局路径规划算法A*机器人具备全局路径规划能力是机器人实现智能化和自主化的重要标志之一，目前在SLAM领域主流的全局路径规划算法是A*算法，A*是一种启发式算法。启发式搜索对状态空间中的每个搜索位置进行评估，得到最佳位置，然后从该位置搜索到目标点。这种方法可以省略很多不必要的搜索路径，提高了全局路径规划的效率。假设机器人要从某个空间的A点移动到B点，但是这两点之间被障碍物挡住了，如下图所示。图4-3障碍物与与目标点示意图在确定机器人全局导航的起点和目标点后，首先将搜索区域划分成方形的格子，这一步简化了搜索区域将搜索区域转换成了二维数组。其中数组的每一项就代表搜索区域中的某一个格子。它有两种状态，一种是机器人可通行的格子，另一种是机器人不可通行的格子。通过计算，找到机器人从A点到达B点所要经过的格子就找到了路径，路径一旦被找到，机器人就可以遍历这些格子到达目标点。在将搜索路径简化为一组可以量化的节点后就可以开始寻找最短路径了。在A*算法中首先从起点节点开始，搜索其相邻的方格，然后向四周扩展，直到找到目标。其主要步骤如下：（1）先建立一个由方格组成的openlist（开放列表），从起点A开始，将其加入到开放列表中，此时开放列表中只有一项，即起点A，后面会加入更多的节点。在开放列表中的格子可能是路径中的，也可能不是路径中的，其本质是一个待检查的方格列表。（2）接下来查看与点A相邻的其他方格，此时忽略障碍物所占的方格区域，将其它可走的或者可到达的方格都加入到开放列表中，把起点A设置为这些格子的父格子点。（3）将A格子从开放列表中移除并加入到封闭列表（closelist）中。如下图4-4所示，绿色的格子为机器人起点，即A点，外框被标注成了亮蓝色，表示该格子已经被加入到了封闭列表中，与其相邻的黑色格子都是需要被检查的，每个黑色的方格会有一个箭头指向其自身的父节点。图4-4开始路径搜索接下来从开放列表中选一个与起点A相邻的格子，通过计算其代价函数决定将哪个格子放入到封闭列表中作为最后路径中的格子。计算出组成路径的方格的代价函数如下：f(n)=g(n)+h(n)（4-1）其中，f(n)是从初始状态经由状态n到目标状态的代价估计，g(n)是在状态空间中从初始状态到状态n的实际代价，h(n)是从状态n到目标状态的最佳路径的代价估计。h(n)的选取保证找到最短路径(最优解的)条件，关键在于估价函数f(n)的选取。以d(n)表达状态n到目标状态的距离，则h(n)的选取大致有如下三种情况:1.如果h(n)<d(n)到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。2.如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行，此时的搜索效率是最高的。3.如果h(n)>d(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。如上所述，将从起点A移动到目标格子产生的移动代价用g来表示，如下图。图4-5A*全局路径搜索示意图1为了避免开方和小数计算，将纵向和横向移动的代价看做10，而对角线的移动代价看做14，都取近似整数。因为g值是根据沿着到达指定方格的路径计算出来的，那么计算出该方格的g值的方法就是找出其父格子的g值，然后按照直线方向或者斜线方向加上10或者11.这里的h值通过曼哈顿方法计算，首先计算从当前方格横向移动或者纵向移动到达目标方格所经过的格子数，然后总数乘以10，在计算过程中需要忽略路径中经过的障碍物，因为这是对剩余距离的估算值，而不是实际值。F值是通过g和h相加而得到，第一步的结果如上图4-5所示，每一个方格都包含了三个值，f、g以及h，如起点右边的方格一样，左上角是f，左下角是g，右下角是h。在标有字母的方格中由于其水平方向从起点到这里只通过一个方格，因此其g值为10。与起点直接相邻的的下方、上方以及左方的格子的g值都是10，对角线上的方格的g值为14。h值通过计算起点到终点的曼哈顿距离得出，其仅仅做横向或者纵向的移动。使用这种方法可以看到，起点右边的格子距离终点格子有三个格子，所以h为30。这个格子上方的方格到终点的格子有四个格子的距离，因此此时的h为40，至于其他格子也可以用同样的方法计算出其距离终点格子的h值和g值。第一步搜索步骤完成后，从开放列表中挑出f值最小的格子节点，对此节点做如下操作：（1）将其从开放列表中取出，放入封闭列表中。（2）和第一步搜索步骤一致，在此搜索与其相邻的其他格子节点，摒弃在封闭列表中不可通行的格子，即障碍物所在的格子，如果搜索到的格子不在开放列表中，则将其放入开放列表中。在搜索过程中，若某个方格已经在开放列表中，则首先检查此条路径是否为更优，若不是，则不做任何操作，若是，则将此格子的父格子设置为当前格子，最后重先计算那个方格的f值和g值。其搜索过程如下图所示。图4-6A*全局路径搜索示意图2如上图所示，在最初的九个方格中，目前还有八个在开放列表中，有一个已经放入了封闭列表中，在如图所示的方格中，起点右边的格子的f值最小，因此选择此方格作为下一个要处理的方格，其外框如图4-6所示用蓝线标出。首先，将其从开放列表中移动到封闭列表中，然后检查与其相邻的格子，因为右边的格子是墙壁，所以忽略，左边的格子是起点所以也忽略，其它四个相邻的方格都在开放列表中，此时通过计算各个格子的g值来判定经由这个格子到达目标点是否是更优的路径，接下来不断重复这个过程，直到将终点也加入到开放列表中，此时整个过程的路径搜索示意图如下图所示。图4-7A*全局路径搜索示意图3如上图所示，在起点下方距离两格的格子的父结点已经与前面的不同了，这是因为在寻路过程中在某处使用新路径时g值经过计算变得更小，因此父节点被重先设定，g值和f值被重先计算。经过对可能在路径中的栅格进行筛选后，就可以确定实际的作为全局路径规划的路线了。从终点开始，按着箭头向父节点移动，这样就可以寻迹到起点，那么这个就是实际的路径了，如下图4-8所示，从起点A移动到终点B就是从路径上的一个方格的中心移动到另一个方格的中心，直至目标。图4-8A*全局路径搜索示意图41.3局部路径规划算法DWA机器人操作系统（ROS）中使用的DWA算法主要是通过获取机器人现在的速度和设置的最大加速度，获取机器人现在可以达到的速度集合。计算这个速度集合中所有的速度的代价，选择代价最小的速度发送给机器人。在动态窗口算法中，首先需要定义合适的机器人运动模型才能够模拟出机器人的运动轨迹，基于此本节主要介绍最新的机器人操作系统中给出的DWA算法结构并对其进行理论分析。动态窗口算法的核心在于采样和动态规划，对速度和运动参数进行采样称之为采样速度，这是在时间维度上的采样，此外还需要对机器人的位置进行采样，称之为轨迹采样。假设机器人是两轮差速驱动的方式，其做圆弧运动的半径的表达式为：（4-1）当旋转的速度w不为0时，则机器人坐标x，y以及角度分别：（4-2）（4-3）（4-4）根据上述方程，可以得到机器人的运动模型，然后根据速度计算出机器人的运动轨迹。在此基础上，通过多次采样来估计速度，然后利用代价函数对这些轨迹进行评价，得到最优轨迹。另外，由于受电机性能的影响，机器人具有最大的加减速极限，因此在机器人轨迹模拟周期内，存在一个动态窗口，其中的速度是机器人能够达到的实际速度，速度的约束范围如下式：（4-5）在机器人实际工作过程中，基于安全考虑，为了使机器人能够在碰撞到障碍物之前停下来，需要为其设定一定的速度范围，如下式：（4-6）当对速度采样完成后，对于动态规划来说，需要考虑局部目标、代价地图、全局路径等，根据这些核心点考虑如何表达代价函数，代价函数的选择可以是多样的，用户有很大的操作空间，这里展示七个主要的代价函数如下图所示。表4-9DWA算法中的主要代价函数DWA局部路径规划算法程序模块的流程大致分为四个部分。首先，初始化DWA算法，加载参数和实例化对象。其次，对速度进行采样，并计算出需要评估的速度样本。然后对样本进行评价，对计算出的速度样本逐一打分，找出最佳样本，最后发布局部路径规划策略，使机器人能够实现局部的路径规划过程，其具体的算法流程框架如下图所示。图4-10DWA算法框架1.4视觉与惯性测量单元IMU数据融合视觉SLAM中，相机能够获取到较丰富的环境信息从而提供稳定的观测数据，但是其缺点也较为明显，随着机器人应用领域的不多拓展，对于环境纹理特征不明显的场景或者图像采集速率过快的情况下，相机的运动估计效果就会差很多，此时需要借助其它的导航系统进行修正才能达到一个比较理想的运动估计结果。惯性测量单元（IMU）通过自身的自主导航能力，在不受外界环境的干扰下，可为载体提供线加速度、角加速度等信息。惯性测量单元与视觉的结合，能够将相机和IMU两者的优点综合利用起来，能够保障机器人在室内环境下具有较高精度的定位效果。相机与IMU进行融合时的坐标转换关系如下图所示。图4-11视觉与惯性测量单元融合坐标转换如上图所示，相机的左摄像头是，右摄像头是，摄像头光心设定为坐标原点。IMU的位置在两个摄像头的中间并将其坐标用表示。相机与IMU进行融合的最终目的是估计出IMU所在的坐标系相对于世界坐标系的运动变换信息，即估计，为导航坐标系，其导航坐标中心在IMU所在坐标系初始时的原点处，重力加速度与G轴和Z轴的方向一致。X轴方向代表系统前方，通过右手准则确定Y轴的方向。在相机与IMU进行组合的流程中，首先利用平滑滤波技术对IMU获取到的加速度和角速度进行处理，经处理会将测量数据中的无效数据过滤掉。在获得相机采集的图像后，通过构造图像金字塔，提取前后帧的特征点并进行匹配。最后，利用剔除误匹配点的特征点对，计算出相机在某一时刻的运动变化信息，其具体流程图如下图所示。图4-12相机与IMU数据融合流程在获取到质量较好的特征匹配点对后，使用3D-2D常用的PNP方法解算相机的运动信息矩阵。为了解决在相机运动变化微小时估计出的相机运动信息精度下降的问题，采用关键帧技术对平移和旋转未超过某一设定阈值的图像帧进行剔除，比如某帧上提取到的特征点数量若小于30时，便将其从关键帧序列中剔除，以避免因数量过低而无法获取到丰富环境特征导致帧间误匹配率高的结果。最后，采用列文博格优化方法对位置和姿态进行优化，得到更准确的运动估计信息。相机与IMU融合算法的变量如下式所示。（4-7）上式中，为相机载体的姿态信息，用四元数表示，，分别是导航坐标系下的速度和位置。1.4.1视觉与惯导联合初始化在对相机和惯导的数据进行融合之前，需要先对它们进行联合初始化，整个系统的初始化过程主要根据相机获取到的关键帧位姿来对IMU的零偏、加速度计偏差、IMU噪声进行初始化并估计IMU的速度，从而实现双目相机的初始化和相机与IMU进行数据融合前的联合初始化过程。1.4.2双目视觉初始化双目视觉初始化就是恢复相机运动结构SFM，根据图像序列对相机的位姿进行估计，对环境中特征点的坐标进行估计。双目相机获取到的图像都是有时间上的先后顺序的，SFM则不同，它使用与时间先后顺序没有关系的图像。本文的双目视觉初始化使用滑动窗口的SFM，主要思想是当一对特征点在滑动窗口中的几个关键帧中都可以观察到，则建立这几个关键帧位姿的约束关系，从而获取轨迹信息。其过程主要有以下四个步骤：（1）创建一个包含数据帧的滑动窗口，然后对相机获取的图像进行ORB特征点提取，根据提取的特征点进行相邻帧之间的匹配。（2）通过特征点匹配得到相机两帧之间的运动关系后，用相机的位姿来估计图像中特征点的空间坐标。（3）根据三维空间中特征点的坐标，采用3D-2D的位姿估计方法求解滑动窗口中剩余图像帧的位姿，从而继续求解出更多的特征点的坐标。（4）使用光束平差法对特征点坐标和相机的位姿进行优化，使特征观测值的重投影误差最小化，得到最优位姿。1.4.3视觉与惯导系统联合初始化系统的初始化对于系统性能来说至关重要，是系统良好运行的关键。由于IMU的初始化可能存在动态初始化使得IMU的初始化难度很大，因此为了使融合后的系统具有更大的普适性，本文提出一种快速双目视觉与IMU进行联合初始化的方法。本文所采取的联合初始化方法是在硬件同步的前提下，首先使用相机的纯视觉位姿获取到系统的初始值，然后根据相机与IMU的联合标定时间差和标定结果，结合IMU的预处理结果进行融合系统的联合初始化。相机与惯导的联合初始化过程主要包含以下几方面：（1）陀螺仪零偏估计陀螺仪由于其输出信号会在均值附近产生波动，波动越小，IMU的测量精度越高，鲁棒性越高，因此零偏估计是体现陀螺仪品质好坏的重要指标。通常是根据相邻的两个关键帧来对陀螺仪的偏差进行估计，通过下式来计算陀螺仪的偏差：（4-8）（2）加速度计偏差估计根据上式可求解出陀螺仪的零偏，通过对更新的结果进行再一次预积分计算消除偏差对预积分产生的负面影响。根据上一节的位置计算方法，忽略加速度计的偏差，可得：（4-9）采用上式对两组连续的关键帧进行描述，用上一节中的速度计算方法得到：（4-10）为便于后面推导，将上式简写为下式形式（4-11）然后使用扰动模型对重力进行求解：（4-12）其中，R是到的旋转矩阵，是的反对称矩阵，表示扰动，G是重力大小，将上式代入式4-9中，可得与求解重力类似的公式：（4-13）上式中参数的表达形式为：（4-14）最后使用奇异值分解的方法对加速度计的偏差进行计算并对重力估计进行优化。（1）速度估计根据式4-14的对关键帧速度的解算可得最后关键帧速度估计为：（4-15）对于在此之前的关键帧的速度估计为：（4-16）（2）IMU数据的噪声假设两个相邻的关键帧之间的IMU偏差不发生改变，则满足下式：（4-17）通过下式从相邻关键帧中分离出噪声和数据：（4-18）其中，（4-19）正态分布的噪声估计用表示，其值为0,。经过预积分处理的噪声使用IMU噪声的线性函数来描述。依据噪声协方差计算出的协方差。（3）全局优化使用光束平差法进行优化，根据上文计算出的陀螺仪的零偏估计、速度估计、噪声估计、加速度计偏差，对全局地图中的地图点和关键点进行优化，其误差项为：（4-20）对上式误差项的重投影误差进行最小化优化得：（4-21）1.4.4视觉和惯导信息融合的紧耦合方法在机器人导航中，定位对导航精度起到决定性的作用，只有机器人知道自己在所处环境中的位置，才能够有效发送速度命令控制机器人到达目标点，所以从某种程度来讲，导航即定位，机器人具有高精度的定位能力最能体现导航的性能。为进一步提高机器人定位精度，本文提出一种将惯性测量单元数据和双目相机数据进行融合然后实现定位的方法，主要思路是首先使用双目视觉里程计技术预测机器人当前的初始位姿，在位姿优化阶段加入IMU预积分约束，用IMU的测量值去修正初始位姿参数，从而消除累计误差，使机器人的定位导航精度得到进一步提升。1.4.5视觉和惯导信息融合紧耦合模型本文提出的融合算法主要是依据视觉的重投影误差和惯导的误差，使用高斯牛顿法进行优化，算法框架如图111所示，针对相机的重投影误差使用鲁棒核函数处理，从而降低了累计误差增长速度，使用高斯牛顿法的目的在于其可以提高在优化过程中待优化量的收敛速度。图4-13IMU与视觉误差项联合优化在本文提出的惯导和双目相机的紧耦合融合中，将IMU的坐标系定义为全局坐标系，当相机获取到第K帧图像时，IMU的状态可用下式来描述。（4-22）如上式所示，在流形上的切空间中，状态和误差之间的差值定位可以作为估计值和真实值之间的差值，接下来根据指数映射和对数映射，IMU在视觉第k帧图像上的误差状态量是：（4