ROS中的机器人导航需要底层轮子驱动十分精确的对行进的速率和角速度进行控制。本文根据大量的实验数据进行图形化分析，找出线性速率、角速度和电机PWM线性关系，并得出计算公式。

1. ROS导航机器人驱动功能分析

ROS的navigation包内容十分丰富，并封装完整。开发机器人其实就是开发轮子驱动与ROS 进行通讯。这是一个双向通讯：

1.1 ROS -> 轮子驱动

ROS 在cmd_vel这个主题上发布Twist消息，这个消息包含的就是机器人的期望前进速度和转向速度。轮子驱动必须要精确的控制电机同时要以某个速度和某个角速度前进或后退。角速度就代表拐弯的弯度.

使用ctrl + alt + t 打开一个新的终端以后，输入如下命令，就可以查看Twist的消息类型了。

rosmsg show geometry_msgs/Twist

Twist消息的结构，其中linear 的x就是代表前进方向的速度，单位为m/s。angular 的z就代表机器人的绕中心旋转的角速度，单位为 弧度/s (rad/s)。本文要研究这两个速度的关系。其余的4个成员可以忽略,它们对平面上的4轮机器人来说没有实际意义.

1.2  轮子驱动 -> ROS

电机执行的效果: 线性速度,角速度和机器人现在地图上的坐标需要实时反馈给ROS. 这是通过向odom主题发送nav_msgs/Odometry导航消息，报告角速度，线速度和巡航角度。

2. PWM, 线性速度，左右轮子速度和角速度的关系

机器人线性速度： RobotV

左右轮速度：       LeftWheelV   RightWheelV

角速度：             YawRate

RobotV = ( LeftWheelV + RightWheelV ) / 2 ／／公式1

YawRate = delta_t * abs( LeftWheelV - RightWheelV ) ／／公式2

PWM 与轮子的关系：

xxxxWheelV = PWM *factor + offset ／／公式3

在实际运行中发现，左右轮的性能因为硬件有所差异，所以 delta_t, factor, offset 对左右两边来讲是不一样的。所以它们各有一套参数。这些参数的确定是要通过大量的数据采集后进行分析得出来的。下面我们讨论如何确定公式2，3中的参数

3. 左右轮子速度和PID控制

PWM与轮子转速的关系并不是不变的，当电池放电一段时间后，要增大factor的值才能达到期望的速度。这就是PID的作用，PID要求给一个初始PWM值，上面的公式3就可以算出这个初始值，剩下就交给反馈调整了， ROS有个pid类。

4. PWM与轮子速度实验数据分析

PWM从2０至1００的区间平均取1０个值，跑1０轮。每轮前行1米，然后后退1米，记录前进后退两个速度。左右两轮各一套数据，每次这样得到了4个数据样本，总共4０个，图表分析如下：

1。左上图 Forward left wheel 前行左轮

2。左下图 Backward left wheel 后退左轮

3。右上图 Forward right wheel 前行右轮

4。右下图 Backward right wheel 后退右轮

横坐标是PWM值，纵坐标是速度(m/s)

采样数据的结果非常完美，标准的线性关系图。同时也能看出来左右两轮确实有不一致的地方，但是前进后退的数据很统一。这样就可以测出factor, offset两个参数，左右两轮个一套。

5.左右轮速度差与角速度实验数据分析

公式2的分析比较复杂，但是一旦确定就不变了，除非改变环境。分析出delta_t

转弯分为1０种类型：

1，左轮不转，右轮PWM从2０至1００的区间平均取1０个值，向前跑1０轮。每轮跑360度，测出角速度和线速度。

2，右轮不转，左轮PWM从2０至1００的区间平均取1０个值，向前跑1０轮。每轮跑360度，测出角速度和线速度。

3，左轮比右轮转的慢，速度差逐渐加大取1０个值，向前跑1０轮。每轮跑360度，测出角速度和线速度。

4，右轮比左轮转的慢，速度差逐渐加大取1０个值，向前跑1０轮。每轮跑360度，测出角速度和线速度。

5，左轮向前，右轮向后，速度差逐渐加大取1０个值，顺时针跑1０轮。每轮跑360度，测出角速度和线速度。

6,   左轮不转，右轮PWM从2０至1００的区间平均取1０个值，向后跑1０轮。每轮跑360度，测出角速度和线速度。

7，右轮不转，左轮PWM从2０至1００的区间平均取1０个值，向后跑1０轮。每轮跑360度，测出角速度和线速度。

8，左轮比右轮转的慢，速度差逐渐加大取1０个值，向后跑1０轮。每轮跑360度，测出角速度和线速度。

