导航和路径规划

路径规划

导航技术前言： 

导航技术的移动机器人技术的核心和关键技术。自主移动机器人的导航就是让机器人可以自主按照内部预定的信息，或者依据传感器获取外部环境进行相应的引导，从而规划出一条适合机器人在环境中行走的路径。定位，就是机器人通过已经观测到的环境信息，结合自身已知的状态进行准确的极端出自身的位姿信息。

室内移动机器人导航框架图： 

、 

导航过程首先要获得相关的地图信息，然后进行路径规划，最后发送数据给机器人，并且通过相应的决策层，使其可以实现自主导航的功能。

costmap 

costmap代价地图，用来存储一些通过传感器获取的外部传感器信息，通过这些信息处理生成二维的栅格地图，然后在对栅格的占有方式来知晓机器人的导航情况。 

每一个costmap的基本单元都有255个不同的值，但是在底层只需要3个状态，占有，空闲，和未知。每个状态都有特殊的值分配到costmap当中。 

我们使用update_frequency的参数设置的更新频率，对地图进行定期的更新， 

机器人碰撞区的示意图： 

这里的lethal表示障碍物的致碰撞。他是机器人的本体的中心，inscribed是以机器人底盘的半径为标准，如果障碍物出现在这个范围内，那机器人肯定是发生了碰撞。circumscribed是以机器人防护栏的外接圆为装，当障碍物在这个范围内，表示机器人接近，或者已经碰到障碍物。我们可以通过inflation_radius来设置机器人膨胀区的半径。 

通过对地图信息的处理，提高自主移动机器人的导航性能，利用costmap对栅格进行分类。以及通过使用膨胀信息逐渐扩展栅格。

机器人的路径规划(move_base) 

移动机器人的路劲规划层可以分为：全局路径规划层，局部路径规划层，行为执行层 

全局路径规划层：global planner层，依据给点的目标，接受权值地图信息生成全局权值地图，规划出从起点到目标位置的全局路径，作为局部路径规划的参考。 

局部路径规划：local planner层，作为导航系统的局部规划部分，接收权值地图生成的局部权值的地图信息。依据附近的障碍物进行局部路劲规划。 

行为执行层：结合上层发送的指令以及路径规划，给出移动机器人当前的行为。 

全局的路径规划 

使用D*或者A*算法，基于地图导航中，首先通过全局const map进行全局的路径规划，以此来计算出移动机器人从出发点到目标位置点的全局规划路线。dijkstra算法是由E.W.Dijkstra于1959年提出，他是贪心算法模式，是非常典型的最短路径算法。用于求得移动机器人行进路线中的一个节点到其他所有节点的最短路径。主要特点：起始点为中心，无向图向外扩展，扩展到最终的目标点为止，通过节点和权值边的关系构成整个路径网络图。 

Dijkstra算法的问题描述：在无向图G=（V，E）当中，假设每条边E[i]的长度为w[i]，找到从顶点V[0]到其余各节点的最短路径 

A*算法 

A*算法是一种非常有效的路径寻优算法，它比Dijkstra算法更快。最早出现在1968年，算法的整体框架是一个图的遍历搜索算法。与其他大多数的图搜索算法不同的是，采用启发函数来估计地图上任意点离目标点的远近程度。通过这种启发式，可以协调选择最好的方向进行搜索。 

A*算法是一种典型的启发式搜索算法。启发中的估价是用估价函数表示的： 

f(n)=g(n)+h(n) 

f(n) 节点n的估价函数 

g(n) 在状态空间从初始节点到n节点的实际代价 

h(n) 从n节点到目标节点最佳路径的估计代价,h(n)是最重要的 

估价函数的正确选择将直接关系到A*算法的成功与否。 

A算法具体实现依靠两个列表：开启列表和关闭列表 

开启列表当中保存所有已经生成但是没有访问的节点，这些节点是从其父节点出发的允许访问节点。

局部路径规划 

路径规划：是在一个自由构型空间C中的起始构型与终点构型之间搜索路径的问题。 

在ROS当中，全局路径规划和局部路径规划是通过local_planner中的cost function这个参数进行规划的，他们当中有一个比重。移动机器人需要在全局路径规划能力的同时具有壁障的功能。

动态窗口法DWA 

