小黑仿生轮腿机器人（三）-SLAM导航
1. 功能描述
本文将利用键盘控制轮腿运动完成slam建图，并能在已建好的地图里进行自主导航。slam导航可以拆分为三步：
第一步：能用键盘控制轮腿机器人运动；
第二步：基于实际场景，用键盘控制轮腿机器人进行slam建图；
第三步：基于已建好的地图，模拟实现slam导航。
2. 功能实现
操作系统：Ubuntu18.04系统，基于Debian GNU/Linux，支持x86、amd64（即x64）、ARM和ppc架构。
仿真系统：基于开源机器人操作系统ROS melodic和开源软件平台Arduino开发，上位机采用ROS melodic，基于Rviz完成全向移动轮腿slam导航运动规划，采用gazebo完成全向移动轮腿物理运动仿真；下位机采用Arduino实现对全向移动轮腿运动的控制。
编程环境：Arduino 1.8.19
2.1 键盘控制轮腿运动
（1）实现思路：按下键盘上指定的键时，可以实现轮腿前进、后退、转向、平移；还可以灵活的设置轮腿的角速度、线速度来调整轮腿的运动。下面是规划的控制轮腿的键盘命令（见下表）。可以先熟悉一下，后续在控制轮腿机器人时会用到。
（2）操作步骤：
① 打开参考例程文件夹（fourmacnum_car_ws\src\arduino_programming\Ros_Arduino_Translation_Programming），下载控制轮腿的程序
Ros_Arduino_Translation_Programming.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-02 https://www.robotway.com/
------------------------------*/
/*
* 上位机（ros）与下位机（arduino）消息发送与接收。
*
*
* 车身位置及传感器\电机接线:
*
* Y
* |
* |
* |
* | 【车身左侧】
* | t:A10 t:A9
* | e:A11 e:A8
* | 舵机[servoPin:26] t:A14 A - - - - - - - - - - - - - - - - - -Y: t:A5 舵机[servoPin:43]
* | e:A15 | | e:A4
* | | | 【车头】
* | t:A13 | | t:A6
* | 舵机[servoPin:27] e:A12 Z - - - - - - - - - - - - - - - - - -X: e:A7 舵机[servoPin:42]
* | t:32 t:46
* | e:31 e:45
* | 【车身右侧】
* |
* 0----------------------------------------------------------------------------------------------------------X
*/
//DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要
//#define USE_USBCON //PAOGRAMMING PORT
//#define USE_NATIVE_USB //NATIVE USB PORT
#define ActionDelayTimes 1500
#define set_delay 1000
//#include
#include
#include
//#include
#include
#include
#define mySerial Serial1
ros::NodeHandle nh;
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 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);
//ServoTimer2 myServo[4];//statement servo
Servo myServo[4];
const int kMessagePubRate = 5;
unsigned long message_pub_time = 0;
const int kReadMotorDeltaT = 5;
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;
float wheel_Speed[4]={0,0,0,0};
char cmd_return[200];
float sudu = 0.87;
//SoftwareSerial mySerial(51, 9);
void setup()
{
delay(1000);
mySerial.begin(115200);
init_Servo();delay(300);
//Servo_Action_Test();delay(1500);
nh.initNode();
nh.subscribe(vel_cmd_sub);
nh.advertise(pose_feedback_pub);
nh.advertise(vel_feedback_pub);
XYSetVel(0.0,0.0,0.0);
moveTest();
position_read_time = millis();
message_pub_time = millis();
}
void loop()
{
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();
}
void moveTest(){
XYSetVel(0.05, 0.0, 0.0);delay(set_delay); //forward 前进
XYSetVel(0.0,0.0, 0.0);delay(set_delay);//停止
XYSetVel(-0.05,0.0,0.0);delay(set_delay); //back 后退
XYSetVel(0.0,0.0, 0.0);delay(set_delay);
// XYSetVel(0.0,0.0,-0.20);delay(set_delay); //left 左转
// XYSetVel(0.0,0.0, 0.0);delay(set_delay);
// XYSetVel(0.0 ,0.0,0.20 );delay(set_delay); //right 右转
// XYSetVel(0.0,0.0, 0.0);delay(set_delay);
// XYSetVel( 0.0, -0.05, 0.0);delay(set_delay);//Left translation 左平移
// XYSetVel( 0.0, 0.05, 0.0);delay(set_delay);//right translation 右平移
XYSetVel(0.0,0.0, 0.0);delay(set_delay);
} ② 启动雷达、键盘、rosserial程序包。打开终端,输入roslaunch robot_navigation_control robot.launch命令（见下图），等待程序的运行启动界面。
终端
成功启动后，可以在终端看到轮腿的当前speed（线速度）为0.04，turn（角速度）为0.08。
注意：speed指的是线速度的大小（包含方向），turn指的是角速度的大小（包含方向）。
启动后的界面（见下图）:
③ 尝试按下键盘命令，控制轮腿的运动。 (注意：一、请先保证终端是激活状态，见上图；二是按键盘时，稍微有点间隔，这样好给轮腿转动的时间)。
假如现在希望先增加轮腿的最大速度，再降低轮腿的最大速度，则分别按下键盘上q、z命令，可以观察到终端的结果（见下图）。
更多命令可自行尝试。
2.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（见下图）。
② 启动构建地图服务。重新打开新终端并输入：roslaunch four_macnum_slam four_macnum_slam.launch（见下图）。
界面启动后，在rviz中选择“map”，可以在可视化界面看到有地图出现；
下面根据实际环境进行建图：按下键盘相关指令（键盘指令请参考上面2.1内容）来控制轮腿在场地内运行，直至在Rviz内可以看到构建地图的轮廓，如下图所示（注意实际地图环境不同，出现的轮廓不同）。
③ 为了后续在已知环境中运行轮腿，需把此次建立的地图进行保存。需要先进入保存地图的文件夹，然后给地图命名保存即可（见下图）。
下面是以保存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三个通道的平均值。
2.3 轮腿导航—Navigation
（1）实现思路：加载上述2.2建好的地图，设置轮腿机器人的初始位置和方向，设定目标位置及方向，轮腿就可以实现导航效果。
Navigation与机器人底盘关系：ROS的二维导航功能包，简单来说，就是根据输入的里程计等传感器的信息流和机器人的全局位置，通过导航算法，计算得出安全可靠的机器人速度控制指令。但是如何在特定的机器人上实现导航功能包的功能，却是一件较为复杂的工程，作为导航功能包使用的必要先决条件，机器人必须运行ROS，发布tf变换树，并发布使用ROS消息类型的传感器数据。同时为了让机器人更好的完成导航任务，开发者还要根据机器人的外形尺寸和性能， 配置导航功能包的一些参数。
在ROS中进行导航需要使用到的三个包是：
① move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
② gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
③ amcl：根据已经有的地图进行定位。
（2）操作步骤：
① 将创建的地图应用到导航程序中。
这里我们需要修改four_macnum_navigation.launch文件,把上述2.2的zhanhui_mapping_test.yaml 添加到此launch文件中。
步骤如下：
打开终端并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch
之后再输入：gedit four_macnum_navigation.launch
② 启动雷达、键盘、rosserial。在该终端继续输入：roslaunch robot_navigation_control robot_car.launch。
③打开新终端并输入：roslaunch four_macnum_navigation four_macnum_navigation.launch（见下图）。
启动界面后，可以看到许多红色的箭头，代表轮腿的姿态估计还不准确。
④ 开始调整轮腿的姿态。使用2D Pose Estimate 标定轮腿位于地图中的初始位置及车头指向（见下图）。
⑤ 鼠标点击有roslaunch robot_navigation_control robot_car.launch的终端后，尝试按下键盘命令控制轮腿运动（键盘命令请参考上面2.1内容），最好尽可能多的消除地图中的箭头（这一步实际是确定轮腿在地图中的实际位置以及车头指向）。下面是多次按下键盘命令控制轮腿运动后，箭头逐渐减少的效果。
下图是最后调整完的轮腿姿态（即轮腿在地图中的实际位置以及车头指向）。
⑥ 尝试给定轮腿目标位置（使用2D Nav Goal），这里的目标位置包含了目标点及车头指向，在可视化界面点击且转动箭头防线，机器人会移动到指定点的位置。
3. 资料内容
slam导航-例程源代码
资料内容详见：小黑仿生轮腿机器人（SLAM导航）