ROS—机器人导航实现

1 准备工作

1.1 分布式架构

需要完成分布式框架的搭建并且能正常运行，在PC端可以远程登录机器人端。

1.2 功能包安装

在机器人端安装导航所需功能包：

• 安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping
• 安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server
• 安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

sudo apt install ros-melodic-gmapping
sudo apt install ros-melodic-map-server
sudo apt install ros-melodic-navigation

安装完毕我们验证是否安装成功：

rospack find gmapping
rospack find map_server
rospack find amcl
rospack find move_base

新建导航功能包（本文名为nav），并导入依赖。

catkin_create_pkg nav roscpp rospy std_msgs gmapping map_server amcl move_base

1.3 机器人模型以及坐标变换

机器人的不同部件有不同的坐标系，我们需要将这些坐标系集成进同一坐标树，实现方案有两种：

1. 不同的部件相对于机器人底盘其位置都是固定的，可以通过发布静态坐标变换以实现集成；
2. 可以通过加载机器人URDF文件结合 robot_state_publisher、joint_state_publisher实现不同坐标系的集成。

方案1在传感器集成中已做演示，接下来介绍方案2的实现。

1.3.1 创建机器人模型相关的功能包

创建功能包：

catkin_create_pkg mycar_description urdf xacro

1.3.2 准备机器人模型文件

在功能包下新建 urdf 目录，编写具体的 urdf 文件。

文件car.urdf.xacro用于集成不同的机器人部件，内容如下：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:include filename="car_base.urdf.xacro" />
    <xacro:include filename="car_laser.urdf.xacro" />

</robot>

文件car_base.urdf.xacro机器人底盘实现，内容如下：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="footprint_radius" value="0.001" />
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${footprint_radius}" />
            </geometry>
        </visual>
    </link>


    <xacro:property name="base_radius" value="0.1" />
    <xacro:property name="base_length" value="0.08" />
    <xacro:property name="lidi" value="0.015" />
    <xacro:property name="base_joint_z" value="${base_length / 2 + lidi}" />
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="0.1" length="0.08" />
            </geometry>

            <origin xyz="0 0 0" rpy="0 0 0" />

            <material name="baselink_color">
                <color rgba="1.0 0.5 0.2 0.5" />
            </material>
        </visual>

    </link>

    <joint name="link2footprint" type="fixed">
        <parent link="base_footprint"  />
        <child link="base_link" />
        <origin xyz="0 0 0.055" rpy="0 0 0" />
    </joint>



    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />
    <xacro:property name="PI" value="3.1415927" />
    <xacro:property name="wheel_joint_z" value="${(base_length / 2 + lidi - wheel_radius) * -1}" />


    <xacro:macro name="wheel_func" params="wheel_name flag">

        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}" />
                </geometry>

                <origin xyz="0 0 0" rpy="${PI / 2} 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>

        </link>

        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${wheel_name}_wheel" />

            <origin xyz="0 ${0.1 * flag} ${wheel_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro>

    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />



    <xacro:property name="small_wheel_radius" value="0.0075" />
    <xacro:property name="small_joint_z" value="${(base_length / 2 + lidi - small_wheel_radius) * -1}" />

    <xacro:macro name="small_wheel_func" params="small_wheel_name flag">
        <link name="${small_wheel_name}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${small_wheel_radius}" />
                </geometry>

                <origin xyz="0 0 0" rpy="0 0 0" />

                <material name="wheel_color">
                    <color rgba="0 0 0 0.3" />
                </material>
            </visual>

        </link>

        <joint name="${small_wheel_name}2link" type="continuous">
            <parent link="base_link"  />
            <child link="${small_wheel_name}_wheel" />

            <origin xyz="${0.08 * flag} 0 ${small_joint_z}" rpy="0 0 0" />
            <axis xyz="0 1 0" />
        </joint>

    </xacro:macro >
    <xacro:small_wheel_func small_wheel_name="front" flag="1"/>
    <xacro:small_wheel_func small_wheel_name="back" flag="-1"/>

</robot>

文件car_laser.urdf.xacro机器人雷达实现，内容如下：

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">

    <xacro:property name="support_radius" value="0.01" />
    <xacro:property name="support_length" value="0.15" />

    <xacro:property name="laser_radius" value="0.03" />
    <xacro:property name="laser_length" value="0.05" />

    <xacro:property name="joint_support_x" value="0" />
    <xacro:property name="joint_support_y" value="0" />
    <xacro:property name="joint_support_z" value="${base_length / 2 + support_length / 2}" />

    <xacro:property name="joint_laser_x" value="0" />
    <xacro:property name="joint_laser_y" value="0" />
    <xacro:property name="joint_laser_z" value="${support_length / 2 + laser_length / 2}" />

    <link name="support">
        <visual>
            <geometry>
                <cylinder radius="${support_radius}" length="${support_length}" />
            </geometry>
            <material name="yellow">
                <color rgba="0.8 0.5 0.0 0.5" />
            </material>
        </visual>

    </link>

    <joint name="support2base" type="fixed">
        <parent link="base_link" />
        <child link="support"/>
        <origin xyz="${joint_support_x} ${joint_support_y} ${joint_support_z}" rpy="0 0 0" />
    </joint>
    <link name="laser">
        <visual>
            <geometry>
                <cylinder radius="${laser_radius}" length="${laser_length}" />
            </geometry>
            <material name="black">
                <color rgba="0 0 0 0.5" />
            </material>
        </visual>

    </link>

    <joint name="laser2support" type="fixed">
        <parent link="support" />
        <child link="laser"/>
        <origin xyz="${joint_laser_x} ${joint_laser_y} ${joint_laser_z}" rpy="0 0 0" />
    </joint>
