ROS 导航

坐标系

里程计定位

• 优点：里程计定位信息是连续的，没有离散的跳跃(无跳变)
• 缺点：里程计存在累计误差，不利于长距离或长期定位
• 产生误差原因：路面不平、测速不准、车轮打滑；长距离，长时间运行会导致误差累计

传感器定位

• 优点：比里程计定位更精准
• 缺点：传感器定位会出现跳变的情况，传感器定位在标志物较少的环境下，其定位精度会大打折扣(特征点较少时，定位精度降低)

两种定位方式优缺点互补，应用时一般二者结合使用。

slam_mapping 建图 nav01_slam.launch

启动
source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch
roslaunch nav_demo nav1_slam.launch
使用键盘控制小车运动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

<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 = "map_frame" value = "map" />
      <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>

map_server 地图服务

map_saver 保存地图 nav02_map_save.launch

launch文件

<launch>
    <arg name="filename" value="$(find nav_demo)/map/nav" />
    <node name="map_save" pkg="map_server" type="map_saver" args="-f $(arg filename)" />
launch>

map_server 地图读取 nav03_map_server.launch

launch文件

<launch>
    
    <arg name="map" default="nav.yaml" />
    
    <node pkg="map_server" type="map_server" name="map_server" args="$(find nav_demo)/map/$(arg map)"/>
launch>

nav.yaml文件

# 1.声明地图图片资源的路径
image: /home/rosnoetic/demo02_ws/src/nav_demo/map/nav.pgm
# 2.地图刻度尺单位是 米/像素 一个像素对应地图0.05m
resolution: 0.050000
# 3.地图的位姿信息(按照右手坐标系(x正轴朝上,y轴正向朝左)，地图右下角相对于rviz中的原点的位姿)
# 值1：x方向的偏移量
# 值2：y方向的偏移量
# 值3：地图的偏航角度(单位是弧度)
origin: [-50.000000, -50.000000, 0.000000]

# 地图中的障碍物判读:
# 最终地图结果: 白色是可通行区域，黑色是障碍物，蓝灰是位置区域
# 判断规则：
# 1.地图中的每一个像素取值在 [0,255] 之间，白色为 255，黑色为 0，该值设为 x
# 2.根据像素值计算一个比例，p = (255-x)/255 白色为0 黑色为1 灰色介于 0 到 1 之间
# 3.判读是否是障碍物 p > occupied_thresh 就是障碍物， p < free_thresh 就是无物，可以自由通行

# 4.占用阈值
occupied_thresh: 0.65
# 5.空闲阈值 二者一起判断地图像素是否是障碍物
free_thresh: 0.196

# 6.是否取反
negate: 0

amcl定位

ctrl+shift+i：可视化

roscd amcl
ls
ls examples
gedit examples/amcl_diff.launch

launch文件 nav04_amcl.launch

<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha5" value="0.1"/>
        <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"/>
        
        <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_model_type" value="likelihood_field"/>
        
        <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="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>

测试文件 test_amcl.launch

<launch>
    
    <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" args = "-d $(find nav_demo)/config/nav.rviz"/>

    
    <include file = "$(find nav_demo)/launch/nav03_map_server.launch" />

    
    <include file = "$(find nav_demo)/launch/nav04_amcl.launch" />

launch>

source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launch
roslaunch nav_demo test_amcl.launch
新开窗口
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

路径规划

代价地图

代价地图组成(多层叠加)

1. 静态层——SLAM绘制的静态地图
2. 障碍物层——导航中传感器感知的障碍物
3. 膨胀层——为了避免碰撞而设置的安全区域
4. 自定义层——根据业务自设置的地图数据(比如虚拟墙)

碰撞算法

横轴是距离机器人中心的距离，纵轴是代价地图中栅格的灰度值。

• 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞
• 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞
• 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞
• 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞
• 自由区域:栅格值为0，此处机器人可以自由通过
• 未知区域:栅格值为255，还没探明是否有障碍物

move_base的使用

launch文件 demo5_path.launch

<launch>
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_demo)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_demo)/param/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_demo)/param/base_local_planner_params.yaml" command="load" />
    node>
launch>

配置文件1 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}

配置文件2 local_costmap_params.yaml

local_costmap:
  global_frame: odom #里程计坐标系
  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，一般与静态地图分辨率保持一致

配置文件3 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.

配置文件4 base_local_planner_params.yaml

运动控制相关

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文件集成 nav06_path.launch

<launch>
    
    <include file = "$(find nav_demo)/launch/nav03_map_server.launch" />

    
    <include file = "$(find nav_demo)/launch/nav04_amcl.launch" />

    
    <include file = "$(find nav_demo)/launch/nav05_path.launch" />

    
    <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>

source ./devel/setup.bash
roslaunch urdf02_gazebo demo03_env.launchslaunch
roslaunch nav_demo nav06_test.launch

导航与slam建图 nav07_slam_auto.launch

<launch>
    
    <include file = "$(find nav_demo)/launch/nav01_slam_launch" />
    
    <include file = "$(find nav_demo)/launch/nav05_path_launch" />
launch>

source ./devel/setup.bash
启动仿真环境
roslaunch urdf02_gazebo demo03_env.launch
启动launch文件
roslaunch nav_demo nav07_slam_auto.launch