我们完成SLAM建图后，如何在导航时让自己的底盘正常工作，是很多新手面临的问题。上位机大家通常采用pi或nvidia jetson nano甚至更强算力的板子，系统一般采用ubuntu，这使得上位机的库几乎是通用的；而底盘不同，单片机有stm32、arduino、51等，电机的种类就更多了，电机的精准度和质量参差不齐，容易在底盘动起来后难以实现精准控制。如何利用电机编码器进行精准的底盘移动并上报odom，不少人会在这里花费大量时间，本文将针对此类问题进行具体讲解。

一、概述

1.整体原理

本文主要讲解执行机构中“底盘电机”与PnC规划控制之间的上下行通信部分。

 

2.控制原理

开始之前，我们先通过ros_control梳理下总体思路。通过下图可以看到，左侧的navigation/moveit与右侧的实体机器人，是通过中间的base_controller/arm_controller + RobotHW来连通的，从而实现了导航算法与底盘/机械臂等不同类型控制器的运动控制通路。本文将主要关注实现base_controller+RobotHW实现轮式机器人移动部分，机械臂的控制在后续的文章中再具体介绍。

这里以pi4上位机+arduino单片机硬件为例（可换成其他硬件，原理相同），简单说明下底盘的导航控制流程：

可以看到上位机与底盘单片机之间的衔接部分主要涉及2个topic：

2.1 /cmd_vel

/cmd_vel是move_base发布的底盘移动命令。包含速度和角度等。在上位机或底盘单片机中监听均可，最终转换为电机指令后由底盘执行。

# 查看当前运行中的topic情况
rostopic list -v
    Subscribed topics:
     * /cmd_vel [geometry_msgs/Twist] 1 subscribers

# 查看topic的MessageType
rostopic type /cmd_vel
    geometry_msgs/Twist

# 查看MessageType结构定义
rosmsg show geometry_msgs/Twist
    geometry_msgs/Vector3 linear
       float64 x
       float64 y
       float64 z
    geometry_msgs/Vector3 angular
       float64 x
       float64 y
       float64 z

2.2 /odom

/odom是move_base监听的底盘里程计信息。包含相对地图原点的pose坐标、twist运动线速度/角速度、以及对应的tf变换等，可通过轮速计、IMU、电机编码器等方式获取或计算得出。在上位机或底盘单片机中发送均可。

 

二、上位机与单片机的通讯方案

1. 方案一：单片机监听/cmd_vel，上位机发布/odom

新手很容易在初期选择这个思路，但它有很大局限性，下边会具体说明。

1.1 通信链路

通过rosserial，在单片机上直接订阅上位机的/cmd_vel消息并控制电机；同时单片机发布自己的运动数据，上位机base_control进行坐标转换后发布/odom和/tf。

1.2 优缺点

优点：

• 底盘直接监听ros /cmd_vel topic，并执行PID等运动，上位机不需要做任何处理

缺点：

• 底盘uno单片机内存不足（uno存储32k/内存2k，换mega2560的话增加成本），无法同时完成/odom的计算和发布，只能将运动信息发给上位机，由上位机完成/odom和tf的发布
• 遇到不能进行按此方案适配的底盘时，需要在上位机特殊实现，会导致产生多种方案，可扩展性不强

1.3 其他

下面以一个使用激光雷达进行导航控制的实例来让大家更清楚的了解具体的控制链路，各节点关系如下图：

核心的节点和topic关系：

• rplidar节点发布/scan，map_server节点发布/map；
• amcl定位节点订阅了上述topic以及初始位姿/initialpose，并发布机器人位姿估计/amcl_pose；
• move_base导航节点订阅了/scan和/map以及规划目标后，发布/cmd_vel运动指令；
• serial底盘节点监听/cmd_vel指令并执行运动，同时发布移动debug信息给base_control；
• base_control监听底盘的/debug，并发布底盘/odom里程计信息给move_base节点以帮助定位；

 

2. 方案二：所有pub/sub逻辑放上位机，单片机仅执行

