PID控制在自动驾驶中的应用举例（二）航向控制

上一篇中介绍了使用PID进行车速控制，控制目标相对简单，如果加入转向的目标，任务复杂程度都会有所增加。

对于环境的配置与之前类似，不再赘述。

from matplotlib import pyplot as plt
from collections import deque
import numpy as np

import gym
import highway_env
%matplotlib inline

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

对于航向控制来说，我自己总结了一种计算方向盘转角的方式，需要输入3个矢量：汽车朝向，目的地要求的汽车朝向和汽车位置与目的地连线的朝向。目的是根据当前汽车的朝向和相对于目的地的方向推算出方向盘合适的转角度数。

将车辆和目的地的方向矢量相加，再与direction的矢量角度进行比较，即可确定方向盘需旋转的角度，如果δ为负（逆时针），则向左打方向盘，若为正（顺时针），向右打方向盘。角度的大小由向量夹角公式决定。

使用代码对以上方法进行表示：

def get_angle(v_p,v_h,dest_p,dest_h):
    v_head_x=np.cos(v_h)
    v_head_y=np.sin(v_h)  
       
    d_head_x=1*np.cos(dest_h)
    d_head_y=1*np.sin(dest_h)
    
    car_dir = np.array([v_head_x,v_head_y])
    path_dir = np.array([dest_p[0]-v_p[0],dest_p[1]-v_p[1]])
    dest_dir = np.array([car_dir[0]+d_head_x,car_dir[1]+d_head_y])
    cos_theta=np.dot(dest_dir,path_dir)/(np.linalg.norm(dest_dir)*np.linalg.norm(path_dir))
    left_right = 0 if np.cross(dest_dir,path_dir)==0 else abs(np.cross(dest_dir,path_dir))/np.cross(dest_dir,path_dir)
    
    angle = np.arccos(cos_theta)*left_right
    return angle

之后可以手动指定车辆和目的地的坐标和方向，坐标是我随机设的点，对于highway-env环境来说，车道不是一个严格的物理模型，汽车开出车道也不会停止模拟。方向的范围是[0,2*pi]。

V_HEADING=np.pi
V_POSITION_X=180
V_POSITION_Y=2

D_HEADING=0
D_POSITION_X=360
D_POSITION_Y=12

之后就可以使用PID等控制方法对汽车进行导航。为了在到达目的地之后停止环境模拟，可以设置一个距离函数，当汽车和目的地的距离小于2m时，认为汽车已经到达目的地，停止模拟。

def get_distance(v_p,dest_p):
    dis=np.sqrt((v_p[0]-dest_p[0])**2+(v_p[1]-dest_p[1])**2)
    return dis

传统控制

传统控制直接将δ值作为方向盘转角steering值。由于物理环境没有阻力，不用对油门踏板进行设置，汽车会一直按照初始速度匀速前进。

env.configure(config)
env.reset()
env.vehicle.heading=V_HEADING
env.vehicle.position=[V_POSITION_X,V_POSITION_Y]
dest_position=[D_POSITION_X,D_POSITION_Y]
dest_heading=D_HEADING
e=0
his_p1=[]

for _ in range(300):
    
    action=[0,e]
    obs, reward, done, info = env.step(action)
    v_p=env.road.vehicles[0].position
    v_h=env.vehicle.heading
    his_p1.append([v_p[0],v_p[1]])
    angle=get_angle(v_p,v_h,dest_position,dest_heading)

    e = 0 if abs(angle)<0.01 else min(max(angle,-1),1)  # [-3.14,3.14] ->  [-1,1]
    dis = get_distance(v_p,dest_position)
    if dis<2:
        break
    env.render()
env.close()

PID控制

与速度控制类似，PID需要开辟一个buffer以便求微分和积分。

env.configure(config)
env.reset()
env.vehicle.heading=V_HEADING
env.vehicle.position=[V_POSITION_X,V_POSITION_Y]
dest_position=[D_POSITION_X,D_POSITION_Y]
dest_heading=D_HEADING
dt=0.1
buffer = deque(maxlen=10)

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

his_p2=[]
for _ in range(300):
    
    action=[0,e]
    obs, reward, done, info = env.step(action)
    v_p=env.road.vehicles[0].position
    v_h=env.vehicle.heading
    his_p2.append([v_p[0],v_p[1]])
    
    angle=get_angle(v_p,v_h,dest_position,dest_heading)    
    e_p = 0 if abs(angle)<0.01 else min(max(angle,-1),1)
    
    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=e_p+0.5*e_i+0.05*e_d
    
    dis = get_distance(v_p,dest_position)
    
    if dis<2:
        break
    env.render()
env.close()

模拟完成后可以画出汽车行驶路径的散点图，以观察两种方法的异同

plt.figure(figsize=(12,8))
plt.scatter(np.transpose(his_p1)[0],-np.transpose(his_p1)[1])
plt.scatter(np.transpose(his_p2)[0],-np.transpose(his_p2)[1])

（地图里y轴正负是反的，我也不理解为啥画出来的图y轴值都是负的）

可以看出来方向控制的变化和速度控制类似，采用PID可以快速将方向调整到的和目标方向一致，但会有小幅震荡。