第五章-常用传感器功能包与数据处理

---

前言

随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器人相关技术，本文就介绍了ROS机器人操作系统中的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

0-准备工作

平台：虚拟机、镜像

1）虚拟机

从U盘拷贝或者网盘下载，安装vmware player15或16软件，有些电脑用15打不开Ubuntu系统，建议安装16版本。解压VM-ubuntu18-202304.7z，用虚拟机软件打开。

或者网盘直接获取：
VMware虚拟机：
链接：

https://pan.baidu.com/s/13_B1m7gJndUDF_LhtHp3Pg 

提取码：

mrxv

2）SSH软件。

从U盘拷贝或者网盘下载，安装bitvise ssh软件。

3）小车

检查小车是否电量充足，打开小车，并使用电脑检测是否有NCUT-EPRobot-xxxxxx名称的wifi。
一定要配对。

第一节 配置分布式环境

1、内容提要

分布式框架简介
连接小车WiFi
主从机配置环境搭建

2、分布式框架简介

ROS是一种分布式软件框架，节点之间通过松耦合的方式进行组合，在很多应用场景下，节点可以运行在不同的计算平台上，通过Topic、Service进行通信。但是ROS中只允许存在一个Master，在多机系统中Master只能运行在一台机器上，其他机器需要通过ssh的方式和Master取得联系。所以在多机ROS系统中需要进行一些配置。
我们以我们的计算机和需要远程操控的小车为例，介绍分布式多机通信的配置，其中小车树莓派的系统作为主机运行Master，我们的计算机作为从机运行节点。

3、连接小车WiFi

EPRobot上的树莓派4B配有WiFi热点，出厂默认为AP模式。小车上电约1分钟后，使用笔记本电脑连接小车WiFi：

WiFi名称： NCUT-EPRobot
密码： 12345678

当计算机连接上小车的wifi之后，小车会自动在192.168.12网段上为计算机分配一个地址。首先，在命令行中查看本机IP地址，输入命令：

ifconfig

如果本机IP（笔记本/虚拟机）为192.168.12.XXX，说明与EPRobot连接正常。通过下图显示我们可以得知，虚拟机系统被分配的地址为192.168.12.216。

如果IP地址不是上述网段，需要在系统的右上角点击小三角图标，关闭网络，并重新启动。

4、WiFi修改方式（选做）

用户也可以根据自己的需要修改wifi名称和密码，同时也可以将WiFi热点改为WiFi客户端，连接其他WiFi。
下面详细说明这几种模式的修改方法：

1、AP模式下，修改AP的SSID和密码。

（1）使用bitvise软件连接小车，打开一个终端，将路径切换至/etc/network/路径下，修改该路径下interfaces文件的权限（密码为ncut1234）：

cd /etc/network
sudo chmod 777 interfaces

（2）使用bitvise软件的tftp功能打开小车系统的文件目录，将/etc/network/interfaces文件打开，将最后三行注释并保存，如下图：

（3）在bitvise软件连接小车的状态下，打开一个终端，将路径切换至/etc/路径下，修改该路径下rc.local文件的权限（密码为ncut1234）：

cd /etc
sudo chmod 777 rc.local

（4）使用bitvise软件打开/etc/network/rc.local文件，修改AP的ssid和密码并保存，如下图：

2、client 模式下，连接指定的 wifi。

（1）使用bitvise软件连接小车，打开一个终端，将路径切换至/etc/network/路径下，修改该路径下interfaces文件的权限（密码为ncut1234）：

cd /etc/network
sudo chmod 777 interfaces

（2）使用bitvise软件的tftp功能打开小车系统的文件目录，将/etc/network/interfaces文件打开，使能最后的三条命令并保存，如下图：

（3）在bitvise软件连接小车的状态下，打开一个终端，将路径切换至/etc/wpa_supplicant路径下，修改该路径下wpa_supplicant.conf文件的权限（密码为ncut1234）：

