机器人操作系统ROS应用实践 课件 第4-6章 移动机器人激光SLAM、移动机器人自主导航、基于多传感器的SLAM

第四章移动机器人激光SLAM机器人操作系统ROS应用实践目录contentSLAM基本原理4.1HectorSLAM算法4.3Gmapping算法4.2本章小结4.44.1SLAM基本原理1SLAM概述移动机器人环境感知技术涉及的主要内容包括定位（Localization）、建图（Mapping）和路径规划（PathPlanning）/运动控制。SLAM技术属于定位和建图的交集部分，如图所示。4.1SLAM基本原理1SLAM概述在未知环境中，机器人自主定位与建图是相互关联和相互依赖的，其既需要利用建立好的地图信息来更新自身的位置和姿态，也需要利用准确的位姿估计信息来建立环境地图。SLAM的基本过程可用下图描述。4.1SLAM基本原理1SLAM概述实现机器人的自主导航需要机器人能够很好地感知周围环境。在一个未知的复杂环境中，人类可以通过眼睛、耳朵、鼻子等多个器官来感知外界环境的变化，从而确定自身所处的状态，这也体现了人类强大的感知能力。但是在未知环境中，机器人无法像人类一样感知外界环境的动态变化，需要依靠自身携带的传感器来感知外界环境。这些传感器通常包括相机、激光雷达、里程计、惯性测量单元等，机器人需要通过分析传感器数据，进行自身位姿的估计和环境地图的构建。4.1SLAM基本原理2移动机器人坐标系为了进行位姿估计和地图构建，需要建立移动机器人的坐标系。ROS中，移动机器人的坐标系一般包括map、base_link、odom、base_laser等。map：地图坐标系，该坐标系为固定坐标系（FixedFrame），与机器人所在的世界坐标系一致。base_link：机器人本体坐标系，又称基坐标系，原点与机器人中心重合。4.1SLAM基本原理2移动机器人坐标系为了进行位姿估计和地图构建，需要建立移动机器人的坐标系。ROS中，移动机器人的坐标系一般包括map、base_link、odom、base_laser等。odom：里程计坐标系，可以看成一个随移动机器人运动而运动的坐标系，只要机器人运动，就会发布base_link到odom的TF。机器人运动开始前（初始时），如机器人在原点处时，odom和map是重合的，即map到odom的TF就是0。随着时间的推移，由于里程计（根据运动学模型计算）的累计误差，odom会出现偏差，这时就需要用传感器进行校正，也就是估计。4.1SLAM基本原理2移动机器人坐标系为了进行位姿估计和地图构建，需要建立移动机器人的坐标系。ROS中，移动机器人的坐标系一般包括map、base_link、odom、base_laser等。base_laser：激光雷达坐标系，由激光雷达的安装位置确定，与base_link的坐标系变换为固定的。4.1SLAM基本原理3ROS导航与定位过程下面以常见的双轮差速移动机器人为例，按照自底向上的顺序介绍ROS导航与定位过程，如图所示。4.1SLAM基本原理3ROS导航与定位过程最底层是轮速和底盘控制，差速轮结构的移动底盘一般包括两个主动轮和一个从动轮，主动轮上安装有带减速器的电机，每个电机轴上有编码器，可以实现里程计功能。电机驱动器上集成有转速控制器，可以直接调用，ROS中也包含了很多第三方的驱动器驱动。4.1SLAM基本原理3ROS导航与定位过程往上是位置控制，如果期望机器人按照设定值，以某个速度前进一定的距离，就需要应用PID进行闭环控制。如果知道当前位置和朝向，以及目标位置和朝向，ROS提供了move_base功能包控制机器人运动到目标点，并实现避障和路径规划（PathPlanner）。4.1SLAM基本原理3ROS导航与定位过程再往上是利用通过Gmapping等功能包创建地图，然后利用AMCL（AdaptiveMonteCarloLocalization，自适应蒙特卡罗定位）功能包，根据当前数据进行定位。最上层是根据语义信息将命令解析成一系列动作，然后驱动下层执行相应命令，使机器人运动到目标位置。4.1SLAM基本原理3ROS导航与定位过程ROS导航与定位一般是针对差速轮式机器人和完整约束轮式机器人的，移动平台只能接收x，y方向速度和偏航角（yaw）角速度；机器人需要激光雷达传感器（或者深度数据模拟的激光数据）创建地图和进行定位；机器人的形状应是尺寸合适的方形或者圆形。4.1SLAM基本原理3ROS导航与定位过程ROS中的Navigation（导航）功能包为机器人的导航提供了一套完整的解决方案。NavigationStack（导航栈）是一个ROS的元功能包，其中包含了路径规划、定位、异常行为恢复、地图服务器等功能，这些开源工具包极大地减少了机器人应用系统的开发工作量，可以快速部署实现相关功能。Navigation功能包中包含了许多导航与定位相关的功能包，其中move_base功能包是一个强大的路径规划器，可以驱动移动底盘运动到世界坐标系上的一个目标点，其包含全局路径规划器和局部路径规划器，同时维护两个costmap（代价地图），一个给全局路径规划器，另一个给局部路径规划器。4.1SLAM基本原理3ROS导航与定位过程这两个costmap可以配置多个地图层：ObstacleMapLayer：障碍地图层，用于动态记录传感器感知到的障碍物信息。InflationLayer：膨胀层，在地图上进行膨胀（向外扩张），以避免机器人撞上障碍物。StaticMapLayer：静态地图层，是基本不变的地图层，通常是SLAM建立完成的静态地图。OtherLayers：通过插件形式实现的代价地图层（目前已有一些开源插件）。4.1SLAM基本原理4环境建图与位姿估计地图是对某种特定环境的描述，常见的地图包括栅格地图、点云地图、拓扑地图。移动机器人的定位与建图是两个紧密相关的问题，是可以同时进行的。在SLAM的过程中，最常用的地图为栅格地图。4.1SLAM基本原理4环境建图与位姿估计在ROS中，栅格地图通常是一张pgm格式的灰度图像，如图所示。图像中的白色像素点表示可行区域，黑色像素点表示障碍物，灰色部分是未知探索区域。4.1SLAM基本原理4环境建图与位姿估计SLAM问题本质上是一个状态估计问题，即如何通过带有噪声的测量数据估计未知的状态变量。其中，测量数据既可以来自机器人自身运动测量传感器（里程计、惯性测量单元等），也可以来自机器人观测外部环境的传感器（相机、激光雷达），需要估计的未知状态变量包括机器人的位姿和环境地图。4.1SLAM基本原理4环境建图与位姿估计从数学表述上来看，一个完整的SLAM过程由两个基本方程组成：运动方程和观测方程，如式所示。运动方程描述的是如何通过运动测量传感器的测量值