与上一方案相比，此方案将单片机与上位机完全解耦，将底盘硬件设配工作全部放在上位机来统一封装。

2.1 通信链路

上位机订阅/cmd_vel消息，计算电机速度后通过串口发给底盘单片机执行；同时单片机通过串口发布自己的编码器信号数据给上位机，上位机base_control进行位置计算和坐标转换后发布/odom和/tf。

2.2 优缺点

优点：

• 不同底盘信号的适配工作统一放在上位机的信号适配层，更换不同底盘时只需要在这里适配下新的底盘上下行信号即可，可扩展性强，一套方案解决所有底盘问题
• 所有的运算逻辑都放在上位机进行，通过串口指令的方式控制单片机，单片机空余算力可用来接更多传感器
• 此方案业界已有类似的封装，比如ros_arduino_bridge，包含了上位机的base_control和单片机的PID和编码器信号处理部分，开发成本会相对较低

缺点：

• 自研底盘，如使用新的电机驱动版或增加新的串口通信协议时，需要进行扩展封装，不过这个活怎么也跑不了，也算不上不足

 

3. 方案三：使用高算力的单片机，收发topic都放到单片机里

这个是对方案一的再次review，即有没有低成本的单片机算力又能满足要求呢？答案是有的，即stm32+arduino，但它同样面临多种底盘可扩展性的问题。

3.1 通信链路

通过rosserial，在单片机上直接订阅上位机的/cmd_vel消息并控制电机；同时单片机根据电机编码器信号进行位置计算后直接发布/odom和/tf。

出现这个方案的原因：

• 与arduino和51单片机相比，stm32有很多高配置的型号，例如STM32F103C8T6这款芯片拥有存储64k，内存20k，还具有更多的GPIO，更高精度的PWM输出（比Nano高64倍），更强大的外设（高精度ADC，若干硬件USART、I2C、SPI）等，但价格与uno差不多

STM32F103C8T6     vs         Arduino Nano

主频：      72MHz      vs        8MHz

SRAM:        20K        vs        2K

存储:        64K           vs          32KFlash+1KEEPROM(ATmega328)

• 现在业界有通过STM32duino的方式利用arduino库的成熟方案，会大幅降低开发成本，同时充分利用stm32的算力

3.2 优缺点

优点：

• 上位机不需要特殊处理，逻辑都下方到单片机里

不足：

• 遇到不能进行适配的底盘时，需要在上位机特殊实现，会导致产生两套方案

 

4. 总结

机器人领域经常会涉及控制不同型号的底盘、电机驱动版、机械臂等，考虑未来可扩展行，最终选择方案二，即所有的运算逻辑和硬件适配都放在上位机进行，通过串口与单片机通信。

 

三、控制通路打通

1.硬件准备

打通前线确定硬件，考虑成本，我选择了以下硬件：

• pi4上位机                    * 1
• arduino uno单片机      * 1
• rplidar单线雷达           * 1
• JGB37-520编码减速电机 * 2
• L298N电机驱动           * 1
• 18650电池                    * 2
• 连接线/轮子/履带/亚克力板   * 若干

2.编码电机连线方法

为了精细控制我们用到了带编码器的减速电机，这类电机不只有正负极2根线，接线方法与普通电机有些区别，这里特殊说明下。以下图的25GA-370电机为例，它有6根线：

连线方法：

• 电机电源+正-负极（红/白线）：接L298 output 1/2或3/4口（供电+驱动板控制）
• L298 input（控制口）：接arduino D7/D8口（用于电机运动控制）
• 电机编码器电源+正-负极（蓝/黑线）：接arduino VIN+GND口（供电）
• 电机编码器A/B相信号（黄/绿线）：接arduino D3/D4口（用于电机编码器信息读取）

实际接线图：

*注：如果代码开启了USE_PS2，则必须接上PS2接收器，否则会影响电机正常运行。

