基于PID的轨迹跟踪

在《一文读懂PID》中，我们已经清楚了PID的原理，为了帮助大家更好的掌握PID控制算法，本期分享《PID在轨迹跟踪中的应用》来进行分析学习，文末附本分析的案例模型，建议收藏学习！

本篇文章介绍一种简单的方法：基于车辆侧向路径位移偏差用PID算法来控制小车的方向盘转角。

01. 侧向路径偏差ey

关于轨迹跟踪控制，我们知道PID控制器可以用下面的表达式来表示：

对于PID控制器而言，方向盘转角(steering angle)是被控量，是PID的输出；e是PID的输入，这里显然需要确定e是什么的误差。

我们这里的思路是基于车辆侧向路径位移偏差来控制车辆，这里的e我们用ey来表示。

先给定一个期望路径，期望路径可以通过很多种方式获得，这里为了方便理解给定了一条期望路径。

这条路径处于大地坐标系X0Y中，车辆在这个平面直角坐标系的位置，我们表示为C(Xc,Yc)，车辆坐标系我们用xcy表示，如图1所示。

图1：期望路径

假设在t时刻，期望路径在车辆坐标系中的方程为y=f(x)，车辆的转向瞬心为O’,车速为V，车速在车辆坐标系x轴的分量为u，沿y轴的分量为v，横摆角速度为r，车辆质心侧偏角为β。

注意这里沿着x轴的加速度假设为0。

令预瞄时间为Tp，Q点是经过时间Tp之后车辆实际所在的位置，P点是车辆在位置C时的预瞄点，也就是车辆经过时间Tp之后的目标位置。

如果我们知道Q点和P点在车辆坐标系下各自的y坐标，那么我们就可以得到在经过Tp之后车辆的侧向路径偏差了。

图2：侧向路径偏差

侧向路径偏差ey就是车辆实际到达的点Q与目标点（预瞄点）P之间的距离，在车辆坐标系中，P点坐标为(xP,yP)，Q点坐标为(xQ,yQ)，线段PQ与y轴的夹角为δ，根据勾股定理，可计算线段PQ的长度为：

当δ非常小时，可认为：cosδ近似为1，那么可得到侧向路径偏差ey：

很显然，如果能计算出yP，yQ，就可以计算出ey。

02. 计算yQ

第一步，先确定yQ

假设路径角与车辆质心侧偏角都很小，那么这里的线段

弧段，近似相等：

车辆从当前位置C沿路径（弧段）CQ行驶到Q点时，所用的时长为Tp，横摆角速度为r，那么车辆转过的角度很快就可以求得：θQ=r·Tp

直线CQ相对于车辆速度的夹角为
，所以直线CQ与车辆坐标x轴的夹角就为这样，我们就可以求得Q点在车辆坐标系y方向的坐标值：

03. 计算yP

第二步，来确定预瞄点P在车辆坐标系下y方向的坐标值。

首先，先介绍一下，我们这里的预瞄点如何来取，如图3所示。

图3：取预瞄点

预瞄点就是车辆在经过控制后到达期望路径上的一个目标点。

这个点的选取跟车辆的速度、航向及预瞄时间相关。

车辆当前位置为C（Xc,Yc），预瞄时间为Tp，车速为V，那么理想状态，车辆从当前位置到预瞄点p运行过的距离为预瞄距离 L=u·Tp。

以C点为圆心，以L为半径画一个圆，可知，这个圆与路径至少有两个交点，我们取车辆运行方向上的那个点为我们的预瞄点。

需要明确的是，目标路径是由一系列的点组成，这些点可以由这个序列来表示{J0,J1,J2,…Jn}，其中J0和Jn分别是路径的起点和终点，如果是实时采集的目标路径，n可以是无穷的。我们这里为了方便，取n=200，（注意：实际控制中，这里可以是无数个点）。

路径上这些点在大地坐标系下的坐标为（Xi，Yi），i=0,1,2，…n ,相邻两个点之间的距离设为L0，这个值可以根据实际控制情况来取。我们这里取L0=1m。

我们要从这些点中去找到预瞄点P。

要注意的是，我们通过预瞄距离画圆与路径的交点并不一定正好是我们设定的目标路径点集中{J0,J1,J2,…Jn}的点，有可能是在两个路径点之间，通常我们选择车辆运行方向上前面的点,如图4所示。

图4：选择预瞄点

接下来，我们就来确定预瞄点P的坐标了。

设车辆当前位置为（Xc,Yc），方位角为φ，那么目标路径上的各个点（Xi,Yi）在车辆坐标系中的坐标我们就可以求得：

我们先找到目标路径上距离车辆当前位置最近的点Jp，p是序号，p的取值在0到n之间（0≤p≤n）。

我们设弧段JpJp+1+Jp+2…Jp+m为目标路径上的预瞄曲线段，根据前面的条件，我们可以知道这个弧段的长度约等于预瞄距离L。

这样一来我们可以得到m的值，

我们前面讲过要取车辆方向上的前一个点，所以m的取值还得做一个变化：

因为这里的p，m都是点的序号，所以用int取整。

p的初值为0，车辆起步后通过实时搜索最近点来确定p的值。

那么就有：
这样预瞄点P在车辆坐标系下的y轴坐标为：

到这里PID控制器的输入——侧向路径偏差就可以计算了：

04. 方向盘转角的控制

在控制算法上，PID相对于其他控制算法要简单很多，我们就基于下面这个公式来构建这个模型。

Kp，Kd，Ki这三个参数的调试是PID控制器的核心。

结合前面的公式：

到这里，基于PID的轨迹跟踪控制算法设计可以说完成大半了，接下来的工作就是通过调节PID的三个参数来调整车辆的行驶效果了。

本文仅用最简单的示例来帮助理解基于PID的轨迹跟踪控制，将车辆看作一个点，只考虑了车辆的侧向路径偏差。车辆实际运行是还需要考虑更多的因素，如车辆的动力学、运动学模型，车辆的航向角、车速、车速稳定性等等。

PS：对于本文分享的案例模型感兴趣的学员，可以联系博主~

本文来源：【汽车学堂Automooc】