来进行机器人位姿

的估计，观测方程描述的是如何通过外部环境观测数据

来进行机器人位姿

的估计和建图（y）。4.1SLAM基本原理4环境建图与位姿估计SLAM系统由前端与后端组成，如图所示。前端的主要任务是完成传感器数据处理、自身定位和局部建图，实时更新机器人的姿态与地图；而后端的主要任务是完成位姿优化及地图优化，不断优化这些姿态的准确性及地图的准确性。4.1SLAM基本原理4环境建图与位姿估计根据后端处理方式不同，SLAM算法可以分为基于滤波器的算法与基于图优化的算法。在SLAM算法的早期应用中，通常采用基于滤波器的算法，其利用贝叶斯原理，根据观测信息及控制信息对系统状态的后验概率进行估计，而根据后验概率表示方法的不同，又存在着多种基于滤波器的算法。常用的基于滤波器的算法有卡尔曼滤波（KalmanFilter，KF）算法、扩展卡尔曼滤波（ExtendedKalmanFilter，EKF）算法、无迹卡尔曼滤波（UnscentedKalmanFilter，UKF，也称无损卡尔曼滤波）算法、粒子滤波（ParticleFilter，PF）算法等。读者可查找相关资料对上述算法进行进一步学习。4.2Gmapping算法1原理分析使用RBPF的Gmapping算法解决SLAM问题的核心思想是利用联合概率密度

估计地图m及机器人的运动位姿

。利用Rao-Blackwellized思想，将上述联合概率分解为式。也就是说，可以先估计机器人位姿，再利用位姿估计地图。而对于位姿的估计，可以采用粒子滤波算法。4.2Gmapping算法2实施流程Gmapping算法的核心是RBPF，采用重要性重采样（SamplingImportanceResampling，SIR）滤波，该算法的实施分为以下四个步骤。粒子采样。对于每一个粒子i，利用建议分布（ProposalDistribution,PD），基于移动机器人的里程计运动模型，从t−1时刻的位姿粒子中

采样出t时刻的位姿粒子

。4.2Gmapping算法2实施流程Gmapping算法的核心是RBPF，采用重要性重采样（SamplingImportanceResampling，SIR）滤波，该算法的实施分为以下四个步骤。权重更新。权重更新使用式进行，为方便编程实现，将其变换成迭代形式：4.2Gmapping算法2实施流程Gmapping算法的核心是RBPF，采用重要性重采样（SamplingImportanceResampling，SIR）滤波，该算法的实施分为以下四个步骤。粒子重采样。每种粒子的数量与它们的权重正相关。由于只能采用有限的粒子来近似连续的分布，随着时间的推移，粒子的多样性将丧失，因此必须引进重采样，以增加粒子的多样性。在目标分布与建议分布不同的情况下，更应引进重采样，这样，经过重采样后所有粒子的权重相同，从而增加了粒子的多样性。重采样阈值依据式）确定：4.2Gmapping算法2实施流程Gmapping算法的核心是RBPF，采用重要性重采样（SamplingImportanceResampling，SIR）滤波，该算法的实施分为以下四个步骤。地图更新。Gmapping算法使用梯度下降法进行搜索匹配，对每个粒子与地图进行扫描匹配，匹配相似度高的得分高，匹配相似度低的得分低，选取得分最高的粒子作为移动机器人的当前姿态，进而用于更新地图。扫描匹配准则依据式确认：4.2Gmapping算法2实施流程Gmapping算法完整的流程如图所示。4.3HectorSLAM算法1原理分析双线性地图插值HectorSLAM算法为了获取平滑的地图，采取双线性插值法对栅格地图进行插值。双线性插值法的核心思想是：在x、y两个方向分别进行一次插值。由图可知，

为障碍物的概率如式所示。4.3HectorSLAM算法1原理分析双线性地图插值其中，

表示栅格

为障碍物的概率。

的偏导数为4.3HectorSLAM算法1原理分析扫描匹配HectorSLAM算法扫描匹配的最优准则为即寻找激光雷达扫描结果与地图最佳对准时的最优变换变量

。

是扫描终点在世界坐标系下的坐标，其函数表达式为式：4.3HectorSLAM算法1原理分析扫描匹配对于给定的任意一个初始估计

，寻找一个最优估计

，使式对

进行一阶泰勒展开，求取展开式的最小值，即对

求取偏导数并令偏导数为0，求得式。4.4本章小结本章主要介绍SLAM基本原理及常见的激光SLAM算法，分析了SLAM中的两大关键技术——环境建图与位姿估计；然后举例讲解了移动机器人坐标变换方法；最后分析了两种经典的激光SLAM算法：Gmapping和HectorSLAM，并通过两个任务，使读者理解SLAM的关键技术，为将来进一步学习或研究其他SLAM算法打下基础。第五章移动机器人自主导航机器人操作系统ROS应用实践目录content基于地图的定位5.15.3基于地图的自主导航5.2本章小结5.1基于地图的定位1蒙特卡罗定位蒙特卡罗定位（MonteCarloLocalization，MCL）算法是一种概率算法，其将机器人当前位置看成许多粒子的密度模型，采用基于粒子滤波的方法来进行定位。每个粒子可以看成机器人在此位置的假设。蒙特卡罗定位算法可以用在带有外部距离传感器（如激光雷达）的机器人系统上。激光雷达可测量各个方向上机器人与最近障碍物之间的距离。在每个时间点上，机器人都会获得激光雷达的测量值。5.1基于地图的定位1蒙特卡罗定位如图所示，图（a）中，三角形代表机器人，直线代表激光束，正方形格子是机器人在该激光束方向上检测到的最近障碍物；图（b）为占据栅格地图（OccupancyGridMap），其中白色格子表示障碍物，黑色格子表示空白位置。（a）

（b）