导航中的局部路径规划层，负责对局部进行壁障的规划，导航当中实现的的算法是Dynamic windows APProach 由于采样速度空间的不同，可以分为DWA和Trajectory Rollout两种方法实现。这两种算法的基本思想都是在机器人的速度空间内进行运动规划，使用评价函数选出最优的一组速度值作为机器人运动控制指令。这两种算法只是适用差速或者万向轮移动机器人。 

动态窗口法是由D.Fox提出的一种直接在控制指令空间内搜索使得目标函数取得最大值的最佳控制指令的自主壁障算法。这种方法可以分成两步：第一，是将搜索空间约束为机器人可控制的指令空间。第二 是使用目标函数对搜索空间内的命令进行评价，选择使目标函数最大化的指令作为最优指令。

机器人行为的执行层 

在行为恢复执行层解释了为什么，当地图不匹配的情况下，turtlebot会不停的旋转。

AMCL定位 

AMCL（adaptive monte carlo localization）是自适应蒙特卡洛定位算法（KLD采样），他使用粒子滤波器对机器人在已知的地图环境中进行位置跟踪。AMCL是自主移动机器人在二维环境中下的基于概率的定位系统。 

AMCL和FastSLAM类似，都是通过粒子的方式表示机器人的位姿信息。AMCL以有限的粒子数表示移动机器人的概率分布。所以如果有足够多的粒子数，可以得到与实际的机器人几乎完全相近的概率分布函数。但是这样做的弊端是：计算量大，导致实时性降低。AMCL算法需要进行采样，权值分配，重采样，与蒙特卡洛算法相比，解决了全局定位失效以及机器人绑架的问题，提高了系统整体的鲁棒性。 

机器人绑架问题：当有外力作用于机器人时，机器人可能会脱离原来的鬼精，最后导致定位失败。 

全局定位失效：当机器人移动到全局地图未知的地方，将会导致定位失败。 

粒子的数量是粒子滤波器发挥较好作用的关键环节。当粒子数量较多的时候，能够降低粒子飘移带来的不良影像。但是这样会增加计算负荷，进而影响滤波的效果。对于例子数量增多带来的计算量大的问题，使用KLD采样方法。 

KLD采样产生是通过观测两个概率分布不同的KL距离。KL距离是用于表示概率分布p和概率分布q之间的逼近误差。即：逼近误差定义: 

在AMCL当中采用KLD采样算法可以动态调整粒子的数量

参数解析：

image: /home/davidhan/catkin_ws/src/p3dx_description/SIG212_02.pgm
resolution: 0.050000 分辨率
origin: [-51.224998, -51.224998, 0.000000] 原点位置
negate: 0  
occupied_thresh: 0.65 栅格占有率
free_thresh: 0.196 空闲网格

• 1
• 2
• 3
• 4
• 5
• 6

• 1
• 2
• 3
• 4
• 5
• 6

静态的障碍物基本上都是完全的占有栅格，但是在办公室的门口就不是

EKF-SLAM PF-SLAM RBPF-SLAM 简介 滤波slam 

在SLAM的解决方案中，基于贝叶斯滤波 的方法一直占主导地位，考虑到移动机器人运动和观测不确定的因素，将这类方案都是描述成移动机器人自身的状态与外部环境联合的后验概率的估计问题，并且运用统计估计理论进行求解。两种典型的解决方案：EKF-SLAM和PF-SLAM分别是扩展卡尔曼滤波器和粒子滤波器。 

EKF-SLAM首先将移动机器人自身的姿态向量和外部环境的特征向量组合成一个高斯状态的向量，建立系统模型，然后通过预测-更新来解决SLAM问题。PF-SLAM是通过蒙塔卡洛定位算法，这种方案使用离散加权的粒子集合表示被估计状态的后验概率，然后通过不断的预测、权值更新以及重采样来解决SLAM问题。但是为了更加精确地逼近后验概率。PF需要大量的粒子，导致算法的复杂度比较高。在这个基础上RBPF-SLAM是1999年Thrun和Montemerlo等人将Rao-Blackwallised粒子滤波器运用到SLAM当中，RBPF-SLAM的估计准确性仅次于EKF-SLAM，但是在计算效率和应用范围上比EKF-SLAM更有优势。 

Graph-based SLAM 图优化理论 以及滤波算法和图优化比较 