9，右轮比左轮转的慢，速度差逐渐加大取1０个值，向后跑1０轮。每轮跑360度，测出角速度和线速度。

1０,左轮向后，右轮向前，速度差逐渐加大取1０个值，逆时针跑1０轮。每轮跑360度，测出角速度和线速度。

1－4为前行，6－9为后行。转速都是逐渐加强。其中旋转角度的测量是通过电子罗盘HMC5883L 测算出来的，该芯片的干扰校准见我的文章

图表分析如下：

左边图表从上向下对应旋转编号为 1，2，3，4，5

右边图表从上向下对应旋转编号为 6，7，8，9，1０

横坐标为左右两轮速度差的绝对值，纵坐标为角速度，观察图像得出下面结论：

1。很明显前进和后退比例系数(斜率)不一样

2。它们向左向下的延长线都是穿过原点的，就是说速度差为０时，没有旋转，这个跟实际很匹配

3。左右两轮转弯性能无差异

4。第3，4行数据点向上平移到1，2行数据点，两组数据可以完全拼接成一条线保持相同的斜率

根据上面的数据可以推断出，公式2中的delta_t在前行和后退时是不一样的，10类转弯可以归纳为2种了。

6.带方向的线性速度和角速度

在ROS中，线性速度向后是负值，向前是正值。角速度方向的规定如图，它符合右手原则，右图逆时针方向就是正值，顺时针为负值。就是上面的左转为正，右转为副。

我们把方向考虑到公式1，2，3中，推导出代码可用的公式从而算出左右轮的速度 LeftWheelV,  RightWheelV, 然后交给pid到树莓派执行。从而达到精确的同时控制线性速度和角速度。其中RobotV 和 YawRate是ROS下发的命令，程序运行时就是已知量。

RobotV = ( LeftWheelV + RightWheelV ) / 2 ／／公式1

YawRate = delta_t * abs( LeftWheelV - RightWheelV ) ／／公式2

当 LeftWheelV 大于 RightWheelV时 是右转，此时YawRate为负，反之为正。公式2中的abs决定值可以去掉了，变成公式4

YawRate = delta_t * ( RightWheelV - LeftWheelV ) ／／公式4

YawRate / delta_t = RightWheelV - LeftWheelV

公式1不变，简单变换一下

RobotV = ( LeftWheelV + RightWheelV ) / 2 ／／公式1

RobotV ＊ 2 ＝ LeftWheelV + RightWheelV

合并公式1 和 4，初中的二元一次方程，得出：

YawRate / delta_t ＋ RobotV * 2 = RightWheelV * 2

两边除2

YawRate / delta_t / 2 + RobotV = RightWheelV

左右互换

RightWheelV = RobotV + YawRate / delta_t / 2

带入公式1

LeftWheelV = RobotV ＊ 2 - ( YawRate / delta_t / 2 + RobotV ) ＝ RobotV － YawRate / delta_t / 2

这里的delta_t取值要根据第5段的结论来定，根据 RobotV 正负值选取。这样左右两个轮子的速度大小和方向都可以计算出来了。但是要注意这里的YawRate必须是弧度制。

Ros 的base_control 的C++代码

#include "ros/ros.h"

#include "std_msgs/String.h"

#include

#include

#include "boost/shared_ptr.hpp"

#include

#include "RunVelocity.h"

boost::shared_ptr m_leftwheel;

boost::shared_ptr m_rightwheel;

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd)

{

double LeftWheelV, RightWheelV;

double RobotV = vel_cmd.linear.x;

double YawRate = vel_cmd.angular.z;

double delta_t = RobotV > 0 ? 5.9 : 4.45;

RightWheelV = RobotV + YawRate / delta_t / 2 ;

LeftWheelV = RobotV - YawRate / delta_t / 2 ;

m_leftwheel->RunSpeedCommand(LeftWheelV);

m_rightwheel->RunSpeedCommand(RightWheelV);

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "base_control");

ros::NodeHandle n;

ros::Subscriber cmd_vel_sub = n.subscribe("/cmd_vel", 10, cmd_vel_callback);

// 树莓派gpio控制轮子

wiringPiSetup();

int left_forward, left_backward, right_forward, right_backward;

n.param("left_forward", left_forward, 1);

n.param("left_backward", left_backward, 4);

n.param("right_forward", right_forward, 6);

n.param("right_backward", right_backward, 5);

m_leftwheel = boost::shared_ptr(new RunVelocity(n, left_forward, left_backward));

m_rightwheel = boost::shared_ptr(new RunVelocity(n, right_forward, right_backward));

ROS_INFO("ready to run on /cmd_vel");

ros::spin();

}