一起自学SLAM算法：13.1 运行机器人上的传感器

连载文章，长期更新，欢迎关注：

---

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

        13.1 运行机器人上的传感器

        13.2 运行SLAM构建地图

        13.3 运行自主导航

        13.4 基于自主导航的应用

---

由于xiihoo机器人中已经为电机控制板、激光雷达、IMU和相机安装好了配套的ROS驱动，因此只要在机器人上开启相应传感器的ROS驱动节点就可以使用这些传感器了。关于这几个传感器的工作原理请参考第4章的相关内容，这里就不再赘述了。

13.1.1 运行底盘的ROS驱动

底盘ROS驱动一方面通过订阅控制话题/cmd_vel并将其解析转发给电机控制板，另一方面从电机控制板获取电机编码器数据并将其解析发布到轮式里程计话题/odom以及odom->base_link的tf关系之中。每种机器人底盘都会提供配套的底盘ROS驱动，在“xiihoo机器人”中底盘ROS驱动为xiihoo_bringup，通过下面的命令进行启动。

#启动底盘
roslaunch xiihoo_bringup minimal.launch

一旦底盘ROS驱动启动以后，就可以向底盘ROS驱动节点的话题/cmd_vel中发送线速度和角速度控制量来控制底盘运动了。其中话题/cmd_vel的消息类型为geometry_msgs::Twist，而geometry_msgs::Twist的数据结构如下。

[geometry_msgs/Twist.msg]
#Raw Message Definition
Vector3  linear
Vector3  angular
#Compact Message Definition
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular

同时从电机控制板获取到的电机编码器数据将被解析成里程计数据并发布到话题/odom以及odom->base_link的tf关系之中。其中话题/odom的消息类型为nav_msgs::Odometry，而nav_msgs::Odometry的数据结构如下。

[nav_msgs/Odometry.msg]
#Raw Message Definition
Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist
#Compact Message Definition
std_msgs/Header header
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/TwistWithCovariance twist

13.1.2 运行激光雷达的ROS驱动

激光雷达ROS驱动节点从激光雷达读取扫描数据并发布到话题/scan，激光雷达ROS驱动由相应厂商提供。在“xiihoo机器人”中，激光雷达数据通过ydlidar驱动发布，通过下面的命令进行启动。

#启动激光雷达
roslaunch ydlidar my_x4.launch 

这需要注意一下激光雷达数据的坐标系，“xiihoo机器人”中的激光雷达采用右手坐标系，雷达正前方为x轴、正左方为y轴、正上方为z轴、以x轴起始逆时针方向为theta轴。激光雷达的扫描数据以极坐标的形式表示，激光雷达正前方是极坐标0度方向、正左方是极坐标90度方向，红色点为扫描到的数据点，如图13-2所示。

图13-2  激光雷达数据格式

一旦激光雷达ROS驱动启动以后，就可以从话题/scan中订阅到激光雷达的扫描数据了。其中话题/scan的消息类型为sensor_msgs::LaserScan，而sensor_msgs::LaserScan的数据结构如下。angle_increment表示激光数据点的极坐标递增角度，ranges数组存放实际的极坐标点距离值。

[sensor_msgs::LaserScan.msg]
#Raw Message Definition
Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
#Compact Message Definition
std_msgs/Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

13.1.3 运行IMU的ROS驱动

IMU的ROS驱动节点从IMU模块读取数据并发布到话题/imu，IMU的ROS驱动由相应厂商提供。在“xiihoo机器人”中，IMU数据通过xiihoo_imu驱动发布，通过下面的命令进行启动。

#启动IMU
roslaunch xiihoo_imu imu.launch

一旦IMU的ROS驱动启动以后，就可以从话题/imu中订阅到IMU的数据了。其中话题/imu的消息类型为sensor_msgs::Imu，而sensor_msgs::Imu的数据结构如下。

[sensor_msgs::Imu.msg]
#Raw Message Definition
Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance
#Compact Message Definition
std_msgs/Header header
geometry_msgs/Quaternion orientation
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance

13.1.4 运行相机的ROS驱动

在“xiihoo机器人”中使用的是USB单目相机，采用usb_cam驱动获取图像数据并发布到话题/usb_cam/image_raw，通过下面的命令进行启动。

#启动相机
roslaunch usb_cam usb_cam.launch

一旦相机的ROS驱动启动以后，就可以从话题/usb_cam/image_raw中订阅到图像数据了。其中话题/usb_cam/image_raw的消息类型为sensor_msgs::Image，而sensor_msgs::Image的数据结构如下。

[sensor_msgs::Image.msg]
#Raw Message Definition
Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data
#Compact Message Definition
std_msgs/Header header
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data

13.1.5 运行底盘的urdf模型

urdf模型描述了机器人底盘的形状、传感器之间的安装关系、各个传感器在tf 树中的关系。其实“xiihoo机器人”底盘的urdf模型主要是提供各个传感器在tf 树中的静态关系，这些静态tf关系将在SLAM和导航算法中被使用。如图13-3为xiihoo机器人底盘中传感器的安装示意图，其中base_footprint为里程计坐标系中心，base_laser_link为激光雷达坐标系中心，imu_link为IMU模块坐标系中心，这些坐标系均为标准右手系。

图13-3  传感器之间的tf关系

以base_footprint为父坐标系，建立base_footprint->base_laser_link的转换关系与base_footprint->imu_link的转换关系，就实现了各个传感器tf关系的构建。构建的具体实现在xiihoo_description/urdf/xiihoo.urdf中完成，其中xiihoo.urdf描述文件的具体内容如下。

最后，用下面的命令启动底盘的urdf模型，这些传感器之间的tf关系就被发布到tf树了，通过订阅/tf就能获取所需的转换关系。

#启动urdf模型
roslaunch xiihoo_description xiihoo_description.launch

13.1.6 传感器一键启动

为了操作方便，可以将要启动的传感器都写入xiihoo_all_sensor.launch这个启动文件，通过这个启动文件就能一键启动机器人的底盘ROS驱动、激光雷达ROS驱动、IMU的ROS驱动、相机的ROS驱动以及底盘的urdf模型，一键启动文件xiihoo_all_sensor.launch的内容如下。

以后在运行SLAM和导航时，就可以通过一键启动文件xiihoo_all_sensor.launch很方便地启动机器人平台相关的节点了。

#一键启动
roslaunch xiihoo_bringup xiihoo_all_sensor.launch

源码仓库

• Github下载：github.com/xiihoo/Books_Robot_SLAM_Navigation

• Gitee下载（国内访问速度快）：gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

参考文献

【1】 张虎，机器人SLAM导航核心技术与实战[M]. 机械工业出版社，2022.