本文涉及的内容都是针对四轮小车的，如果你的是两轮或者其他形式的小车，请参考其他文章。当然如果你感兴趣的话，本文也有一定的参考价值。另外，本文的内容仅供参考，如有错误，望各位不吝赐教。
运动学分析
在用代码实现之前，我们最好先简单了解一下相关的原理。这种4WD轮式机器人准确的说是采用所谓”滑动控制“(Skid-steer Drive)，这是一种类似于差速控制(Differential Drive)的运动方式。
NOTE:具体可以参考我的相关博文:机器人差速驱动方式(Differential Drive)和 机器人滑动转向驱动方式(Skid-steer Drive)。
参考阿克曼(Ackerman)转向几何学原理，即在汽车转向时4个轮胎都近似围绕一个中心点旋转以保证汽车的行驶稳定性，但是由于四轮差速转向小车没有转向机构，很难保证这一点，而且，小车转向时很容易和地面发生滑移。把汽车的形心作为质心，并且忽略路面情况变化等的影响，可得出四轮差速转向小车的运动学模型如下图所示。
在该图中、
分别为前左轮和后左轮，前右轮和后右轮的转角；
为左右轮距离；
为前后轮轴距；
和
分别为车子质心的线速度和角速度，
,
,
,
分别为各个轮中心的实际运动方向。
根据上图可以得出各速度的和转动角度的关系：
由公式可得：
式中，。
则电机的角速度为：
式中，为电动机的减速比，
为车轮的半径。电机的转速可以根据
得到。
计算车轮转速
在明白了转向原理之后，我们可以根据实际小车的尺寸算出我们要的数据。我使用的小车的各项参数如下：
- 前后轮轴距
- 左右轮距离
- 车轮直径
- 电动机减速比
在这里我们需要各个轮子的转速，我们指定小车转向的角速度为
，转向半径
为
。由上面的公式便可得出各个轮子的转速：
代码实现
首先我们要包含所需要的头文件：
#include <AFMotor.h>
#include <ros.h>
#include <geometry_msgs/Twist.h>
其中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<geometry_msgs::Twist> 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。本文中的示例代码可以在这里下载。
参考资料
- 杨俊驹,林睿,等.轮式移动机器人运动控制系统研究与设计[J].苏州大学：现代电子技术，2016，39(2):23-27.
- ROS Wiki: rosserial_arduino Tutorials
- adafruit.com: Using DC Motors
注：本文迁移自我的CSDN博客：https://blog.csdn.net/github_30605157/article/details/51297066。
