移动机器人激光SLAM导航（二）：运动控制与传感器篇

参考引用

• 机器人工匠阿杰
• wpr_simulation

移动机器人激光SLAM导航（文章链接汇总）

1. 机器人运动控制

1.1 测试环境安装

• wpr_simulation 安装

  $ mkdir -p catkin_ws/src
  $ cd catkin_ws/src
  $ git clone https://github.com/6-robot/wpr_simulation.git
  $ cd wpr_simulation/scripts/
  $ ./install_for_melodic.sh  # 自动下载和安装编译需要的依赖项
  $ cd ~/catkin_ws
  $ catkin_make
  

1.2 控制量纲与消息包

• 矢量运动速度量纲为 m/s，旋转运动速度量纲为 rad/s，如：3.14 代表 1s 转半圈 180°
• x、y、z 方向通过右手定则确定：食指指向正前方（+x），中指指向正左方（+y），大拇指指向正上方（+z）
  

• 速度控制消息包：geometry_msgs/Twist
  

• 消息包具体定义：linear & angular
  

1.3 控制 x 方向速度 /cmd_vel

• 构建一个新的功能包 vel_pkg，并在 vel_pkg 功能包的 src 下新建 vel_node.cpp 文件

  $ cd catkin_ws/src
  $ catkin_create_pkg vel_pkg roscpp rospy geometry_msgs
  

  #include 
  #include 
  
  int main(int argc, char *argv[]) {
      ros::init(argc, argv, "vel_node");
  
      ros::NodeHandle n;
      // 向 ROS 大管家 NodeHandle 申请发布话题 /cmd_vel 并拿到发布对象 vel_pub
      ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
      // 构建一个 geometry_msgs/Twist 类型的消息包 vel_msg，用来承载要发送的速度值
      geometry_msgs::Twist vel_msg;
      vel_msg.linear.x = 0.1;
      vel_msg.linear.y = 0.0;
      vel_msg.linear.z = 0.0;
      vel_msg.angular.x = 0;
      vel_msg.angular.y = 0;
      vel_msg.angular.z = 0;
  
      // 开启一个 while 循环，不停的使用 vel_pub 对象发送速度消息包 vel_msg
      ros::Rate r(30);
      while (ros::ok()) {
          vel_pub.publish(vel_msg);
          r.sleep();
      }
      
      return 0;
  }
  // 修改 CMakeLists.txt 文件此处略
  // 最后到 catkin_ws 目录下编译
  

• 测试运行

  $ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
  $ roslaunch wpr_simulation wpb_simple.launch
  $ rosrun vel_pkg vel_node
  

2. 使用 Rviz 观测传感器数据

• Gazebo 是模拟真实机器人发出传感器数据的工具（虚拟的环境），现实中不存在并被真实的实体机器人和真实环境所代替
• Rviz 是接收传感器数据并进行显示的工具（真实的环境），只有需要观察某些数据实时变化的时候才用到

  $ cd ~/catkin_ws/
  $ roslaunch wpr_simulation wpb_simple.launch
  $ roslaunch wpr_simulation wpb_rviz.launch
  

3. 激光雷达 /scan

3.1 消息包类型：sensor_msgs/LaserScan

# 1 / scan_time = LiDAR 扫描频率 
# --noarr 防止数组消息刷屏
$ rostopic echo /scan --noarr

上图中超过雷达测距范围的点数值变成 INF

3.2 获取激光雷达数据节点

• 构建一个新的功能包，包名叫做 lidar_pkg，并在软件包中新建一个节点，节点名叫做 lidar_node

  $ cd catkin_ws/src
  $ catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
  

  #include 
  #include 
  
  // 构建回调函数 LidarCallback()，用来接收和处理雷达数据
  void LidarCallback(const sensor_msgs::LaserScan msg) {
      float fMidDist = msg.ranges[180];
      // 调用 ROS_INFO() 打印雷达检测到的前方障碍物距离
      ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);
  }
  
  int main(int argc, char *argv[]) {
      setlocale(LC_ALL, "");
      ros::init(argc, argv, "lidar_node");
  
      // 向 ROS 大管家 NodeHandle 申请订阅话题 /scan，并设置回调函数为 LidarCallback()
      ros::NodeHandle n;
      ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
  
      ros::spin();
  
      return 0;
  }
  // 修改 CMakeLists.txt 文件此处略
  // 最后到 catkin_ws 目录下编译
  

3.3 激光雷达避障节点

// 在 3.2 小节基础上修改
#include 
#include 
#include 

ros::Publisher vel_pub;  // 全局变量，保证回调函数也能使用
int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg) {
    float fMidDist = msg.ranges[180];
    ROS_INFO("qianfangceju ranges[180] = %f m", fMidDist);

    if(nCount > 0) {
        nCount--;
        return;
    }

    // 构建速度控制消息包 vel_cmd
    geometry_msgs::Twist vel_cmd; 
    // 根据激光雷达的测距数值，实时调整机器人运动速度，从而避开障碍物
    if (fMidDist < 1.5) {
        vel_cmd.angular.z = 0.3;
        nCount = 40;  // 确保机器人旋转的角度足够避开障碍物
    } else {
        vel_cmd.linear.x = 0.1;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char *argv[]) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "lidar_node");

    // 让大管家 NodeHandle 发布速度控制话题 /cmd_vel
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10); // 发布速度控制话题 /cmd_vel

    ros::spin();

    return 0;
}

4. IMU

4.1 消息包类型 sensor_msgs/Imu

4.2 获取 IMU 数据节点

• ROS 官方 IMU 话题种类标准
  