5.1基于地图的定位1蒙特卡罗定位如图所示，蒙特卡罗定位算法的步骤如下。初始化粒子群。定义粒子群大小为M，在机器人初始位姿已知的情况下，可复制M个初始位姿作为初始粒子群；而在初始位姿未知的情况下，可利用高斯分布随机采样的方法初始化粒子群，即使粒子均匀分布在地图上。5.1基于地图的定位1蒙特卡罗定位如图所示，蒙特卡罗定位算法的步骤如下。模拟粒子运动。模拟粒子运动可以根据机器人的运动模型，对任意粒子做下一时刻的运动估计，从而得到下一时刻的粒子群，这种方法需要准确地对机器人做运动建模；然而有些时候机器人的运动模型难以建立，可以利用机器人运动的连续性（在两个相邻的采样时间间隔内机器人的运动范围是有限的），使用随机函数生成粒子来描述机器人可能运动到的位置。5.1基于地图的定位1蒙特卡罗定位如图所示，蒙特卡罗定位算法的步骤如下。计算粒子评分。对于激光雷达采集的每个点，激光雷达读数与地图的匹配有四种情况，如表所示。5.1基于地图的定位1蒙特卡罗定位如图所示，蒙特卡罗定位算法的步骤如下。粒子群重采样。在每次评分结束后，会发现有些粒子评分很低，即这些粒子严重偏离了机器人的可能位姿，这些粒子必须舍弃掉，以提高粒子群的质量与收敛速度；而有些粒子的评分则很高，并且这些粒子的评分都很接近，则需要保留这些粒子。但直接将评分过低的粒子舍弃必然会减少总粒子数，为了保持粒子群规模大小不变，可以对高得分的粒子进行简单的复制重采样，以保持粒子总数不变。这就是粒子群重采样的过程。5.1基于地图的定位1蒙特卡罗定位根据上述四个步骤可知，蒙特卡罗定位算法具有如下优点。无须对机器人进行运动建模即可以对机器人的全局位姿进行估计。无须知道机器人的初始位姿。计算过程消耗的内存较少，计算量较小，计算结果较为准确。机器人运动位姿的估计非常平滑，适合做移动机器人的导航控制。算法容易实现。5.1基于地图的定位1蒙特卡罗定位根据上述四个步骤可知，蒙特卡罗定位算法具有如下缺点。一旦机器人位置出现跳变，蒙特卡罗定位算法将失效（存在“机器人绑架”问题）。定位精度与粒子数有关，提高定位精度需要使用大量粒子。定位收敛速度较慢。5.1基于地图的定位2自适应蒙特卡罗定位为解决蒙特卡罗定位算法的一些弊端，人们开发出了它的许多改进版本，其中自适应蒙特卡罗定位（AdaptiveMonteCarloLocalization，AMCL）算法颇为有效。由于蒙特卡罗定位算法在粒子群重采样步骤中，可能会意外丢弃所有正确位姿附近的粒子。当粒子数很小（如50个），并且扩散到比较大的区域中（如全局定位过程）时，这个问题的严重性就充分显示出来了。5.1基于地图的定位2自适应蒙特卡罗定位这就需要对蒙特卡罗定位算法进行优化，自适应蒙特卡罗定位算法的“自适应”主要体现在两方面：一是解决粒子数固定的问题，当移动机器人定位收敛时，粒子基本都集中在一处，此时可以适当减少粒子数；二是解决“机器人绑架”问题，当发现粒子的平均分数突然降低了（正确的粒子在某次迭代中被抛弃了）时，在全局区域再重新散布一些粒子。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包的主题与服务amcl功能包订阅的主题如下。tf（tf/tfMessage）：表示坐标变换信息。initialpose（geometry_msgs/PoseWithCovarianceStamped）：表示用于初始化粒子滤波器的均值和协方差。scan（sensor_msgs/LaserScan）：表示激光雷达测量数据。map（nav_msgs/OccupancyGrid）：设置use_map_topic参数后，amcl会订阅map主题来获取地图数据，用于激光雷达定位。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包的主题与服务amcl功能包发布的主题如下。amcl_pose（geometry_msgs/PoseWithCovarianceStamped）：表示机器人在地图中的估计位姿，并带有协方差信息。particlecloud（geometry_msgs/PoseArray）：表示由粒子滤波器维护的位姿估计集合。tf（tf/tfMessage）：表示发布从odom（可以通过~odom_frame_id参数重新映射）到map的变换。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包的主题与服务amcl功能包的服务如下。global_localization（std_srvs/Empty）：用于启动全局定位，所有粒子在地图的空闲空间中随机分散。request_nomotion_update（std_srvs/Empty）：用于手动执行更新和发布更新粒子的服务。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包的主题与服务amcl功能包调用的服务如下。static_map（nav_msgs/GetMap）：amcl调用该服务来获取地图数据。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包中的参数总体过滤器参数~min_particles（int，默认值：100）：允许的最小粒子数。~max_particles（int，默认值：5000）：允许的最大粒子数。~kld_err（double，默认值：0.01）：真实分布和估计分布之间的最大误差。~kld_z（double，默认：0.99）：（1−p）的上标准正常分位数，其中p是估计分布误差小于kld_err的概率。5.1基于地图的定位2自适应蒙特卡罗定位amcl功能包中的参数激光模型参数~laser_min_range（double，默认值：−1.0）：最小扫描范围，−1.0表示使用激光雷达默认的最小扫描范围。~laser_max_range（double，默认值：−1.0）：最大扫描范围，−1.0表示使用激光雷达默认的最大扫描范围。~laser_max_beams（int，默认值：30）：表示在更新过滤器时要在每次扫描时使用多少束均匀间隔的光束。5.2基于地图的自主导航1导航框架机器人导航包含定位和路径规划两个核心内容，ROS提供的amcl和move_base功能包能够建立起一套完整的导航框架，实现机器人的导航功能。Navigation（导航）功能包的核心是move_base功能包，其包含了global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors五个包。move_base功能包订阅TF（坐标系转换）、odom（里程计数据）、map（地图）、sensordata（激光雷达数据或点云）及goal（目标位置）等主题，之后发布cmd_vel主题。5.2基于地图的自主导航1导航框架ROS的导航功能框架如图所示。5.2基于地图的自主导航1导航框架TF：利用ROS的TF工具发布坐标系之间的转换关系，包括：/map->/odom，/odom->/base_link，/base_link->/sensor等。odom：导航需要用到里程计数据，需将其用TF工具和nav_msgs/Odometry消息发布出来。map：在导航前，提供一张全局地图，需要提前创建（但不是必须的）。sensordata：用于避障、建图等，可以是激光雷达数据或点云数据（sensor_msgs/LaserScanorsensor_msgs/PointCloud）。goal：目标位置在全局地图中的坐标，用geometry_msgs/PoseStamped消息格式发布。5.2基于地图的自主导航1导航框架ROS的导航功能框架中几个主要组件的功能如下。sensortransforms组件。该组件涉及使用TF进行传感器坐标的变换，机器人中心往往不是传感器中心，需要把传感器的测量数据变换成以机器人中心为原点的坐标信息。例如，若传感器获取的数据是基于激光雷达坐标系base_laser的，而控制机器人时，是以机器人本体坐标系base_link进行的，则需要根据两者的位置关系进行坐标变换。5.2基于地图的自主导航1导航框架ROS的导航功能框架中几个主要组件的功能如下。sensorsources组件。这是机器人导航传感器的数据输入组件，数据一般只有两种：激光雷达数据、点云数据。odometrysource组件。该组件负责提供机器人导航需要输入的里程计数据。5.2基于地图的自主导航1导航框架ROS的导航功能框架中几个主要组件的功能如下。basecontroller组件。该组件负责将导航过程中得到的数据封装成具体的线速度和转向角度，发布给硬件平台。map_server组件。ROS的costmap地图采用网格形式，每个网格值的范围为0～255，分为三种状态：占用（障碍物）、无用（自由的）、未知。5.2基于地图的自主导航1导航框架尽管导航功能包设计得尽可能通用，但是其仍然对机器人的硬件有以下三个要求：导航功能包仅对差分轮式机器人和完整约束轮式机器人有效，并且假设可直接使用速度命令对机器人进行控制，速度命令的格式为：x方向速度、y方向速度、速度向量角度。导航功能包要求机器人安装有激光雷达等二维平面测距传感器，以建图和定位。5.2基于地图的自主导航1导航框架尽管导航功能包设计得尽可能通用，但是其仍然对机器人的硬件有以下三个要求：导航功能包以方形或者圆形的机器人为模型进行开发，对于其他外形的机器人，虽然可以正常使用，但是支持度可能不佳。在ROS的导航功能框架中可以看到，move_base功能包提供了ROS导航的配置、运行和交互接口，它主要包括两个部分：全局路径规划和局部路径规划。5.2基于地图的自主导航2全局路径规划全局路径规划是在已知的环境中，从机器人当前位置找到一条到达目标位置最佳路径的过程。这里所说的最佳路径指的是路径上没有障碍物的情况下的最短路径。如果机器人在按照规划好的全局路径进行运动的过程中，遇到阻碍物，则要进行局部路径规划，绕开障碍物，到达目标位置。5.2基于地图的自主导航2全局路径规划Dijkstra算法Dijkstra（迪杰特斯拉）算法是典型的单源最短路径算法，它用于计算图中一个顶点到其他顶点之间的最短路径。该算法的基本思想是：以起点为中心，每次找到离起点最近的一个顶点，然后以该顶点为中心向外逐层扩展，直到扩展到目标点为止，最终得到起点到其余所有点的最短路径。5.2基于地图的自主导航2全局路径规划Dijkstra算法Dijkstra算法具体的步骤如下。初始化。S中只包含起点，即S={V0}，V0到V0的距离为0。U中包含除起点之外的其他点。若V0与U中的顶点Vi之间有边可达，则Vi,V0之间有正常权值，一般就是距离值。若二者之间没有边可达，则Vi,V0之间的权值为无穷大（编程时设为一个较大的数值，如9999）。5.2基于地图的自主导航2全局路径规划Dijkstra算法Dijkstra算法具体的步骤如下。从U中选取一个距离V0最近的顶点Vk，把Vk加入S中，选定的权值就是V0到Vk的最短路径长度。5.2基于地图的自主导航2全局路径规划Dijkstra算法Dijkstra算法具体的步骤如下。以Vk为新的中间点，修改U中各顶点的权值：如果从V0经过Vk到顶点Vm的距离比从V0直接到Vm的距离短，则修改顶点Vm的权值，使其等于V0到Vk的距离与Vk到Vm的距离之和；如果从V0经过Vk到顶点Vm的距离比从V0直接到Vm的距离长，则维持顶点Vm的权值不变。5.2基于地图的自主导航2全局路径规划Dijkstra算法Dijkstra算法具体的步骤如下。重复以上步骤直到所有的顶点都被包含在集合S中。5.2基于地图的自主导航2全局路径规划Dijkstra算法如图所示是一个典型的无向图。5.2基于地图的自主导航2全局路径规划A*算法A*算法是在静态网络中求解最短路径的算法，它把启发式算法（HeuristicApproach）如BFS（Breadth-FirstSearch，广度优先搜索）算法和Dijkstra算法结合在一起，能保证找到一条最短路径。A*算法的核心思想是找到一条路径，使得机器人在该路径上行走时，每一步的移动代价G与估算成本H的和都是最小的。那么，问题也就转化为求取G与H的和最小的问题，下面将详细介绍G与H的意义及如何使G与H的和最小。5.2基于地图的自主导航2全局路径规划A*算法实现A*算法需要以下条件。地图：一个存储静态路网的结构，由栅格组成。栅格：组成地图的基本单位，也可以叫节点。每个栅格都具有五种属性：坐标、G、H、F（G和H的和）、父亲。Open列表：用于存储等待处理的栅格。Close列表：用于存储实际路径上的栅格。起点与目标点：机器人初始位置与目标位置，是算法的两个输入。5.2基于地图的自主导航2全局路径规划A*算法以上这些存储结构是实现A*算法所需要的，下面给出地图结构体、Close列表、Open列表的抽象定义。地图结构体的定义包含了每个栅格的坐标、可达性、相邻栅格个数及栅格的值。5.2基于地图的自主导航2全局路径规划A*算法以上这些存储结构是实现A*算法所需要的，下面给出地图结构体、Close列表、Open列表的抽象定义。Close列表定义如下：5.2基于地图的自主导航2全局路径规划A*算法以上这些存储结构是实现A*算法所需要的，下面给出地图结构体、Close列表、Open列表的抽象定义。Open列表定义如下：5.2基于地图的自主导航2全局路径规划Dijkstra算法和A*算法的区别Dijkstra算法和A*算法都是求解最短路径的有效算法，它们之间的区别主要体现在以下方面。Dijkstra算法注重计算起点到其他所有点的最短路径长度，可以看成只有一个输入，即指定的起点；A*算法关注的是点与点之间的最短路径，每次计算会有两个输入：起点与目标点，可以直接得出一条具体的路径。5.2基于地图的自主导航2全局路径规划Dijkstra算法和A*算法的区别Dijkstra算法和A*算法都是求解最短路径的有效算法，它们之间的区别主要体现在以下方面。Dijkstra算法建立在比较抽象的图论层面，而A*算法可以更直接地应用于栅格地图中。A*算法的搜索范围小于Dijkstra算法，但A*算法的效率更高。5.2基于地图的自主导航2全局路径规划Dijkstra算法和A*算法的区别Dijkstra算法和A*算法都是求解最短路径的有效算法，它们之间的区别主要体现在以下方面。当目标点很多时，A*算法会代入较多重复的数据和复杂的估价函数，所以如果不要求获得具体路径而只要求比较路径长短时，Dijkstra算法会更加高效。5.2基于地图的自主导航3局部路径规划进行局部路径规划时，需要对障碍物的情况进行详细判断，特别是需要应对多变的动态环境，采用适应性强的处理方式，并简化算法复杂度。如图是在栅格地图上进行路径搜索的流程图，机器人先进行全局路径规划，当碰到障碍物时，会选取相应环境参数进行避障处理。5.2基于地图的自主导航3局部路径规划机器人按照规划好的路径行走时，会利用红外测距传感器、超声波雷达或激光雷达进行障碍物检测和实时测距，依据机器人的实际大小，当机器人检测到其与障碍物的距离小于或等于安全距离时，就获取障碍物四周的情况进行避障。因此，局部路径规划是根据机器人附近的障碍物信息进行的实时避障路径规划。5.2基于地图的自主导航3局部路径规划如图所示，根据障碍物的宽度和其与机器人之间的距离，将障碍物标准化为一个矩形。5.2基于地图的自主导航3局部路径规划人工势场法斥力函数的选取由于障碍物是机器人不能靠近的，因此在势场中，障碍物应该对机器人表现出一种排斥效果，且距离越小，排斥效果越明显，机器人具有的势能越大，反之势能越小。因此，势能和距离成反比，取斥力函数如下：5.2基于地图的自主导航3局部路径规划人工势场法斥力函数的选取其中，