</robot>

1.3.3 在launch文件加载机器人模型

launch 文件（文件名称自定义，比如：car.launch）内容示例如下：

<launch>
    <param name="robot_description" command="$(find xacro)/xacro $(find mycar_description)/urdf/car.urdf.xacro" />
    <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" />
</launch>

为了使用方便，还可以将该文件包含进启动机器人的launch文件中，示例如下：

<launch>
        <include file="$(find ros_arduino_python)/launch/arduino.launch" />
        <include file="$(find rplidar_ros)/launch/rplidar_a1.launch" />
        <!-- 机器人模型加载文件 -->
        <include file="$(find mycar_description)/launch/car.launch" />
</launch>

1.4 结果演示

2 SLAM建图

2.1 编写gmapping节点相关launch文件

在创建的导航功能包中新建launch目录，并新建launch文件（本文名为gmapping.launch），代码示例如下:

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

关键代码解释：

<remap from="scan" to="scan"/><!-- 雷达话题 -->
<param name="base_frame" value="base_footprint"/><!--底盘坐标系-->
<param name="odom_frame" value="odom"/> <!--里程计坐标系-->

2.2 执行

1. 执行相关launch文件，启动机器人并加载机器人模型：roslaunch mycar_start start.launch
2. 启动地图绘制的 launch 文件：roslaunch nav gmapping.launch
3. 启动键盘键盘控制节点，用于控制机器人运动建图：rosrun teleop_twist_keyboard teleop_twist_keyboard.py
4. 在 rviz 中添加地图显示组件，通过键盘控制机器人运动。同时，在rviz中可以显示gmapping发布的栅格地图数据了。
   

3 地图服务

3.1 地图保存launch文件

  首先在自定义的导航功能包下新建 map 目录，用于保存生成的地图数据。地图保存的语法比较简单，编写一个launch文件(本文名为map_save.launch)，内容如下:

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

测试：
运行launch文件：

roslaunch nav map_save.launch

可以看到在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml。

3.2 地图读取

通过 map_server 的 map_server 节点可以读取栅格地图数据，编写 launch 文件(本文名为map_server.launch)如下:

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/>
</launch>

  其中参数是地图描述文件的资源路径，执行该launch文件，该节点会发布话 题:map(nav_msgs/OccupancyGrid)，最后，在 rviz 中使用 map 组件可以显示栅格地图。

4 定位

在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。

4.1 编写amcl节点相关的launch文件

在功能包nav下编写launch文件（本文名为：amcl.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"/><!-- 添加地图坐标系 -->

  </node>
</launch>

4.2 编写测试launch文件

  amcl节点是不可以单独运行的，运行 amcl 节点之前，需要先加载全局地图，然后启动 rviz 显示定位结果，上述节点可以集成进launch文件（本文名为test_amcl.launch），内容示例如下:

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find nav)/launch/amcl.launch" />
</launch>

launch文件中地图服务节点和amcl节点中的包名、文件名需要根据自己的设置修改。

4.3 执行

1. 执行相关launch文件，启动机器人并加载机器人模型：roslaunch mycar_start start.launch。
2. 启动键盘控制节点：rosrun teleop_twist_keyboard teleop_twist_keyboard.py。
3. 启动上一步中集成地图服务、amcl 的 launch 文件：roslaunch nav test_amcl.launch。
4. 启动rviz并添加RobotModel、Map组件，分别显示机器人模型与地图，添加 posearray 插件，设置topic为particlecloud来显示 amcl 预估的当前机器人的位姿，箭头越是密集，说明当前机器人处于此位置的概率越高；
5. 通过键盘控制机器人运动，会发现 posearray 也随之而改变。

5 路径规划

路径规划使用 navigation 功能包中的 move_base 功能包。

5.1 编写launch文件

在功能包nav中编写launch文件（本文名为move_base.launch）。

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

5.2 编写配置文件

5.2.1 costmap_common_params.yaml

  该文件是 move_base 在全局路径规划与本地路径规划时调用的通用参数，包括:机器人的尺寸、距离障碍物的安全距离、传感器信息等。配置参考如下:

#机器人几何参，如果机器人是圆形，设置 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}

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

5.2.3 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，一般与静态地图分辨率保持一致

5.2.4 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

5.3 launch文件集成

若要实现导航，需要集成地图服务、amcl 、move_base 等，集成示例如下:

<launch>
    <!-- 设置地图的配置文件 -->
    <arg name="map" default="nav.yaml" />
    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav)/map/$(arg map)"/>
    <!-- 启动AMCL节点 -->
    <include file="$(find nav)/launch/amcl.launch" />
    <!-- 运行move_base节点 -->
    <include file="$(find nav)/launch/move_base.launch" />
</launch>

5.4 导航测试

1. 执行相关launch文件，启动机器人并加载机器人模型：roslaunch mycar_start start.launch；
2. 启动导航相关的 launch 文件：roslaunch nav nav.launch；
3. 添加Rviz组件实现导航。