*注：如使用QGPMotorShield电机驱动模块，则左电机电源需要反接，并在encoder解码器中也*-1；同时，这块板子的供电和设计原因，即使用使用相同直流电机，也必须单独调试各轮PID参数，实测上看与L298P板子差的非常大，所以如果大家用不同的电驱板接上去运行异常的话，则必须每个轮子单独调试下转动方向和PID参数。

3.ros_arduino_firmware库概述

该库包含Arduino端和用来控制Arduino的ROS驱动包，具备在ROS下控制Arduino机器人的完整解决方案。它集成了兼容不同电驱机器人的基本控制器（base controller），可以接收ROS Twist类型的消息，可以发布里程数据到ROS端。

代码库位置：https://github.com/yanjingang/robot_base_control

# pc/pi安装 robot_base_control 库
cd ~/catkin_ws/src
git clone git@github.com:yanjingang/robot_base_control.git
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

3.1 模块

• ros_arduino_bridge： metapackage (元功能包)，catkin_make安装即可
• ros_arduino_msgs：  消息定义包
• ros_arduino_firmware： 固件包，更新到Arduino（执行运动指令、发送电机编码器数据，通过serial与上位机通讯）
• ros_arduino_python： ROS相关的Python包，用于上位机，树莓派等开发板或电脑等（监听/cmd_vel并转换为移动指令下发给电机；将电机编码器数据转换为里程计数据发到/odom）

3.2 文件结构

├ ros_arduino_bridge # metapackage (元功能包)
├── CMakeLists.txt
└──  package.xml

├ ros_arduino_firmware #固件包，更新到Arduino
├── CMakeLists.txt
├── package.xml
└──  src
    └── libraries                       #库目录
        ├── MegaRobogaiaPololu          #针对Pololu电机控制器，MegaRobogaia编码器的头文件定义
            ├── commands.h              #定义命令头文件
            ├── diff_controller.h       #差分轮PID控制头文件
            ├── MegaRobogaiaPololu.ino  #PID实现文件
            ├── sensors.h               #传感器相关实现，超声波测距，Ping函数
            └──  servos.h                #伺服器头文件
        └── ROSArduinoBridge            #Arduino相关库定义
            ├── commands.h              #定义命令
            ├── diff_controller.h       #差分轮PID控制头文件
            ├── encoder_driver.h        #编码器驱动头文件
            ├── encoder_driver.ino      #编码器驱动实现, 读取编码器数据，重置编码器等
            ├── motor_driver.h          #电机驱动头文件
            ├── motor_driver.ino        #电机驱动实现，初始化控制器，设置速度
            ├── ROSArduinoBridge.ino    #核心功能实现，程序入口
            ├── sensors.h               #传感器头文件及实现
            ├── servos.h                #伺服器头文件，定义插脚，类
            └── servos.ino              #伺服器实现

├ ros_arduino_msgs #消息定义包
├── CMakeLists.txt
├── msg                                 #定义消息
    ├── AnalogFloat.msg                 #定义模拟IO浮点消息
    ├── Analog.msg                      #定义模拟IO数字消息
    ├── ArduinoConstants.msg            #定义常量消息
    ├── Digital.msg                     #定义数字IO消息
    └──  SensorState.msg                 #定义传感器状态消息
├── package.xml
└──  srv                                 #定义服务
    ├── AnalogRead.srv                  #模拟IO输入
    ├── AnalogWrite.srv                 #模拟IO输出
    ├── DigitalRead.srv                 #数字IO输入
    ├── DigitalSetDirection.srv　　　　 #数字IO设置方向
    ├── DigitalWrite.srv                #数字IO输入
    ├── ServoRead.srv                   #伺服电机输入
    └── ServoWrite.srv                  #伺服电机输出

└ ros_arduino_python #ROS相关的Python包，用于上位机，树莓派等开发板或电脑等。
├── CMakeLists.txt
├── config                              #配置目录
    └──  arduino_params.yaml             #定义相关参数，端口，rate，PID，sensors等默认参数。由arduino.launch调用