为机器人参考点与障碍物之间的距离，

为势场作用的最大范围，

为加权系数。机器人所受到的斥力为5.2基于地图的自主导航3局部路径规划人工势场法斥力函数的选取斥力的角度计算公式如下：5.2基于地图的自主导航3局部路径规划人工势场法引力函数的选取机器人的运动始终是朝着目标点进行的，因而目标点对机器人的作用力可以看成是引力，而且这种引力与机器人和目标点之间的距离有关，二者相距越远，引力就越大。当二者距离为0时，引力也就为0，此时机器人已经到达目标点。根据以上规律，可以将机器人的引力函数设置如下：5.2基于地图的自主导航3局部路径规划人工势场法引力函数的选取引力的角度计算公式如下：由式（5-10）可以看出，引力的方向是指向目标点的，且引力值实际上是一个常数。在实际应用中，可以通过调节系数来设置引力值的大小。5.2基于地图的自主导航3局部路径规划人工势场法合力的计算在算出机器人所受的引力和斥力后，通过力的分解与合成就可以得出机器人所受到的合力的大小和方向。计算方法如下：计算出机器人所受到的斥力

和引力

；将斥力和引力在x轴和y轴上进行分解，得到斥力分量Frx与Fry，引力分量Fgx与Fgy，则合力大小为5.2基于地图的自主导航3局部路径规划动态窗口法动态窗口法（DynamicWindowApproaches，DWA）：在机器人速度空间中，采样多组速度并模拟机器人在这些速度下一定时间内的运动轨迹，得到多条路径后，对这些路径进行评价，选取最优路径所对应的速度来驱动机器人运动，以避开障碍物。5.2基于地图的自主导航3局部路径规划动态窗口法该算法一般用于在二维平面上进行局部路径规划，适用于静态环境，需要的计算资源少，考虑了机器人的运动学、动力学模型，但其代价函数容易陷进局部最优。DWA的具体步骤如下：在机器人速度空间中进行多组离散采样，得到(dx,dy,

)；对每个采样的速度进行模拟，计算机器人以该速度运动一定时间后的状态，得出多条路径；评价得到的每条路径并舍弃非法路径，评价目标如下：是否接近障碍物、是否接近目标点、是否接近全局路径；5.2基于地图的自主导航3局部路径规划动态窗口法该算法一般用于在二维平面上进行局部路径规划，适用于静态环境，需要的计算资源少，考虑了机器人的运动学、动力学模型，但其代价函数容易陷进局部最优。DWA的具体步骤如下：选择得分最高的路径，发送对应的速度给移动机器人；重复上面的过程。DWA需要在机器人运动之前模拟生成许多运动轨迹，使得计算量增大，容易造成避障不及时的问题。当障碍物较多时，会造成机器人打转，导致路径不是最优的等问题，降低机器人的安全性和运动效率。5.2基于地图的自主导航3局部路径规划动态窗口法动态窗口子图的更新原则是，只有当机器人的运动距离超过设定的阈值时，才更新子图。因此动态窗口子图的更新阈值很关键，若设置得太小，会导致整个系统计算量增加，降低实时性；若设置得太大，则会造成机器人避障的准确性下降。同时，动态窗口子图范围的大小设置也很重要，若设置得过小，会造成机器人避障的准确性下降；若设置得太大，则增加系统的计算量。因此，动态窗口子图的更新阈值与子图范围的大小，需要根据实际效果进行调整。5.2基于地图的自主导航3局部路径规划动态窗口法当把动态窗口子图中的障碍物坐标变换到全局地图坐标系下之后，需要在全局地图中将该坐标对应的位置标识为障碍物区域，并进行障碍物范围扩充处理，使机器人在运动时与障碍物之间存在合适的安全距离，然后控制机器人进行避障。当机器人绕过障碍物后，再清除动态窗口子图融合到全局地图中的障碍物。这样不断循环，直到机器人运动到全局目标点为止。5.2基于地图的自主导航3局部路径规划TEB算法连接起点和目标点的路径，会受到内外力的影响而变形，将这条可以变形的路径定义为ElasticBand（橡皮筋），变形的条件就是“橡皮筋”受到的内外力约束变化。在起点和目标点之间，存在N个使“橡皮筋”变形的控制点（机器人避障时改变运动方向的点），“橡皮筋”内外力相互平衡，使路径收缩，同时与障碍物保持一定的距离。由于机器人在这些点之间运动时需要一定的时间，因此这种算法称为TEB算法。5.2基于地图的自主导航3局部路径规划TEB算法TEB算法的内外力约束主要如下。跟随全局规划路径和避障约束力：跟随全局规划路径的约束力将“橡皮筋”拉向全局路径，而避障约束力使得“橡皮筋”远离障碍物。速度和加速度约束：属于运动学、动力学约束，即移动机器人运动时的最大速度和加速度。5.2基于地图的自主导航3局部路径规划TEB算法TEB算法的内外力约束主要如下。非完整（Non-Holonomic）运动学约束：差速移动机器人在平面上运动时只有两个自由度，只能以朝向的方向进行直线运动或旋转。这种运动学约束使得差速移动机器人沿着由若干弧线组成的平滑轨迹运动。最短路径时间约束：该约束为时间间隔序列的平方，该约束使得机器人获得具有最短运动时间的路径，而非传统的空间上的最短路径。5.2基于地图的自主导航3局部路径规划TEB算法对于两轮差速移动底盘，TED算法在运动中调节朝向会使运动路径不流畅，机器人在起点和即将到达目标点时会出现不必要的倒退。这在某些应用场景里是不允许的，因为机器人倒退可能会碰到障碍物。因此，需要根据场景对TED算法进行一些优化：机器人在起点不朝向目标点时，TEB算法规划出的路径会考虑倒退旋转，修正朝向。但在一些场景中，这样的倒退和修正是不允许的。所以针对两轮差速的运动模型，可以先让底盘原地旋转，使其朝向目标点，再执行优化函数，得到最优轨迹。5.2基于地图的自主导航3局部路径规划TEB算法对于两轮差速移动底盘，TED算法在运动中调节朝向会使运动路径不流畅，机器人在起点和即将到达目标点时会出现不必要的倒退。这在某些应用场景里是不允许的，因为机器人倒退可能会碰到障碍物。因此，需要根据场景对TED算法进行一些优化：将机器人到达目标点时的朝向缓存下来，当机器人到达目标点时，对比此时朝向与缓存的目标朝向，当两个朝向有差异时，原地旋转到目标朝向。5.2基于地图的自主导航3局部路径规划TEB算法对于两轮差速移动底盘，TED算法在运动中调节朝向会使运动路径不流畅，机器人在起点和即将到达目标点时会出现不必要的倒退。这在某些应用场景里是不允许的，因为机器人倒退可能会碰到障碍物。因此，需要根据场景对TED算法进行一些优化：根据障碍物信息，找到前方距离障碍物的最短距离，然后根据障碍物的远近调整最大线速度（max_vel_x）约束，改善避障效果。5.2基于地图的自主导航3局部路径规划TEB算法对于两轮差速移动底盘，TED算法在运动中调节朝向会使运动路径不流畅，机器人在起点和即将到达目标点时会出现不必要的倒退。这在某些应用场景里是不允许的，因为机器人倒退可能会碰到障碍物。因此，需要根据场景对TED算法进行一些优化：根据机器人当前位置距离全局目标点的距离来调整最大线速度约束，距离全局目标点远时可以将最大线速度设置得大一点，距离全局目标点近时可以将最大线速度设置得小一点，以防止出现过冲现象。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包订阅的动作move_base/goal（move_base_msgs/MoveBaseActionGoal）：表示move_base功能包的运动规划目标。move_base/cancel（actionlib_msgs/GoalID）：表示撤销指定目标的请求。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包发布的动作move_base/feedback（move_base_msgs/MoveBaseActionFeedback）：表示反馈信息，包含机器人底盘的坐标。move_base/status（actionlib_msgs/GoalStatusArray）：表示发布到move_base功能包的目标状态信息。move_base/result（move_base_msgs/MoveBaseActionResult）：表示对move_base功能包的操作结果为空。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包订阅的主题move_base_simple/goal（geometry_msgs/PoseStamped）：为用户提供一个到move_base功能包的非动作接口，不需要关注跟踪目标的执行状态。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包发布的主题cmd_vel（geometry_msgs/Twist）：输入机器人底盘的速度命令。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包的服务~make_plan（nav_msgs/GetPlan）：允许用户从move_base功能包请求一个到给定位姿的规划，但不会使move_base功能包执行该规划。~clear_unknown_space（std_srvs/Empty）：允许用户清理机器人周围区域的未知空间。当costmap（代价地图）在环境中已经停止了很长时间，并在一个新的位置重新启用时有用。5.2基于地图的自主导航4导航功能包move_base功能包接口move_base功能包中的参数~base_global_planner（string,默认值：navfn/NavfnROS）：设置move_base功能包使用的全局路径规划插件的名称。~base_local_planner（string,默认值：base_local_planner/TrajectoryPlannerROS）：设置move_base功能包使用的局部路径规划插件的名称。~controller_frequency（double,默认值：20.0）：发布控制命令的循环频率（单位：Hz）。5.2基于地图的自主导航4导航功能包代价地图配置通用配置文件导航功能包使用代价地图存储障碍物信息，创建一个名为costmap_common_params.yaml的通用配置文件，具体内容如下：5.2基于地图的自主导航4导航功能包代价地图配置通用配置文件导航功能包使用代价地图存储障碍物信息，创建一个名为costmap_common_params.yaml的通用配置文件，具体内容如下：5.2基于地图的自主导航4导航功能包代价地图配置全局规划配置文件全局规划配置文件名为global_costmap_params.yaml，用于存储配置全局代价地图的参数，内容如下：5.2基于地图的自主导航4导航功能包代价地图配置局部规划配置文件局部规划配置文件名为local_costmap_params.yaml，用于存储配置局部代价地图的参数，内容如下：5.2基于地图的自主导航4导航功能包局部路径规划器配置base_local_planner局部路径规划器的主要作用是根据规划的全局路径计算发送给机器人底盘的速度控制命令。局部路径规划器需要根据机器人规格尺寸进行参数配置。5.3本章小结本章介绍了如何利用传感器信息使机器人获取在已知地图中的定位，并进行自主导航。通过自适应蒙特卡罗定位功能包amcl获取机器人在地图中的准确姿态，然后利用move_base功能包完成全局路径规划和局部路径规划，实现机器人避障。第六章基于多传感器的SLAM机器人操作系统ROS应用实践目录content惯性测量单元模型与标定6.16.5激光雷达与IMU的外参标定6.2Cartographer算法差速轮式移动机器人的运动里程计模型6.3基于卡尔曼滤波的多传感器融合6.46.6本章小结6.1惯性测量单元模型与标定六轴IMU包含加速度计和陀螺仪，加速度计的三个轴与陀螺仪的三个轴分别对应于三维笛卡尔坐标系的X轴、Y轴、Z轴。加速度计通常由质量块、阻尼器、弹性元件、敏感软件和处理电路等部分组成，用于测量轴向的加速度。陀螺仪的基本原理是，在物体旋转时，旋转轴所指的方向在没有外力干扰下会保持不变，据此可测量物体绕轴旋转的角速度。6.1惯性测量单元模型与标定1加速度计测量模型加速度计测量模型如下：

