无人车系统(四):轨迹跟踪PID控制

本篇介绍如何利用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=0ke(i)+kd(e(k)e(k1))(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 PDI。所以,微分与积分先调哪一个,真的与实际系统有关,并不是一招鲜吃遍天的。

我也思考了(胡思乱想的,还请专业人士解惑),无人车轨迹跟踪任务只用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 PDI依次调参效果展示

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参数也影响较大)。后面将继续介绍让我印象更加深刻的无人车轨迹跟踪控制方法。


以上

相关推荐
©️2020 CSDN 皮肤主题: 鲸 设计师:meimeiellie 返回首页