cd /etc/wpa_supplicant
sudo chmod 777 wpa_supplicant.conf

（4）使用bitvise软件打开/etc/wpa_supplicant/wpa_supplicant.conf文件，修改要连接的wifi名称和密码并保存，如下图：

5、主从机配置环境搭建

1、目的：

以上的步骤主要完成了将小车和虚拟机系统建立在了同一个局域网下，保证两个设备能够正常的进行网络通信。
接下来，我们要进行ROS的主从机环境配置。

2、虚拟机配置（个人笔记本）

1、虚拟机 hosts 配置：

首先在虚拟机ubuntu系统的/etc路径下配置hosts文件如下图所示，此配置的作用是将小车的IP地址写到ubuntu系统中，让虚拟机知道小车在网络中的名称和对应的IP地址。

2、虚拟机 bashrc 配置：

接下来需要进入虚拟机ubuntu系统的home路径下，然后按ctrl+H键显示home路径下的隐藏文件。在显示出来的文件找.bashrc文件。打开.bashrc文件，并在文件末尾添加如下两句代码，其意义是将小车 EPRobot 设置为主机。

3、小车配置

1、小车hosts配置：

然后通过windows系统下的Bitvise SSH Client软件登录，点击New SFTP window

然后在/etc路径下打开hosts文件：

将我们查到的 ubuntu 系统的IP地址写到小车的 hosts 文件中，用以匹配作为从机的计算机。同时将127.0.1.1设置为自己的名称。

2、小车bashrc配置：

下面我们进入小车系统的 /home/EPRobot 路径下，然后按 ctrl+H 键显示home路径下的隐藏文件。在显示出来的文件找到并打开 .bashrc 文件，并在文件末尾添加如下两句代码，其意义是将小车EPRobot设置为主机。

这样，我们就完成了ROS远程分布开发环境的搭建。

6、常见问题：
★ ROS Master未启动
★ rviz能显示数据，不能下发指令
★ 指令运行的设备不对

第二节 机器人常用传感器原理及应用

1、课程目标：

了解常用传感器的种类、采集原理以及应用场景。
基本概念：
传感器原理、数据采样
见：PPT

---

第三节 机器人ROS中的传感器仿真

1、课程目标：

熟悉ROS中的传感器模拟器的使用方法，能够模拟不同类型传感器的数据。
基本概念：
Gazebo仿真环境、激光雷达、摄像头、IMU、传感器消息
实验设置：
1、搭建Gazebo仿真环境
2、新建并配置常用传感器模拟器
3、编写ROS节点，订阅传感器数据，进数据处理和可视化展示
见：PPT

---

第四节 激光雷达的数据采集与处理

1、课程内容：

激光雷达工作原理
雷达数据格式
启动雷达
测试结果

2、激光雷达工作原理

激光雷达最基本的工作原理与无线电雷达没有区别，即由雷达发射系统发送一个信号,打到地面的树木、道路、桥梁和建筑物上，引起散射，经目标反射后被接收系统收集，通过测量反射光的运行时间而确定目标的距离。至于目标的径向速度，可以由反射光的多普勒频移来确定，也可以测量两个或多个距离，并计算其变化率而求得速度，这也是直接探测型雷达的基本工作原理。

激光雷达的作用就是精确测量目标的位置（距离与角度）、形状（大小）及状态（速度、姿态），从而达到探测、识别、跟踪目标的目的。激光雷达是一种雷达系统，是一种主动传感器，所形成的数据是点云形式。
其工作光谱段在红外到紫外之间，主要发射机、接收机、测量控制和电源组成。
通过旋转的机械镜面测量激光发出和收到回波的时间差，从而确定目标的方位和距离。由于激光雷达主动发射激光，因此受环境变化的影响小，测距精准。

典型激光雷达的系统组成包括激光发射源、接收器、伺服电机、斜面镜和光学旋转编码器（又称圆光栅）。

