原创博客:转载请标明出处:http://www.cnblogs.com/zxouxuewei/
导航与定位是机器人研究中的重要部分。
一般机器人在陌生的环境下需要使用激光传感器（或者深度传感器转换成激光数据），先进行地图建模，然后在根据建立的地图进行导航、定位。在ROS中也有很多完善的包可以直接使用。
在ROS中，进行导航需要使用到的三个包是：
（1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
（2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
（3） amcl：根据已经有的地图进行定位。
参考链接：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup
http://www.ros.org/wiki/navigation/Tutorials
整体导航包的格局如下图所示：
其中白色框内的是ROS已经为我们准备好的必须使用的组件，灰色框内的是ROS中可选的组件，蓝色的是用户需要提供的机器人平台上的组件。
1、sensor transforms
其中涉及到使用tf进行传感器坐标的转换，因为我们使用的机器人的控制中心不一定是在传感器上，所以要把传感器的数据转换成在控制中心上的坐标信息。如下 图所示，传感器获取的数据是在base_laser的坐标系统中的，但是我们控制的时候是以base_link为中心，所以要根据两者的位置关心进行坐标 的变换。
变换的过程不需要我们自己处理，只需要将base_laser和base_link两者之间的位置关系告诉tf，就可以自动转换了。具体的实现可以参见：http://blog.csdn.net/hcx25909/article/details/9255001。
2、sensor sources
这里是机器人导航传感器数据输入，一般只有两种：
（1） 激光传感器数据
（2） 点云数据
具体实现见：http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Sensors
3、odometry source
机器人的导航需要输入里程计的数据（tf，nav_msgs/Odometry_message），具体实现见：
http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom
4、base controller
在导航过程中，该部分负责将之前得出来的数据转封装成具体的线速度和转向角度信息（Twist），并且发布给硬件平台。
5、map_server
在导航过程中，地图并不是必须的，此时相当于是在一个无限大的空地上进行导航，并没有任何障碍物。但是考虑到实际情况，在我们使用导航的过程中还是要一个地图的。 具体实现见：http://www.ros.org/wiki/slam_gmapping/Tutorials/MappingFromLoggedData 在ROS的导航中，costmap_2d这个包主要负责根据传感器的信息建立和更新二维或三维的地图。ROS的地图（costmap）采用网格 （grid）的形式，每个网格的值从0~255，分为三种状态：占用（有障碍物）、无用（空闲的）、未知。具体状态和值的对应关系如下：
上图共分为五个部分：（下面的红色框图是机器人的轮廓，旁边的黑框是上图的映射位置）
（1） Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。
（2） Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。
（3） Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
（4） Freespace（自由空间）：没有障碍物的空间。
（5） Unknown（未知）：未知的空间。
具体可见：http://www.ros.org/wiki/costmap_2d
slam算法说明：
SLAM 算法最早出现在 SmithSelf 和 Cheeseman 的论文中,他 们 运用扩展卡尔曼滤波对状态空间中的机器人位姿和特征 ( 路标 ) 位置进行同 时 估 计 。 计 算 复 杂 度 高 一 直 是 SLAM 的 难 题 之 一 ，在 基 于 特 征 的SLAM 算法中,标准 EKF SLAM 方法的计算复杂度与环境特征个 数 呈二次方关系,这使得它只能在一般不超过上百个特征的较小范围内应用。 因此,研究一种计算量可随地图大小进行缩放的 SLAM 算法成为一个公共难题。由于 SLAM 问题针对未知且不确定的环境,一般使用概率方法来描述该问题。 SLAM 的常用概率解法有两类, EKF 算法 和 粒 子 滤 波 算法。 对于非线性系统的 SLAM 问题 EKF 算法和快速 SLAM(FastSLAM)算法是比较典型的两种算法 。 粒子滤波方法对模型进行直接近似,它不要 求 控 制 向 量 和 观 测 值 满 足 高 斯 分 布 假 设 将 粒 子 滤 波 方 法 应 用 到高维 SLAM 问题,计算量非常大, FastSLAM 算法 把 联 合SLAM状 态 分成运动部分和条件地图部分以缩小采样空间 ,机器人的位姿用不同权值的粒子 表 示 ,地 图 由 独 立 的 高 斯 分 布 解 析 表 示 ,机 器 人 位 姿 状 态 的递归估计采用粒子滤波方法,地图状态的递归估计采用 EKF 方法。
主要算法：
（1），EKF 的 SLAM 算法
传统的 SLAM 算法需要将不同时间、不同地点的感知信息进行匹配和联合 ,存 在 局 部 数 据 之 间 的 关 联 问 题 ,也 存 在 局 部 数 据 和 全 局 数
据的关联与匹配问题 [3] 。随着 SLAM 问题研究的深入, 人们发 现 EKF 方 法 的 瓶 颈 在 于 其计算复杂性 。 对于其最优滤波,无论对滤波方程如何整理和计算,其计算复杂度都至少与地图中特征个数的平方成正比 ,难以满足构建大规模地图和实时性的要求。 对于大规模地图, EKF SLAM 算法难以避免出现不一致现象。 一致性 (consistency) 是指算法能得到无偏估计且能正确地给 出 估 计 方 差 ,近 几 年 ,国 外 许 多 学 者 在 这 方 面 已 经 进 行 了 研究 , 分析了模型误差、线性化误差等因素对一致性的影响。 这些研究表明, EKF 算法并不适合大规模的 SLAM 。基于扩展卡尔曼滤波的 SLAM 算法是一种经典方法,虽然该方法计算复杂度高且无法处理“闭环”问题,但其收敛性是目前所有算法中最好的,因此至今仍在陆地、水下及空中等场合得到广泛的应用。
（2）.FAST SLAM
与基于 Kalman 滤波的 SLAM 不同, RBPF SLAM 采用粒子 来 表 示机器人的位姿,而环境特征的估计依然利用 EKF 解析计算。 这种将状态分解为采样部分和解析部分的粒子滤波方法一般称为 RBPF 。 由于这 种 分 解 , RBPF SLAM 的 计 算 复 杂 度 为 O(NM) ,其 中 N , M 分 别 为 所用粒子的数目及特征个数,如给定粒子数 N , RBPF SLAM 的 计 算 复 杂度与特征个数 M 成线性关系,而传统 EKF SLAM 的复杂度为 O(M) ,因此 RBPF SLAM 也被称为 FAST SLAM , 这种算法降低了计算复杂度 ,是 SLAM 的一种常用算法 。FAST SLAM 算 法 是 同 时 定 位 与 地 图 创 建 领 域 的 一 类 重 要 方 法 ,UPF-IEKF FastSLAM 2.0 算法采用 UPF 估计机器人的路径,地图估计
则采用 IEKF 算法 UPF 算法使粒子向后验概率高的区域运动,提高了估 计 精 度 同 时 降 低 计 算 复 杂 度 ; IEKF 算 法 通 过 迭 代 观 测 更 新 过 程 来提高估计精度。 地图估计和路径估计是相互关联的 ,路径和地图估计的精度主要取决于路径估计的方法,采用 UKF 估计地图时,虽然估计精度有所提高,计算量却高于 EKF 算法。 综合考虑估计精度和计算复杂度, 认为 “ UPF-IEKF ” 是一种更合理的 FastSLAM 算法滤波架构,UPF-IEKF FastSLAM 2.0 ( n=2 )算 法 是 FastSLAM 2.0 系 列 算 法 中 最 理
想的算法。