基于滤波技术的SLAM解决方案在实时处理观测信息的过程中会丢失部分信息，是的在整个构图的过程中不能回顾所有的已观测信息。为此，部分研究者提出离线技术的解决方案：在移动机器人探索过程中，仅仅保存所有的观测信息，当机器人等下来后再进行地图构建。离线地图避免的信息的丢失，很大程度上降低了移动机器人的计算负担。离线地图通常是基于图论的思想，将SLAM问题转换成稀疏的有向图，通过优化方案求解SLAM的后验概率。将这类SLAM问题的解决方案统称为“Graph-based SLAM” 

Graph-based SLAM解决方案在1997年首次由于Lu和MiliOS提出，他们采用ICP算法对相邻的扫描数据进行配准，并且利用迭代线性化对图结构进行优化。但是因为缺乏高效的最优化算法，Graph-based SLAM解决方案一直停歇不前。近些年来，随着线性解析器的出现和发展，Graph-based SLAM解决方案逐渐成为主流。Howard采用松弛法对移动机器人进行定位和地图构建。Duckett等人采用Gauss-Seide松弛法，对图结构中的约束误差进行最小化优化，提出了一种非线性的SLAM方案。 

大多数研究者将图结构的最优化问题看成是非线性的最小二乘问题，常用的解决途径是：首先利用当前的状态构建线性化系统。最后采用迭代算法求解。

地图的描述 

从维数上面划分：三维地图和二维地图 

三维地图的优点：包含丰富的环境信息，能够真实的展示外部环境 

缺点：对地图构建的算法要求高，更容易受到环境的影响，利用三维地图进行自主导航和路径规划计算量大。 

二维地图的优点：二维地图对外部环境进行简化，忽略其高度信息，虽然不能准确的表征外部特征，但是二维地图的信息量比较小，构建和维护比较简单，很强的扩展性。 

缺点：不能准确的表示外部特征。 

二维地图分成以下四种：

• 栅格地图（grid-based map）
• 几何特征地图（geometric feature map）
• 拓扑地图（topological map）
• 混合地图（mixed map）

栅格地图是由Elfes和Moraves首先提出，栅格地图首先将外部环境划分成许多小的单元，称为栅格，并且用概率值表示每个栅格被占据的可能性。栅格地图的优点：栅格地图是对静态环境的近似表征，易于创建和维护，并且对传感器的精度要求很低。缺点：精度要求不高，随着外部环境的增大，存储的信息量显著增大。 

目前hector slam采用的就是栅格地图。

几何特征地图：是将外部环境用一系列的路标特征进行表示。讲外部环境的几何特征用点线面进行近似。几何特征地图优点：存储的信息量小，易于计算。缺点：对噪声比较敏感。目前只是适用于结构化的环境当中。

拓扑地图首先是由Kuipers和Byun提出，由cassandra和Ulrich等人将拓扑地图应用到移动机器人的自身定位当中。拓扑地图是用一个带节点和无向边的拓扑结构图表征外部环境。其中，我们用节点来对应外部环境的一个特定的位置，例如拐角等。边用来表示节点之间的连接，例如走廊。拓扑地图的优点：紧凑，忽略了外部环境的几何特征，便于表示，存储空间下。在不需要移动机器人准确的位姿信息下，具有高效的路径规划能力。缺点：缺乏精确的尺度信息，难于创建和维护，当外部环境有很多相似的位置的时候，很难判断为拓扑地图的同一节点。

混合地图是在拓扑地图的基础上引入几何特征，丰富了地图信息。混合地图的优点，不仅具有拓扑地图的高效性，而且具有几何特征的精确性和一致性。

2DSLAM 

2DSLAM问题通常被认为是后验概率估计问题。其解决方案大多数是基于贝叶斯滤波器的概率模型方法。根据随机估计可知，整个SLAM问题可以描述成求解如下分布： 

其含义是在已知观测值集合，控制输出信息集合以及移动机器人的初始位姿的情况下，求出k时刻移动机器人的位姿和环境地图的m的联合后验条件概率. 

SLAM问题的解决方案一般是基于递归的贝叶斯估计方法。已知k-1时刻的概率分布，利用观测模型和状态转移模型（运动模型）求解出K时刻的概率分布并且进行更新。观测模型描述了传感器对系统状态的作用，运动模型描述控制输入信息对系统状态的作用。 

观测模型描述了当移动机器人姿态信息和环境路标位置已知的情况时，观测的概率。观测可以利用当前的姿态和环境地图m的概率分布函数进行表示。观测模型的一般形式为： 

