树莓派+ROS+Arduino造一台导航小车(完整代码+硬件调试)

目录

1、小车平台架构

1.1 实验概述

1.2 预期目标

2、硬件平台简介

2.1 执行机构

2.1.1 底盘结构

2.1.2 减速电机

2.2 下位驱动系统简介

2.2.1 驱动控制单元Arduino

2.2.2 电机驱动单元 L298P

2.3 上位控制系统简介

2.3.1 树莓派

2.3.2 摄像头型号

2.3.3 激光雷达型号

3、驱动系统开发

3.1 arduino IDE 开发环境搭建

3.1.1 arduino 连接 ubuntu

3.1.2 arduino IDE 

3.2 arduino 案例与基本语法

3.3 arduino 编程实现底盘运动控制

3.3.1 电机驱动

3.3.2 编码器测速

3.3.3 PID 控制车速

3.4 底盘运动控制库

3.4.1 编码器驱动定义

3.4.2 电机驱动定义

3.4.3 PID 库文件源码分析

3.4.4 PID 库文件调试实现

4、控制系统开发

4.1 树莓派 ubuntu 系统配置

4.1.1 安装 Ubuntu Server 18.04

4.1.2 连接 WIFI

4.1.2 更新软件仓库源、配置桌面

4.1.3 配置静态 IP

4.1.5 安装过程中的其他问题

4.2 ubuntu 分布式框架搭建

4.1.1 NAT 模式和桥接模式

4.1.2 创建 ssh 客户端、服务器

4.3 ROS 安装与通信

4.3.1 ROS-melodic 安装

4.3.2 ROS 分布式通信

4.3.3 ros_arduino_bridge 上位机参数配置

4.3.4 测试 ROS 远程控制小车（实车场景）

4.3.5 VScode 远程开发

4.3.6 常见问题：

4.4 激光雷达

4.4.1 硬件添加

4.4.2 软件测试

4.4.3 常见问题

4.5 相机

4.6 传感器集成

4.6.1 launch 文件

4.6.2 坐标变换

5、小车导航系统

5.1 添加模型

5.1.1 创建 urdf 模型文件

5.1.2 启动 car.launch 文件

5.1.3 启动 start.launch （集成小车模型、雷达、相机）

 5.2 地图服务

5.2.1地图绘制 gmapping

5.2.2 地图保存 map_saver

5.2.3 地图读取 map_server

5.3 AMCL 定位

5.3.2 amcl.launch 文件

5.3.2 执行 test.amcl.launch 文件

5.4 路径规划

5.4.1 move_base.launch 文件

5.4.2 执行路径规划

5.4.3 调试常见问题

5.4.4 自主建图

6、总结

1、小车平台架构

1.1 实验概述

主要介绍 ROS 小车导航平台的软硬件搭建，包括下位机 Arduino 的硬件搭建、底盘驱动程序、上位机树莓派 ROS 分布式环境搭建、ROS navigation 导航功能包的使用，最终实现小车的自主移动与主动避障。

执行机构

由车身底盘、直流减速电机、以及用于保持平衡的万向轮构成。

驱动系统

由驱动控制系统 Arduino、电机驱动单元 L298P 构成。Arduino 将上位机控制系统下发的速度指令转换为直流电机所需的 PWM 脉冲信号，完成底盘的驱动。

控制系统

包括搭载在树莓派上的 ROS 系统、ROS 导航功能包、及外围传感器：激光雷达、相机，实现 ROS 小车的导航定位及自主移动功能。根据目标位置及自身定位功能，完成路径规划，并将速度消息发布，由下位机接收，实现对下位机的控制。

传感器

包括电机测速的霍尔编码器，用于测车轮速度、及外维传感器：摄像头、激光雷达。

1.2 预期目标

完成差速轮式车的软硬件平台搭建，实现 SLAM 建图、自身定位、路径规划、底盘控制，通过人机交互界面对目标点位置的选取，实现小车的自主移动及主动避障。

2、硬件平台简介

包括：底盘车轮结构、减速电机、编码器、电机驱动板、Arduino 电路板、树莓派、激光雷达、摄像头。

2.1 执行机构

由安装在底板上的直流电机构成

2.1.1 底盘结构

底盘由车轮、电机、底板组成，其中前轮分为差速式和阿克曼式，本章采用的结构为差速式。

差速底盘：通过控制左后轮、右后轮的速度差，实现小车转向的控制。

阿克曼转向底盘：后轮控制纵向速度，前轮控制转角，实现转向控制。

2.1.2 减速电机

电机参数如下：

编码器参数：

2.2 下位驱动系统简介

由驱动控制单元 Arduino 和电机驱动单元 L298P 构成。

2.2.1 驱动控制单元Arduino

2.2.2 电机驱动单元 L298P

由于 arduino 的输出电流不足以直接驱动电机，需要通过电机驱动板放大电机控制信号，这里选用的驱动板为 L298P，L298P 的部分引脚和 arduino 相对应，可以直插在 arduino 上方，从而为 arduino 供电，同时 arduino 的模拟引脚 11、13 输出的 PWM 脉冲也可以控制 L298P 的电机驱动引脚，从而达到调速的目的。