为IMU参考坐标系下第k帧的加速度计测量值

为第k帧的真实加速度值

为需要在线估计的第k帧的加速度零点误差

为参考坐标系到世界坐标系的变换矩阵

为世界坐标系下的重力加速度加速度计测量模型g为当地重力加速度

为加速度白噪声6.1惯性测量单元模型与标定1加速度计测量模型加速度计测量模型如下：加速度计测量模型通常，加速度白噪声

符合零均值高斯分布：假定加速度零点误差

随时间随机游走，其对时间的导数

符合零均值高斯分布：6.1惯性测量单元模型与标定1加速度计测量模型陀螺仪测量模型陀螺仪测量模型如下：

为第k帧的陀螺仪测量值

为第k帧的真实角速度值

为需要在线估计的第k帧的角速度零点误差

为加速度白噪声6.1惯性测量单元模型与标定1加速度计测量模型陀螺仪测量模型陀螺仪测量模型如下：通常，角速度白噪声符合零均值高斯分布：假定角速度零点误差

随时间随机游走，其对时间的导数

符合零均值高斯分布：6.1惯性测量单元模型与标定2系统误差的预标定加速度计与陀螺仪的测量模型仅考虑了传感器的零点误差与白噪声，但是对于实际的IMU来说，这不足以建立完全的实际测量模型。在使用IMU之前，需要明确IMU的系统误差和随机误差，对IMU进行预标定，以进一步补偿测量误差，本节介绍系统误差的预标定。6.1惯性测量单元模型与标定2系统误差的预标定系统误差也叫确定性误差，包括轴向误差和比例系数，轴向误差是用来修正由于制造工艺精度导致的X、Y、Z三个轴无法准确正交的问题的；比例系数也叫尺度因子，是用来修正真实值和传感器输出的测量值之间的比值的，即修正传感器数字信号到物理量转换的误差。通常情况下，IMU在出厂时会标定好确定性误差，从而我们可以将轴向误差和比例系数视为确定值。轴向误差和比例系数如图所示。6.1惯性测量单元模型与标定2系统误差的预标定这里介绍一种无须外部基准装置的IMU标定方法，能够提供“轴向误差”“比例系数”“零点误差”的标定结果。设加速度计测量模型为a0为正交坐标系下的加速度计测量值as为加速度计实际坐标系下的原始测量值加速度计轴向误差6.1惯性测量单元模型与标定2系统误差的预标定设加速度计测量模型为比例系数零点误差