├── launch
    └──  arduino.launch                  #启动文件
├── nodes
    └──  arduino_node.py                 #python文件,实际处理节点，由arduino.launch调用，即可单独调用。
├── package.xml
├── setup.py
└── src                                 #Python类包目录
    └── ros_arduino_python
        ├── arduino_driver.py           #Arduino驱动类
        ├── arduino_sensors.py          #Arduino传感器类
        ├── base_controller.py          #基本控制类，订阅cmd_vel话题，发布odom话题
        └── __init__.py                 #类包默认空文件

3.4 总结

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

• ros_arduino_firmware/src/libraries/ROSArduinoBridge：Arduino端的固件包实现，需要修改并上传至Arduino电路板；
• ros_arduino_python/config/arduino_params.yaml：ROS端的一个配置文件，相关驱动已经封装完毕，我们只需要修改配置信息即可

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

 

4.单片机逻辑实现

如上所述，arduino板子我们采用ros_arduino_firmware固件库，它封装了执行运动指令、发送电机编码器数据，通过串口与上位机通讯等。

4.1 文件说明

• ROSArduinoBridge.ino 主程序
• commands.h                  串口命令的预定义
• diff_controller.h              PID控制代码
• encoder_driver.h            编码器，这里只是针对了Arduino UNO，使用了中断接口D2,D3,和模拟接口A4，A5；所以电机编码器的输出接线需要按照此规则接线，另外要注意编码器要有两路输出
  左侧电机的编码输出接D2,D3；右侧电机的编码输出接A4,A5
• encoder_driver.ino         编码器的实现代码
• motor_driver.h                马达驱动的接口定义，用不动的马达驱动板都要实现此文件定义的三个函数
• motor_driver.ino             马达驱动实现代码，根据预定义选择不同的驱动板库，在这里我使用了L298P，所以需要自己实现一个新的驱动库，后面会介绍
• sensors.h                        传感器的实现文件
• servos.h                          舵机的实现文件

4.2 固件命令

固件通过串行端口接受单字母命令，用于控制伺服电机和读取编码器数据等。这些命令可以通过任何串行接口发送到 Arduino，包括 Arduino IDE 中的串行监视器。

命令列表可以在文件commands.h 中找到，包括：

#define ANALOG_READ    'a'  // 获取模拟引脚上的读数（例如a 3）
#define GET_BAUDRATE   'b'  // 获取波特率
#define PIN_MODE       'c'  // 设置引脚模式（0 input, 1 output，例如c 3 1）
#define DIGITAL_READ   'd'  // 获取数字引脚上的读数（例如d 4）
#define READ_ENCODERS  'e'  // 获取电机编码器计数
#define MOTOR_SPEEDS   'm'  // 指定速度（例如m 20 20）
#define PING           'p'  // 
#define RESET_ENCODERS 'r'  // 复位
#define SERVO_WRITE    's'  // 设置伺服的位置 -弧度(0-3.14)
#define SERVO_READ     't'  // 读伺服的位置
#define UPDATE_PID     'u'  // 更新PID
#define DIGITAL_WRITE  'w'  // 向数字引脚发送低(0)或高(1)信号
#define ANALOG_WRITE   'x'  // 向模拟引脚发送模拟信号

使用示例：

获取当前编码器计数：
  e
要以每秒10个脉冲/秒 移动机器人：
  m 10 10
重置：
  r

4.3 具体实现

为了满足控制需要，我们需要配置ROSArduinoBridge.ino，打开底盘控制、L298电机驱动，关闭舵机：

//是否启用底盘控制器
#define USE_BASE      // Enable the base controller code
//#undef USE_BASE     // Disable the base controller code

/* Define the motor controller and encoder library you are using */
//启用基座控制器需要设置的电机驱动以及编码器驱动
#ifdef USE_BASE
   /* The Pololu VNH5019 dual motor driver shield */
   //#define POLOLU_VNH5019

   /* The Pololu MC33926 dual motor driver shield */
   //#define POLOLU_MC33926

   /* The RoboGaia encoder shield */
   //#define ROBOGAIA
   
   /* 电机编码器信号处理 Encoders directly attached to Arduino board */
   #define ARDUINO_ENC_COUNTER

   /* L298P电机驱动版 Motor driver*/
   #define L298P_MOTOR_DRIVER
