在ROS中通过Arduino实现对4WD轮式机器人的简单控制

本文涉及的内容都是针对四轮小车的，如果你的是两轮或者其他形式的小车，请参考其他文章。当然如果你感兴趣的话，本文也有一定的参考价值。另外，本文的内容仅供参考，如有错误，望各位不吝赐教。

目录

运动学分析

在用代码实现之前，我们最好先简单了解一下相关的原理。这种4WD轮式机器人准确的说是采用所谓”滑动控制“(Skid-steer Drive)，这是一种类似于差速控制(Differential Drive)的运动方式。

NOTE:具体可以参考我的相关博文:机器人差速驱动方式(Differential Drive)和 机器人滑动转向驱动方式(Skid-steer Drive)。

参考阿克曼(Ackerman)转向几何学原理，即在汽车转向时4个轮胎都近似围绕一个中心点旋转以保证汽车的行驶稳定性，但是由于四轮差速转向小车没有转向机构，很难保证这一点，而且，小车转向时很容易和地面发生滑移。把汽车的形心作为质心，并且忽略路面情况变化等的影响，可得出四轮差速转向小车的运动学模型如下图所示。

在该图中 α1 、 α2 分别为前左轮和后左轮，前右轮和后右轮的转角； 2L 为左右轮距离； 2K 为前后轮轴距； v 和 w 分别为车子质心的线速度和角速度， V1 , V2 , V3 , V4 分别为各个轮中心的实际运动方向。
根据上图可以得出各速度的和转动角度的关系：

V1=w⋅R1=w⋅Ksinα1

V2=w⋅R2=w⋅Ksinα2

V3=V1=w⋅Ksinα1

V4=V2=w⋅Ksinα2

由公式 Vy=V⋅cosα 可得：

V1y=V1cosα1=w⋅Ktanα1=w(R−L)

V2y=V2cosα2=w⋅Ktanα2=w(R+L)

V3y=V3cosα1=w⋅Ktanα1=w(R−L)

V4y=V4cosα2=w⋅Ktanα2=w(R+L)

式中， R=vw 。
则电机的角速度为：

wn=Vny⋅ir,n=1,2,3,4

式中， i 为电动机的减速比， r 为车轮的半径。电机的转速可以根据 n=w2π 得到。

计算车轮转速

在明白了转向原理之后，我们可以根据实际小车的尺寸算出我们要的数据。我使用的小车的各项参数如下：

• 前后轮轴距 2K=168mm
• 左右轮距离 2L=266mm
• 车轮直径 r=130
• 电动机减速比 1:30
  在这里我们需要各个轮子的转速，我们指定小车转向的角速度 w 为 5rad/s ，转向半径 R 为 100mm 。由上面的公式便可得出各个轮子的转速：
  
  n1=n3=18.3m/s
  
  
  n2=n4=116.1m/s

代码实现

首先我们要包含所需要的头文件：

#include 
#include 
#include 

其中AFMotor.h驱动电动机驱动板（我所用的是L293D）的头文件，另外两个请参考ROS的wiki文档。

//setting each motor
AF_DCMotor rightFront(3);
AF_DCMotor leftFront(4);
AF_DCMotor leftBack(1);
AF_DCMotor rightBack(2);

设置每个电机在驱动板上对应的接口。

//x轴方向的速度
double lin_vel = 0.0;
//y轴方向的速度
double ang_vel = 0.0;
int cmd_ctrl = 0;
//注册ROS节点
ros::NodeHandle nh;
//回调函数
void motor_cb(const geometry_msgs::Twist& vel)
{
  lin_vel = vel.linear.x;
  ang_vel = vel.angular.z;
  cmd_ctrl = 1 * lin_vel + 3 * ang_vel;
}
//设置订阅的消息类型和发布的主题
ros::Subscriber sub("/turtle1/cmd_vel", motor_cb);

这一部分的作用主要是订阅相关的控制消息。

void setup() {
  nh.initNode();
  nh.subscribe(sub);
  // Turn on all 4 motors
  rightFront.setSpeed(200);
  rightFront.run(RELEASE);
  leftFront.setSpeed(200);
  leftFront.run(RELEASE);
  leftBack.setSpeed(200);
  leftBack.run(RELEASE);
  rightBack.setSpeed(200);
  rightBack.run(RELEASE);
}

初始化节点、车轮速度等。