2.3 上位控制系统简介

上位机控制系统包括搭载 ROS 的树莓派，以及用于测距的激光雷达、摄像头。

2.3.1 树莓派

使用 Raspberry Pi 4b 作为上位机 ROS 系统的搭载平台，还需要用到读卡器、16G 及以上的 SD卡、显示屏或 HDMI 采集卡、网线。

2.3.2 摄像头型号

树莓派 500万像素原装摄像头。

2.3.3 激光雷达型号

型号：思岚A1 单线激光雷达。

3、驱动系统开发

下位机底盘驱动系统常用的电子开发平台有 STM32、Arduino等，arduino 的入门简单，开发语言基于 C / C++ 。可以进行简单的电路控制设计、通过传感器感知环境、控制 LED 灯、电机等装置。

3.1 arduino IDE 开发环境搭建

arduino 的硬件部分用来做电子元器件、传感器、电子装置的连接，软件部分是 arduino IDE，将程序烧录至电路板后开始工作。

3.1.1 arduino 连接 ubuntu

a. 将 arduino Mega 2560 开发板连接至 USB 端口，选择连接到虚拟机

b. 在ubuntu 中查看USB设备是否连接，名称为 /dev/ttyUSB0 或者 /dev/ttyACM0

 c. 当前用户还没有添加到 dialout 用户组，对该设备没有操作权限。

 d. 添加当前用户名到 dialout 用户组，以便获得对 arduino 的访问权限。

e 重启后再次查看是否已添加到 dialout 用户组

3.1.2 arduino IDE 

a. 下载、安装、启动，步骤略。

b. 选择开发板型号

c. 设置端口

3.2 arduino 案例与基本语法

a. 案例1：控制LED 灯闪烁

b. 案例2：串口打印 

这时可以在开发板上看到 RX指示灯常亮，代表开发板正在接收数据 

c. 通过串口，由 PC 向 arduino 发送数据

 d. PWM 控制 LED 灯亮度

e. 时间相关函数 millis() 

3.3 arduino 编程实现底盘运动控制

在 ROS 小车的实际运行过程中，上位机控制系统向下位机驱动系统发布速度话题。arduino 作为下位机订阅到该话题后，闭环控制电机转速，跟随期望速度，并将实时测量的速度信息作为话题发布给上位机，从而在上位机控制系统中计算里程计信息。这里首先展开的是 arduino 如何驱动电机转动，至于使电机转速跟随期望速度的PID 算法将在后续详述。

3.3.1 电机驱动

通过对 Arduino 单板上的模拟量引脚进行 PWM 脉宽调制，实现对电机的调速。

引脚说明：端子 10、12 负责对 motor A 调速，端子 11、13 负责对 motor B 调速。端子10、11 是数字量引脚，取值范围为 HIGH 或 LOW，HIGH 表示电机使能，LOW 表示电机停止。端子12、13 为模拟量引脚，负责脉宽调制，取值范围为 0 ~ 255，其中0 ~ 127 表示电机正转、128 ~ 255 表示电机反转。

接线说明：将 12 V 外部电源的端子接入 L298P 的电源端子，将 L298P 的驱动电压输出端子引出到电机上。每个电机有两个电源端子，共两个电机，所以在 arduino 上共有 4 个端子负责输出驱动电压，有 2 个输入端子接收 12V 的电池供电。

c. 代码实现

3.3.2 编码器测速

测速原理：单位时间内的脉冲个数 / 时间。编码器的分辨率为 13 PPR，即电机每转一圈输出13个脉冲；电机减速比为 30：1，即车轮每转一圈对应电机转30圈；所以车轮转一圈，对应输出 30 ×13 = 390 个脉冲。

接线方案：编码器需要 5V 的电源输入，借用 L298P 驱动板上的5V 电源输出，另外编码器 A B 相分别连接至 arduino 的中断引脚20、21上，接线图如下：

arduino 中断模式涉及：LOW, CHANGE, RISING, FALLING，这里选择 CHANGE 模式，即当电平发生跳变时便进入中断模式。  根据不同的计数精度，分为单倍频、双倍频、四倍频模式：

a. 单倍频计数原理：A 相上升时进入中断模式，在中断函数中进行脉冲计数。如果 B 相为高电平，则为正转，计数+1；B 相为低电平，则为反转，计数 -1。

单倍频代码实现：

单倍频计数的缺点：精度低，只考虑了 A 相上升沿跳变时的计数，没有考虑下降沿。因此引入双倍频计数模式。

b. 双倍频计数原理：增加在 A 相下降沿的计数，即 A 相下降时也进入中断模式。在中断函数中，判断如果 B 相为低电平，则为正转，计数+1；B 相为高电平，则为反转，计数 -1。双倍频将计数精度提高了2倍，但由于没有利用到 B 相跳变时刻的计数，精度仍然有待提高，为此引入四倍频计数。

c. 四倍频计数原理：增加在 B 相上升/下降沿的计数，精度提高4倍。判断逻辑同上，代码实现如下。

计算转速

减速比为 30，编码器分辨率为 13 PPR，计数方式为 4 倍频。所以车轮转速(单位：rpm) = {单位时间内脉冲个数 / 30 / 13 / 4 } / {单位时间(ms) * 1000 * 60}