#endif

//是否启用舵机
//#define USE_SERVOS  // Enable use of PWM servos as defined in servos.h
#undef USE_SERVOS     // Disable use of PWM servos

根据实际情况调整端口设置：

# 电机引脚
vim motor_driver.h
    #elif defined L298P_MOTOR_DRIVER    // L298P
        // PWM调速引脚
        #define LEFT_MOTOR_PWM    10  // A motor PWM调速(控制转速)
        #define RIGHT_MOTOR_PWM   11  // B motor PWM调速(控制转速)
      
        // 普通数字引脚（控制方向）
        #define LEFT_MOTOR_DIR   12  // A motor 使能(控制转动方向)
        #define RIGHT_MOTOR_DIR  13  // B motor 使能(控制转动方向)
      
    #endif

# 电机编码器引脚
vim encoder_driver.h
    #ifdef ARDUINO_ENC_COUNTER
      // 左轮电机编码器信号读引脚（A motor 编码信号线接 D2/3）
      #define LEFT_ENC_PIN_A PD2  //pin 2
      #define LEFT_ENC_PIN_B PD3  //pin 3
      
      // 右轮电机编码器信号读引脚（B motor 编码信号线连 A4/5）
      #define RIGHT_ENC_PIN_A PC4  //pin A4
      #define RIGHT_ENC_PIN_B PC5   //pin A5
    #endif

调整完毕后烧制到arduino uno板子里。

4.4 PID调节

理解PID的Kp Ki Kd参数作用：

PID参数调试方法：

vim diff_control.h

//1、打开电机码盘注释用于pid绘图
Serial.println(input);   

//2、先调试单个电机的PID，先注释另一个电机的doPID()
doPID(&rightPID);
//doPID(&leftPID);

//3、打开“arduino-工具-串口绘图器“，选择回车，输入不同速度指令，查看pid曲线
m 10 10
m 15 15
m 30 30
m 100 100

//4、为了方便观察和调试PID变化过程，可以把move命令持续执行时间临时改大写
#define AUTO_STOP_INTERVAL 5000

 

5.上位机逻辑实现

如上所述，上位机我们采用ros_arduino_python库，它实现了订阅cmd_vel话题，发布odom话题，通过串口与arduino通讯等功能。

5.1 具体实现

配置机器人的尺寸、PID 参数和传感器参数：

roscd ros_arduino_python/config
cp arduino_params.yaml my_arduino_params.yaml

vim my_arduino_params.yaml
    port: /dev/ttyACM0
    baud: 57600
    timeout: 0.1

    rate: 50  # 轮询速率50hz
    sensorstate_rate: 10

    use_base_controller: True #启用基座控制器
    base_controller_rate: 10

    base_frame: base_footprint #base_frame 设置

    # === 电机参数 Robot drivetrain parameters
    wheel_diameter: 0.067 # 轮胎直径(米)
    wheel_track: 0.23 # 两个轮胎间距(米)
    encoder_resolution: 38.5 # 编码器精度(手动推车移动1米，用1米的电机编码器值*(self.wheel_diameter * pi)/self.gear_reduction可以计算出此编码器精度)
    gear_reduction: 30 # 减速比(马达参数表：jgb37-520 12v 空转330rpm 减速比20)
    #motors_reversed: True # 转向取反

    # === PID parameters PID参数，需要自己调节
    Kp: 20 #0.25   # 提高响应速度，调大后惯性过大，容易导致超过目标值后的震荡回调（通常先把其他参数置0，将Kp参数由小向大调，以能达到最快响应又无无大超调为最佳参数。）
    Kd: 12 #0      # 主要用于对Kp的未来走势进行预测，以避免Kp冲过头（通常为0.5*延迟时间，过大会导致超调震荡而难以收敛。通常在P、I调节好后，再调节D，一般的系统D=0,1或2即可，只有滞后较大的系统，D值才可能调大些）
    Ki: 0 #0.2   # 根据历史曲线调节控制误差（调大会导致趋于稳定的时间变长，调小会导致超调量过大而回调震荡幅度过大。通常从大往小调整，调小的同时也要缩小Kp）
    Ko: 50
    accel_limit: 1.0

    # === Sensor definitions.  Examples only - edit for your robot.
    sensors: {
      #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
      #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
      #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
      #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
      #arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
    }

