开源四轮ROS差速机器人控制方案

文章摘要：在电机驱动方案选型中，对比了亚博四路驱动板（串口控制简单）和TB6612驱动板（代码迁移容易但接线复杂），最终选择亚博驱动板实现四轮差速控制。系统采用"虚拟两轮"简化策略，将左右侧轮组同步控制（vL/vR），通过速差实现转向。编码器反馈取两侧轮速平均值，形成闭环控制。该设计兼顾了接线简便性与控制逻辑清晰性。

阿阿阿阿阿里郎

392人浏览 · 2025-05-27 22:23:28

阿阿阿阿阿里郎 · 2025-05-27 22:23:28 发布

代码开源：https://gitee.com/alina-yyl/ros_arduino_bridge.git

系统组成：

上位机: Jetson

下位机：arduino_mage_2560

电机：MG540直流减速电机

在刚开始元器件选型前确定了两种方案：

型号	亚博四路电机驱动板	tb6612四路电机驱动板
优点	通过串口协议控制，接线简单	代码实现较简单，之前用过2路的，容易迁移
缺点	代码层面实现需要有一定的debug能力	需要将四路的驱动板线全接单片机，接线复杂

最后选择了亚博的，用来做电机控制板，整体系统框图如下所示

1.四轮差速简化控制原理

核心思想：将四轮差速车视为“虚拟两轮差速车”，将左右两侧的两个轮子视为一个整体（即左组轮和右组轮）。
控制方式：
- 左前轮和左后轮 速度同步（vL=v左前=v左后）。
- 右前轮和右后轮 速度同步（vR=v右前=v右后）。
- 通过左右轮速差（vR−vL）实现转向，通过同速（vL=vR）实现直行。

两轮差速模式下的行为

动作	左组轮速度 vL	右组轮速度 vR	运动效果
直行	vL=vR	vR=vL	四轮同步，直线前进/后退
左转	vL<vR	vR>vL	绕左侧轮组中心转向
右转	vL>vR	vR<vL	绕右侧轮组中心转向

编码器反馈处理
- 每组轮子的两个编码器信号需取平均值或加权值，作为该侧的实际速度反馈。
  例如：
  v左实际=(v左前编码器+v左后编码器)/2

2.建立Arduino与电机驱动板通信

使用4p的PH2.0，根据引脚连接Arduino的串口。
将电机驱动板输入电源负极接入ArduinoGND(共地)
在setup()中新定义刚接的串口