3.3.3 PID 控制车速

a. PID 库文件配置

下载 PID 库: git clone https://github.com/br3ttb/Arduino-PID-Library

将下载好的库文件移动到 arduino/libraries目录：sudo cp -r Arduino-PID-Library /home/用户名/Arduino/libraries

重命名文件，即将"-"符号删除：sudo mv Arduino-PID-Library ArduinoPIDLibrary

重启 ArduinoIDE，菜单栏：File => Examples => ，查看 PID 库是否成功安装。

b. 代码实现

/* 复制测速代码、编写控制电机运动代码、PID库
 * 修改测速代码：测速单位时间 2000 => 50毫秒
 * 添加电机转向、PWM 控制引脚
 * 包含 PID 头文件、创建 PID 对象
 * 在 setup()中启用 PID 对象，调用 PID.compute()，输出控制电机 PWM 值
 */
 #include<PID_v1.h>
 int DIR_LEFT = 10; // L298P 电机驱动板的电机控制脉冲输出口
 int PWM_LEFT = 12;
 
 int encoder_A = 21; // 中断逻辑口：2
 int encoder_B = 20; // 中断逻辑口：3
 volatile int count = 0;
 
 void count_a(){
  if(digitalRead(encoder_A)==HIGH){
      if(digitalRead(encoder_B)==HIGH){
        count++;
      }else{ count--; }
  }else{
    if(digitalRead(encoder_B)==LOW){
      count++;
    }else{count--;} }}

 void count_b(){
  if(digitalRead(encoder_B)==HIGH){
      if(digitalRead(encoder_A)== LOW){
        count++;
      }else{ count--;}
  }else{
    if(digitalRead(encoder_A)== HIGH){
      count++;
    }else{
      count--; } } }

long start_time = millis();
int interval_time = 50;
int per_round = 30 * 13 * 4;
double vel; // 全局变量：车轮速度

void get_current_vel() {
  long right_now = millis();
  long pass_time = right_now - start_time;
  if(pass_time >= interval_time) {
    noInterrupts();
    vel = (double)count / per_round /interval_time * 1000 * 60;
    // Serial.println("vel = ");
    Serial.println(vel);
    count = 0;
    start_time = right_now; 
    interrupts(); } }

double pwm;
double target = 30; // 目标速度
double kp     = 1.5;
double ki     = 3.0;
double kd     = 0.1;
PID pid(&vel, &pwm, &target, kp, ki, kd,DIRECT); // DIRECT ，REVERSE：对 KID 参数取反

void update_vel() {
  get_current_vel(); // 更新 vel 数值
  pid.Compute();     // 计算 pwm
  int pwm_new = 128 + (int)pwm / 2 ;
  digitalWrite(DIR_LEFT,HIGH);
  analogWrite(PWM_LEFT, pwm_new);// 最小149 
}
void setup() {
  Serial.begin(57600);
  pinMode(encoder_A, INPUT);
  pinMode(encoder_B, INPUT);
  attachInterrupt(2, count_a, CHANGE);
  attachInterrupt(3, count_b, CHANGE);
  pinMode(DIR_LEFT,OUTPUT);
  pinMode(PWM_LEFT,OUTPUT);
  pid.SetMode(AUTOMATIC);
}
void loop() {
  delay(10);
  update_vel();
}

c. 查看控制效果

d. 比例控制 

 e. 比例+积分控制

 f. 比例+积分+微分控制

3.4 底盘运动控制库

使用 ros 封装的 ros_arduino_bridge 驱动包，可以更便捷地实现底盘的控制，极大得提高开发效率。 ros_arduino_bridge 的源代码下载路径如下：

git clone https://github.com/hbrobotics/ros_arduino_bridge.git

需要修改固件包的源代码，并烧录至 arduino 电路板，从而实现对不同硬件的适配。 

使用封装的驱动包文件便捷之处在于，通过输入精简的指令和参数即可执行对编码器、电机等的调用，在 arduino 串口监视器中可调用的宏定义命令有：

3.4.1 编码器驱动定义

a. 启用基控制器、添加自定义的编码器驱动名

b. 修改编码器头文件

 c. 修改编码器源文件

d. 初始化编码器

 e. 测试

3.4.2 电机驱动定义

a. 添加自定义的电机驱动名

 b. 修改电机驱动头文件

c. 修改电机驱动源文件（追加了补充内容）

追加修正内容： 

 d. 测试

3.4.3 PID 库文件源码分析

a. 主程序中的 PID 定义，part 1：

part 2：

part 3:

b.  差速控制器 diff_controller 头文件实现

part 1：PID 参数初始化

 part 2：updatePID()

 par 3：doPID() 核心算法

3.4.4 PID 库文件调试实现

调试左电机时，为了避免右电机干扰，将右轮的 PID 实现函数，即 doPID( &rightPID ) 暂时屏蔽。同时在 diff_controller.h 文件中添加串口打印值，用于串口绘图器中查看实时速度值：

下面以左电机为例，进行 PID 参数整定。

a. 纯比例控制：

