三轮全向底盘实现SLAM导航功能

1. 功能说明

    本文我们将利用键盘控制三轮全向底盘（R313b）小车运动完成slam建图，并能在已建好的地图里进行自主导航。slam导航可以拆分为三步：

    第一步：能用键盘控制底盘的运动；

    第二步：基于实际场景，用键盘控制底盘小车进行slam建图；

    第三步：基于已建好的地图，模拟实现slam导航。

2. 电子硬件

   本实验中采用了以下硬件：

主控板	Arduino mega2560控制板
扩展板	STM32扩展板
电池	11.1V动力电池
电池	树莓派、雷达、显示屏、键盘、鼠标、充电宝/手机充电器

 按下图进行电路连接：

第一步：先将步进电机与主板的扩展板进行连接（提示：侧重看x、y、z的接线位置，装有雷达的一侧代表底盘小车的车头。雷达左侧的步进电机  

控制小车的X轴方向；雷达的右侧的步进电机控制小车的Y轴方向；雷达对侧的步进电机控制小车的Z轴方向，具体的接线引脚见下图）

第二步：将主控板的扩展板跟树莓派连接起来（见图中蓝色的线），并用锂电池进行供电。

第三步：将鼠标、键盘、显示屏与树莓派进行连接（见下图）。其中显示屏和树莓派用充电宝供电，鼠标、键盘这里用的是无线的。

 第四步：将雷达与树莓派进行连接（建议把雷达连接在树莓派的usb3.0上，见下图）。

经过上边几步的电路连接后，就完成了相应的电路连接（见下图）。 

3. 功能实现

• 操作系统：Ubuntu18.04系统，基于Debian GNU/Linux，支持x86、amd64（即x64）、ARM和ppc架构。
• 仿真系统：基于开源机器人操作系统ROS melodic和开源软件平台Arduino开发，上位机采用ROS melodic，基于Rviz完成全向移动底盘小车slam导航运动规划，采用gazebo完成全向移动底盘小车物理运动仿真；下位机采用Arduino实现对全向移动底盘小车运动的控制。
• 编程环境：Arduino 1.8.19

3.1 键盘控制底盘小车运动

（1）实现思路

      当按下键盘上指定的键时，可以实现三轮全向底盘小车前进、后退、转向、平移；还可以灵活的设置底盘小车的角速度、线速度来调整底盘小车的运动。下面是本实验中规划的控制底盘小车的键盘命令（见下表），可以先熟悉一下，后续在控制底盘小车时会用到。

分类	运动指令
	键盘快捷键	指令含义
基本运动	i	前进
	，	后退
	j	左转
	l	右转
平移	J	左平移
	L	右平移
调整角速度与线速度	q	增大底盘小车最大速度的10%（包含角速度与线速度）
	z	减小底盘小车最大速度的10%（包含角速度与线速度）
调整线速度	w	仅仅增大底盘小车线速度的10%
	x	仅仅减小底盘小车线速度的10%
调整角速度	e	仅仅增大底盘小车角速度的10%
	c	仅仅减小底盘小车角速度的10%
	除了上面的按键其他按键（如：k）	停止
	Ctrl+c	程序结束

（2）操作步骤

① 打开参考例程文件夹（src\Arduino_Programing\Control_FuLaiLun_Car），下载程序Control_FuLaiLun_Car.ino：

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-16 https://www.robotway.com/

  ------------------------------*/

/***************************************************************************************************

实验功能：

         驱动全向三轮底盘实现前进、后退、左转、右转、左平移、右平移


实验接线：

         步进电机细分：16细分

         步进电机方向引脚：dir: x: 44, y: 32, z: 42, a: 30

         步进电机步进引脚：stp: x: 45, y: 33, z: 43, a: 31


实验现象：

         上电后，小车前进3秒，停止5秒；后退3秒，停止5秒；

         左平移3秒，停止5秒；右平移3秒，停止5秒；左转3秒，停5秒，右转3秒，停5秒，小车停止。

***************************************************************************************************/

//DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要

//#define USE_USBCON          //PAOGRAMMING PORT

#define USE_NATIVE_USB      //NATIVE USB PORT

#include <ros.h>

#include <ros/time.h>

#include <geometry_msgs/Vector3.h>

ros::NodeHandle   nh;

#define Action_implement_delay 3000 //单个动作执行所用时间

#define Action_wait_for_delay 5000   //动作与动作之间的间隔时间

#define Set_Speed_Test 0.04

void XYRun(double vx, double vy, double w);

void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}

ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );


geometry_msgs::Vector3 pose_message;

ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);