运动模型描述了移动机器人在控制输入信息的作用下不同状态之间的转移概率。移动机器人k时刻的位姿可以利用上一时刻的位姿和当前的控制向量的条件概率函数进行表示。运动模型的转移概率为： 

递归的贝叶斯估计方法主要分成预测和更新两步，假设我们已知k-1时刻的概率分布，我们需要通过观测和控制向量来计算k时刻的概率分布 

第一步，利用运动模型对联合后验概率进行预测 

第二步，利用观测模型对联合后验概率进行更新： 

EKF-SLAM系统 

EKF-SLAM算法主要有两个过程：预测和更新。首先根据上一时刻的系统状态和控制向量，对当前时刻的系统状态与状态误差协方差进行预测，在利用系统环境特征路标以及传感器的观测值校正更新状态，最终获取系统状态和状态误差协方差的估计值。 

算法模型具体流程如下： 

表示t时刻系统状态向量的先验估计值和后验校正值；分别表示t时刻系统的先验估计误差的协方差和后验校正误差的协方差。表示t-1时刻系统的控制向量；为t-1时刻的状态噪声，在EKF-SLAM算法当中通常假设高斯白噪声并且设置为0向量； 

分别表示状态输入和观测的噪声的协方差

Markov过程就是指，机器人k时刻的状态只于k-1时刻的状态和k时刻的控制输入信息有关。

基于RBPF的同时定位与地图重建原理 

SLAM问题在概率论的观点下可以理解为：在系统初始状态（地图m和位姿x）给定的情况下，从开始到时刻t的传感器观测信息与移动机器人的里程计的运动信息，来估计机器人的运动轨迹和地图的mt的后验概率，根据贝叶斯滤波递归原理，可得到求解SLAM的递归公式： 

可知t时刻后验概率可以表示为t-1时刻的后验概率、时刻观测模型和运动模型的迭代形式。 

SLAM当中有两个系统模型，运动模型P(xt|xt-1,ut-1)与观测模型P(zt|xt,m).运动模型表示上一时刻移动机器人轨迹xt-1和控制命令ut-1 已知的情况下，机器人下一时刻的位姿xt的概率密度。观测模型表示，移动机器人地图m和这一时刻的位姿xt已知的情况下，传感器获取环境的不确定性。RBPF核心在于计算机器人路径和地图m的后验概率RBPF-SLAM通过贝叶斯滤波器状态空间进行分解，分解如下： 

这一分解吧SLAM问题分解成两个独立的后验的概率的乘积。这样的话，我们就可以选对机器人的轨迹进行估计，然后再结合观察模型对地图进行更新。 

一个最常见的粒子滤波器是重要性重采样（SIR）滤波器。主要分成以下4步： 

1、采样 

依据提议分布q, 下一代粒子集合由上一代粒子集合产生，通常由机器人的运动模型作为采样的提议分布。 

2、粒子权重 

为了弥补采样的时，提议分布跟目标分布的差距，需要计算每一个独立粒子的权重 

3、重采样 

因为粒子是按照其权重比例来选取饿，经过重采样，所有的粒子都具有相同的权重。 

4、地图更新 

对每个粒子来说，可以用轨迹和观测信息来计算对应的，从而对地图进行更新。 

当发现一条新的路径的时候，需要对机器人的轨迹权值进行重新估算。

视觉定位技术 

亏欠了大家很久的SIFT算法 

Harris角点检测算子，这种方法具有计算简单，提取的角点特征均匀和算子稳定的特点，但是在实际的特征匹配国政中，如果从两幅图片比例不固定的图像当中提取角点，那么两幅图像的角点匹配不出来。因此这种角点检测方法不具有抗尺度缩放能力。 

David G.Lowe 在1999年提出一种基于尺度空间的，对图像的缩放，旋转甚至仿射变换保持不变性的图像局部特征描述算子的SIFT算子。 

SIFT算法的用途：

• 检测影像当中的局部特征
• 在尺度空间当中寻找极值点
• 提取出极值点的位置和尺度

欧式距离 

kd-tree 

kd-tree是二分查找的多维扩展

amcl的讲解链接： 

http://blog.csdn.net/chenxingwangzi/article/details/50038413

move_base的包使用插件 

http://blog.csdn.net/heyijia0327/article/details/45030929

路径规划算法有很多种，在导航中经常提到的就是A*算法和Dijkstra算法。 

A*算法是导航算法中比较经典的算法，他比Dijkstra多个一个估算函数，如果估算函数为0那么A*算法也就是退化成了Dijkstra算法。 

