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_pT_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_pe(k)表示通过比例项增益K_p对误差进行粗调，向着减小误差的方向调整，给出第一部分输出；积分项\frac{K_p}{T_t}\sum_{0}^{k}e(k)表示对误差的累计求和，如果存在稳态误差，这一项通过累加放大并向着减小误差的方向调整，给出第二部分输出；微分项\frac{K_pT_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{aligned} &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{aligned}
其中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