void loop() {
  nh.spinOnce();

  switch (cmd_ctrl)
  {
   case 2: 
  {
    Serial.print("Go Forward!\n");
    //delay(1000);
    rightFront.setSpeed(116);
    leftFront.setSpeed(116);
    leftBack.setSpeed(116);
    rightBack.setSpeed(116);
    rightFront.run(FORWARD);
    leftFront.run(FORWARD);
    leftBack.run(FORWARD);
    rightBack.run(FORWARD);
    delay(1000);
    rightFront.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    rightBack.run(RELEASE);
    break;
  }
  case -2:
  {
    Serial.print("Go Backward!\n");
    //delay(1000);
    rightFront.setSpeed(116);
    leftFront.setSpeed(116);
    leftBack.setSpeed(116);
    rightBack.setSpeed(116);
    rightFront.run(BACKWARD);
    leftFront.run(BACKWARD);
    leftBack.run(BACKWARD);
    rightBack.run(BACKWARD);
    delay(1000);
    rightFront.run(RELEASE);
    rightBack.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    break;
  }
  case 6:
  {
    Serial.print("Turn Left!\n");
    //delay(1000);
    rightFront.setSpeed(116);
    rightFront.run(FORWARD);
    rightBack.setSpeed(116);
    rightBack.run(FORWARD);
    leftFront.setSpeed(18);
    leftFront.run(BACKWARD);
    leftBack.setSpeed(18);
    leftBack.run(BACKWARD);
    delay(1000);
    rightFront.run(RELEASE);
    rightBack.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    break;
  }
  case -6:
  {
    Serial.print("Turn Right!");
    //delay(1000);
    rightFront.setSpeed(18);
    rightFront.run(BACKWARD);
    rightBack.setSpeed(18);
    rightBack.run(BACKWARD);
    leftFront.setSpeed(116);
    leftFront.run(FORWARD);
    leftBack.setSpeed(116);
    leftBack.run(FORWARD);
    delay(1000);
    rightFront.run(RELEASE);
    rightBack.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    break;
  }
  default:
  {
    //delay(1000);
    rightFront.run(RELEASE);
    rightBack.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    delay(1000);    
    break;
  }
}
    rightFront.run(RELEASE);
    rightBack.run(RELEASE);
    leftFront.run(RELEASE);
    leftBack.run(RELEASE);
    cmd_ctrl = 0;
}

当接收到右转消息时，内轮速度设为18，外轮速度设置为116；左转同理。

硬件连接

将电机驱动板L293D和Arduino板的相应接口(数字一一对应)通过杜邦线连接起来，具体针脚连接方式如下：

如果只想使用直流/步进电机应该连接以下引脚：

• 数字端口11：直流电机#1/步进#1（PWM）
• 数字端口 3：直流电机#2/步进#1（PWM）
• 数字端口 5：直流电机#3/步进#2（PWM）
• 数字端口 6：直流电机#4/步进#2（PWM）

如果要控制直流/步进电机应该增加以下引脚：

• 数字引脚4：DIR CLK触发
• 数字引脚7：DIR EN指令的允许端EN
• 数字引脚8：DIR SER
• 数字引脚12：DIR ATCH中断连接

另外，GND、5V引脚必须也要连接，否则的话就无法稳定地控制直流电动机。

具体接线图如下所示：

NOTE:当然如果你的Arduino上不连接其他传感器的话，你可以直接将L293D直接”骑“到Arduino板子上，即所有对应的引脚都连接起来，这种方法比较简单但是所有的引脚都被占用了。

测试你的代码

首先在你的Arduino编译并上传写好的代码，然后运行下面的命令以指定串口来连接Arduino板。
note:在运行节点之前别忘了启动节点管理器（roscore）。

rosrun rosserial_python serial_node.py /dev/ttyACM0

最后一个参数要根据你在Arduino IDE中选择的串口而定，这里我们使用的是ACM0。
接着运行：

rosrun turtlesim turtle_teleop_key

使用方向键你就可以自由地控制你的机器人了。

后记

本文只是介绍实现对差速轮式机器人的简单控制，无法实现精准的控制。如要更深入的学习请参考ROS的官方wiki。本文中的示例代码可以在这里下载。

参考资料

1. 杨俊驹,林睿,等.轮式移动机器人运动控制系统研究与设计[J].苏州大学：现代电子技术，2016，39(2):23-27.
2. ROS Wiki: rosserial_arduino Tutorials
3. adafruit.com: Using DC Motors

本作品采用知识共享署名-非商业性使用-相同方式共享 4.0 国际许可协议进行许可。