为加速度白噪声6.1惯性测量单元模型与标定2系统误差的预标定设陀螺仪测量模型为w0为正交坐标系下的陀螺仪测量值ws为陀螺仪实际坐标系下的原始测量值陀螺仪轴向误差比例系数零点误差

为加速度白噪声6.1惯性测量单元模型与标定2系统误差的预标定IMU的系统误差预标定过程分为以下步骤。将IMU接入采集设备，并静置50s，以估计陀螺仪零点误差。拿起IMU，沿着IMU每个轴的正、负方向进行足够快的旋转（每次旋转后静置5s），直到对陀螺仪三个轴的正、负共六个方向进行充分的旋转激励，以标定陀螺仪的“轴向误差”和“比例系数”。6.1惯性测量单元模型与标定2系统误差的预标定IMU的系统误差预标定过程分为以下步骤。手拿IMU，然后沿着IMU每个轴的正、负方向进行快速直线运动（每次运动后静置5s），直到对加速度计三个轴的正、负共六个方向进行有效的激励，以此标定加速度计的“轴向误差”“比例系数”“零点误差”。将记录的传感器数据转换为合适的格式后，使用开源工具imu_tk，标定上述Ta、ka、ba、Tg、Kg、bg。6.1惯性测量单元模型与标定2系统误差的预标定IMU的系统误差预标定过程分为以下步骤。编写ROS节点，将实际坐标系下的原始传感器测量值as、ws，分别使用预标定得到的参数进行补偿，将得到的正交坐标系下的传感器测量值a0、w0作为式（6-1）与式（6-4）中的