但是在一般的嵌入式硬件上面，由于性能和内存的限制要求，不能直接使用A*算法。所有有很多改进的算法。例如：利用地图数据分层的思想，简化地图中和道路中的网络结构。也能提高路径规划的性能。 

Dijstra算法虽然运行的比较慢，但是他能能哦故找到一条最短的路径。 

http://blog.csdn.net/changbaohua/article/details/3860307 

这个博客还是要看的，但是我感觉现在没有那么多的是时间去细细的看。

航迹推演导航： 

采用里程计、陀螺仪等来测量机器人的速度，绝对方向和目标位置。但是随着导航里程计的增加，累计误差的不断增加，需要采取额外的措施进行改进。

Kuipers等人引入的启发式A*算法，通过评估函数搜索出一条代价最优的路径。在局部路径规划方面，经典的算法有人工势场法，神经网络法，模糊推理法，向量场矩阵法，以及动态窗口法。基于SICK的二维激光测距的激光里程计，通过将连续的两帧激光数据进行匹配，然后估算出机器人相对位姿发生的变化，。通过连续对位姿变换的叠加实现对机器人位姿的跟踪。根据激光扫描匹配算法给出的机器人里程计信息的建议分布进行改进，大大减少了粒子的数量。提高了计算的效率。 

从上面一个步骤得到的栅格地图。结合利用机器人自身的参数。利用机器人的两轮的宽度，自己的长宽高，以及激光雷达相对自己的位姿。构建出代价地图。

关于A*算法 

A*算法通过起点和目标点之间进行搜索，然后根据代价函数（最短的距离和时间）的标准选出一条适合机器人运动的路径。这种算法基于加权地图形式进行表述。从地图当中起点开始，然后不断向重点进行扩散，直到扩散到终点的位置。在主循环内的每次迭代的过程当中，A*算法需要决定那条局部路径需要扩展成最终的路径。这是通过估计通过到达目标点的代价来实现的。 

这个地方说白了，就是一个通用的表达式。 

A*算法的流程框图。 

全上面的这张图可以看出，基于A*算法虽然能够给出全部无碰撞的有效路径，但是并不是符合人们对于AGV行驶路径的期望，人们期望规划的路径能够尽量走在正中间。 

目前几种路径规划的算法比较 

我觉有空可以找找人工势场法的插件

我现在才发现，当你想要改进某种算法的时候，你直接去知网上找某算的改进算法，然后就会有一大堆思路，然后你想想，你想要改进那种算法就可以了。

通过这次写论文给我的一个感触，第一就自己平时写的太少了，很多时候，自己图懒，很多代码和仿过程都自己进行认真的思考，当时自己就是想着能够快速学完，然后很多问题都不求甚解。 

第二就是看论文的时候，尽量看英文吧，至少这样查重好弄一些，基本上都不会重复。另外英文的质量相对还是高一些。 

然后我打算，每周都写一篇这样的文章，一定要把自己看的东西，写出来。其实这样慢慢就习惯了。也包括看到别人好的代码，想着能够按照自己的方式实现一遍，才能够将叫做真正的理解。然后就是每周整理一下自己的论文的将，自己看看过的论文，做好标注，那些是有价值的论文，尽量还是看一些比较好的论文。

相关阅读

淘宝店铺导航二级页面怎么优化?二级页面装修技巧!

在淘宝店铺上面存在一些导航二级页面，可是大家是否清楚的知道淘宝店铺的导航二级页面到底要如何去进行优化呢?此外，大家是否有去了

淘宝店铺导航页面装修怎么做？

淘宝店铺导航页面装修怎么做?很多朋友不知道淘宝店铺导航页面装修怎么做，要知道，淘宝店铺导航页面装修是一个可大可小的问题，但是如

淘宝店铺首页图片化妆品导航怎么设置和添加?

用心经营着的卖家都希望打造一个高大上的淘宝店铺，从基础标配上看，店铺装修很重要。那么，淘宝店铺首页图片化妆品导航如何做呢?今天

APP设计模式：浅析导航栏设计

文章总结了APP种常见的几类导航栏及特点，希望给大家带来些启发。1 一级导航/主导航1.1 标签式导航标签式导航又叫Tab式导航，是目前

AngularJS(九)面包屑导航

AngularJS(九)面包屑导航 1 itemCatService.js // 根据上级 ID 查询下级列表 this.findByParentId = function(parentI