本篇介绍如何利用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()
-
跟踪直线的结果
运行结果如下图所示,蓝色点划线为无人车需要跟踪的参考轨迹,红色的点为无人车每个决策时刻的位置。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
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参数也影响较大)。后面将继续介绍让我印象更加深刻的无人车轨迹跟踪控制方法。
以上