b. 比例+积分控制 

由于左右电机是分别调试的 PID 参数，可能会导致左右车轮控制效果不同，小车无法直线前进。 

总结：到目前为止，关于下位机的固件包 ros_arduino_firmware 已经完全烧录到 arduino 电子板中了，后面章节的关于上位机控制系统的前提就是本章节的下位机 arduino 单板已经有底层的驱动程序，现在已经实现了底盘自驱动，这部分内容是做车辆底盘控制的重点，后续上位控制系统会在此基础上做一些导航、规划控制的算法的实现。

4、控制系统开发

主要介绍树莓派安装 ubuntu 后的基本配置、与远程 PC 的分布式系统搭建、主从机开始 ssh 服务、ros_arduino_bridge 的安装与配置、实际场景中控制小车的运动。

4.1 树莓派 ubuntu 系统配置

软件包括：ubuntu server 18.04 镜像包、烧录器

硬件包括：树莓派4b、鼠标、键盘、HDMI 采集卡、SD 卡、读卡器、PC

4.1.1 安装 Ubuntu Server 18.04

1、 下载 ubuntu Server 18.04 的树莓派镜像包

https://mirrors.tuna.tsinghua.edu.cn/ubuntu-cdimage/ubuntu-mate/releases/18.04/release/ubuntu-mate-18.04.5-desktop-amd64.iso

2、树莓派官网下载烧录软件，选择离线下载好的 ubuntu 18.04 镜像包、选择 SD 卡（需要先格式化）、点击右下角的设置选项，可以将用户名、WIFI信息、开启 ssh、语言设置、时区设置等提前写入系统，设置好后开始烧录，这里提前添加了用户名和 WIFI 热点。

 3、烧录完成后，将 SD 卡插入树莓派的卡槽中，连接树莓派电源、鼠标、键盘、通过 HDMI 采集卡连接至 PC 的 USB 端口，启动相机、树莓派上电后自动开机进入 ubuntu server18.04

4、现在安装的 ubuntu 是 server 版，配置桌面前需要先连接 WIFI、更换国内的软件源。

4.1.2 连接 WIFI

1、修改配置文件

 2、添加 WIFI 名和密码

 3、尝试连接 WIFI，通常不报错表示访问 OK

4、ifconfig 查看到网卡上已经分配了动态 IP 地址

5、这时候 ping 通外网就表示 WIFI 连接成功。

4.1.2 更新软件仓库源、配置桌面

1、备份源文件

2、修改源文件

 3、替换仓库源地址

 4、检查已安装软件的可用更新

 5、安装桌面，重启后自动进入桌面

        sudo apt install ubuntu-desktop

4.1.3 配置静态 IP

1、route -n 查看网关

2、vim /etc/netplan/01-network-manager-all.yaml 编辑网络配置文件

 3、修改静态 IP

 4、重新 ifconfig 查看网卡

4.1.5 安装过程中的其他问题

1、解决 HDMI 屏幕反转问题

2、sudo apt upgrade 无法安装软件：

4.2 ubuntu 分布式框架搭建

4.1.1 NAT 模式和桥接模式

通常创建一个 Linux 虚拟机时，网络配置模式有：桥接、NAT、host-ony 三种模式。

NAT 模式下，宿主机的 IP 和虚拟机的 IP 不在同一网段，虚拟网卡和物理网卡的连接是通过虚拟交换机来完成的。这时宿主机相当于有两张网卡，一张真实网卡访问外网，一张虚拟网卡连接虚拟机，二者处于不同网段，通过虚拟交换机进行连接。

1、查看宿主机的真实 IP

     宿主机的虚拟网卡 IP：

      虚拟机 IP，与宿主机的虚拟 IP 为同一网段

2、现在将虚拟机的网卡连接方式改为：桥接模式

 重启虚拟机，再次查看 IP，已经自动与宿主机真实 IP 保持在同一网段

 再次查看宿主机的 IP，已经没有虚拟 IP 地址

 在虚拟机上访问远程的树莓派结果OK

4.1.2 创建 ssh 客户端、服务器

SSH 服务在架构上分为客户端与服务器，客户端是本地 PC，为请求数据的发送方，服务端是树莓派，为请求的接收方。

1、树莓派开启SSH 服务

 2、 虚拟机开启 SSH 服务

3、虚拟机访问树莓派

4、从服务端下载文件测试

5、添加 ssh 密钥对

密钥对可以实现免密访问服务器，提高访问效率。生成一堆公钥、私钥，将私钥存储在客户端，将公钥上传至服务器，每次访问服务器时，通过本地上传私钥至服务器，与服务器的公钥匹配后，客户端成为合法用户，直接创建 ssh 连接，不再需要手动输入密码。

6、不输入密码直接 ssh 访问服务器

4.3 ROS 安装与通信

安装 ros-melodic 版本、测试 ros 的分布式控制、配置上位机 ros_arduino_bridge，测试 ros 远程控制小车运动。 

4.3.1 ROS-melodic 安装

melodic/Installation/Ubuntu - ROS Wiki

4.3.2 ROS 分布式通信