激光雷达的关键部件按照信号处理的信号链包括控制硬件DSP(数字信号处理器)、激光驱动、激光发射二极管、发射光学镜头、接收光学镜头、ADP(雪崩光学二极管)、TIA（可变跨导放大器）和探测器。其中除了发射和接收光学镜头外，都是电子部件。

3、雷达数据格式

雷达扫描之后上传给树莓派的格式为雷达扫描360度范围内障碍物与雷达正前方的夹角θ以及障碍物距离雷达的距离d两个数值对组成的数组。如果我们要通过雷达扫描实现对距离雷达最近的物体的追踪，那么我们就可以通过分析上传的雷达扫描周围物体的数据进行判断，如下图所示：

/home/EPRobot/robot_ws/src/hibot_follower/src/hibot_follower.cpp

在ROS中，激光雷达的数据类型为sensor_msgs/LaserScan，我们在ROS中查询该类型的具体结构组成，可得以下结果：

对我们真正有用的数据主要有angle_min、angle_max、angle_increment、range_min、range_max和ranges。我们可以订阅雷达数据进行验证：

rostopic echo 话题

对思岚A1雷达而言，角度的起始为-180°，结束为180°，角度增量为0.5°，最小检测距离为0.15米，最大距离为12米。Ranges这个数组为从-180°到180°之间，每隔0.5°的距离值。下图为思岚A1雷达角度示意图：

4、启动雷达

我们通过对每一个扫描的激光点进行判断，得出每一个激光点对应的扫描到的物体的角度θ及距离d，通过比对，找出距离雷达最近的物体，当这个物体移动时，树莓派就根据物体对应的位置向下位机发送小车的速度指令，从而控制电机运动，使小车向移动物体运动，实现物体的追踪。
我们在小车终端执行EPRobot_start.launch文件，EPRobot_start.launch文件中我们可以看到在其中会启动雷达的rplidar.launch文件，此文件就会将我们的雷达启动起来。

接下来我们进到rplidar.launch文件中，如下图所示：

我们可以看到此launch文件启动了rplidarNode这个节点，其中并配置了数据传送的串口、波特率等一系列参数。这样就顺利的将雷达启动起来了，我们会从终端看到如下打印。

5、显示结果

当我们启动Rviz之后，在global option中设置Fixed Frame为/odom_combined，就可以在图形中看到小车和周围的障碍物，如下图所示

---

第五节 案例：使用激光雷达完成移动物体的实时跟踪

EPRobot 配备的激光雷达主要用于扫描周围的障碍物，以及时避让。同时，雷达还能够实时跟踪小车周围的移动物体，并进行跟随，具体操作步骤如下（本节操作基于windows 系统下的 SSH 软件和我们提供的虚拟机系统）：

1）打开小车，连接小车 WiFi。打开 Bitvise SSH Client 软件，远程连接小车，打
开一个终端，并启动小车的底层驱动程序：

roslaunch eprobot_start EPRobot_start.launch

2）在虚拟机中打开一个新的终端，启动 rviz：

rviz

在 global option 中设置 Fixed Frame 为 base_footprint，就可以在图形中看到
小车和周围的障碍物。

3）在 Bitvise SSH Client 打开一个新的终端，启动跟随程序：

roslaunch hibot_follower hibot_follower.launch

打开成功后，终端会输出以下信息，其中第一个值就是小车前方最近的物体的距离，
目前程序设定小车跟踪前方距离 0.4m-1.0m 之间的移动物体，如需修改小车的跟随速
度、转向幅度、最大跟随距离以及最小跟随距离，可以修改以下路径的 launch 文件中
的相关参数

/home/EPRobot/robot_ws/src/hibot_follower/launch/hibot_follower.launch
上图 launch 文件参数及路径。

以下输出信息，表示了小车最近的跟踪对象距离、相对角度以及 x,y 坐标。

终端输出信息。

