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

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

算法

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状态下运动的。

φ=2⋅arcsin(Qz)

角度和距离

两点之间的距离：

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

两点之间的角度：

θ=atan2(y2−y1,x2−x1)(angle)

误差

1. 距离误差 ：参照公式distance
2. 角度误差，首先要计算真实的角度（与起始点相差的角度）：
   
   φa=φo−φs
   
   

φa : 实际角度
φo: 里程计测算角度
φs ： 起点角度

PID(PSD)控制器

vA(t)=kPe(t)+kDde(t)dt+kI∫0te(t)dt(PID)

vA(t) PID输出
e(t) 误差
kP 是比例常数
kD 微分常量
kI 积分常量

vA(tn)=kPe(tn)+kDe(tn)−e(tn−1)tn−tn−1+kS∑i=0ne(ti)⋅(tn−tn−1)(PSD)

tn : n次迭代 kS对应kI常量

code

https://github.com/will1991/rosintrodution