ROS 是分布式计算环境，一个 ROS 系统可以包含多个节点，每个节点可以部署在不同的计算机上，比如来自控制层的节点需要订阅来自轨迹规划节点的消息。任何节点之间随时需要与任何其他节点进行通信。一个 ROS 系统中只能有一台主机启动 roscore 作为管理节点，但可以与多台从机的 ROS节点进行通信。这里树莓派作为主机端（启动 roscore 的设备），本地虚拟机作为从机。

1、配置从机（虚拟机）的静态 IP：通过桌面形式，与修改配置文件的效果相同。

 修改为静态 IP、添加子网掩码、网关、自动获取 DNS

2、修改 /etc/hosts 文件

修改从机（虚拟机端）/etc/hosts：加入主机（树莓派）的 IP 地址和计算机名，重启后生效

 修改主机（树莓派端）的 /etc/hosts ，加入从机（虚拟机）IP 地址和计算机名，重启后生效

重启后在主机端和从机端分别互相 Ping 对方的 hostname，代表 /etc/hosts 配置生效。

3、修改 .bashrc 文件

修改从机（虚拟机端）

source .bashrc 使环境变量配置生效

修改主机（树莓派端）

4、测试 ros 分布式通信

a. 在主机端（树莓派）启动 roscore、小乌龟，订阅速度消息，等待从机端发布

 b. 在从机端（虚拟机）启动发布节点，通过键盘控制发布速度指令

c. 在主机查看到小乌龟订阅到了来自从机的速度消息，开始运动，实现了分布式控制。

4.3.3 ros_arduino_bridge 上位机参数配置

1、首先在树莓派中安装 python-serial 功能包

2、回到虚拟机端，驱动包 ros_arduino_bridge 关于上位控制系统 ros 的功能主要是来自于 ros_arduino_python，程序入口是该包的 launch 文件下的 arduino.launch 文件

 找到该 launch 文件中包含的 yaml 配置文件模板，复制并重命名为自定义的yaml配置文件。

3、修改上位机参数配置

4、将从机中的配置文件上传至主机：ssh 远程访问的方式登录树莓派，创建一个工作空间。

 5、在从机端将 ros_arduino_bridge 文件通过 scp 命令上传至树莓派的 catkin_ws/src 路径。

6、在主机端确认文件接收 OK，编译成功代表配置生效。

4.3.4 测试 ROS 远程控制小车（实车场景）

1、将 arduino 的 USB 数据线连接至树莓派的 USB 3.0 端口上（之前是连接在 PC 上）,将小车上电、恢复用 arduino_bridge 驱动包控制时的硬件连接状态。

 2、在树莓派中先 source devel.setup.bash，然后 roslaunch 启动 arduino 的控制节点

这时可以看到 rosmaster 在启动后读取了 my_arduino.yaml 文件中的配置参数，并将里程计消息数据进行了实时发布。

 3、在从机上启动键盘控制节点，发布速度指令

4、这时小车已经开始运动，并且可以在 RVIZ 中通过添加 odometry 来观察小车的运动轨迹

4.3.5 VScode 远程开发

1、添加 ssh 主机

启动VSCode、打开插件、应用商店搜索 remote develop、安装远程开发插件

ctrl + shift + p 打开命令行、输入 remote-ssh:connect to 

选择添加一个 ssh 远程主机

添加 ssh 命令

选择第一个选项、或回车完成添加

完成后有提示

 2、远程开发

ctrl + shift + p 进入命令行、选择第一项

选择远程 IP

 进入远程终端

 file => open file 打开远程文件

ctrl + shift + p 更换目录、ctrl + shift + ~进入终端

4.3.6 常见问题：

1、USB 端口号冲突导致 launch 启动失败

roslaunch ros_arduino_python arduino.launch 启动失败，无法连接 arduino、或者其他设备（比如：雷达）的 launch 文件启动异常，关于雷达的设备名在 4.4.1 章节中描述。

解决方案: 先检查硬件连接是否异常、检查 arduino 上电指示灯是否常亮，排除硬件异常后检查USB 端口是否冲突：这里发现原本属于 arduino 的USB0 端口号自动映射到了雷达上

修改 ros_arduino_launch 的配置文件中的USB端口号

 再次 roslaunch ros_arduino_python arduino.launch 后发现设备名已经切换，arduino 启动成功

2、waiting for subscriber to connected to cmd_vel

问题描述：树莓派（ros 从机）启动 arduino.launch 文件、虚拟机（从机）启动键盘控制节点，主机无法订阅到该消息，持续线束：等待订阅者订阅该消息。表示从机端的 ros 并没有接收到远程主机端 ros 所发布的话题请求，说明 ros 的分布式环境出现问题。

 排查 1：查看从机端是否能接收到话题，能接收到话题但发布的消息订阅不到

排查2：查看从机端、主机端的 /etc/hosts 文件配置是否异常，都添加了对方的hostname、IP

 排查3：查看 ~/.bashrc 文件配置是否异常，发现主从机的配置仍然存在

排查4：ifconfig 查看网卡 IP 是否发生变化 ，这里发现从机端静态 IP 尾号从105 变成 100，导致 ros 主机发布的话题请求无法传达到从机端，这里需要检查 IP 漂移的原因。