5.2 控制测试

# pc/pi准备好 robot_base_control/robot_navigation
cd ~/catkin_ws/src
git clone git@github.com:yanjingang/robot_base_control.git
git clone git@github.com:yanjingang/robot_navigation.git
git clone git@github.com:ros-teleop/teleop_twist_keyboard.git
cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

#arduino 
#烧制OSArduinoBridge.ino到板子上

# pc/pi启动测试
roslaunch ros_arduino_python arduino.launch

# 尝试发布 Twist 命令：
rostopic pub /cmd_vel geometry_msgs/Twist -r 1 -- '[1.0, 0.0, 0.0]' '[0.0, 0.0, 1.0]'  # 线速度和角速度为1时，画圆（参数：[linear的x,y,z] [angular的x,y,z]）
rostopic pub /cmd_vel geometry_msgs/Twist -r 1 '{ linear: {x: 0.5, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0} }'  # 前进
rostopic pub /cmd_vel geometry_msgs/Twist -r 1 '{ linear: {x: 1} }'  # 直行
rostopic pub /cmd_vel geometry_msgs/Twist -1 '{ angular: {z: 1} }'  # 旋转40度

# pc键盘控制：确保前后左右方向控制正确
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

# 查看指令和里程计数据是否正常：
rostopic echo /cmd_vel
rostopic echo /odom

# 如果发布Twist后里程计数为0或/odom坐标不变，检查下编码器的电源和信号接线是否正确

可以打开rviz，添加TF查看底盘的朝向和运动方向是否正常（在 Rviz 中，默认 X 轴是红色、Y 轴时绿色、Z 轴是蓝色，也就是 XYZ 对应 RGB）。

代码中经常会看到欧拉角的转换，这里简单普及下，欧拉角其实就是表示绕 Z 、Y 、X 轴的旋转的角度，分别称为 Yaw（偏航）、Pitch（俯仰）、Roll（翻滚），跟zyx的区别是线变成了旋转轴。这个简单了解下就行，调试代码还是要关注x轴和z轴的值（除了麦克纳姆轮等能直接左右平移的底盘外，普通底盘只会涉及到x和z）。

5.3精准度标定

按照上边的方法我们打通了自研底盘与ros的导航控制流程，但实际使用后你可能会遇到走一定距离后，定位位置出现偏移，这是精度不足的表现，我们还需要对机器人进行标定，即让机器人能完全按照给定的线速度，和角速度行进而不产生偏差。

5.3.1.线速度标定

线速度的标定方法，就是设置一个1m的距离，代码控制让机器人行驶1米，看是否刚好走了1米，误差在可接受的范围内即可，如下是进行标定的代码：

https://github.com/yanjingang/robot_base_control/ros_arduino_python/src/calibrate/calibrate_linear.py

接下来运行如下命令，控制小车前进

rosrun ros_arduino_python calibrate_linear.py

如果能一次运行刚好是1米那当然是理想的效果，如果不理想，可能要调整my_arduino_params.yaml文件中有关机器人的参数了，首先需要检查机器人的参数是否与实际的想否，如果不想否修改为与实际想否的数据，另外要注意的是ROS里面使用的单位是 米，一定要注意单位的换算；