激光雷达的具体数据格式解析，参考雷达介绍。

作业一、雷达测距：

思路参考:

1. 构建一个新的软件包，包名叫做lidar_pkg.
2. 在软件包中新建一个节点，节点名叫做lidar_node。
3. 在节点中，向ROS大管家NodeHandle申请订阅话题/scan，并设置回调函数为LidarCallback()
4. 构建回调函数LidarCallback()，用来接收和处理雷达数据
5. 调用ROS INFO()显示雷达检测到的前方障碍物距离。

参考以下图片完成雷达测距功能：

src：lidar_node.cpp

CMakeLists.txt

作业二、雷达避障：

参考以下图片完成雷达避障功能：
src：lidar_node.cpp

思路参考:
1.引入速度控制头文件：

2. 让NodeHandle 发布速度控制话题 /cmd vel
切记vel_pub需要声明全局变量；

思考一下为什么？

3.构建速度控制消息包 vel_cmd.

4.根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物。

这里注意小车速度；首先先测试一下速度。
5.避障优化：

python ？

第六节 IMU的数据采集与处理

1、课程内容：

• 陀螺仪的工作原理
• 四元数与欧拉角
• 使用陀螺仪实现姿态解算
• 使用STM32单片机采集陀螺仪数据
• 姿态数据的滤波算法
• ROS中的IMU消息格式

2、IMU工作原理

机器人或智能车的控制中，姿态是非常重的要控制环节。要保证机器人的空间姿态能够按照人为设定，必须要准确快速的采集机器人的当前姿态。目前最常用的解决方案和传感器就是使用MEMS陀螺仪，进行控制对象的姿态测量。

1、陀螺仪的工作原理

陀螺仪是用来测量角速率的器件，在加速度功能基础上，可以进一步发展，构建陀螺仪。陀螺仪的内部原理是这样的：对固定指施加电压，并交替改变电压，让一个质量块做振荡式来回运动，当旋转时，会产生科里奥利加速度，此时就可以对其进行测量；这有点类似于加速度计，解码方法大致相同，都会用到放大器。
三轴陀螺仪也叫作微机械陀螺仪，而微机械陀螺仪也会被称作MEMS陀螺仪。它的特点在于能够同时进行六个方向的位置测定工作，还能对该方向移动的轨迹及加速的测定。最早的单轴陀螺仪的只能进行一个方向的测量。一个三轴陀螺仪能完成三个单轴陀螺仪的工作量，如果在一个系统需要三个陀螺仪，三轴陀螺仪可以完美替代三个单轴陀螺仪。三轴陀螺仪具有体积小、重量轻、结构简单、可靠性好等优点，是激光陀螺的发展趋势。简而言之，三轴陀螺仪最大的作用就是“测量角速度，以判别物体的运动状态，所以也称为运动传感器。

在实际应用中，我们需要的不一定只有加速度信息，更重要的是3轴的角度信息。这些角度可以通过3轴的角速度、加速度进行计算得到。

3、四元数与欧拉角

在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。我们经常需要对两种表达方式进行转换，计算公式采用3D笛卡尔坐标系：

▲3维迪卡尔坐标系

定义ψ，θ，φ分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。
▲Tait-Bryan angle

1、欧拉角分类

欧拉角按照旋转轴分为经典欧拉角（Proper Euler Angle）和泰特布莱恩角（Tait–Bryan angles），共 12种旋转方式：

经典欧拉角-Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)
使用两个轴的旋转角度表示，第一个旋转角度和第三个旋转角度都是绕同一个轴。
泰特-布莱恩角-Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z)
使用三个轴的旋转角度表示。

4、使用陀螺仪实现姿态解算

首先，对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。
不难看出，第一种方法测得的数据中存在低频噪声，而第二种方法测得的数据中存在高频噪声，因此可通过向量外积补偿也就是我们常说的互补滤波来进行数据融合。
具体见文档 19；