主机 Ping 不通 从机 IP，从机可以 ping 通主机，间接说明了从机端 IP 地址发生变化，但仍然在同一网段，所以在从机端可以订阅到主机的话题，但主机端接收不到来到从机端发布的话题。查看 IP 配置如下：

 原因可能是之前尝试从桥接模式切换了 NAT，再次切换回来以后的原来的 IP 配置被系统删除。按照 4.3.2 节的静态 IP 重新配置为静态 IP

 重新测试 ros 分布式控制：在树莓派启动 arduino.launch 文件，虚拟机端启动键盘控制节点

4.4 激光雷达

4.4.1 硬件添加

1、连接至树莓派 USB3.0接口，上电自启动。

 2、在树莓派中查看设备的端口号

 3、添加到用户组

sudo usermod -a -G dialout your_user_name

4、下载雷达驱动包

git clone https://github.com/slamtec/rplidar_ros

5、返回工作空间重新编译、source 环境、为端口设置别名

cd src/rplidar_ros/scripts/
./create_udev_rules.sh

6、查看设备别名

4.4.2 软件测试

1、radar.launch 文件内容修改

 2、执行 rplidar.launch 文件

roslaunch rplidar_ros rplidar.launch

发现异常，ros 路径下找不到雷达的节点文件

 尝试安装 ros 对应的功能包

重新运行 rplidar.launch 文件，雷达启动成功

3、rviz 中查看探测结果 

4.4.3 常见问题

1、无法启动 rplidar.launch 文件

解决：在连接多个 USB 设备后，端口名容易发生混淆，使用别名可以将设备名自动映射为USB端口号，修改后重新启动成功。

4.5 相机

本次实验由激光雷达测距、建图，相机部分不再做详细介绍。

1、设备名称

ll /dev/video1

2、下载 ros 的相机驱动

sudo apt-get install ros-ROS版本-usb-cam

 3、查找是否安装成功

4、查看 launch 文件内容

5、直接启动 相机 launch 文件

roslaunch usb_cam usb_cam-test.launch

无法打开窗口报错：

5、注释掉窗口查看的节点 2，重新启动 launch 文件。

4.6 传感器集成

4.6.1 launch 文件

1、新建集成雷达、相机的功能包、新建 集成的 launch 文件

 2、编写新的 launch 文件，添加 arduino、相机、雷达的 launch 文件的包含路径

 3、回到工作空间中编译

 ​​​4、启动 mycar_launch 文件 

 5、在虚拟机端启动 rviz 节点，在 rviz 中添加雷达组件

6、启动键盘控制节点

 7、添加里程计组件

4.6.2 坐标变换

现在虽然可以在 rviz 中分别启动里程计、雷达，但二者使用的坐标系不同，且二者没有任何关系，所以没有正确的 TF 坐标变换，无法在同一坐标系下显示，所以需要添加静态坐标变换。

1、在 launch 文件夹下，创建关于坐标变换的 start_tf.launch 文件

2、添加静态坐标转换节点，分别测量雷达、摄像头中心位置关于跟坐标系（穿过车轮中心的地面垂线与地面交点）的相对位置关系，将参数传入静态坐标系节点的参数中，并且再将旧的 start.launch 文件路径也包含在新的 start_tf.launch 文件中。

3、启动新的 launch 文件

4、在虚拟机端启动 rviz，通过 TF 查看坐标树关系是否建立OK 

 5、将里程计数据、雷达数据、摄像头数据同时添加

6、在虚拟机端启动键盘控制节点，查看小车运动轨迹

5、小车导航系统

ROS 提供了一套完整的 2D 机器人导航 (Naigation) 功能包，包含： SLAM 建图、地图服务、定位、路径规划、底盘控制，这套导航系统的缺点在于它是针对差速轮式机器人设计的，实现导航前提是机器人底盘只接收 3 个运动命令：纵向速度 Vx、横向速度 Vy、横摆角速度。

ROS 导航系统与无人驾驶的实现类似，都涉及环境感知、地图构建、自身定位、路径规划、底盘控制等关键技术，区别在于机器人导航通常研究负责室内某些特定功能的小车，如：AGV 送货、送餐等，而无人驾驶的意义更在于改善交通系统，所以研究侧重点的是舵机转向的四轮车辆。

5.1 添加模型

为了在导航过程中更直观地显示小车的实时位置、姿态，可以添加小车的 urdf 模型文件，另外由于 urdf 中会要求给出各个组件的坐标关系，如：laser 相对 base_link、base_link 相对 base_footprint 等，所以不再需要单独发布不同坐标系之间的静态坐标变换。

5.1.1 创建 urdf 模型文件

1、首先在工作空间的 src 路径下，创建一个用于描述小车模型的功能包 mycar_description、添加依赖项 urdf、xacro

2、在该功能包下新建 urdf 文件夹，添加 car_base.urdf.xacro、car_camera.urdf.xacro、car_laser.urdf.xacro 文件，分别是小车底盘、摄像头、雷达的模型文件

小车底盘模型： car_base.urdf.xacro 参数部分

雷达模型 car_laser.urdf.xacro 参数部分

