4. ROS编程入门--PID控制器

介绍：
这篇教程是交给大家怎么在ROS里写一个PID控制器，PID控制器有三部分:比例部分（P）、积分部分（I）、微分部分（D）,PID的输出是这三部分的加和。
PID可以控制机器人的位置和航向角，期望机器人向前走x米，x即为参考值（PID控制器的输入期望），旋转期望角度$\varphi$。

算法

PID输入是里程计消息。首先你要计算当前值和目标值之间的差值。另一个要计算的是PSD控制器的干涉值（intervention）（译者注：估计是PID输出吧）。由于用的是离散版本PID,因此积分变为加和形式。这些都在NodePID 类中进行实现。回掉函数负责数据的收集。

输入输出处理

程序的输入消息是里程计消息：

std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64 covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64 covariance

你实际需要的是位置和时间信息。位置信息(x,y)存储在pose.pose.position中，Time信息存储在header.stamp中.唯一的问题是如何得到机器人的旋转信息。geometry_msgs/Quaternion 包含三位空间旋转的 float64 （x,y,z,w) 四元数，我么的机器人是在2D状态下运动的。

$$\varphi=2\cdot\arcsin(Q_{z})$$

角度和距离

两点之间的距离：

d=(y2−y1)2+(x2−x1)2−−−−−−−−−−−−−−−−