geometry_msgs::Vector3 vel_message;

ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);


const int kMessagePubRate = 5;

unsigned long message_pub_time = 0;

const int kReadMotorDeltaT = 50;

unsigned long position_read_time = 0;

float current_x = 0,current_y = 0,current_a = 0;

float current_vx = 0,current_vy = 0,current_va = 0;


void setup()

{

  delay(1000);

  Serial.begin(57600);

  initMotor();

//   Stepper_move_test();

  nh.initNode();

  nh.subscribe(vel_cmd_sub);

  nh.advertise(pose_feedback_pub);

  nh.advertise(vel_feedback_pub);

  XYSetVel(0.0,0.0,0.0);

  position_read_time = millis();

  message_pub_time = millis();

}


void loop()

{

  XYRun();  

  if(millis()>position_read_time)

  {

    XYRead();

    position_read_time += kReadMotorDeltaT;

  }

  if(millis()>message_pub_time)

  {

    pose_message.x = current_x;

    pose_message.y = current_y;

    pose_message.z = current_a;

    vel_message.x = current_vx;

    vel_message.y = current_vy;

    vel_message.z = current_va;

    pose_feedback_pub.publish(&pose_message);

    vel_feedback_pub.publish(&vel_message);

    message_pub_time+=1000.0/kMessagePubRate;

  }

  nh.spinOnce();  

}

② 启动雷达、键盘、rosserial程序包。打开终端,输入roslaunch robot_navigation_control robot.launch命令（见下图），等待程序的运行启动界面

 成功启动后，可以在终端看到底盘小车的当前speed（线速度）为0.04，turn（角速度）为0.08。【注意：speed指的是线速度的大小（包含方向），turn指的是角速度的大小（包含方向）】

 启动后的界面（见下图）:

 ③ 尝试按下键盘命令，控制底盘小车的运动。【注意：一、请先保证终端是激活状态，见上图；二、按键盘时，稍微有点间隔，这样能给底盘小车转动的时间；三、一定要打开扩展板上的电源开关，否则会发现小车位置在rviz中位置虽然变化，但车不会运动】

假如现在希望先增加底盘小车的最大速度，再降低底盘小车的最大速度，则分别按下键盘上q、z命令，便可以观察到终端的结果（见下图）。

更多命令请自行尝试。

3.2 底盘小车同步定位与建图-SLAM

（1）实现思路     

      利用Gmapping算法对底盘小车所在的未知环境进行建立地图、同步定位、最后保存此地图。此地图可供后续底盘小车导航使用。

      Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法， 它基于粒子滤波，采用RBPF的方法，效果稳定。本次三轮全向底盘小车slam建图采用的是gmapping_slam包。下面介绍Gmapping SLAM计算图。Gmapping的作用是根据激光雷达和里程计（Odometry）的信息，对环境地图进行构建，并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据，而输出应当有自身位置和地图。下面我们从计算图（消息的流向）的角度来看看gmapping算法在实际运行中的结构：

位于中心的是我们运行的slam_gmapping节点，这个节点负责整个gmapping SLAM的工作。它的输入需要有两个：

输入

     /tf以及/tf_static：坐标变换，类型为第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有两个tf，一个是base_frame 与 laser_frame 之间的tf，即机器人底盘和激光雷达之间的变换；一个是base_frame 与 odom_frame之间的tf，即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。

      /scan：激光雷达数据，类型为sensor_msgs/LaserScan

      /scan很好理解，Gmapping SLAM所必须的激光雷达数据，而/tf 是一个比较容易忽 视的细节。尽管/tf 这个Topic听起来很简单，但它维护了整个ROS三维世界里的转换关系，而 slam_gmapping要从中读取的数据是base_frame 与 laser_frame 之间的tf，只有这样才能够把周围障碍物变换到机器人坐标系下，更重要的是base_frame 与 odom_frame之间的tf，这个tf 反映了里程计（电机的光电码盘、视觉里程计、IMU）的监测数据，也就是机器人里程计测得走了多少距离，它会把这段变换发布到odom_frame 和 laser_frame 92之间。因此slam_gmapping 会从/tf 中获得机器人里程计的数据。

输出

      /tf：主要是输出 map_frame 和 odom_frame 之间的变换

      /slam_gmapping/entropy：std_msgs/Float64 类型，反映了机器人位姿估计的分 散程度

      /map：slam_gmapping 建立的地图

      /map_metadata：地图的相关信息

     输出的/tf 里又一个很重要的信息，就是map_frame和odom_frame之间的变换，这 其实就是对机器人的定位。通过连通map_frame和odom_frame，这样map_frame与 base_frame 甚至与 laser_frame 都连通了。这样便实现了机器人在地图上的定位。