void setup() {
  Serial.begin(BAUDRATE);
  Serial2.begin(MOTOR_BAUDRATE);

3.驱动模块通讯协议

指令	说明	例子	备注
$mtype:x#	配置电机的型号	$mtype:1#	x:是电机的类型数值代表的电机类型 1: 520电机 2：310电机 3: TT电机(带有编码器的) 4: TT电机(不带编码器)




$deadzone:xxxx#	配置电机的pwm脉冲死区	$deadzone:1650#	xxxx:是死区的值，这个需要测量得出,可通过改变此值消除电机最小震荡。死区值的范围 (0-3600)




$mline:xx#	配置电机的相位线	$mline:13#	xx:这个是电机的减速比参数，此值需要查商家电机的参数表可得




$wdiameter:xx#	配置轮子的直径	$wdiameter:50#	xx:这个是轮子的直径，此值可以测量或者用商家的信息得出




$MPID:x.xx,x.xx,x.xx#	配置pid参数	$MPID:1.5,0.03,0.1#	x.xx,x.xx,x.xx:这个分别是控制电机p ,i ,d 的参数每对数值进行一次变更，芯片会重启，会把正在运动的电机停止。属于正常情况





$flash_reset#	恢复出厂

$spd:0,0,0,0#	控制4个电机的速度	$spd:100,-100,0,50#	0,0,0,0 :代表板子丝印上的 M1,M2,M3,M4。速度的范围为(-1000~1000) 超出范围无效




$pwm:0,0,0,0#	控制4个电机的pwm输出	$spd:0,520,300,800#	0,0,0,0 :代表板子丝印上的 M1,M2,M3,M4。速度的范围为(-3600~3600) 超出范围无效





$upload:0,0,0#	接收编码器数据	$upload:1,0,0#	$upload:0,0,0#:第一个0代表的是 :轮子转动的总编码器数据第二个0代表的是:轮子转动实时转动编码器数据(10ms) 第三个0代表的是:轮子的速度






$read_flash#	查询flash变量

$read_vol#	查询电池电量

4.Arduino读取编码器数据

串口通讯中，发送下面命令即可让驱动模块不断发送编码器数据，返回信息格式为：
- "$MAll:M1,M2,M3,M4#"，所以在Arduino中需要接收到这段数据格式后进行处理。

$upload:1,0,0#

接收编码器数据

编码器初始化

  void initEncoders(){
    left_count = 0L;
    right_count = 0L;
    serial_left_count=0L;
    serial_right_count = 0L;
    Serial2.println("$upload:1,0,0#");
  }

在setup()中会进行编码器初始化，因此在初始化时向驱动模块发送接收编码器命令。

编码器数据处理
- 编码器数据在ROS中会不断轮询
- 当读取到MAII协议头时进行编码器数据处理
- 在核心函数readEncoder()中，将读取到的四个电机编码器值写入数组。
- 将四路编码器数据合并成两路，根据与ROS通信格式处理为字符串并return。

#elif defined MY_ARDUINO_COUNTER
  volatile long left_count = 0L;//左轮计数器
  volatile long right_count = 0L;//右轮计数器
  volatile long serial_left_count = 0L;//左前轮电机驱动器中的编码器数据
  volatile long serial_right_count = 0L;//右前轮电机驱动器中的编码器数据

  // 全局变量定义
  #define RXBUFF_LEN 64
  uint8_t g_recv_buff[RXBUFF_LEN] = {0};
  bool g_recv_flag = false;
  int Encoder_Now[4] = {0};
  String lastEncoderValues = "0 0";  // 持久化存储上次有效值

  // 协议验证函数
  bool validate_protocol(const char* data) {
      // 协议头快速验证
      if(strncmp(data, "MAll:",5) != 0) return false;

      // 参数格式验证
      const char* p = data + 5;
      for(int seg=0; seg<4; seg++) {
          if(*p == '-') p++;  // 符号检查
          
          bool has_digit = false;
          while(isdigit(*p) || *p == '.') {
              if(isdigit(*p)) has_digit = true;
              p++;
          }
          if(!has_digit || (seg<3 && *p++ != ',')) return false;
      }
      return *p == '\0';
  }

  // 串口接收中断服务程序
  void serialEvent2() {
      static bool capturing = false;
      static uint8_t idx = 0;
      
      while(Serial2.available()) {
          char c = Serial2.read();
          
          if(c == '$') {
              capturing = true;
              idx = 0;
              continue;
          }
          
          if(capturing) {
              if(c == '#') {  // 帧结束处理
                  capturing = false;
                  g_recv_buff[idx] = '\0';
                  if(validate_protocol((char*)g_recv_buff)) {
                      g_recv_flag = true;
                  }
                  break;
              }
              
              if(idx < RXBUFF_LEN-1) {
                  g_recv_buff[idx++] = c;
              } else {  // 缓冲区溢出保护
                  capturing = false;
                  idx = 0;
              }
          }
      }
  }

  // 核心数据解析函数
  String readEncoder() {
      static unsigned long lastUpdate = 0;
      const unsigned long TIMEOUT = 100; // 100ms超时
      
      if(g_recv_flag) {
          // 提取M1-M4参数
          char* dataStart = (char*)g_recv_buff + 5;
          char* params[4];
          uint8_t paramIndex = 0;
          
          // 快速逗号分割
          params[paramIndex++] = dataStart;
          for(char* p = dataStart; *p && paramIndex<4; p++) {
              if(*p == ',') {
                  *p = '\0';
                  params[paramIndex++] = p+1;
              }
          }
          
          // 更新编码器值
          if(paramIndex == 4) {
              Encoder_Now[0] = atoi(params[0]); // M1
              Encoder_Now[1] = atoi(params[1]); // M2 
              Encoder_Now[2] = atoi(params[2]); // M3
              Encoder_Now[3] = atoi(params[3]); // M4
              
              // 更新持久化值
              lastEncoderValues = String((Encoder_Now[0]+Encoder_Now[1])/2) + " " + String((Encoder_Now[2]+Encoder_Now[3])/2);
              lastUpdate = millis();
          }
          
          g_recv_flag = false;
      }
      
      // 超时处理
      if(millis() - lastUpdate > TIMEOUT) {
          return lastEncoderValues;
      }
      
      return lastEncoderValues;
  }

5.电机速度控制

setup（）中需要将电机初始化，因此向驱动模块发送速度0指令，使电机锁死

void initMotorController() {
    Serial2.println("$spd:0,0,0,0#");
  }

ROS中，通过m x x 发送电机控制指令，所以在Arduino只需要将左右轮控制指令分别分配给四个电机即可

void setMotorSpeeds(int leftSpeed, int rightSpeed) {
      // 安全格式化（带缓冲区溢出保护）
      sprintf(send_buff,"$spd:%d,%d,%d,%d#",leftSpeed,leftSpeed,rightSpeed,rightSpeed);

      // 可靠串口传输
      Serial2.println(send_buff);

}