ROS小车-上位机代码：两驱小车直线行走校准

前言
要想实现对小车的精准控制、定位精度的准确，那必然前提就是找到小车速度控制的数学逻辑。

本文实现的目的是：通过teleop_twist_keyboard键盘速度控制节点，给小车下发速度指令，希望在I(i)指令下，小车做直线运动(可匀速或可变加减速)，但是行走轨迹必须尽量笔直。

PID 的误解

当我实现了通过串口协议的move指令对小车车轮的PWM控制之后，我急切地寻找让两个轮子在同一速度下运行的方法，也就是让小车走直线。

小车为什么不能走直线？原因挺多的。
电机原因。同一批次同一规格同一减速比等同一参数的电机彼此之间都有不同。造成的结果就是相同电压下，转速不一样。
驱动芯片原因。驱动芯片对左右两桥的输出电压不等大，跟电机一个道理。
电路设计原因。电路设计时走线长短的不对称，或其他原因造成两边阻抗不等大。
小车设计原因。小车的设计造成两轮歪斜，两轮受力不均匀等原因。

总之，想依靠工艺手段达到两轮等速的目的是不大现实的。特别是手工制品。工厂机器生产还能够控制误差在一定范围之内。那对我们这种手工制作的小车就没招了吗？那当然还是有办法，就是通过软件来弥补这个缺陷。

在我搜索了大半个百度之后，我得到了PID能够调速实现小车直线行走的校准方法的这一种想法。于是我使劲地翻PID理论，学习PID的算法。但是当我想要搜索“PID的作用”时，我发现百度搜不到这个答案。它只告诉我“P”“I”“D”这三个值分别的作用是什么。

那我就要提问了：PID真的能够校准速度吗？实现两轮等速吗？
答案是：可以。

那我为什么还说我误解它了呢？
答案是：费劲。

为了清晰地回答这个问题，需要先讲清楚PID算法的作用是什么。PID算法的作用就是让系统或快或慢地达到预期值。慢则稳，荡却快。也就是说PID是与我们设定的预期目标有关系，它的作用是把控到达目标值的快慢关系。理解起来其实就是加速度的意思。可以控制这个速度多快到达预期速度，那就是加速度嘛。

那为啥它能够有校准速度的效果？也就是两轮等速的效果。你想想，加速度在它的把控之下，是否就能够操作速度的快慢？对速度形成拉扯？当某个轮子一时间跑得快了，我把它的P压低，你上升时间就长，那么某一段时间内，两个轮子的速度是差不多的，所以就有看似等速的效果。等过了这一段时间，该歪还是歪。因为两轮的目标速度并不一样。

所以说，用PID来校准速度真的是费劲。治标不治本。我们应该抓到问题的核心，那就是目标速度。我们要的结果就是两轮等速，目标就是两轮速度一样嘛。那从何下手？

我们回到最初的思考：小车的速度跟什么有关？
回答：两轮的速度有关。

两轮的速度跟什么有关？
回答：PWM有关。

PWM！这就是答案！小车速度的关系就是跟PWM有关。你肯定想说，这不是废话吗？是废话，我一直在说废话，对不起。那既然速度跟PWM有关，还要整PID干嘛？

速度与 PWM

当我们回到问题的本质上来之后，我们是不是应该思考：怎样建立速度与PWM的关系？

这两者肯定是有关系，但是它们是什么关系？你问我，我哪知道！只能瞎猜咯。线性关系？你加1，我加1，我们一起做兄弟！这恐怕不是吧…

根据我学过的电路知识，电机并不属于线性元件。那么是不是曲线关系呢？那答案是必须的。不是线性肯定就是非线性嘛，也就是曲线性嘛。

于是我翻开了大佬的笔记：
ROS 学习系列-- 四轮机器人线性速率、角速度和电机PWM线性关系的定量分析

我跟着大佬的思路一步一个脚印做着实验。结果我得到了如下这个图：说好的线性呢？好吧。果然是非线性的。横坐标是速度(m/s)，纵坐标是PWM(0-255)。但是如图上所见，PWM在某段区间内，确实可以把它看成是一条线段。这也是电机的特性。

运动学分解

再次翻开大佬笔记：两轮差速底盘的运动模型分析：运动控制与里程计解算

具体的内容，希望看本文的你，能够仔仔细细看一下上面的链接。这里只说应用。

上面的长篇大论，我们知道两个电机的模型肯定是有一定区别的。那么小车若想笔直行走两电机的转速或者说两轮的速度肯定是不一样的。而电机的速度又是跟所给的PWM有关系的。

根据运动学分解：
左轮速度 = 小车线速度 - 小车角线速度 × 两轮距离的一半
右轮速度 = 小车线速度 + 小车角线速度 × 两轮距离的一半

小车的线速度与角速度来源与对/cmd_vel话题的订阅。/cmd_vel话题消息来自teleop_twist_keyboard节点的发布。到这里，理想状态下(仿真环境)，小车是可以按预期速度跑起来了。

代码实现如下

#include <ros/ros.h> //ros头文件
#include <geometry_msgs/Twist.h> //速度消息头文件
// 消息格式头文件
#include "std_msgs/Empty.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"#define PI acos(-1)float wheel_track;//轮子间距int main(int argc, char *argv[])
{setlocale(LC_ALL,"");ros::init(argc, argv, "ArduinoClient");ros::NodeHandle node("~");//订阅速度话题ros::Subscriber sub_car_cmd_vel = node.subscribe("/cmd_vel",100,car_vel_callback);//获取轮子间距node.param<float>("wheel_track", wheel_track,0.0);ROS_INFO("轮子间距:%f m",wheel_track);ros::spin();//订阅回调
}void car_vel_callback(const geometry_msgs::Twist& cmd_vel)
{geometry_msgs::Twist twist = cmd_vel;double m_x_speed  = twist.linear.x;     //线速度 m/sdouble m_th_speed = twist.angular.z;  //角速度 rad/s// ROS_INFO_STREAM("cmd_vel接收到 <-- 线速度:"<<m_x_speed<<"\t"<<"cmd_vel 角速度:"<<m_th_speed);double left_speed =0,right_speed=0;left_speed  = m_x_speed - m_th_speed * wheel_track / 2;right_speed = m_x_speed + m_th_speed * wheel_track / 2;// ROS_INFO_STREAM("即将下发 -->> 左轮速度:"<<left_speed<<"\t"<<"右轮速度:"<<right_speed);
}

需要说明的是：geometry_msgs::Twist 速度消息下有六个值，分别是twist.linear(x,y,z)，twist.angular(x,y,z)。
小车线速度 = twist.linear.x
小车角速度 = twist.angular.z

速度校准

速度校准的准备工作是需要先完成小车的主体，主控与Arduino的通信协议之后，才能继续。