（2）操作步骤

① 先搭建一个实际场景，便于建立地图。【注意：为了方便识别，搭建的实际测试地图最好是不规则的区域。如下图测试的场地，在长方形区域的一边是不规则的，这样便于识别建立的仿真地图】

② 启动雷达、键盘、rosserial；打开终端并输入命令：roslaunch robot_navigation_control robot_car.launch （见下图）

③ 启动构建地图服务。按（ctrl+shift+T）打开第二个终端并输入：roslaunch four_macnum_slam four_macnum_slam.launch（见下图）

界面启动后，在rviz中选择“map”，可以在可视化界面看到有地图出现。下面根据实际环境进行建图：

【注意：一、请先保证键盘控制的终端是激活状态（请用鼠标点击一下此终端）；二、按键盘时，稍微有点间隔，这样能给底盘小车转动的时间】

按下键盘相关指令（键盘指令请参考3.1）来控制底盘小车在场地内运行，直至在Rviz内可以看到构建地图的轮廓，如下图所示（注意：地图环境会根据实际场景建立）。

底盘小车在未知环境建立地图

④ 为了后续在已知环境中运行底盘小车，需把此次建立的地图进行保存。需要先进入保存地图的文件夹，然后给地图命名，保存即可。例如我们把此次建立的地图命名为your_map_name。【提示：your_map_name为自定义的名称，您也可以按自己的需求进行命名】

下面是本实验中的具体操作

重新打开新终端（ctrl+shift+t）并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/maps

之后输入：rosrun map_server map_saver -f ./your_map_name

返回下图红色框的信息，代表成功保存地图。

查看生成的地图文件：

      上面我们用map_server来保存地图。这里我们来简单了解和查看一下刚才保存的地图文件（见下图）。其中：map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。

      地图文件，通常为pgm格式；

      地图的描述文件，通常为yaml格式

打开your_map_name.pgm，见下图：

your_map_name.yaml ，各个参数见下图。

其中占据的概率 occ = (255-color_avg)/255.0，color_avg为RGB三个通道的平均值。

3.3 底盘小车导航-Navigation

（1）实现思路

        在上述3.2搭建好的实际场景中，实现三轮全向底盘小车的导航效果。     

• 具体思路：加载上述3.2建好的地图，设置底盘小车的初始位置和方向，设定目标位置及方向，底盘小车就可以实现导航效果。     
• Navigation与底盘关系：ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程，作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能， 配置导航功能包的一些参数。      

在ROS中进行导航需要使用到的三个包是：

       ① move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；

       ② gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；

       ③ amcl：根据已经有的地图进行定位。

基于上图，我们来看看底盘简单对应关系。

（2）操作步骤

① 将创建的地图应用到导航程序中。

这里我们需要修改four_macnum_navigation.launch文件，把上述3.2的your_map_name.yaml 添加到此launch文件中。步骤如下：

打开终端并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch

之后再输入：gedit four_macnum_navigation.launch

② 启动雷达、键盘、rosserial。

在该终端继续输入：roslaunch robot_navigation_control robot_car.launch（见下图）

③ 按下（ctrl+shift+T）打开第二个终端并输入：roslaunch four_macnum_navigation four_macnum_navigation.launch（见下图）

启动界面后，可以看到许多红色的箭头，代表底盘小车的姿态估计还不准确。

④ 开始调整底盘小车的姿态。

第一步：请先旋转地图，确保地图和实际场景的方向基本一致。

第二步：使用2D Pose Estimate 标定底盘小车位于地图中的初始位置及车头指向（见下图）。

第三步：鼠标点击有roslaunch robot_navigation_control robot_car.launch的终端后，尝试按下键盘命令控制底盘小车运动（键盘命令请参考3.1），最好尽可能多的消除地图中的箭头【这一步实际是确定底盘小车在地图中的实际位置以及车头指向】。下图是多次按下键盘命令控制底盘小车运动后，箭头逐渐减少的效果。

下图是最后调整完的底盘小车姿态（即底盘小车在地图中的实际位置以及车头指向）。

 尝试给定三轮全向底盘小车目标位置。使用2D Nav Goal，这里的目标位置包含了目标点及车头指向，在可视化界面点击且转动箭头防线，机器人会移动到指定点的位置。

4. 资料下载

三轮全向底盘-SLAM导航

①slam导航-例程源代码

②slam导航-样机3D文件

资料内容详见：三轮全向底盘-SLAM导航