、

，用于机器人的位姿估计。6.1惯性测量单元模型与标定3随机误差的预标定由于外界环境不断变化，加速度计与陀螺仪的测量模型中会携带随机白噪音信号，且该信号符合零均值高斯分布，同时两者的零点误差也会随时间游走，其对时间的导数也符合零均值高斯分布，这两类随机噪音信号会造成测量值在采集过程中携带误差，影响精度，因此，需要标定加速度计与陀螺仪的白噪音参数和零点误差随机游走参数。随着IMU数据的不断积分，随机误差会越来越大。因此，有必要对IMU随机误差进行标定。6.1惯性测量单元模型与标定3随机误差的预标定在IEEE标准中，使用Allan方差法进行单轴光纤陀螺仪的测试。Allan方差法是一种时域分析方法，用于确定信号的噪声。下面使用Allan方差法估计加速度计与陀螺仪的随机误差。首先计算原始测量数据在不同带宽

的低通滤波器下的方差

，然后将滤波器带宽

作为X轴变量，方差作为Y轴变量，绘制对数曲线图（Allan标准差图），分析方差

随滤波器带宽的变化趋势；最后，根据Allan标准差图，得到陀螺仪与加速度计信号的以下参数。6.1惯性测量单元模型与标定3随机误差的预标定N——白噪声参数。陀螺仪数据的单位：加速度计数据的单位：该值对应Allan标准差图中斜率为

的直线部分在

处的截距。其物理意义是，对于采样频率为f的陀螺仪（或加速度计），实际测量值具有标准差为

的白噪声。6.1惯性测量单元模型与标定3随机误差的预标定B——零点误差不稳定性参数。陀螺仪数据的单位：rad/s加速度计数据的单位：m/s2该值对应Allan标准差图中

的最小值。陀螺仪零点误差不稳定性参数的物理意义是，陀螺仪在准确地补偿静态零点误差后，每小时角度积分的误差小于

。6.1惯性测量单元模型与标定3随机误差的预标定K——速度零点误差随机游走参数。陀螺仪数据的单位：加速度计数据的单位：该值对应Allan标准差图中斜率为

