PID轨迹跟踪|python实现

PID轨迹跟踪 | python实现

基本概念

关于PID的基本概念网上有很多，简单来说就是用P项（比例项）实现负反馈效果，用I项（积分项）消除稳态误差，用D项（微分项）提供误差信号微分的预测效果。

连续时间下的PID公式如下所示：
$$u(t)=K_p\left(e(t)+\frac{1}{T_t}{\int }_0^{t}e(t)dt+T_D\frac{de(t)}{dt}\right)$$
式中，$u(t)$表示PID控制器的输出，系统的控制输入；$e(t)=ref-feedback$表示误差；$K_p$表示比例增益系数；$T_t$表示积分时间常数，$K_i=\frac{K_p}{T_t}$表示积分增益系数；$T_D$表示微分时间常数，$K_d=K_p{T}_D$表示微分增益系数。

离散时间下的PID公式如下所示：
$$u(k)=K_p\left(e(k)+\frac{1}{T_t}\sum _0^{k}e(k)+T_D\frac{e(k)-e(k-1)}{dt}\right)$$
式中，$dt$表示离散的周期，与连续时间公式中的$dt$表示微分时间单元不同；其它符号的含义与连续时间公式中的含义相同。离散时间的PID用求和代替了积分，用后向差分代替了微分。实际计算机控制时只能是离散的，所以对离散控制进行分析。

PID是一个无模型控制器，没有太多具体的数学上的原理，更多是根据工程中经验而来，因此很难解释公式为什么这么设计、为什么要用这三项，不必在此纠结，只需了解各项作用即可。

比例项$K_{p}e(k)$表示通过比例项增益$K_p$对误差进行粗调，向着减小误差的方向调整，给出第一部分输出；积分项$\frac{K_p}{T_t}{\sum }_0^{k}e(k)$表示对误差的累计求和，如果存在稳态误差，这一项通过累加放大并向着减小误差的方向调整，给出第二部分输出；微分项$\frac{K_p{T}_D}{dt}(e(k)-e(k-1))$表示通过误差信号过去对时间的导数，对误差信号未来的一个预测（这里是假设误差信号一阶时间连续，而在实际情况中也大多如此），提前向着减小误差的方向调整，给出第三部分输出。最后对三部分的输出进行求和，得到最终的PID输出$u(t)$。

问题描述

用PID实现轨迹跟踪，就是计算前轮转向角${\delta }_f$，使得车辆与参考轨迹上的参考点$P_r=(x_r,y_r)$之间的横向误差$e_y$趋近与0。那么这里有两个问题，一是参考点$P_r$怎么找，二是横向误差$e_y$怎么算。

找参考点

参考点就是人为定义的一个点，可以找当前离自车最近的点为参考点，也可以类似于纯跟踪控制里面找一个预瞄点作为参考点。

实际仿真做下来感觉找离自车最近的点有一个问题，就是车必须在有移动的情况下打方向才会向着参考轨迹移动。但是以参考线上最近点为参考点的话，会导致车还没靠近前一个参考点就要向后一个参考点运动了，就是说留给车的运动空间或者调整距离不够，整个车的行为很滞后。

因此可以考虑找一个预瞄点为参考点，这样就能打一个提前量，车的行为不至于滞后，在轨迹跟踪时能表现好一点。

算横向误差$e_y$

图3 PID控制横向误差

$$\begin{array}{rl}& e_y=l_d\sin ({\theta }_e)\\ & l_d=\sqrt{(x_r-x_h{)}^2+(y_r-y_h{)}^2}\\ & {\theta }_e=\alpha -\psi \\ & \alpha =\arctan (\frac{y_r-y_h}{x_r-x_h})\end{array}$$
其中$P_h=(x_h,y_h)$表示自车的当前坐标点。

PID控制的目标是让某个误差量为0，这里以横向误差$e_y$为误差量，其中$l_d$始终为正，误差量的正负号由$\sin ({\theta }_e)$提供。就是说参考点在自车左侧时误差为正，参考点在自车右侧时误差为负。需要注意下编程时跟自车方向盘打角的一个正负关系对应，方向盘打角往左为负往右为正时需要加一个负号。

应用demo

PID轨迹跟踪演示的完整代码在github仓库，这里仅给出PID计算部分。图中轨迹跟踪效果比较一般的原因个人理解有几点，一是PID是无模型控制，这里只用了一套参数进行控制，像LQR和MPC好歹模型参数也在实时更新以及在实时求解；二是控制周期给太短了，参考点实时在更新；三是参数没有仔细整定。有兴趣的话可以从这三个角度入手进行改进，以提升轨迹跟踪效果。

图3 PID控制轨迹跟踪

class PID:
    """位置式实现
    """
    def __init__(self, upper, lower, k = [1., 0., 0.]):
        self.kp, self.ki, self.kd = k

        self.e = 0.0      # error
        self.pre_e = 0.0  # previous error
        self.sum_e = 0.0  # sum of error

        self.upper_bound = upper    # upper bound of output
        self.lower_bound = lower    # lower bound of output

    def set_param(self, k, upper, lower):
        self.kp, self.ki, self.kd = k
        self.upper_bound = upper
        self.lower_bound = lower

    def cal_output(self, ref, feedback):   # calculate output
        self.e = ref - feedback

        pid_out = self.e * self.kp + self.sum_e * self.ki + (self.e - self.pre_e) * self.kd
        if pid_out < self.lower_bound:
            pid_out = self.lower_bound
        elif pid_out > self.upper_bound:
            pid_out = self.upper_bound

        self.pre_e = self.e
        self.sum_e += self.e

        return pid_out