# === 马达参数 Robot drivetrain parameters
wheel_diameter: 0.067 #0.067     # 轮胎直径(米) 
wheel_track: 0.23 #0.11        # 两个轮胎间距(米)
encoder_resolution: 38.5    # 编码器精度(手动推车移动1米，用1米的电机编码器值*(self.wheel_diameter * pi)/self.gear_reduction可以计算出此编码器精度)
gear_reduction: 30          # 减速比(马达参数表：jgb37-520 12v 空转330rpm 减速比20)
#motors_reversed: True      # 转向取反

即使是数据与实际的测量数据符合，但也可能达不到你的要求，这可能与电机的性能和负重有关系，本人的经验是，轮径、轮间距、减速比按实际的填写，编码器精度通过手动计算得出，即可以达到满意的效果。

当然要达到高精度的控制效果，硬件的精度也要非常高，例如两侧电机在PWM饱和255时可能行走距离不同，这需要在diff_controller.h里通过两轮的轮速差比diamete_ratio来调整PWM的output。

typedef struct {
  ...
  double diamete_ratio;         //yanjingang:左轮相对于右轮轮径比系数(255饱和输出直行，往哪侧偏，则将对侧轮调大)
  ...
}
SetPointInfo;


void resetPID(){
   ...
   leftPID.diamete_ratio = 1.01; //yanjingang:左轮相对右轮的饱和输出时的轮速差比，需要反复试，确保pwm255饱和输出时能走直线不偏向一侧（>1.0表示此轮较快，需减慢）
   ...
   rightPID.diamete_ratio = 1.0;
   ...
}

// 左右电机具体PID调试函数
void doPID(SetPointInfo * p) {
  ...
  if (output >= MAX_PWM)
    output = MAX_PWM / p->diamete_ratio;  // 输出饱和时，处理左右电机的轮速差
  else if (output <= -MAX_PWM)
    output = -MAX_PWM / p->diamete_ratio;  // 输出饱和时，处理左右电机的轮速差
  ...
}

 

5.3.2.角速度标定

角速度标定，即控制机器人人转动固定的角度，看机器人是否按照控制指令完成，在这里我们让机器人转40度，代码如下：

https://github.com/yanjingang/robot_base_control/ros_arduino_python/src/calibrate/calibrate_angular.py

接下来运行如下命令，控制小车旋转：

rosrun ros_arduino_python calibrate_angular.py

影响角速度的主要参数是两个轮胎间距参数wheel_track，所以如果发现机器人旋转角度不准，可以调整此参数（实际角度过大时调大，角度不足时调小）。

*注：后边增加了turn_reduction参数，转向角度不准可以通过这个参数来标定。

5.3.3.激光雷达tf

激光雷达相对于底盘中心点的位置也很重要，如果安装的不正，或者激光雷达的朝向与底盘的朝向不同，都需要调整它们的tf变换。我的底盘因为加了深度摄像头，底座螺丝孔冲突，不得不把激光雷达朝向与底座相反，所以我的tf配置为：

vim robot_navigation/launch/lidar.launch
    <group if="$(eval base_type == 'MarsRobot')">
	<!-- 激光雷达相比与底盘，高0.15m，z轴旋转180度 -->
        <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
            <!--args="x y z yaw pitch roll frame_id child_frame_id period_in_ms"-->
            args="0.0 0.0 0.15 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
        </node>
    </group>

5.4 集成到ros导航库

将之前SLAM机器人的base_control切换到刚才调好的ros_arduino_python里的

# 修改base_control配置，使用ros_arduino_python里的base_control
vim ~/catkin_ws/src/robot_navigation/launch/robot_lidar.launch
   <!--include file="$(find robot_navigation)/launch/base_control.launch">
       <arg name="pub_imu" value="$(arg pub_imu)"/>
   </include-->
   <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
       <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
   </node> 


# SLAM建图测试
roslaunch robot_navigation robot_slam_laser.launch planner:=teb
# PC端可视化
roslaunch robot_navigation slam_rviz.launch
# PC键盘控制移动
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