5、使用STM32单片机采集陀螺仪数据

我们的控制器中板载了最常用的陀螺仪芯片MPU6050，能同时检测三轴加速度、三轴陀螺仪(三轴角速度)的运动数据以及温度数据。利用MPU6050芯片内部的DMP模块（Digital Motion Processor数字运动处理器），可对传感器数据进行滤波、融合处理，它直接通过I2C接口向主控器输出姿态解算后的姿态数据，降低主控器的运算量。其姿态解算频率最高可达200Hz，非常适合用于对姿态控制实时要求较高的领域。常见应用于手机、智能手环、四轴飞行器及计步器等的姿态检测。

▲MPU6050传感器的坐标及方向

我们需要使用STM32单片机的I2C接口读取MPU6050内部的寄存器数据，以获取传感器的实时数据，再通过姿态解算的算法，将3轴的加速度和角速度转化为3轴的欧拉角和四元数，用于实际机器人的姿态控制。原理图如下图所示。

▲板载MPU6050原理图

6、姿态数据的滤波算法

然而，在实际环境中，运动物体上的陀螺仪数据抖动较为严重，同时会偶尔出现一些波动较大的无效数据。由于最终的欧拉角，特别是偏航角，是通过角速度的积分得到的。数据的波动会使得误差不断累积，最终得到的角度与实际偏离较大。
所以我们必须要使用有效的滤波算法，将波动数据进行处理，才能尽量保证最终的结果与实际相对吻合。
由于其原理和推到比较复杂，本节内容就不再展开，会专门安排一节内容详细讨论。

7、ROS中的IMU消息格式

在Ubuntu系统中，我们可以输入以下命令来查看ROS中对于IMU数据格式的定义。

rosmsg show sensor_msgs/Imu

执行效果如下图所示。

ROS内置消息中除基本数据类型之外，基本都会包含header消息头，
header包含seq、stamp以及frame_id，分别表示序列号、时间戳和帧id：

• seq：一般从0开始，每次增加1，表示消息序列id；
• stamp：为时间戳，表示ros消息产生的时间。
• frame_id：表示消息发布者，这里的frame常表示坐标系，例如gps、imu和lidar等。

裸数据：

angular_velocity ：表示角速度；
angular_velocity_covariance： 角速度协方差；

linear_acceleration： 表示线加速度；
linear_acceleration_covariance： 表示线加速度协方差；

裸数据解算

orientation：表示姿态，使用四元数表示；
orientation_covariance：表示姿态协方差；

orientation是由linear_acceleration和angular_velocity计算而得，但并不是所有IMU设备都直接提供orientation，如果没有提供，将orientation各项置为0，orientation_covariance各项置为-1.这里协方差表示各个数据的误差，一般由器件厂商给出。

可以看出，Imu结构体中分别包含了四元数、三轴角速度矢量和三轴线速度矢量。我们在实际使用中时，一般不需要用到所有的数据，所以，只需要填充需要的数据到相应位置即可。

作业：IMU数据获取

思路参考:

1. 构建一个新的软件包，包名叫做imu_pkg;
   

2. 在软件包中新建一个节点，节点名叫做imu_node;
   

3. 在节点中，向ROS大管家NodeHandle申请订阅话题/imu/data,并设置回调函数为IMUCallback();
   
   ros::spin() 保持节点持续运行

4. 构建回调函数IMUCallback()，用来接收和处理IMU数据;
   
   判断数据是否有效；

5. 使用TF工具将四元数转换成欧拉角;
   
   这个RPY 就是 roll 滚转 pitch 俯仰和 yaw 航向角的缩写；
   这时候三个欧拉角变量里的数值单位是弧度，我们要将其转换成 360°的角度值，更直观一些。

6. 调用ROS INFO()显示转换后的欧拉角数值；
   

7. 完善编译规则，后编译。
   
   文件结构：
   

---

总结

以上就是今天要讲的内容。