• 构建一个新的软件包叫做 imu_pkg，在软件包中新建一个节点叫做 imu_node.cpp

  $ cd catkin_ws/src
  $ catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
  

  #include 
  #include 
  #include 
  
  void IMUCallback(sensor_msgs::Imu msg) {
      // 先对四元数 orientation 的协方差矩阵第一个数值进行判断，如果小于0
      // 说明四元数的值不存在，具体查看sensor_msgs/Imu Message说明
      if(msg.orientation_covariance[0] < 0) {
          return;
      }
      // 使用 tf 工具将四元数转换成 tf 四元数对象
      tf::Quaternion quaternion ( 
          msg.orientation.x,
          msg.orientation.y,
          msg.orientation.z,
          msg.orientation.w
      );
      double roll, pitch, yaw; // 用来装载转换后的欧拉角结果
      // 先将 quaternion 转换成一个 tf 的 3×3 矩阵对象，然后调用 getRPY 转换成欧拉角
      tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
      // 将弧度值转换为角度值
      roll = roll * 180 / M_PI;
      pitch = pitch * 180 / M_PI;
      yaw = yaw * 180 / M_PI;
  
      // 调用 ROS_INFO() 显示转换后的欧拉角数值
      ROS_INFO("roll = %.0f pitch = %.0f yaw = %.0f", roll, pitch, yaw);
  }
  
  int main(int argc, char *argv[]) {
      setlocale(LC_ALL, "");
      ros::init(argc, argv, "imu_node");
  
      // 在节点中向 ROS 大管家 NodeHandle 申请订阅话题 /imu/data，并设置回调函数为 IMUCallback()
      ros::NodeHandle n;
      ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);
  
      ros::spin();
  
      return 0;
  }
  // 修改 CMakeLists.txt 文件此处略
  // 到 catkin_ws 目录下编译 
  

4.3 IMU 航向锁定节点

// 在 4.2 小节基础上修改
#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;

// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg) {
    // 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转 = %.0f 俯仰 = %.0f 朝向 = %.0f", roll, pitch, yaw);
    // 速度消息包
    geometry_msgs::Twist vel_cmd;

    // 设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角
    double target_yaw = 90;  // 目标朝向角
    // 计算速度
    double diff_angle = target_yaw - yaw;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x = 0.1;
    vel_pub.publish(vel_cmd);
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "imu_node"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 让 ROS 大管家 NodeHandle 发布速度控制话题 /cmd_vel
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    
    ros::spin();
    return 0;
}

5. ROS 常用消息包

5.1 标准消息包 std_msgs

5.2 几何消息包 geometry_msgs

其中一些消息包带了 stamped 关键词，这些消息包都是多了一个 Header，也就是多了时间戳信息和坐标系（frame） ID，将空间量和时间量进行了绑定，多用于一些预测和滤波算法中

5.3 传感器消息包 sensor_msgs

6. 占据栅格地图

导航所使用的地图数据，就是 ROS 导航软件包里的 map_server 节点在话题 /map 中发布的消息数据，消息类型是 nav_msgs 消息包的 OccupancyGrid（占据栅格）地图，就是一种正方形小格子组成的地图，每个格子里填入一个数值，表示障碍物占据情况

6.1 占据栅格地图形象解释

• 在地面上划分出一个个大小一样的正方形栅格，然后在栅格中填充不同的颜色表示占据情况，没有障碍物的栅格为白色，被占据的栅格为黑色（除去中间的墙面，剩下的黑白方格便是栅格地图）
  

• 栅格尺寸大小可变，越小则黑色的区域越精细，也就越接近障碍物的轮廓，但同时地图的数据量就越大，处理的时候计算量就越大，所以一般会给栅格设置一个适当的尺寸
  

• 栅格尺寸：指的是栅格的单边长度，体现了地图的精细程度，常被用来表示栅格地图的分辨率，ROS 中栅格地图的默认分辨率为 0.05m，也就是每个栅格的边长为5cm
  

• 数字表示栅格
  

• 将栅格一行一行拼接起来变成一个数组，有了这个数组，再加上栅格的行、列数等信息，就能通过具体的数值将这个栅格地图描述清楚了
  

6.2 占据栅格地图消息类型

6.3 占据栅格地图发布节点

• 构建一个软件包 map_pkg，依赖项里加上 nav_msgs

  $ cd ~/catkin_ws/src
  $ catkin_create_pkg map_pkg roscpp rospy nav_msgs
  $ cd ..
  $ code .
  

• 在 map_pkg 里创建一个节点 map_pub_node

  #include 
  #include 
  
  int main(int argc, char *argv[]) {
      ros::init(argc, argv, "map_pub_node");
      // 通过大管家发布话题 /map，消息类型为 nav_msgs::OccupancyGrid
      ros::NodeHandle n;
      ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);
  
      ros::Rate r(1);
  
      while(ros::ok()) {
          // 构建一个 nav_msgs::OccupancyGrid 地图消息包，并对其进行赋值
          nav_msgs::OccupancyGrid msg;
  
          msg.header.frame_id = "map";
          msg.header.stamp = ros::Time::now();
  
          msg.info.origin.position.x = 0;
          msg.info.origin.position.y = 0;
          msg.info.resolution = 1.0;
          msg.info.width = 4;
          msg.info.height = 2;
  
          msg.data.resize(4 * 2);
          msg.data[0] = 100;
          msg.data[1] = 100;
          msg.data[2] = 0;
          msg.data[3] = -1;
  
          pub.publish(msg);
          r.sleep();
      }
   
      return 0;
  }
  // 修改 CMakeLists.txt 文件此处略
  // 到 catkin_ws 目录下编译
  

• 启动 Rviz，订阅话题 /map，显示地图

  $ roscore
  $ rosrun map_pkg map_pub_node
  $ rviz