相机 car_camer.urdf.xacro 参数部分

3、在同一个 urdf 目录下，创建 car.urdf.xacro，包含底盘、相机、雷达的 urdf 文件

4、在 mycar_description 功能包下，创建 launch 文件夹，添加 car.launch：包含小车的模型描述文件 mycar_description；另外启动 robot_state.publisher、joint_state.publisher，分别实现静态坐标转换、关节自由运动

5.1.2 启动 car.launch 文件

1、在主机中启动 car.launch 文件

2、回到虚拟机端，启动 rviz、添加 RobotModel、TF、观察 TF tree 中各个坐标之间的关系

5.1.3 启动 start.launch （集成小车模型、雷达、相机）

1、将车辆模型描述文件添加到 start.launch 文件，这样可以一次性启动 arduino、雷达、相机、及车辆模型、并且车轮模型中已经添加了雷达、相机等组件相对 base_footprint 的相对坐标关系。相对于启动 start_tf.launch 文件来比，这种方式还可以在 rviz 中观察到车辆的模型。

2、启动 start.launch 文件

3、在虚拟机上启动 rviz、添加 robotModel、 TF、LaserScan、Odometry

 5.2 地图服务

SLAM(simultaneous localization and mapping) 为实时定位与建图：小车在未知环境中从任一点移动，在移动过程中根据位置估计和地图进行实时定位，在定位的同时构建增量式地图，覆盖完整场地后绘制出一张全局地图。 gmappin 是一种常用的 SLAM 技术，需要小车有环境感知能力，依赖于激光雷达、深度相机等传感器。

首先在树莓派中安装 ros-melodic-gmapping 、map-server 功能包

sudo apt install ros-<ROS版本>-gmapping // 构建地图
sudo apt install ros-<ROS版本>-map-server // 保存、读取地图

在工作空间 src 下创建功能包 nav，导入依赖项：roscpp、rospy、std_msgs、gmapping 、map_server、 amcl、 move_base，执行 catkin_make 确认依赖项添加 OK

5.2.1地图绘制 gmapping

1、在 nav 文件夹下新建 launch 文件夹、编写 gmapping.launch 文件

2、启动 start.launch 文件

3、执行 gmapping.launch 文件

4、在虚拟机端启动 teleop_twist_keyboard 和 rviz，添加：robotModel、TF、Map 、LaserScan，通过键盘控制节点，控制小车行驶轨迹覆盖场地后，自动创建地图，创建完成后不要关闭，等待下一步地图保存的服务。

5.2.2 地图保存 map_saver

1、先在 nav 文件夹下新建 map 文件夹，用于存储地图文件；在 launch 文件夹下，新建 map_save.launch 文件，将 /map/nav 添加到节点参数中，其中 nav 是地图文件名，可以自定义

 2、在 gmapping 为开启的状态下，执行 map_save.launch 

3、完成后可以关闭所有节点，生成的地图文件为 nav/map 路径下的 nav.yaml 和 nav.pgm

5.2.3 地图读取 map_server

1、在 launch 路径下新建一个 map_server.launch 文件，用于启动离线地图读取服务

2、执行 map_server.launch 文件

3、在虚拟机端启动 rviz、添加 Map、话题 /map

5.3 AMCL 定位

AMCL(Adaptive Monte Carlo Localization) 是用于平面机器人的概率定位系统，可以根据已有的地图，使用粒子滤波器推算机器人的位置。里程计本身也可以用于定位，但由于里程计存在累积误差，且机器人出现打滑时测算不准，容易造成定位误差，而 AMCL 则通过估计机器人在地图坐标系下的姿态，结合里程计数据，提高定位精度。

5.3.2 amcl.launch 文件

1、在 nav/launch 路径下，新建 amcl.launch 文件

2、由于 amcl 的执行依赖于离线地图文件 nav.yaml，所以这里需要新建一个 test_amcl.launch 集成文件，包含 amcl.launch 文件、map_server

5.3.2 执行 test.amcl.launch 文件

1、先启动机器人的 start.launch 文件

2、再启动 test_amcl.launch

3、在虚拟机端启动 teleop_twist_keyboard、 rviz，添加 robotModel、Map、LaserScan、 添加 posearray（ topic 为 particlecloud）查看小车当前的预估位姿，箭头的密集程度表示小车处于该位置的概率

4、另外注意到，雷达图像与离线地图不重叠，解决方法是：在菜单栏中点击 2D poseEstimate ，在地图中选取小车所在的实际位置，然后通过键盘控制节点微调小车的位置、或者微调 2D poseEstimate 的选取点位，使雷达图像与离线地图重合

5.4 路径规划

路径规划即机器人从 A 点运动到 B 点的过程，机器人根据目标位置搜索全局路径，并且在运动过程中，根据障碍物的信息，调整运动路线，直至到达终点。ROS 的导航功能包中提供了 move_base 作为路径规划器，包括全局路径规划与局部路径规划。其中全局路径规划器 global_planner 使用了 Dijkstra 或 A* 搜索算法，给出最优全局路径； 局部路径规划器 local_planner 使用 Dynamic Window Approaches 算法对障碍物进行规避，结合全局路径，计算出当前的最优局部路径。