的直线部分在

处的截距。其物理意义是，对于采样频率为f的陀螺仪（或加速度计），零点误差对时间的导数符合标准差为

的高斯分布。6.1惯性测量单元模型与标定3随机误差的预标定随机误差的具体标定过程分为以下步骤。将IMU固定于稳固平面上，避免振动影响标定精度，与安装在机器人上时的安装方向相同，以减少重力加速度对陀螺仪测量的影响。将IMU通过延长线连接到采集设备上，与PC建立通信。静止等待若干分钟，等待IMU进入稳定状态。6.1惯性测量单元模型与标定3随机误差的预标定随机误差的具体标定过程分为以下步骤。使用ROS的rosbag工具记录2～5小时的IMU数据，这期间保持IMU静止，否则会影响标定结果；使用基于Allan方差法的开源工具imu_utils，将源码中IMU的采样频率修改为实际值后，通过公式拟合，得到标定结果。6.1惯性测量单元模型与标定3随机误差的预标定BMI055的随机误差标定结果见下表，标定结果与数据手册中给出的数值基本一致。6.2激光雷达与IMU的外参标定在基于多传感器的SLAM系统中，传感器的数据类型不同，采样频率也不同。例如，一个移动机器人系统中，激光雷达的采样频率为10Hz，相机的采样频率为15Hz，IMU的采样频率为400Hz。在进行数据融合之前，需要保证不同传感器的数据是同一时刻获得的，即进行数据时间同步。选择这些传感器中最低的采样频率作为基准，即将激光雷达的采样时间作为对齐标志，如图所示。6.2激光雷达与IMU的外参标定通过ROS功能包采集的数据每一帧都被标记上了时间戳，由于各传感器之间没有硬件同步功能，因此无法保证采集到的数据帧中有相同的时间戳。下面以激光雷达为基准，保留距离激光雷达数据帧时间戳最近的相机帧。对于IMU传感器来说，则通过加权均值法进行时间同步处理，如式所示：6.2激光雷达与IMU的外参标定激光雷达获得的是点云数据，粗略地反映了场景中物体的轮廓。激光雷达随着移动机器人的运动而运动，以激光雷达的初始位姿所在的坐标系作为世界坐标系，如图所示。6.2激光雷达与IMU的外参标定ICP算法假定有两个待处理的点云数据集合P,Q，记P为源点云数据集合，Q为目标点云数据集合，存在未知空间变换T=[Rt]使得它们能够进行空间匹配，即将P所在的坐标系变换到Q所在的坐标系下。设向量，n为两张点云图中最近邻近点云对数，ICP算法的目的就是找出最邻近的点云对

，使得

。用点云之间的欧氏距离来度量匹配误差：于是可以建立最小二乘的优化目标函数：6.2激光雷达与IMU的外参标定结束迭代优化的条件有两个，满足其中之一即可：迭代次数达到设定阈值；前后两次均方根误差之差的绝对值小于设定阈值，并且前后两次内点比例之差的绝对值小于设定阈值。在实际处理过程中，源点云和目标点云是不完全一样的，需要剔除误匹配的点云对，从而使迭代优化收敛得更准确。可使用自适应阈值法进行误匹配剔除，具体方法是，对点云对进行统计分析，计算出自适应阈值，从而剔除大于该阈值的点云对。6.2激光雷达与IMU的外参标定计算出每次迭代过程中，对应点距离值的均值和标准差：将自适应阈值设定为6.2激光雷达与IMU的外参标定下表是某个移动机器人系统的激光雷达-IMU外参变换矩阵标定结果，其中平移向量单位为m。于是，可以根据传感器两两之间的位姿变换关系，建立起移动机器人平台上的所有传感器坐标系的变换矩阵，为后续的多传感器融合算法奠定基础。6.3差速轮式移动机器人的运动里程计模型SLAM算法都需要依据机器人的运动模型进行位姿估计，然后结合其他传感器观测进行位姿校正。假设机器人在二维平面上运动，且不存在车轮打滑的情况，分别在机器人左右轮上安装编码器，基于上述假定条件，对差速轮式移动机器人进行运动学建模。6.3差速轮式移动机器人的运动里程计模型如图所示，假定机器人在XY平面上运动，机器人中心点在左轮与右轮中间，左轮与右轮之间的距离为2L，左侧轮式编码器测得的速度为vL，右侧轮式编码器测得的速度为vR，则差速轮式移动机器人的运动学参数由速度v和角速度ω构成，它们可以根据机器人的左轮速度和右轮速度求解得出。6.3差速轮式移动机器人的运动里程计模型设差速轮式移动机器人绕着圆心A做圆周运动，圆周半径为AB1=R，当机器人沿着圆心A运动很短的时间t后，差速轮式移动机器人由A1运动到A2，则左轮运动的距离为ΔSL，右轮运动距离为ΔSR，机器人整体运动距离为ΔS。根据圆周的弧长与半径R、夹角θ的关系，可得方程组：左轮与右轮运动的弧长可由轮式编码器对时间积分获得，即ΔSL、ΔSR、L为已知量，联合上式中的两式，可分别求得差速轮式移动机器人做圆周运动的半径R与夹角θ。6.3差速轮式移动机器人的运动里程计模型上式中，

，

。机器人沿着圆心A运动很短的时间t后，可由运动弧长求解圆周的半径R与夹角θ。当时间t逐渐趋近于零时，对式中的θ与ΔS中的时间t做微分，可得机器人速度v与角速度w如下：6.3差速轮式移动机器人的运动里程计模型如图所示，假设某一时刻机器人速度为v，角速度为

，位姿为

，其中x、y表示二维坐标，

表示机器人的航向角。

为机器人运动圆心的坐标。6.3差速轮式移动机器人的运动里程计模型由速度与角速度的关系可知：机器人的运动速度不会跳变，在一个极短的时间

内，运动速度为常量。采用三角法，在无噪声情况下，机器人运动了

，同时转向了

角度。因此，机器人位姿

可表示为下式。6.3差速轮式移动机器人的运动里程计模型下式中，

和

为误差（噪声）模型。因此，在使用里程计进行机器人位姿估计时，位姿估计精度低，可靠性差，需结合其他传感器观测数据共同求解。6.4基于卡尔曼滤波的多传感器融合SLAM的过程，可以认为是机器人在未知环境中从某个位姿开始运动，在运动过程中不断重复利用自身携带的传感器（如激光雷达、IMU、相机等）观测到的地图特征（如墙角、柱子等）进行自身位姿的确