自动驾驶控制算法---横向控制（MPC/LQR）和纵向控制（PID算法及基于CARLA的Python代码实现）_carla的mpc

1、给控制的规划轨迹

　　给出一条轨迹，然后去找匹配点（与真实位置距离最短的点），再由匹配点去算出投影点，然后让车按照真实位置与投影点的误差去算出u得到控制量使得误差$e_{rr}$为0。但实际上无人驾驶环境是动态的，事先设计的轨迹未必是合理的轨迹，而且每次寻找匹配点都要遍历一遍耗费算力。

　　因此，规划的轨迹一般是带有时间的。假设初始条件$x(0)$、$\dot{x(0)}$、$\ddot{x(0)}$、$y(0)$、$y^{'}(0)$、$y^{''}(0)$和终止条件$x(T)$、$\dot{x(T)}$、$\ddot{x(T)}$、$y(x_{end})$、$y^{'}(x_{end})$、$y^{''}(x_{end})$，通过这些边界条件可以算出五次多项式（六个未知数）$x(t)$与$y(x)$（$y(x)$代表对曲线的斜率有要求）。

　　通过中间变量求导得出：

$y(t)=y(x(t))$

$\dot{y}(t)=y^{'}(x(t))\cdot \dot{x(t)}$

$\ddot{y}(t)=y^{''}(x(t))\cdot \dot{x(t)}^{2}+y^{'}(x(t))\cdot \ddot{x(t)}$

　　这就有了$y(t)$。

　1.1、横向控制接口

　　t时刻轨迹给出此时刻$x_{r}$与$y_{r}$的值，则：

$x _{r}\left ( t \right )=x(t)$

$y _{r}\left ( t \right )=y(t)$

$\theta _{r}\left ( t \right )=\arctan \left \{ y^{'}\left [ x\left ( t \right ) \right ] \right \}$

$k_{r}\left ( t \right )=\frac{y^{''}\left [ x(t) \right ]}{(1+y^{'}\left [ x(t) \right ]^{2})^{1.5}}$

　　得到这些横向控制接口，然后就可以用反馈控制LQR+前馈控制$\delta _{f}$进行横向控制。

　1.2、纵向控制接口

　　如上图，纵向位置误差为$-e_{s}=-(\vec{x}-\vec{x_{r}})\cdot \vec{\tau _{m}}$

　　速度误差为$v_{p}-\dot{s}=\sqrt{\dot{x_{r}(t)}^{2}+\dot{y_{r}(t)}^{2}}-\frac{v_{x}\cos (\varphi -\theta _{r}(t))-v_{y}\sin (\varphi -\theta _{r}(t))}{1-k_{r}(t)\cdot e_{d}}$

　　加速度为$a_{p}=\sqrt{\ddot{x_{r}(t)}^{2}+\ddot{y_{r}(t)}^{2}}$

　　纵向控制去用双PID去控制速度，即用$e_{s}$作为位置PID的输入，速度差和位置PID的输出作为速度PID的输入，速度PID的输出和加速度一起作为加速度给汽车。

2、PID纵向控制

　　速度和加速度匹配：规划的速度是10，因此希望车以10的速度在跑。因为加速度一开始是0，必然要有一个很大的加速度让车加速起来，然后在加速过程加速度逐渐的减小，速度刚好是10的时候加速度刚好是0，速度没有到10的时候，一直有加速度，一直不断在加速，这叫匹配的加速度。

　　用PID控制纵向速度，可以自动的匹配加速度，期望的速度去减当前的速度，差值作为加速度信号输进去。速度-速度=加速度（忽略量纲），看成一个数减一个数，只要满足在刚开始的时候离目标很远加速度很大，在接近目标的时候加速度逐渐减缓，达到目标时候加速度变为0就可以了。

$u(t)=K_{p}(e(t)+\frac{1}{T_{i}}\int_{0}^{t}e(t)dt+T_{d}\frac{de(t)}{dt})$

　　PID：比例P（proportional）、积分I（integral）、微分D（derivative）。

　　（1）、P

　　P就是个比例项，即对误差信号乘个数值。

　　比例项P的作用：可以加快信号到达目标的速度，将误差信号（期望速度和实际速度之差）乘个比例项P能够讲误差信号扩大几倍，然后加快达到期望速度的速度，缩小稳态误差。

　　比例项P的问题：

　　①、而且不能消除稳态误差，因为不管放大10倍、100倍，总有时刻因为误差信号乘完比例项P后依然太小了不起作用。

　　②、比例项P值较小的情况下，响应速度变慢了，而且最终离目标的误差较大；比例项P值较大的情况下，虽然响应速度比较快，但是振荡很严重，可能一点点微小的误差加速度就会放得很大，就会很敏感、波动。

　　（2）、I

　　I就是对信号积分，然后输出积分的值（有滞后的作用）。　　

　　积分项I的作用：为了消除稳态误差。只要实际速度和期望速度时间的差值（即误差信号）不是0，那么积分项I就会一直积分，信号一直有，就会消除稳态误差。积分会随着时间的增长而越来越大，积分控制器会明显的消除稳态误差。

　　积分项I的问题：会带来超调。比如本来目标是10，是从50降到10的，但是降到了0，再从0上升到10（虽然收敛了，但是产生了超调），我们理想的控制是从50逐渐的降到10而不带有超调。

　　（3）、D

　　D就是对信号微分，然后输出微分的值（这里的微分D不是数学意义上的微分，严格来说是一个数值微分，因为信号离散了没有办法做数学意义上的解析的微分，所以要进行相关滤波操作，因为数值微分很容易震荡） 。

　　微分D的作用：抑制超调（微分就是可以提前控制，达到抑制超调的目的）。

　　在实际应用中比例项也会引起超调，因为在实际应用中信号是有延迟的，无论你踩到刹车还是油门，发动机还是电机开始转进行工作都有一定的延迟，一旦有延迟的话比例项就会产生超调，微分D就起作用了。

总结：比例项P可以加快信号到达目标的速度，p过大在实际应用中会带来超调（因为延迟），就会用微分D来消掉实际应用中比例项P产生的超调。积分项I一般不常用（微分D降比例P的超调还是比较容易的，但是降积分I的超调就比较难 ），虽然积分项I能够消除稳态误差，但是会带来很大的超调，而且消起来比较麻烦（除非稳态误差特别大），而且一般积分带来的超调只能通过降低比例项（比例P降低一点）来减小超调，但是比例P减小导致响应变慢了。一般来说在控制的时候用比例项P、微分D就够了，保证误差在一定容忍范围内。

一般来说经验是先调比例项P、再调微分D、最后再调积分项I。只要控制的精度达到一定范围就可以了，不要求完全的精确，因为要达到特别精确一定要增大积分项I过渡的消除稳态误差，必然会导致超调，然后再去调增大微分D去抑制超调，必然会导致噪声增大（伯德图的对数幅频曲线往上翘 ）。

P+D：又快又满足精度要求，I：给个非常小的积分I起一点作用，减小稳态误差。

3、结合自动驾驶问题的纵向速度控制PID代码实现

　　1、导入库文件

import numpy as np
import math
import carla
import cvxopt
from collections import deque

　　2、初始化信息（比例系数k_p、积分系数k_i、微分系数k_d等）

class Longitudinal_PID_controller(object):    #纵向控制
    def __init__(self, ego_vehicle,  k_p=1.15, k_i=0, k_d=0, d_t=0.01):  # 手动调节参数k_p、k_i、k_d
        """
        采用PID进行纵向速度控制，包括比例项P、积分项I、微分项D
        ego_vehicle: 是carla中的主车
        k_p: 比例项系数
        k_i: 积分项系数
        k_d: 微分项系数
        d_t: 控制间隔
        """
 
        self.vehicle = ego_vehicle  # ego_vehicle是carla中的主车
        self.k_p = k_p   # 比例系数
        self.k_i = k_i   # 积分系数
        self.k_d = k_d   # 微分系数
        self.d_t = d_t   # 控制间隔
        self.target_speed = None  # 目标速度
        self.error_buffer = deque(maxlen=60)  # 设置一个误差缓存区，用于积分项和差分项的计算
        self.error_threshold = 1  # 设定一个阈值

　　3、PID的实现（输出速度控制量u）

    def PID_control(self, target_speed):
        """
        函数：计算PID控制的输出
        return: u
        """
 
        v = self.vehicle.get_velocity()  # self.vehicle.get_velocity()的格式：Vector3D(x=0.000000, y=0.000000, z=-0.194462)
        v_length = 3.6 * math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z)  # 速度大小,m/s转为km/h，1m/s = 3.6km/h
        self.target_speed = target_speed      # 目标速度
        error = self.target_speed - v_length  # 速度误差
        self.error_buffer.append(error)  # 将新的速度误差放入缓存区，如果缓存区满了，最左边的溢出，整体数据左移一位，新的数据加在最右边
 
        if len(self.error_buffer) >= 2:
            integral_error = sum(self.error_buffer) * self.d_t  # 积分误差，为了解决稳态误差
            derivative_error = (self.error_buffer[-1] - self.error_buffer[-2]) / self.d_t  # 微分误差，为了缓解超调
        else:
            integral_error = 0.0
            derivative_error = 0.0
        if abs(error) > self.error_threshold:   # 一旦出现误差大于阈值的情况，只有比例项发挥作用
            integral_error = 0.0
            self.error_buffer.clear()
 
        u = self.k_p * error + self.k_i * integral_error + self.k_d * derivative_error
 
        return u

　　4、整个控制算法（结合MPC/LQR的横向控制和PID的纵向控制）

class Vehicle_control(object):
    def __init__(self, ego_vehicle, vehicle_para, pathway, controller_type="MPC_controller"):
        """
        初始化车辆的油门范围、转角范围、刹车范围
        """
        
        self.vehicle = ego_vehicle  # ego_vehicle是carla中的主车
        self.max_throttle = 1       # 最大油门 = 1，最小油门 = 0
        self.max_brake = 1          # 最大刹车 = 1，最小刹车 = 0
        self.max_steer = 1          # 最大转角 = 1
        self.min_steer = -1         # 最小转角 = 1
        self.Lateral = None
        if controller_type == "MPC_controller":    # 采用MPC横向控制
            self.Lateral = "MPC_controller"
            self.Lateral_control = Lateral_MPC_controller(ego_vehicle, vehicle_para, pathway)
        elif controller_type == "LQR_controller":  # 采用LQR横向控制
            self.Lateral = "LQR_controller"
            self.Lateral_control = Lateral_LQR_controller(ego_vehicle, vehicle_para, pathway)
        self.Longitudinal_control = Longitudinal_PID_controller(ego_vehicle)
 
    def run_step(self, target_speed):
        """
        函数：计算横向控制与纵向控制的流程
        return: control
        """
        
        control = carla.VehicleControl()     # 驾驶控制来管理车辆的基本运动,包括throttle、steer、brake、hand_brake、reverse、Manual_gear_shift、gear
        control.hand_brake = False           # 不使用手刹
        control.reverse = False              # 车辆不向后移动
        control.manual_gear_shift = False    # 不通过手动换档来控制车辆
        control.gear = 1                     # 车辆行驶的档位 = 1
        if self.Lateral == "MPC_controller":
            steer_r = self.Lateral_control.MPC_control()  # 返回横向控制的转角
        if self.Lateral == "LQR_controller":
            steer_r = self.Lateral_control.LQR_control()  # 返回横向控制的转角
        acceleration_r = self.Longitudinal_control.PID_control(target_speed)    # 返回纵向控制的加速度
 
        # 横向控制限定范围
        if steer_r >= 0:
            control.steer = min(self.max_steer, steer_r)
        else:
            control.steer = max(self.min_steer, steer_r)
 
        # 纵向控制限定范围
        if acceleration_r >= 0:
            control.throttle = min(self.max_throttle, acceleration_r)
            control.brake = 0
        else:
            control.throttle = 0
            control.brake = max(self.max_brake, acceleration_r)
 
        v = self.vehicle.get_velocity()  # self.vehicle.get_velocity()的格式：Vector3D(x=0.000000, y=0.000000, z=-0.194462)
        v_length = math.sqrt(v.x * v.x + v.y * v.y + v.z * v.z)  # 速度大小,m/s
 
        return control
 
control_object = Vehicle_control(ego_vehicle, vehicle_para, pathway, controller_type="MPC_controller")
control_object.run_step(target_speed)
ego_vehicle.apply_control(control_object)  # apply_control：将横向控制以及纵向控制输出的信息交给主车ego_vehicle去执行命令

到此完成了整个控制算法的讲解以及代码编写。感谢！！！