基础教程连接：

Introduction · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程http://www.autolabor.com.cn/book/ROSTutorials/

一：机器人平台设计

机器人组成部分主要有：

执行机构: 主体使用亚克力板拼装，由两个直流电机带动主动轮以及保持平衡的两个万向轮实现机器人行走，由于执行机构比较简单，不再做单独介绍。

驱动系统: 电池、arduino 以及电机驱动模块；

控制系统: 树莓派；

传感系统: 编码器、单线激光雷达；

1.机器人平台设计之arduino基础

使用的 arduino mega 2560，软件部分则是Arduino IDE。

1.1  通过串口，由 arduino 向计算机发送数据

/*
 * 需求:通过串口，由 arduino 向计算机发送数据
 * 实现:
 *  1.setup中设置波特率
 *  2.setup 或 loop 中使用 Serial.print 或 Serial.println() 发送数据
 * 
 * 
 * 
 */
void setup() {
  Serial.begin(57600);
  Serial.println("setup");
}

void loop() {
  delay(3000);
  Serial.print("loop");
  Serial.print("  ");
  Serial.println("hello");
}

 1.2 通过串口，由计算机向Arduino发送数据

/*
 * 需求:通过串口，由计算机向 arduino 发送数据
 * 实现:
 *  1.setup中设置波特率
 *  2.loop 中接收发送的数据，并打印
 * 
 * 
 * 
 */
char num;
void setup() {
  Serial.begin(57600);
}

void loop() {
  if(Serial.available() > 0){
    num = Serial.read();
    Serial.print("I accept:");
    Serial.println(num);  
  }
}

2.机器人平台设计之电机驱动

机器人平台使用的电机为直流减速电机，电机驱动板为基于L298P实现的电路板。

2.1 直流减速电机

主要由三部分构成: 减速箱、电机主体、编码器。

编码器参数：M1: 电机电源+(和M2对调可以正反转 )、GND: 编码器电源-、C2: 信号线、C1: 信号线、VCC:编码器电源+、M2: 电机电源-(和M1对调可以正反转)。

2.2 电机驱动板

采用一款基于L298P优化的电机驱动板。

2.3 电机基本控制实现

1. 左电机的M1与M2对应的是引脚4(DIRA)和引脚5(PWMA)，引脚4控制转向，引脚5输出PWM。右电机的M1与M2对应的是引脚6(PWMB)和引脚7(DIRB)，引脚7控制转向，引脚6输出PWM。  

2. 可以通过PWM控制电机转速。

  digitalWrite(DIRA,HIGH);
  analogWrite(PWMA,100);
  /*
   * 注意: 
   * 1.可以通过将DIRA设置为HIGH或LOW来控制电机转向，但是哪个标志位正转或反转需要根据需求判断，转向是相对的。
   * 2.PWM的取值为 [0,255],该值可自己设置。
   * 
   */

2.4 PID控制理论

P： 如果实现上述场景中的车速控制，一种简单的实现方式是: 确定目标速度，获取当前速度，使用(目标速度-当前速度)*某一系数 计算结果为输出的PWM，再获取当前速度，使用(目标速度-当前速度)*某一系数 计算结果为输出的PWM并输出...如此循环在上述模型中，调速实现是一个闭环，每一次循环都会根据当前时速与目标时速的差值，再乘以以固定系数，计算出需要输出的PWM值，这其中的系数，称之为比例。

I：上述模型算法中，最终速度与预期速度存在稳态误差，这意味着最终结果可能永远无法达成预期，解决的方法就是使用积分I。每次调速时，输出的PWM还要累加根据积分I计算的结果，以消除静态误差。

D：当I值设置的过大时，可能会出先"超速"的情况，超速之后可能需要多次调整，产生系统震荡，解决这种情况可以使用D微分，当速度越是接近目标速度时，D就会越施加反方向力，减弱P的控制，起到类似”阻尼“的作用。通过D的使用可以减小系统震荡。

3.机器人平台设计之底盘实现

在ros中还提供了一个已经封装了的模块: ros_arduino_bridge，该模块由下位机驱动和上位机控制两部分组成，通过该模块可以更为快捷、方便的实现自己的机器人平台。

3.1 ros_arduino_bridge 简介

该功能包包含Arduino库和用来控制Arduino的ROS驱动包，它旨在成为在ROS下运行Arduino控制的机器人的完整解决方案。

• 如果只是安装调试下位机，那么不必须安装ROS系统，只要有 arduino 开发环境即可；

• 而上位机调试，适用于 ROS Indigo 及更高版本，但是暂不支持最新版本 noetic，所以上位机需要使用其它版本的ROS(建议 melodic)；

在ros_arduino_bridge 架构中，

结构虽然复杂，但是关注的只有两大部分:

• ros_arduino_bridge/ros_arduino_firmware/src/libraries/ROSArduinoBridge
• ros_arduino_bridge/ros_arduino_python/config/arduino_params.yaml

