文章目录
前言
SLAM（Simultaneous Localization And Mapping）是指在机器人或移动设备等自主移动系统的运动过程中，同时实时地构建出环境地图并确定自己的位置的技术。SLAM技术已经广泛应用于无人驾驶、机器人导航、虚拟现实等领域。
在SLAM技术中，机器人需要通过自身的传感器，如激光雷达、摄像头、惯性测量单元等，获取环境信息，然后通过算法实现自主移动和位置估计，同时将环境信息转化为地图。SLAM技术可以分为前端和后端两个主要部分。
前端负责从传感器数据中提取特征点或者地标，并通过各种算法对这些信息进行跟踪和匹配，从而实现机器人的实时定位和建图。常用的前端算法包括基于激光雷达的算法、基于视觉的算法和基于惯性测量单元的算法等。
后端则负责对前端得到的数据进行处理和优化，将多次测量得到的位置信息融合起来，生成一张更加精确的地图，并对机器人的轨迹进行校正和优化。常用的后端算法包括基于图优化的算法和基于滤波的算法等。
常用的基于激光雷达的SLAM算法包括经典的Gmapping算法和Hector SLAM算法等；常用的基于视觉的SLAM算法包括ORB-SLAM、LSD-SLAM、DSO等
一、Gmapping建图算法
GMapping算法是经典的激光SLAM算法，在移动机器人建图中被广泛使用，它使用粒子滤波器来估计移动机器人在环境中的位置，并同时构建环境的地图。
1、Gmapping算法流程原理
SLAM问题可以描述为，移动机器人从开机到t时刻一系列传感器测量数据 z 1 : t z1:t z1:t（这里当然是/scan）以及一系列控制数据 u 1 : t u1:t u1:t (这里认为是/odom)的条件下，同时对地图 m m m、机器人位姿 x 1 : t x1:t x1:t 进行的估计，显然是一个条件联合概率分布
粒子滤波器是一种基于蒙特卡罗方法的概率滤波器，用于估计随时间变化的状态量。它通过在状态空间中随机采样一组粒子来表示当前状态的不确定性，并通过对这些粒子进行加权来计算状态的后验概率分布。在机器人定位和建图问题中，粒子滤波器通常用于估计机器人在环境中的位姿和地图的特征。
设 u t u_t ut 表示机器人在时间 t t t 到 t + 1 t+1 t+1 之间的运动模型， z t z_t zt 表示机器人在时间 t t t 的传感器测量结果。粒子滤波器通过一组粒子 s t [ 1 : M ] s_t^{[1:M]} st[1:M] 来表示机器人在时间 t t t 的位置和地图特征的不确定性，其中 M M M 表示粒子的数量，每个粒子包含位姿向量 x t [ m ] x_t^{[m]} xt[m] 和地图特征向量 m t [ m ] m_{t}^{[m]} mt[m] 。
在预测步骤中，对于每个粒子 x t [ m ] x_t^{[m]} xt[m]，根据机器人的运动模型进行预测：
x ˉ t + 1 [ m ] = u t ( x t [ m ] ) + ϵ t [ m ] \bar{x}_{t+1}^{[m]} = u_t(x_t^{[m]}) + \epsilon_t^{[m]} xˉt+1[m]=ut(xt[m])+ϵt[m]
其中 x ˉ t + 1 [ m ] \bar{x}_{t+1}^{[m]} xˉt+1[m] 表示预测的下一时刻的位姿状态， u t ( x t [ m ] ) u_t(x_t^{[m]}) ut(xt[m]) 表示机器人根据运动模型预测的状态， ϵ t [ m ] \epsilon_t^{[m]} ϵt[m] 表示预测误差。这里假设预测误差服从零均值高斯分布，即 ϵ t [ m ] ∼ N ( 0 , Σ t ) \epsilon_t^{[m]} \sim \mathcal{N}(0, \Sigma_t) ϵt[m]∼N(0,Σt)，其中 Σ t \Sigma_t Σt 是预测误差的协方差矩阵。在测量更新步骤中，对于每个粒子的位姿 x t + 1 [ m ] x_{t+1}^{[m]} xt+1[m]，计算其权重 w t + 1 [ m ] w_{t+1}^{[m]} wt+1[m]，表示当前粒子的后验概率分布：
w t + 1 [ m ] = P ( z t + 1 ∣ x t + 1 [ m ] , m t [ m ] ) ⋅ P ( x t + 1 [ m ] ∣ x t [ m ] , u t ) w_{t+1}^{[m]} = P(z_{t+1}|x_{t+1}^{[m]}, m_{t}^{[m]}) \cdot P(x_{t+1}^{[m]}|x_t^{[m]}, u_t) wt+1[m]=P(zt+1∣xt+1[m],mt[m])⋅P(xt+1[m]∣xt[m],ut)
其中 P ( z t + 1 ∣ x t + 1 [ m ] , m t [ m ] ) P(z_{t+1}|x_{t+1}^{[m]},m_{t}^{[m]}) P(zt+1∣xt+1[m],mt[m]) 表示传感器测量结果与地图特征的匹配程度， m t [ m ] m_{t}^{[m]} mt[m] 表示当前地图的特征。 P ( x t + 1 [ m ] ∣ x t [ m ] , u t ) P(x_{t+1}^{[m]}|x_t^{[m]}, u_t) P(xt+1[m]∣xt[m],ut) 表示机器人在时间 t t t 到 t + 1 t+1 t+1 之间的运动模型的概率密度函数。
在重采样步骤中，根据权重对粒子进行重采样。具体来说，对于每个粒子 s t [ m ] s_t^{[m]} st[m]，根据其权重 w t [ m ] w_t^{[m]} wt[m] 对其进行抽样，得到一组新的粒子 s t + 1 [ 1 : M ] s_{t+1}^{[1:M]} st+1[1:M]，使得权重较大的粒子被采样的概率更高。
最终，通过对所有粒子进行加权平均，可以得到机器人在时间 t + 1 t+1 t+1 的位置和地图特征的估计值：
s ^ t + 1 = ∑ m = 1 M w t + 1 [ m ] s t + 1 [ m ] \hat{s}_{t+1} = \sum_{m=1}^M w_{t+1}^{[m]} s_{t+1}^{[m]} s^t+1=m=1∑Mwt+1[m]st+1[m]
通过不断迭代上述步骤，粒子滤波器能够逐渐收敛到正确的机器人位姿和地图特征。
2、Gmapping建图实操
首先打开终端，安装对应ROS版本的Gmapping功能包、一个键盘控制包、地图服务包
sudo apt-get install ros-melodic-gmapping
sudo apt-get install ros-melodic-teleop_twist_keyboard
sudo apt install ros-melodic-map-server
gmapping 功能包中的核心节点是:slam_gmapping，该节点订阅的话题、发布的话题、服务以及相关参数详情如下：
2.1订阅的Topic
tf (tf/tfMessage)：用于雷达、底盘与里程计之间的坐标变换消息。
scan(sensor_msgs/LaserScan)：SLAM所需的雷达信息。
2.2发布的Topic
map_metadata(nav_msgs/MapMetaData)：地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。
map(nav_msgs/OccupancyGrid)：地图栅格数据，一般会在rviz中以图形化的方式显示。
~entropy(std_msgs/Float64)：机器人姿态分布熵估计(值越大，不确定性越大)。
2.3服务
dynamic_map(nav_msgs/GetMap)：用于获取地图数据。
2.4参数
~base_frame(string, default:“base_link”)：机器人基坐标系。
~map_frame(string, default:“map”)：地图坐标系。
~odom_frame(string, default:“odom”)：里程计坐标系。
~map_update_interval(float, default: 5.0)：地图更新频率，根据指定的值设计更新间隔。
~maxUrange(float, default: 80.0)：激光探测的最大可用范围(超出此阈值，被截断)。
~maxRange(float)：激光探测的最大范围。
在工作空间下新建一个launch文件夹和launch文件，把下面的launch文件参数配置复制上去
<launch>
<param name="use_sim_time" value="true"/> <!--使用仿真时间-->
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<remap from="scan" to="scan"/>
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->
<param name="map_update_interval" value="5.0"/>
<param name="maxUrange" value="16.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.5"/>
<param name="temporalUpdate" value="3.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="30"/>
<param name="xmin" value="-50.0"/>
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
</node>
<node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" />
</launch>
然后启动Gazebo仿真环境并加载机器人模型，再用roslaunch命令执行该launch文件
roslaunch 包名 launch文件名
在rviz配置栏里面加载RobotModel、Map、LaserScan等插件，并在终端启动键盘控制节点
rosrun teleop_twist_keyboard teleop_twist_keyboard
然后用键盘控制机器人运动并实时建图，得到的栅格地图可以通过map_server节点保存
在终端跑rosrun命令运行map_server节点将栅格地图保存到指定路径下
rosrun map_server map_saver map:=/<Map Topic> -f PATH_TO_YOUR_FILE/mymap
二、AMCL蒙特卡洛定位
1、自适应蒙特卡洛定位算法原理
AMCL（自适应蒙特卡洛定位）是一种基于粒子滤波器的机器人定位方法，其主要目标是根据机器人的传感器数据来估计其在环境中的位姿。
我们的目标是估计机器人在时刻t的位姿，用 x t x_t xt表示，用 u t u_t ut表示从时刻t-1到t的运动模型， z t z_t zt表示时刻t的观测数据。根据贝叶斯定理，可以表示机器人位姿在运动模型和观测数据条件估计为：
p ( x t ∣ z 1 : t , u 1 : t ) = p ( z t ∣ x t , z 1 : t − 1 , u 1 : t ) ⋅ p ( x t ∣ z 1 : t − 1 , u 1 : t ) p ( z t ∣ z 1 : t − 1 , u 1 : t ) p(x_t | z_{1:t}, u_{1:t}) = \frac{p(z_t | x_t, z_{1:t-1}, u_{1:t}) \cdot p(x_t | z_{1:t-1}, u_{1:t})}{p(z_t | z_{1:t-1}, u_{1:t})} p(xt∣z1:t,u1:t)=p(zt∣z1:t−1,u1:t)p(zt∣xt,z1:t−1,u1:t)⋅p(xt∣z1:t−1,u1:t)
为了计算 p ( x t ∣ z 1 : t − 1 , u 1 : t ) p(x_t | z_{1:t-1}, u_{1:t}) p(xt∣z1:t−1,u1:t)，我们需要引入一个条件概率 p ( x t ∣ x t − 1 , u t ) p(x_t | x_{t-1}, u_t) p(xt∣xt−1,ut)表示。它描述了在给定上一个时刻的位姿 x t − 1 x_{t-1} xt−1和当前的运动模型 u t u_t ut下，机器人当前位姿 x t x_t xt的概率分布。应用全概率公式，运动模型可以根据机器人的具体运动特性来表示
p ( x t ∣ z 1 : t − 1 , u 1 : t ) = ∫ p ( x t ∣ x t − 1 , u t ) ⋅ p ( x t − 1 ∣ z 1 : t − 1 , u 1 : t − 1 ) d x t − 1 p(x_t | z_{1:t-1}, u_{1:t}) = \int p(x_t | x_{t-1}, u_t) \cdot p(x_{t-1} | z_{1:t-1}, u_{1:t-1}) dx_{t-1} p(xt∣z1:t−1,u1:t)=∫p(xt∣xt−1,ut)⋅p(xt−1∣z1:t−1,u1:t−1)dxt−1
为了计算 p ( z t ∣ x t , z 1 : t − 1 , u 1 : t ) p(z_t | x_t, z_{1:t-1}, u_{1:t}) p(zt∣xt,z1:t−1,u1:t)，我们需要引入一个观测模型，用 p ( z t ∣ x t ) p(z_t | x_t) p(zt∣xt)表示。这个模型描述了在给定当前位置 x t x_t xt下，观测数据 z t z_t zt的概率分布。
蒙特卡洛定位方法采用粒子滤波器（Particle Filter）来近似表示和计算上述概率分布。我们使用一组粒子 X t = x t ( 1 ) , x t ( 2 ) , ⋯ , x t ( M ) X_t = {x_t^{(1)}, x_t^{(2)}, \cdots, x_t^{(M)}} Xt=xt(1),xt(2),⋯,xt(M)来表示时刻t的位姿分布， M M M是粒子的数量。
根据运动模型，从时刻t-1的粒子集合 X t − 1 X_{t-1} Xt−1和运动模型 u t u_t ut预测时刻t的粒子集合 X t X_t Xt，对于每个粒子 x t − 1 ( i ) x_{t-1}^{(i)} xt−1(i)，我们可以根据运动模型 p ( x t ∣ x t − 1 ( i ) , u t ) p(x_t | x_{t-1}^{(i)}, u_t) p(xt∣xt−1(i),ut)采样获得一个新的粒子 x t ( i ) x_t^{(i)} xt(i)。
根据观测模型，计算每个新粒子 x t ( i ) x_t^{(i)} xt(i)的权重 w t ( i ) w_t^{(i)} wt(i)，对于每个粒子 x t ( i ) x_t^{(i)} xt(i)，我们可以计算观测数据 z t z_t zt的似然 p ( z t ∣ x t ( i ) ) p(z_t | x_t^{(i)}) p(zt∣xt(i))，然后将其作为粒子的权重。根据新粒子的权重 w t ( i ) w_t^{(i)} wt(i)进行重采样，生成一个新的粒子集合 X t X_t Xt。在这个过程中，权重较大的粒子有更高的概率被多次采样，而权重较小的粒子可能被丢弃。这个过程有助于将粒子集合集中在高概率区域，从而提高定位精度。
AMCL的一个关键特性是自适应地调整重采样频率，根据粒子权重的有效样本大小（Effective Sample Size，ESS）来自适应调整，ESS计算如下：
N e f f = 1 ∑ i = 1 M w t ( i ) 2 N_{eff} = \frac{1}{\sum_{i=1}^{M} {w_t^{(i)}}^2} Neff=∑i=1Mwt(i)21
当 N e f f N_{eff} Neff低于一个阈值（例如总粒子数的一半）时，执行重采样步骤。这样可以防止过早重采样导致的粒子集退化，并减少计算开销。AMCL蒙特卡洛定位方法通过粒子滤波器和自适应重采样策略对机器人位置进行估计，这种方法在机器人定位问题中表现出了较好的鲁棒性和实时性。
2、AMCL定位实操
首先打开终端，安装对应ROS版本的导航功能包，该功能包包含了amcl定位功能
sudo apt-get install ros-melodic-navigation
amcl 功能包中的核心节点是:amcl，该节点订阅的话题、发布的话题、服务以及相关参数详情如下：
3.1订阅的Topic
scan(sensor_msgs/LaserScan)：激光雷达数据。
tf(tf/tfMessage)：坐标变换消息。
initialpose(geometry_msgs/PoseWithCovarianceStamped)：用来初始化粒子滤波器的均值和协方差。
map(nav_msgs/OccupancyGrid)：获取地图数据。
3.2发布的Topic
amcl_pose(geometry_msgs/PoseWithCovarianceStamped)：机器人在地图中的位姿估计。
particlecloud(geometry_msgs/PoseArray)：位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。
tf(tf/tfMessage)：发布从 odom 到 map 的转换。
3.3服务
global_localization(std_srvs/Empty)：初始化全局定位的服务。
request_nomotion_update(std_srvs/Empty)：手动执行更新和发布更新的粒子的服务。
set_map(nav_msgs/SetMap)：手动设置新地图和姿态的服务。
static_map(nav_msgs/GetMap)：调用此服务获取地图数据。
3.5参数
~odom_model_type(string, default:“diff”)：里程计模型选择: “diff”,“omni”,“diff-corrected”,“omni-corrected” (diff 差速、omni 全向轮)
~odom_frame_id(string, default:“odom”)：里程计坐标系。
~base_frame_id(string, default:“base_link”)：机器人极坐标系。
~global_frame_id(string, default:“map”)：地图坐标系。
在工作空间下的launch文件夹下新建launch文件，把下面的launch文件参数配置复制上去
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/><!-- 里程计模式为差分 -->
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/><!-- 里程计坐标系 -->
<param name="base_frame_id" value="base_footprint"/><!-- 添加机器人基坐标系 -->
<param name="global_frame_id" value="map"/><!-- 添加地图坐标系 -->
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>
再新建一个launch文件作为运行脚本，里面include一下上面amcl的launch配置文件
<launch>
<!-- 设置地图的配置文件 -->
<arg name="map" default="nav.yaml" />
<!-- 运行地图服务器，并且加载设置的地图-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
<!-- include一下上面amcl的launch配置文件，启动AMCL节点 -->
<include file="$(find mycar_nav)/launch/amcl.launch" />
<!-- 运行rviz -->
<node pkg="rviz" type="rviz" name="rviz"/>
</launch>
然后启动Gazebo仿真环境并加载机器人模型，再用roslaunch命令执行该launch文件
roslaunch 包名 launch文件名
在rviz配置栏里面加载RobotModel、Map、PoseArray等插件，并在终端启动键盘控制节点
rosrun teleop_twist_keyboard teleop_twist_keyboard
然后用键盘控制机器人运动，观察到机器人周围一圈的红色箭头代表对机器人的实时定位估计，箭头越密集的区域机器人出现的可能性越高
三、move_base路径规划
move_base 是一个用于移动底盘的功能包，提供了一种方式让机器人在地图中安全地导航到目标位置。move_base 将全局路径规划器和局部路径规划器结合起来，形成一个完整的导航框架。
1、路径规划算法简介
move_base 使用多种路径规划和导航算法，包括全局路径规划和局部路径规划。全局路径规划主要用于在已知地图中找到从机器人当前位置到目标位置的最优路径，局部路径规划主要用于根据全局规划的路径实时调整机器人的运动，使其能够避免在地图中未知的障碍物。
常用的全局路径规划算法有：
A 算法：A* 是一种启发式搜索算法，结合了最优性（Dijkstra 算法）和启发式信息（Greedy Best-First-Search 算法）来寻找最短路径。A* 算法的关键思想是在每一步都估计从起始点到目标点的最小代价，并在搜索树中选择具有最小估计代价的节点进行扩展。
Dijkstra 算法：Dijkstra 算法也是一种搜索算法，用于找到从源节点到图中所有其他节点的最短路径。它在每个步骤中选择当前未处理节点中具有最小距离值的节点，并根据这个节点的邻居节点更新距离值。
RRT (Rapidly-exploring Random Trees) 算法：RRT 是一种基于随机采样的增量式路径规划算法。它以树的形式在搜索空间中快速扩展，直到找到一条从起点到目标点的路径。RRT 算法适用于处理高维度和复杂的环境。
局部路径规划算法有：
DWA (Dynamic Window Approach)：DWA 是一种基于速度空间搜索的局部路径规划算法。它在机器人的速度空间中搜索一组可行的速度，然后根据全局路径和局部障碍物选择最优速度来控制机器人的运动。
TEB (Timed Elastic Band)：TEB 是一种基于优化的局部路径规划算法。它将全局路径表示为一系列时间相关的配置（位置和速度），并将其视为一个弹性带。通过优化弹性带的形状，可以在满足动态约束的同时避免局部障碍物。
2、代价地图简介
代价地图（Costmap）是在ROS中进行机器人路径规划和障碍物避难时常用的一种地图类型，它表示了机器人在不同位置和方向上的行进代价。代价地图通常是基于栅格（Grid）表示的，每个栅格表示地图上的一个小区域，其值表示在该区域内行进的代价大小。
move_base中的代价地图可以通过以下几种方式构建：
staticLayer静态地图层：静态地图是在机器人运行前由激光雷达或深度相机等传感器采集到的地图，可以通过ROS中的map_server软件包进行发布。move_base可以直接订阅静态地图消息，将其转换为代价地图，静态地图层只是简单地将灰度图中的像素值换算成ros代价地图中的代价值
obstacleLayer障碍地图层：在机器人开始移动之前，move_base会添加一个障碍物地图层，这个层由机器人周围的障碍物构成，如人、车辆、家具等，这个层会随着机器人的移动根据激光雷达等传感器实时更新，以确保机器人能够避免碰撞
inflationLayer膨胀层：move_base会在障碍物地图层的基础上添加一个膨胀层，这个层可以将机器人周围的障碍物膨胀成一个安全距离，以便代价地图更清晰显示机器人的可利用空间，避免机器人的外壳或者突出误撞上障碍物
move_base中的依据代价地图进行代价计算方式：
由上面的代价曲线图可以得知，横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值，在参数配置时，需要指定代价地图中各个栅格的代价值计算公式，以及障碍物、斜率和其他因素的权重，以保证代价地图的准确性和适应性
（1）致命障碍:栅格值为254；
（2）内切障碍:栅格值为253；
（3）外切障碍:栅格值为[128,252]；
（4）非自由空间:栅格值为(0,127]；
（5）自由区域:栅格值为0；
（6）未知区域:栅格值为255；
2、move_base路径规划实操
move_base功能包中的核心节点是:move_base，该节点的action、订阅的话题、发布的话题、服务以及相关参数详情如下：
2.1动作
move_base/goal(move_base_msgs/MoveBaseActionGoal)：move_base 的运动规划目标。
move_base/cancel(actionlib_msgs/GoalID)：取消目标。
move_base/feedback(move_base_msgs/MoveBaseActionFeedback)：连续反馈的信息，包含机器人底盘坐标。
move_base/status(actionlib_msgs/GoalStatusArray)：发送到move_base的目标状态信息。
move_base/result(move_base_msgs/MoveBaseActionResult)
2.2订阅的Topic
move_base_simple/goal(geometry_msgs/PoseStamped)：运动规划目标(与action相比，没有连续反馈，无法追踪机器人执行状态)。
2.3发布的Topic
cmd_vel(geometry_msgs/Twist)：输出到机器人底盘的运动控制消息。
2.4服务
~make_plan(nav_msgs/GetPlan)：请求该服务，可以获取给定目标的规划路径，但是并不执行该路径规划。
~clear_unknown_space(std_srvs/Empty)：允许用户直接清除机器人周围的未知空间。
~clear_costmaps(std_srvs/Empty)：允许清除代价地图中的障碍物，可能会导致机器人与障碍物碰撞，请慎用。
在功能包下新建 param 目录，在 param 目录下新建costmap_common_params.yaml、local_costmap_params.yaml、global_costmap_params.yaml、base_local_planner_params.yaml这几个代价地图配置文件
costmap_common_params.yaml
#机器人几何参数，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状
obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物
#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0
#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}
global_costmap_params.yaml
global_costmap:
global_frame: map #地图坐标系
robot_base_frame: base_footprint #机器人坐标系
# 以此实现坐标变换
update_frequency: 1.0 #代价地图更新频率
publish_frequency: 1.0 #代价地图的发布频率
transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间
static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.
local_costmap_params.yaml
local_costmap:
global_frame: map #地图坐标系
robot_base_frame: base_footprint #机器人坐标系
update_frequency: 10.0 #代价地图更新频率
publish_frequency: 10.0 #代价地图的发布频率
transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间
static_map: false #不需要静态地图，可以提升导航效果
rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
width: 3 # 局部地图宽度 单位是 m
height: 3 # 局部地图高度 单位是 m
resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致
base_local_planner_params
TrajectoryPlannerROS:
# Robot Configuration Parameters
max_vel_x: 0.5 # X 方向最大速度
min_vel_x: 0.1 # X 方向最小速速
max_vel_theta: 1.0 #
min_vel_theta: -1.0
min_in_place_vel_theta: 1.0
acc_lim_x: 1.0 # X 加速限制
acc_lim_y: 0.0 # Y 加速限制
acc_lim_theta: 0.6 # 角速度加速限制
# Goal Tolerance Parameters，目标公差
xy_goal_tolerance: 0.10
yaw_goal_tolerance: 0.05
# Differential-drive robot configuration
# 是否是全向移动机器人
holonomic_robot: false
# Forward Simulation Parameters，前进模拟参数
sim_time: 0.8
vx_samples: 18
vtheta_samples: 20
sim_granularity: 0.05
接着编写launch文件，配置好move_base节点、amcl定位节点和地图服务节点
<launch>
<!-- 设置地图的配置文件 -->
<arg name="map" default="nav.yaml" />
<!-- 运行地图服务器，并且加载设置的地图-->
<node name="map_server" pkg="map_server" type="map_server" args="$(find mycar_nav)/map/$(arg map)"/>
<!-- 启动AMCL节点 -->
<include file="$(find mycar_nav)/launch/amcl.launch" />
<!-- 运行move_base节点 -->
<include file="$(find mycar_nav)/launch/path.launch" />
<!-- 运行rviz -->
<node pkg="rviz" type="rviz" name="rviz" />
</launch>
然后启动Gazebo仿真环境并加载机器人模型，再用roslaunch命令执行该launch文件
roslaunch 包名 launch文件名
在rviz配置栏里面加载RobotModel、Map、Odometry、Path等插件，在工具栏的 2D Nav Goal设置目的地实现路径规划
总结
以上就是SLAM之Gmapping建图与路径规划学习笔记的全部内容。Gmapping算法能够通过对机器人传感器数据的实时分析和处理，构建出高精度的环境地图，并且能够实现环境地图的实时更新，为路径规划提供了重要的基础。应用路径规划算法，帮助机器人在环境中寻找最短路径，并且能够实时更新路径规划结果以适应环境变化。