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=\sqrt{(y_{2}-y_{1})^{2}+(x_{2}-x_{1})^{2}}\tag{distance}$$
两点之间的角度：
$$\theta=atan2(y_{2}-y_{1},x_{2}-x_{1})\tag{angle}$$

误差

1. 
   
2. 距离误差 ：参照公式distance
3. 角度误差，首先要计算真实的角度（与起始点相差的角度）：
   $$\varphi_{a}=\varphi_{o}-\varphi_{s}$$
   

$\varphi_{a}$ : 实际角度
$\varphi_{o}$: 里程计测算角度
$\varphi_{s}$ ： 起点角度

PID(PSD)控制器

$$v_{A}(t)=k_{P}e(t)+k_{D}\frac{de(t)}{dt}+k_{I}\intop_{0}^{t}e(t)dt\tag{PID}$$

$v_{A}(t)$ PID输出
$e(t)$ 误差
$k_{P}$ 是比例常数
$k_{D}$ 微分常量
$k_{I}$ 积分常量

$$v_{A}(t_{n})=k_{P}e(t_{n})+k_{D}\frac{e(t_{n})-e(t_{n-1})}{t_{n}-t_{n-1}}+k_{S}\sum_{i=0}^{n}e(t_{i})\cdot(t_{n}-t_{n-1})\tag{PSD}$$

$t_{n}$ : n次迭代 $k_{S}$对应$k_{I}$常量

code

https://github.com/will1991/rosintrodution