前者是Arduino端的固件包实现，需要修改并上传至Arduino电路板；

后者是ROS端的一个配置文件，相关驱动已经封装完毕，我们只需要修改配置信息即可。

整体而言，借助于 ros_arduino_bridge可以大大提高我们的开发效率。

4.机器人平台设计之控制系统

ROS是一种分布式设计框架，针对小型或微型机器人平台的控制系统，可以选择多处理器的实现策略。具体实现是“PC + 嵌入式”，使用嵌入式系统(树莓派)充当机器人本体的控制系统，而PC则实现远程监控，通过前者实现数据采集与直接的底盘控制，而后者则远程实现图形显示以及功能运算。

4.1 控制系统实现_分布式框架

为树莓派连接无线网络并为为树莓派配置静态IP（原本是动态ip，自动获取）。

（1）ROS分布式通信对网络配置有某些要求，先要保证不同计算机处于同一网络中，最好分别设置固定IP，如果为虚拟机，需要将网络适配器改为桥接模式；

（2）配置文件修改，分别修改不同计算机的 /etc/hosts 文件，在该文件中加入对方的IP地址和计算机名。

#主机端：

从机的IP    从机计算机名

#从机端：
主机的IP    主机计算机名

设置完毕，可以通过 ping 命令测试网络通信是否正常。

IP地址查看名: ifconfig

计算机名称查看: hostname

（3）配置主机和从机的IP，在主机和从机的~/.bashrc 追加：

export ROS_MASTER_URI=http://主机IP:11311
export ROS_HOSTNAME=主机IP

4.2 控制系统实现_ssh远程连接

具体实现看教程。

4.3 控制系统实现_安装ros_arduino_bridge

ros_arduino_bridge的ROS端功能包主要是使用 ros_arduino_python，程序入口是该包launch目录下的arduino.launch文件。

需要载入yaml格式的配置文件，该文件在 config 目录下已经提供了模板，配置文件里面主要写了一些参数的值。

5. 机器人平台设计之传感器

5.1 传感器_雷达使用

使用的是思岚A1单线激光雷达，

USB查看命令:

ll /dev/ttyUSB*

授权(将当前用户添加进dialout组，与arduino类似):

sudo usermod -a -G dialout your_user_name

重启之后才可以生效。

需要下载相关雷达驱动包并编译，启动其中的launch文件，即可启动雷达，雷达开始发布话题。

5.2 传感器_相机使用

USB查看命令:

ll /dev/video

软件准备:

安装USB摄像头软件包，命令如下:

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

或者也可以从 github 直接下载源码:

git clone https://github.com/ros-drivers/usb_cam.git

 其中软件包内含有启动用到的launch文件，内容包括：相机的启动节点usb_cam、图形化窗口的方式显示图像数据的节点image_view。

5.3 传感器集成

所谓集成主要是优化底盘、雷达、相机相关节点的启动并通过坐标变换实现机器人底盘与里程计、雷达和相机的关联，实现步骤如下:

1. 编写用于集成的 launch 文件；（可以同时运行多个节点）
2. 发布TF坐标变换；（建立不同坐标系只见的联系）

二：实体机器人导航实现

1.准备工作

1.1 功能包的安装：

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

• 安装 gmapping 包(用于构建地图):sudo apt install ros-<ROS版本>-gmapping

• 安装地图服务包(用于保存与读取地图):sudo apt install ros-<ROS版本>-map-server

• 安装 navigation 包(用于定位以及路径规划):sudo apt install ros-<ROS版本>-navigation

新建功能（包名自定义，比如：nav），并导入依赖: gmapping map_server amcl move_base

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

创建功能包:

catkin_create_pkg mycar_description urdf xacro。

2. 导航实现SLAM建图

2.1 gmapping简介

gmapping 是ROS开源社区中较为常用且比较成熟的SLAM算法之一，gmapping可以根据移动机器人里程计数据和激光雷达数据来绘制二维的栅格地图。

gmapping 安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-gmapping

2.2 gmapping节点说明

gmapping 功能包中的核心节点是:slam_gmapping。为了方便调用，需要先了解该节点订阅的话题、发布的话题、服务以及相关参数。

（1）订阅的Topic

tf (tf/tfMessage)

• 用于雷达、底盘与里程计之间的坐标变换消息。

scan(sensor_msgs/LaserScan)

• SLAM所需的雷达信息。

（2）发布的Topic

map_metadata(nav_msgs/MapMetaData)

• 地图元数据，包括地图的宽度、高度、分辨率等，该消息会固定更新。

map(nav_msgs/OccupancyGrid)

• 地图栅格数据，一般会在rviz中以图形化的方式显示。

~entropy(std_msgs/Float64)

• 机器人姿态分布熵估计(值越大，不确定性越大)。

（3）服务

dynamic_map(nav_msgs/GetMap)

• 用于获取地图数据。

2.3 编写gmapping节点相关launch文件