# 保存地图
roscd robot_navigation/maps/
rosrun map_server map_saver -f map

# 使用地图导航测试
roslaunch robot_navigation robot_navigation.launch
roslaunch robot_navigation navigation_rviz.launch  #pc

# rviz里修正初始位姿并设置导航目标后，查看指令/里程计数据以及底盘移动是否正常
rostopic echo /cmd_vel
rostopic echo /odom

5.5 转向速度修正

测试时发现，规划路径为直线时，小车会一直打转，检查/cmd_vel发现angular.z上是有值的，但很小，怀疑是小车转向时速度过快导致的。经过反复测试，在AUTO_STOP_INTERVAL 5000时通过teleop_twist_keyboard发送angular.z=1指令，小车旋转到40度是比较合理的角度，最终通过在base_controller.py的th值计算时，增加了turn_reduction降速逻辑。调整后，小车即可以通过move_base合理精准的行进。

# 添加转向减速比参数配置
vim ros_arduino_python/config/my_arduino_params.yaml
turn_reduction: 63.0           # 转向减速比（使转向速度降低，确保angular.z=1持续5s时旋转<40度，需要反复测量）

# 转向减速
vim ros_arduino_python/base_controller.py
self.turn_reduction = rospy.get_param('~turn_reduction', 2.0)   # yanjingang: 转向减速比
def cmdVelCallback(self, req):
    ...
    th = th * self.wheel_track  * self.gear_reduction / self.turn_reduction    # 旋转角速度根据电机和轮径调整，使转向速度降低（yanjingang: /18.0是为了angular.z=1时从旋转360度降低为旋转40度）
    ...

5.6 全局/局部路径规划避障修正

在实际测试过程中发现，全局和局部路径规划喜欢贴着地图或障碍物边缘前进，很容易导致卡住无法通过。经过反复调试，调整以下参数后规划路径及可远离障碍物：

# 机器人的轮廓尺寸
vim param/MarsRobot/costmap_common_params.yaml
robot_radius: 0.13  # 底盘圆形半径

# 全局路径规划器
vim param/MarsRobot/global_planner_params.yaml
orientation_mode: 3 #0     # 设置每个点的朝向。 (None=0, Forward向前=1, Interpolate插值=2, ForwardThenInterpolate向前然后插值=3, Backward向后=4, Leftward向左=5, Rightward向右=6)(动态配置)
use_dijkstra: true         # 如果设置为false就用A*

# 局部轨迹规划器: teb
vim param/MarsRobot/teb_local_planner_params.yaml
footprint_model: # 主要是碰撞检测中的距离计算 types: "point", "circular", "two_circles", "line", "polygon"
   type: "circular"      # 机器人模型为圆形
   radius: 0.13          # 圆半径

导航代码库位置：https://github.com/yanjingang/robot_navigation

5.7 运行效果

建图效果：

导航效果：

至此，我们实现了一个可以精准控制的自研底盘，它通过激光雷达+amcl+odom里程计实现定位、规划路径后通过move_base控制底盘移动、通过电机编码器生成odom里程计，最终实现一个完整的定位/建图和导航流程。

今天先到这里，下次将教大家如何实现一个基于纯视觉的自主导航机器人，并在后续教程里增加语音控制、机械臂控制等。

你期望拥有一个什么样的机器人？留言告诉我，我来带你一起实现～

 

yan 22.1.1

 

参考：

ROS小车启动功能包编写

译：在ROS上发布Odometry信息

TF坐标变换

ROS机器人Diego 1#制作（三）Base Controller—Ros_arduino_bridge

机器人搭建与调试 5：线速度、角速度校准

PID 参数整定过程及一些常规步骤

ROS机器人Diego 1#制作（四）Base Controller—线速度的标定

ROS机器人Diego 1#制作（五）Base Controller—角速度的标定

局部路径规划器teb_local_planner详解6：关于动态障碍物与常见问题

局部路径规划器teb_local_planner详解2：关于避障

欢迎关注下方“非著名资深码农“公众号进行交流～