PID控制在自动驾驶中的应用举例（一）车速跟踪

之前研究了一些端到端基于数据的自动驾驶算法，但是如果希望深入自动驾驶的研究还是要掌握基础的自动控制知识。本文从基础的PID控制车速开始练习，希望虚拟的车辆可以以速度，之后逐步增加控制目标的复杂度。

安装highway-env环境

highway-env是基于gym的开源环境，可以提供内置车辆运动学模型的虚拟2D强化学习驾驶环境，本文不涉及自动驾驶或强化学习的算法，只是借用其平台和运动学模型进行对车辆控制的实验，具体安装环境的过程见我以前的文章。

安装好后选择基础的四车道高速公路环境，进行基本的配置即可，因为当前只需对一辆车进行控制，所以要将环境车辆的数量设为0。因为对于一辆车的控制比较精细，所以要将action的类型手动设置为“连续型”。

import gym
import highway_env

env = gym.make('highway-v0')
config = \
    {
    "observation": 
         {
        "type": "Kinematics",
        "vehicles_count": 1,
        "features": ["presence", "vx", "vy"],
        "features_range": 
            {
            "vx": [-100, 100],
            "vy": [-100, 100]
            },
        "absolute": True,
        "order": "sorted"
        },
    "action": {
        "type": "ContinuousAction"
    },    
    "simulation_frequency": 5,  # [Hz]
    "policy_frequency": 1,  # [Hz]
     'vehicles_count': 0,
     'reward_speed_range': [20, 80],
    }

负反馈

负反馈是闭环控制的核心，其原理就类似于“面多了加水，水多了加面”，通过矫正使系统的输出和期望值的偏差最小，最好始终保持在一个平衡的零界点。比如车速比预期快（反馈）要抬油门（控制），车速比预期慢（反馈）要踩油门（控制），使车速始终与期望的巡航速度保持一致。因此在设计车速控制器时，可以结合环境进行如下定义：
$$\begin{gathered} e=v_t-v \\ a=K\ast e \end{gathered}$$

其中vt代表目标车速，v表示当前车速，a表示油门控制量（在highway-env环境里直接和加速度绑定），K为系数。

这样就完成了对车速的基本控制，当车速与目标速度差越大时，加速度（包括加速和减速两种情况）越大，当车速与目标速度渐渐接近时，加速度也越来越接近于0（当前环境没有阻力等条件）。可以尝试在环境中模拟这一过程：

vt=0.2
env.configure(config)
env.reset()
env.vehicle.speed=0
his_v1=[]
his_e1=[]
K=10
a=0
for _ in range(100):
    action=[a,0]
    obs, reward, done, info = env.step(action)
    v=obs[0][1]
    e=v-vt
    a=-K*x
    his_v1.append(v)
    his_e1.append(a)
    env.render()
env.close()

为了方便归一化，将速度的上下限设为了[-100m/s,100m/s],目标速度为60km/h，归一化后约为0.167，汽车从静止开始加速，到60km/h时开始匀速行驶。

PID控制

PID是控制理论中一种常用的控制策略，分为三个部分：比例（P）、积分（I）、微分（D）。就是说把控制信号分为3个部分，最后求和得出误差进行反馈矫正。可以进行如下定义：
$$\begin{gathered} e_p=v_t-v \\ {e}_i=\int \left(v_t-v\right)dt \\ {e}_d=\frac{d(v_t-v)}{dt} \\ a=K_p\ast e_p+K_i\ast e_i+K_d\ast e_d \end{gathered}$$

可以看出上一步的反馈控制就是PID中的比例部分，但是加上了积分和微分部分，结果会有一些不一样（代码里要按离散数据处理，设置一个buffer保存前前若干个epoch的瞬时速度值，再计算微分和积分）。

from collections import deque

env.configure(config)
env.reset()
env.vehicle.speed=0
his_v2=[]
his_e2=[]
buffer = deque(maxlen=10)

e=0
e_p=0
e_i=0
e_d=0

dt=0.2 #policy frequency = 5 Hz

for _ in range(100):

    action=[e,0]
    obs, reward, done, info = env.step(action)
    v=obs[0][1]
    
    e_p=v-vt
    
    buffer.append(e_p)
    e_i=np.sum(buffer)*dt
    
    if len(buffer)>=2:        
        e_d=(buffer[-1]-buffer[-2])/dt
    else:
        e_d=0
        
    e=-(10*e_p+3*e_i+0.1*e_d)
    his_v2.append(v)
    his_e2.append(e)
    env.render()
env.close()

运行结束后可以观察结果的曲线：

会发现曲线变化规律和只有比例控制的曲线有所不同，将二者放在一起观察：

from matplotlib import pyplot as plt
import numpy as np

plt.plot(np.transpose([his_v1,his_v2]))

可以发现PID的特点是收敛速度更快（因为有积分部分），可以用更短的时间达到跟踪目标，但容易用力过猛超出目标并在反复调节时震荡，具体两种控制方法的表现可以通过设置比例系数Ki来调整，过程和机器学习调参类似，可以自己动手实验的时候慢慢体会其中精髓。

我本人之前对自动控制理论了解并不多，但如果想从事自动驾驶相关研究还是必须要有一些自动控制方面的知识作为基础，所以现在也在慢慢摸索，如有问题欢迎大家评论私信，积极交流、共同进步。