move_base 功能包订阅 move_base_msgs/MoveBaseActionGoal 类型的消息作为运动规划的目标，同时发布 move_base_msgs/MoveBaseActionFeedback 将底盘坐标作为反馈消息进行发布。底盘的运动控制则是由 move_base发布的 geometry_msgs/Twist 类型的消息给下位机，下位机如 Arduino、stm32 等控制器将接收到的速度指令转化为驱动电机的脉冲信号，从而实现底盘的驱动

5.4.1 move_base.launch 文件

1、首先安装 ros-melodic-navigation 功能包

sudo apt install ros-<ROS版本>-navigation // 路径规划、运动控制

2、在 nav / launch 下创建 move_base.launch ，用于启动 move_base 节点、 params 文件

3、在 / nav 下创建 param 文件夹、创建 yaml 文件，这里的 yaml 是自定义的参数配置文件

通用配置文件 costmap_common_params.yaml，用来存储障碍信息、传感器

全局规划配置文件 global_costmap.params.yaml 存储全局代价地图的参数

本地规划配置文件 local_costmap_params.yaml 存储代价地图的配置参数

本地规划器 base_local_planner_params.yaml 根据全局路径计算速度指令，将消息发布

3、单独的 move_base 功能包无法执行，需要加载全局地图、自身定位、以及 move_base 的 launch 文件。在 nav / launch 文件夹下创建 nav.launch 文件

5.4.2 执行路径规划

1、启动机器人 start.launch 文件（包含：arduino、摄像头、雷达、TF）

2、启动 nav.launch 文件

3、虚拟机端启动 rviz，添加 Map、robotModel、laserScan

4、添加 posearray、Map_global（由 Map 重命名，话题为：global_costmap）、Map（由 Map  重命名，话题为：local_costmap）、path_local（由 path 重命名，表示全局路径）、 path_global（本地路径），将 rviz 配置文件保存为 /home/car_nav.rviz

5、在菜单栏选择 2D Nav Goal，将鼠标放置在地图目标点上，会自动规划出全局路径（绿色线条）以及局部路径（自定义为红色），（为了更好地显示小车的运动轨迹，将 posearray 关闭​​​​）

绿色长线表示全局路径，红色短线条表示局部路径

6、潜在异常：通过 2D Nav Goal 发布了目标位置后，小车静止不动，并且在终端窗口抛出关于坐标变化的异常 Timed out waiting for transform from base_footprint to map to become available before running costmap，即坐标变换等待超时，这是由于 amcl 的超时等待时间过短造成的.

解决方法 1：在 nav/launch/amcl.launch 文件中将 transform_tolerance 的值修改为 0.5，重新启动并加载所有 launch 文件后，小车行进正常

解决方法 2：在虚拟机中通过启动 rqt 的方式修改 transform_tolerance 为 0.5，观察到小车已经开始朝目标点运动。

5.4.3 调试常见问题

问题 1：行驶过程中出现多次原地转圈。造成的原因可能是局部路径过短，当小车旋转速度太大时，偏离全局路径程度太大，这时需要适当增大 sim_time ，即延长局部路径规划器的仿真预测时间

在 rqt 中实现动态调参：在虚拟机端命令行输入 rqt，在插件中添加 Dynamic Reconfigure

设置 sim_time 为 2.0 或更大

这时局部路径路线更长，减少旋转过冲现象（更新了小车模型和静态地图）

问题 2：在进入障碍物的膨胀区、或撞到障碍物后，小车无法规划出新的路径，造成迷失

原因：全局地图和本地地图的障碍物膨胀系数设置不合理

改善：将全局地图的障碍物膨胀系数调大，以便在规划全局路径时尽可能远离障碍物，将局部地图的障碍物膨胀系数减小，以便在靠近障碍物时尽可能避免进入膨胀区域。

5.4.4 自主建图

1、在 nav/launch 路径下新建 auto_slam.launch，用于集成 SLAM 与 move_base 的相关节点

 ​​​​​​2、启动 start.launch 节点、auto_slam.launch 节点

roslaunch mycar_start start.launch # 启动机器人模型
roslaunch nav auto_slam.launch     # 启动自动建图

3、在虚拟机端启动 rviz，打开 /home/car_nav.rviz 配置文件，取消掉 Map_global 和 Map_local的组件，通过 2D Nav Goal 引导小车覆盖场地后完成建图

4、启动 map_save.launch 节点，将地图保存为本地磁盘 nav/map/map.yaml

roslaunch nav map_save.launch # 静态地图序列化，保存在磁盘中

6、总结

ROS navigation 导航功能包包含了 SLAM 建图、AMCL 定位、以及 move_base 路径规划器的基本功能，实现了小车的自主移动以及主动避障功能，但局限性在于该功能包是针对差速轮式机器人设计的，move_base 发布的差速指令决定了该功能包无法直接移植到前轮转向、后轮驱动的无人驾驶车辆控制系统。

参考文献：

1.2 ROS安装 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程文章来源地址https://www.toymoban.com/news/detail-455032.html

到了这里，关于树莓派+ROS+Arduino造一台导航小车(完整代码+硬件调试)的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！