无人车系统（四）：轨迹跟踪PID控制

windSeS

已于 2023-04-24 13:21:54 修改

阅读量5.3w

点赞数 105

分类专栏：无人驾驶技术系统文章标签： PID控制无人车轨迹跟踪控制 bang-bang控制 PID原理

于 2019-12-11 23:36:26 首次发布

本文为博主原创文章，未经博主允许不得转载。

本文链接：https://blog.csdn.net/u013468614/article/details/103492847

版权

无人驾驶技术系统专栏收录该内容

16 篇文章 545 订阅

订阅专栏

本篇介绍如何利用PID控制实现无人车轨迹跟踪。本篇利用无人车的横向跟踪误差作为控制器的反馈，因此首先介绍计算横向跟踪误差的公式。然而，介绍一个又蠢又萌（简单or傻）的控制方法：Bang-Bang控制,又被人形象的称为"砰-砰"控制。紧接着介绍我的主角：PID控制。以上控制方法都应用于无人车系统的轨迹跟踪任务。轨迹跟踪任务比较简单、形象、也被我们所熟知，因此阅读本篇也可以加深对PID控制或者说反馈控制原理的理解。

1. 横向跟踪误差

横向跟踪误差(cross track error, 简称CTE)为前轴中心点 $r_x, r_y)$ 到最近路径点 $p_x, p_y)$ 的距离，具体如下图所示。

在这里插入图片描述

以上图为基础进行简略分析，如果参考轨迹点在无人车的左边 $\theta_{e}\in [0, \pi]$ ，则应该向左打方向盘；反之 $\theta_{e} \in [0, -\pi]$ 则向右打方向盘。

经分析可得横向跟踪误差（CTE）计算公式如下：

$e_y=l_d\sin \theta_{e} \tag{1}$

其中， $l_d=||\vec{p}-\vec{r}||_2$ 为机器人后轴中心离当前路点的距离，也被称为前视距离。

在本篇利用仿真环境验证PID控制效果时，发现利用横向跟踪误差，PID很难调出一个较好的控制器。反而是它的简化版本（如式（2）所示）控制效果好一些（建议大家用两种误差分别试试，也望内行的人解惑）。

后面PID控制利用的误差反馈形式如下：
$e=l_d sign(\sin \theta_{e}) \tag{2}$
其中， $s i g n (.)$ 为符号函数，当变量大于0时，取-1；当变量小于0时，取 $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$ ，也即横向跟踪误差为0。

对于轨迹跟踪任务，bang-bang控制器可设计如下：

$\delta=sign(e_y)\delta_0 \tag{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控制器

$\delta(k)=k_p e(k)+k_i \sum_{i=0}^{k}e(i)+k_d (e(k)-e(k-1)) \tag{4}$

其中， $e (i), i = 0, 1, 2, ..., k$ 为 $k$ 步对应的系统误差， $k_p, k_i, k_d$ 分别为P,I,D控制器的参数。

三个参数需要根据任务调试得到一组较合适的值。调参过程一般为：先将 $k_i, k_d$ 设定为零，单独调P控制器，直到系统响应达到一个最好的效果：响应速度能够接受，超调也挺小（比较主观）。然后固定 $k_p$ ，调积分控制器的参数 $k_d$ 。有歌为证（+_+）：

PID调参口讯
参数整定找最佳，从小到大顺序查。
先是比例后积分，最后再把微分加。
曲线振荡很频繁，比例度盘要放大。
曲线漂浮绕大弯，比例度盘往小扳。
曲线偏离回复慢，积分时间往下降。
曲线波动周期长，积分时间再加长。
曲线振荡频率快，先把微分降下来。
动差大来波动慢，微分时间应加长。
理想曲线两个波，前高后低四比一。
一看二调多分析，调节质量不会低。

但是，当我按照先P，然后I，最后D的顺序调无人车轨迹跟踪控制时，P加上I后，超调只会越来越大。然后我在P后先调微分D，效果立马好很多。所以，本文的顺序是 $P\rightarrow D \rightarrow 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\rightarrow D \rightarrow I$ 依次调参效果展示

3.3.2.1 调节P控制器的参数 $k_p$

$k_p=0.1$ 时
$k_p=1.0$ 时

对比以上两个参数的效果，我们发现，当 $k_p$ 较小时，超调现象也较小，不过需要更长时间响应。

3.3.2.2 保持 $k_p=1.0$ 不变，继续调节 $k_d$

当 $k_d=20.0$ 时

我们发现，加入微分控制后，系统的超调明显降低，并维持在一个较小的稳态误差。

3.3.2.3 保持 $k_p=1.0,k_d=20.0$ ，继续调节 $k_i$

当 $k_i=0.001$ 时

当加入积分控制后，也就形成完整的PID控制，我们发现最后稳态误差变得非常小。

3.3.3 PID跟踪正弦曲线效果

直接利用上面调出的PID参数： $k_p=1.0, k_i=0.001, k_d=20.0$ 。运行结果如下图:
在这里插入图片描述

4. 总结

本文针对无人车轨迹跟踪任务，介绍了bang-bang控制，PID控制。并针对每种控制方法的原理、控制器设计进行介绍，并给出python示例代码，最后利用直线与正弦曲线轨迹跟踪结果，说明各控制方法的特点。~~PID能够让低速运行的无人车跟踪上较平缓的轨迹~~ (PID控制对高速不平缓轨迹的跟踪效果有待验证，PID有多大的能力，跟我们怎么设计它，给它构造什么样的误差反馈有很大关系，最后PID参数也影响较大)。后面将继续介绍让我印象更加深刻的无人车轨迹跟踪控制方法。

以上

windSeS

关注

105
点赞
踩
705

收藏

觉得还不错? 一键收藏
打赏
52
评论
无人车系统（四）：轨迹跟踪PID控制

本篇介绍如何利用PID控制实现无人车轨迹跟踪。1. 横向跟踪误差横向跟踪误差(cross track error, 简称CTE)为前轴中心点(rx,ry)(r_x, r_y)(rx,ry)到最近路径点(px,py)(p_x, p_y)(px,py)的距离，具体如下图所示。以上图为基础进行简略分析，如果参考轨迹点在无人车的左边θe∈[0,π]\theta_{e}\in [0, \......
复制链接

扫一扫