gmapping是下载的功能包，我们不能对其源码进行修改，只能修改其内部设置好的一些参数，通过编写gmapping节点相关launch文件进行参数的设置。

主要的一些参数有：雷达的话题名称，底盘的坐标系，里程计坐标系，地图更新频率，激光探测的有效距离等。

3. 导航实现地图服务

3.1 map_server简介

map_server功能包中提供了两个节点: map_saver 和 map_server，前者用于将栅格地图保存到磁盘，后者读取磁盘的栅格地图并以服务的方式提供出去。

map_server安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-map-server

3.2 map_saver 和 map_server节点的使用

（1）map_saver

订阅的topic:

map(nav_msgs/OccupancyGrid)

• 订阅此话题用于生成地图文件。

使用的话，可以编写一个launch文件。

结果会在指定路径下会生成两个文件，xxx.pgm 与 xxx.yaml：

xxx.pgm 本质是一张图片，直接使用图片查看程序即可打开。

xxx.yaml 保存的是地图的元数据信息，用于描述图片。

（2） map_server

发布的话题：

map_metadata（nav_msgs / MapMetaData）

• 发布地图元数据。

map（nav_msgs / OccupancyGrid）

• 地图数据。

服务

static_map（nav_msgs / GetMap）

• 通过此服务获取地图。

参数

〜frame_id（字符串，默认值：“map”）

• 地图坐标系。

4. 导航实现定位

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

4.1 amcl简介

AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。

amcl已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

4.2 amcl节点说明

amcl 功能包中的核心节点是:amcl。

订阅的Topic

scan(sensor_msgs/LaserScan)

• 激光雷达数据。

tf(tf/tfMessage)

• 坐标变换消息。

initialpose(geometry_msgs/PoseWithCovarianceStamped)

• 用来初始化粒子滤波器的均值和协方差。

map(nav_msgs/OccupancyGrid)

• 获取地图数据。

发布的Topic

amcl_pose(geometry_msgs/PoseWithCovarianceStamped)

• 机器人在地图中的位姿估计。

particlecloud(geometry_msgs/PoseArray)

• 位姿估计集合，rviz中可以被 PoseArray 订阅然后图形化显示机器人的位姿估计集合。

tf(tf/tfMessage)

• 发布从 odom 到 map 的转换。

服务

global_localization(std_srvs/Empty)

• 初始化全局定位的服务。

request_nomotion_update(std_srvs/Empty)

• 手动执行更新和发布更新的粒子的服务。

set_map(nav_msgs/SetMap)

• 手动设置新地图和姿态的服务。

调用的服务

static_map(nav_msgs/GetMap)

• 调用此服务获取地图数据。

4.3 坐标变换

里程计本身也是可以协助机器人定位的，不过里程计存在累计误差且一些特殊情况时(车轮打滑)会出现定位错误的情况，amcl 则可以通过估算机器人在地图坐标系下的姿态，再结合里程计提高定位准确度。

5. 导航实现路径规划

在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。

5.1 move_base简介

move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。如前所述, move_base主要由全局路径规划与本地路径规划组成。

move_base已经被集成到了navigation包，navigation安装前面也有介绍，命令如下:

sudo apt install ros-<ROS版本>-navigation

5.2 move_base节点

move_base功能包中的核心节点是:move_base。

具体看教程。

5.3 代价地图

ROS中的地图其实就是一张图片，这张图片有宽度、高度、分辨率等元数据，在图片中使用灰度值来表示障碍物存在的概率。不过SLAM构建的地图在导航中是不可以直接使用，其基础之上需要添加一些辅助信息的地图，比如时时获取的障碍物数据，基于静态地图添加的膨胀区等数据。

代价地图有两张:global_costmap(全局代价地图) 和 local_costmap(本地代价地图)，前者用于全局路径规划，后者用于本地路径规划。

两张代价地图都可以多层叠加,一般有以下层级:

• Static Map Layer：静态地图层，SLAM构建的静态地图。

• Obstacle Map Layer：障碍地图层，传感器感知的障碍物信息。

• Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。

• Other Layers：自定义costmap。

多个layer可以按需自由搭配。

5.4 碰撞算法

代价地图中栅格的灰度值表示如下：

• 致命障碍:栅格值为254，此时障碍物与机器人中心重叠，必然发生碰撞；

• 内切障碍:栅格值为253，此时障碍物处于机器人的内切圆内，必然发生碰撞；

• 外切障碍:栅格值为[128,252]，此时障碍物处于其机器人的外切圆内，处于碰撞临界，不一定发生碰撞；

• 非自由空间:栅格值为(0,127]，此时机器人处于障碍物附近，属于危险警戒区，进入此区域，将来可能会发生碰撞；

• 自由区域:栅格值为0，此处机器人可以自由通过；

• 未知区域:栅格值为255，还没探明是否有障碍物。

膨胀空间的设置可以参考非自由空间。