本篇介绍如何利用PID控制实现无人车轨迹跟踪。本篇利用无人车的横向跟踪误差作为控制器的反馈，因此首先介绍计算横向跟踪误差的公式。然而，介绍一个又蠢又萌（简单or傻）的控制方法：
Bang-Bang控制,又被人形象的称为"砰-砰"控制。紧接着介绍我的主角：PID控制。以上控制方法都应用于无人车系统的轨迹跟踪任务。轨迹跟踪任务比较简单、形象、也被我们所熟知，因此阅读本篇也可以加深对PID控制或者说反馈控制原理的理解。
1. 横向跟踪误差
横向跟踪误差(cross track error, 简称CTE)为前轴中心点 ( r x , r y ) (r_x, r_y) (rx,ry)到最近路径点 ( p x , p y ) (p_x, p_y) (px,py)的距离，具体如下图所示。
以上图为基础进行简略分析，如果参考轨迹点在无人车的左边 θ e ∈ [ 0 , π ] \theta_{e}\in [0, \pi] θe∈[0,π]，则应该向左打方向盘；反之 θ e ∈ [ 0 , − π ] \theta_{e} \in [0, -\pi] θe∈[0,−π]则向右打方向盘。
经分析可得横向跟踪误差（CTE）计算公式如下：
e y = l d sin θ e (1) e_y=l_d\sin \theta_{e} \tag{1} ey=ldsinθe(1)
其中， l d = ∣ ∣ p ⃗ − r ⃗ ∣ ∣ 2 l_d=||\vec{p}-\vec{r}||_2 ld=∣∣p −r ∣∣2为机器人后轴中心离当前路点的距离，也被称为前视距离。
在本篇利用仿真环境验证PID控制效果时，发现利用横向跟踪误差，PID很难调出一个较好的控制器。反而是它的简化版本（如式（2）所示）控制效果好一些（建议大家用两种误差分别试试，也望内行的人解惑）。
后面PID控制利用的误差反馈形式如下：
e = l d s i g n ( sin θ e ) (2) e=l_d sign(\sin \theta_{e}) \tag{2} e=ldsign(sinθe)(2)
其中， s i g n ( . ) sign(.) sign(.)为符号函数，当变量大于0时，取-1；当变量小于0时，取 1 1 1；当变量等于0时，取0。
2. Bang-Bang控制
2.1 bang-bang控制原理
无人驾驶系列(一) PID控制详解中形容的非常形象。一个新手司机在马路上行驶时，有时就习惯按照固定的幅度打方向盘，特别是在紧张的情形下，愈是如此。新手司机朝左打、朝右打都是为了让车在车道中间行驶，但实际上车在路上走成S形。
如果bang-bang控制能为自己分辨，它肯定会说：我又不擅长干这个，我有我合适的任务啊（例如：热水器）。
没有人真的拿bang-bang控制来做无人车的轨迹跟踪控制器。我没忍住在篇强加进来，单纯觉得这个控制方法太好玩了，蠢萌蠢萌的。
即然写了它，那么就好好介绍一下吧。bang-bang控制是工程领域中最为常见的一种综合控制形式。它的原理是把最优问题归结为：将状态空间划分为两个区域，一个区域对应于控制变量取正最大值，另一个区域对应于控制变量取负最大值。这两个区域的分界面称为开关面，而决定砰砰控制的具体形式的关键就是决定开关面。
2.2 bang-bang控制器设计
对于无人车轨迹跟踪任务，这个开关面为: e y = 0 e_y=0 ey=0，也即横向跟踪误差为0。
对于轨迹跟踪任务，bang-bang控制器可设计如下：
δ = s i g n ( e y ) δ 0 (3) \delta=sign(e_y)\delta_0 \tag{3} δ=sign(ey)δ0(3)
其中, δ \delta δ为控制器的输出，此处为期望的方向盘转向角。
2.3 python示例代码
本部分利用跟踪直接轨迹与正弦曲线轨迹来看看砰-砰控制的呆萌行为。
class UGV_model:
""""""
Seeing 【https://blog.csdn.net/u013468614/article/details/103489350】 for the complete code of UGV_model.
""""""
from scipy.spatial import KDTree
# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 1000, 1000)
# refer_path[:,1] = 5*np.sin(refer_path[:,0]/5.0) # generating sin reference trajectory
refer_tree = KDTree(refer_path) # reference trajectory
plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=5.0)
# Initial: pos_x is 0, pos_y is 1.0 m, heading is 0 m,
# wheelbase is 2.0 m, speed is 2.0 m/s, decision period is 0.1s.
ugv = UGV_model(0, 1.0, 0, 2.0, 2.0, 0.1)
pind = 0
ind = 0
for i in range(1000):
robot_state = np.zeros(2)
robot_state[0] = ugv.x
robot_state[1] = ugv.y
_, ind = refer_tree.query(robot_state)
if ind < pind: ind = pind
else: pind = ind
dist = np.linalg.norm(robot_state-refer_path[ind])
dx, dy = refer_path[ind] - robot_state
alpha = math.atan2(dy, dx)
e = np.sign(np.sin(alpha-ugv.theta))*dist # bang-bang controller
delta = np.sign(e)*np.pi/6.0
ugv.update(2.0, delta)
ugv.plot_duration()
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
-
跟踪直线的结果
运行结果如下图所示，蓝色点划线为无人车需要跟踪的参考轨迹，红色的点为无人车每个决策时刻的位置。bang-bang控制始终偿试向参考轨迹靠近，但是一直走S形。如果把车开成这样，那人不废，车也得废了。
-
跟踪正弦曲线的结果
它把车开回头了！什么也不用说了，这车肯定废了。
3. PID控制
3.1 PID控制原理
PID控制是应用最为广泛的控制器，没有之一。PID控制器问世至今已有将近70年历史，它以结构简单、稳定性好、工作可靠、调整方便一直是工业控制主要技术之一。PID控制原理可以阅读PID控制原理：看完这个故事你就明白了 与pid控制原理实例说明,这两篇博文用非常有意思且形象的例子来说明PID控制原理。
PID控制器由比例控制、积分控制以及微分控制组合而成。具体结构如下图所示：
-
比例控制（P）：比例控制器的输出与当前状态离目标状态的差值成比例，比例大，则更快逼近目标值，但比例大容易超调；比例小时，超调现象减弱，但是响应时间会变的很长。并且，当仅有比例控制时，系统输出存在稳态误差。
-
积分控制（I）：积分控制器的输出与输入误差信息号的积分成正比关系。积分控制一般被用来消除系统的稳态误差。
-
微分控制（D）：微分控制的输出与输入误差信号的变化率成正比关系。微分控制可以用来减小纯比例控制的超调。
用一个不怎么恰当的说法形容PID中，三者的角色：P相当于显微镜的粗调旋钮，I与D相当于精调旋钮。
3.2 PID控制器
δ ( k ) = k p e ( k ) + k i ∑ i = 0 k e ( i ) + k d ( e ( k ) − e ( k − 1 ) ) (4) \delta(k)=k_p e(k)+k_i \sum_{i=0}^{k}e(i)+k_d (e(k)-e(k-1)) \tag{4} δ(k)=kpe(k)+kii=0∑ke(i)+kd(e(k)−e(k−1))(4)
其中， e ( i ) , i = 0 , 1 , 2 , . . . , k e(i), i=0,1,2,...,k e(i),i=0,1,2,...,k为 k k k步对应的系统误差， k p , k i , k d k_p, k_i, k_d kp,ki,kd分别为P,I,D控制器的参数。
三个参数需要根据任务调试得到一组较合适的值。调参过程一般为：先将 k i , k d k_i, k_d ki,kd设定为零，单独调P控制器，直到系统响应达到一个最好的效果：响应速度能够接受，超调也挺小（比较主观）。然后固定 k p k_p kp，调积分控制器的参数 k d k_d kd。有歌为证（+_+）：
PID调参口讯
参数整定找最佳， 从小到大顺序查。
先是比例后积分， 最后再把微分加。
曲线振荡很频繁， 比例度盘要放大。
曲线漂浮绕大弯， 比例度盘往小扳。
曲线偏离回复慢， 积分时间往下降。
曲线波动周期长， 积分时间再加长。
曲线振荡频率快， 先把微分降下来。
动差大来波动慢， 微分时间应加长。
理想曲线两个波， 前高后低四比一。
一看二调多分析， 调节质量不会低。
但是，当我按照先P，然后I，最后D的顺序调无人车轨迹跟踪控制时，P加上I后，超调只会越来越大。然后我在P后先调微分D，效果立马好很多。所以，本文的顺序是 P → D → I P\rightarrow D \rightarrow I P→D→I。所以，微分与积分先调哪一个，真的与实际系统有关，并不是一招鲜吃遍天的。
我也思考了（胡思乱想的，还请专业人士解惑），无人车轨迹跟踪任务只用PI肯定超调的原因，是因为控制对象是方向盘，控制目的横向位移，这两者之间有一定的延迟性。方向盘转向角与横向位移之间还有一个无人车的航向角，延迟非常大。积分只会被这个延迟的误差误导。
3.3 python示例代码
3.3.1 PID控制python代码
class PID:
def __init__(self, kp, ki, kd):
self.kp = kp
self.ki = ki
self.kd = kd
self.ep = 0.0
self.ei = 0.0
self.ed = 0.0
self.dt = 0.1
def update_e(self, e):
print(e)
self.ed = e - self.ep
self.ei += e
self.ep = copy.deepcopy(e)
def get_u(self):
u = self.kp*self.ep+self.ki*self.ei+self.kd*self.ed
if u > np.pi/6: u = np.pi/6
if u < -np.pi/6: u = -np.pi/6
print(u)
return u
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
3.3.2 P → D → I P\rightarrow D \rightarrow I P→D→I依次调参效果展示
3.3.2.1 调节P控制器的参数 k p k_p kp
- k p = 0.1 k_p=0.1 kp=0.1时
- k p = 1.0 k_p=1.0 kp=1.0时
对比以上两个参数的效果，我们发现，当 k p k_p kp较小时，超调现象也较小，不过需要更长时间响应。
3.3.2.2 保持 k p = 1.0 k_p=1.0 kp=1.0不变，继续调节 k d k_d kd
- 当 k d = 20.0 k_d=20.0 kd=20.0时
我们发现，加入微分控制后，系统的超调明显降低，并维持在一个较小的稳态误差。
3.3.2.3 保持 k p = 1.0 , k d = 20.0 k_p=1.0,k_d=20.0 kp=1.0,kd=20.0，继续调节 k i k_i ki
- 当 k i = 0.001 k_i=0.001 ki=0.001时
当加入积分控制后，也就形成完整的PID控制，我们发现最后稳态误差变得非常小。
3.3.3 PID跟踪正弦曲线效果
直接利用上面调出的PID参数： k p = 1.0 , k i = 0.001 , k d = 20.0 k_p=1.0, k_i=0.001, k_d=20.0 kp=1.0,ki=0.001,kd=20.0。运行结果如下图:
4. 总结
本文针对无人车轨迹跟踪任务，介绍了bang-bang控制，PID控制。并针对每种控制方法的原理、控制器设计进行介绍，并给出python示例代码，最后利用直线与正弦曲线轨迹跟踪结果，说明各控制方法的特点。PID能够让低速运行的无人车跟踪上较平缓的轨迹 (PID控制对高速不平缓轨迹的跟踪效果有待验证，PID有多大的能力，跟我们怎么设计它，给它构造什么样的误差反馈有很大关系，最后PID参数也影响较大)。后面将继续介绍让我印象更加深刻的无人车轨迹跟踪控制方法。
以上
672
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
