Carsim-Simulink无人驾驶Pure Pursuit算法实现

一、基础问题

1. Carsim中的参考路径（双移线A）

在博主相关的stanley算法一文中，介绍了与carsim-simulink 进行联合仿真时的注意事项和基本概念。这里补充一种双移线路径的生成模型:
$$z_1=\frac{shape}{d_{x1}}(X_{phi}-X_{s1})-\frac{shape}{2};$$
$$z_2=\frac{shape}{d_{x2}}(X_{phi}-X_{s2})-\frac{shape}{2};$$
$$Y_{ref}=\frac{d_{y1}}{2}(1+tanh(z_1))-\frac{d_{y2}}{2}(1+tanh(z_2))$$
其中，$X_{s1,2}$ $d_{x1,2}$ $d_{y1,2}$ $shape$是控制曲线的参数

% generate the reference path: double path trajectory
% parameters 
shape=2.4;%参数名称，用于参考轨迹生成
dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
Xs1=27.19;Xs2=56.46;%参数名称

% generation model
X_phi=1:1:220;%这个点的区间是根据纵向速度（x_dot）来定的，如果速度为10m/s则区间=10*0.1=1
z1=shape/dx1*(X_phi-Xs1)-shape/2;
z2=shape/dx2*(X_phi-Xs2)-shape/2;
Y_ref=dy1/2.*(1+tanh(z1))-dy2/2.*(1+tanh(z2));
figure(1);
plot(X_phi, Y_ref,'r--','LineWidth',2);
hold on;

2. Carsim中的参考路径（双移线B）

% 生成双移线参考路径的另一种方法
cx = 0:0.1:50;                            %每0.1长度取一次预瞄点的x值
for i = 1:length(cx)                      %全局路径c(y)生成 路径初始化
     cy(i) = cos(cx(i)/5)*cx(i)/4;
end
figure(1);
plot(cx,cy);

二、Pure Pursuit的原理

pp算法的基本原理是几何原理，具备基本数学知识即可理解，可以阅读以下资料：
https://blog.csdn.net/gophae/article/details/100012763

1. 阿克曼转向几何关系

$$tan(\delta )=\frac{L}{R}$$

2.考虑预瞄距离$L_d$的几何关系:

pure pursuit 需要给出预瞄距离Ld, 可以对这个预瞄距离进行tuning,但在某些情况下使用该模型效果不佳。给定预瞄距离后, 在预定轨迹上获得 采样点$g$：后轴中心点，采样点$g$，及车辆的运动学半径中心，构成等腰三角形，等边边长为半径$R$，根据sin正弦函数三角公式，得如下：

$$\frac{l_d}{sin(2\alpha )}=\frac{R}{sin(\frac{\pi }{2}-\alpha )}$$
$$\Rightarrow \frac{l_d}{2sin(\alpha )cos(\alpha )}=\frac{R}{cos(\alpha )}$$
$$\Rightarrow \kappa =\frac{1}{R}=\frac{2sin(\alpha )}{l_d}$$

3. PP控制算法

根据上面两个式子：联立消去R，得到控制量$\delta$为：
$$\delta =ta{n}^{-1}\frac{2Lsin\alpha }{l_d}$$
其中，输入参数为：预瞄距离$l_d$, 车辆前后轴距常数L， 及车辆纵轴与指向预瞄点的方向的夹角$alpha$；输出为实时计算出的期望车辆转角$\delta$。

• 通常情况下选取$l_d$的原则是与车速成线性关系，在实际中可用$l_d=k{V}_x$ ，需要使用者tuning的部分就是这里的系数k值。当然地这个选择可以根据设计更改，也可以是非线性的。
• pp算法可以理解为一种比例控制的范式：偏差理解为$\alpha$，即为车辆纵轴与预瞄点的夹角，比例系数（尽管不是线性比例，但是单调关系）理解为与正弦（$sin$）、系数($\frac{2L}{l_d}$)、反正切($ta{n}^{-}1$)都单调关系有关联。即：偏差角度越大，控制转角$\delta$越大；预瞄距离越远，控制转角作用越弱（系数$\frac{2L}{l_d}$越小）。
• 偏差还可以理解为横向偏差距离$e_d$，如下图：
  $$sin(\alpha )=\frac{e_d}{l_d}$$
  代入控制方程，得到与横向偏差有关的控制方程：
  $$\delta =ta{n}^{-1}\frac{2L{e}_d}{l_d^2}$$
  这样，系数$\frac{2L}{L_d^2}$可视作一个纯P控制的增益。
  

4. PP vs. Stanley

• pure pursuit预瞄距离系数K越大，跟随效果会越差，随着K减少，效果变好，继续减少，跟随效果会突然变差，最终跟随发散，无法工作。
• 曲率或曲率半径对pure pursuit工作效果影响很大，某个曲率下，给定的pure模型可能工作的很好，在另一个曲率下，工作效果会突然变差，这是因为pure仅仅考虑了curvature，因为对curvature的依赖性就很强。并且这种效果会随着车速的增加越发增强。
• stanley 比pure好的地方是考虑到了预定点heading angle 和真实车辆heading angle的差， 作为一个前馈控制，提前补足了在转弯过程中的车辆转角，设计者可以根据需求，对前馈值进一步增加增益系数。
• 两种方法都推荐使用与速度相关的可变增益系数，可以更好的弥补此类仅考虑车辆运动学，而忽略车辆动力学的模型的不足。增益系数的设定方法可以参考PID系数整定法。

三、参考代码

最终实现如下图的跟随的代码：

1. python代码

import numpy as np
import math
import matplotlib.pyplot as plt
#定义常数
k = 0.1  # look forward gain
Lfc = 1.0  # look-ahead distance
Kp = 1.0  # speed propotional gain
dt = 0.1  # [s]
L = 2.9  # [m] wheel base of vehicle


show_animation = True


class VehicleState:# 定义一个类，用于调用车辆状态信息

    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v


def update(state, a, delta):#更新车辆状态信息

    state.x = state.x + state.v * math.cos(state.yaw) * dt
    state.y = state.y + state.v * math.sin(state.yaw) * dt
    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
    state.v = state.v + a * dt

    return state


def PIDControl(target, current):#PID控制，定速巡航
    a = Kp * (target - current)

    return a


def pure_pursuit_control(state, cx, cy, pind):# 纯跟踪控制器

    ind = calc_target_index(state, cx, cy)#找到最近点的函数，输出最近点位置

    if pind >= ind:
        ind = pind

    if ind < len(cx):
        tx = cx[ind]
        ty = cy[ind]
    else:
        tx = cx[-1]
        ty = cy[-1]
        ind = len(cx) - 1

    alpha = math.atan2(ty - state.y, tx - state.x) - state.yaw

    if state.v < 0:  # 如果是倒车的话，就要反过来
        alpha = math.pi - alpha

    Lf = k * state.v + Lfc

    delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0)#核心计算公式

    return delta, ind


def calc_target_index(state, cx, cy):
# 找到与车辆当前位置最近点的序号

    # search nearest point index
    dx = [state.x - icx for icx in cx]
    dy = [state.y - icy for icy in cy]
    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
    ind = d.index(min(d))
    L = 0.0

    Lf = k * state.v + Lfc

    # search look ahead target point index
    while Lf > L and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cx[ind + 1] - cx[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1

    return ind

def main():
    #  target course ，随机出来一条sin函数曲线
    cx = np.arange(0, 50, 0.1)
    cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx]

    target_speed = 10.0 / 3.6  # [m/s]

    T = 100.0  # max simulation time

    # initial state
    state = VehicleState(x=-0.0, y=-3.0, yaw=0.0, v=0.0)

    lastIndex = len(cx) - 1
    time = 0.0
    x = [state.x]
    y = [state.y]
    yaw = [state.yaw]
    v = [state.v]
    t = [0.0]
    target_ind = calc_target_index(state, cx, cy)
    # 不断执行更新操作
    while T >= time and lastIndex > target_ind:
        ai = PIDControl(target_speed, state.v)
        di, target_ind = pure_pursuit_control(state, cx, cy, target_ind)
        state = update(state, ai, di)

        time = time + dt

        x.append(state.x)
        y.append(state.y)
        yaw.append(state.yaw)
        v.append(state.v)
        t.append(time)
        if show_animation:
            plt.cla()
            plt.plot(cx, cy, ".r", label="course")
            plt.plot(x, y, "-b", label="trajectory")
            plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
            plt.axis("equal")
            plt.grid(True)
            plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4])
            plt.pause(0.001)
            plt.show()

            
if __name__ == '__main__':
    print("Pure pursuit path tracking simulation start")
    main()

2. MATLAB代码

%% 纯跟踪算法跟随效果与预瞄距离关系很大。就像人开车时眼睛往前看多远，看的太近了，到了弯道才慌忙打方向盘；看的太远了，又容易导致最近一段的轨迹跟随效果不好。

%% initialization
k = 0.1;                                  % look forward gain  前向预测距离所用增益
Kp = 1.0 ;                                % speed propotional gain
Lfc = 1;                                  % 基础预瞄距离
dt = 0.1;                                 % [s]
L = 2.9  ;                                % [m] wheel base of vehicle
cx = 0:0.1:50;                            %每0.1长度取一次预瞄点的x值


for i = 1:length(cx)                      %全局路径c(y)生成 路径初始化
     cy(i) = cos(cx(i)/5)*cx(i)/4;
end


i = 1;
target_speed = 10/3.6;                    %设定速度  如果实际自动驾驶，这个就是设定要到达的速度，
                                          %附加：目前这里没有规划，如果有规划，就在过弯的时候给出横向加速度，所以车身方向速度就会减小。
T = 500;
lastIndex = length(cx);                   %返回cx的向量长度   有501个预瞄点x值   有501个序号
x = 0; y = 4; yaw = 0; v = 0;             %车辆初始位置  
time = 0;
Ld = k * v + Lfc;                         %初始速度为0时的预瞄距离Ld=Lfc
 
 figure(1);
while T > time 
  target_index = calc_target_index(x,y,cx,cy);                 %输出距离车辆当前最近的点的位置
  ai = PIDcontrol(target_speed,v,Kp); %
  Ld = k * v + Lfc ;
  delta = pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Ld); %前轮要转的角度
   
  [x,y,yaw,v] = update(x,y,yaw,v, ai, delta,dt,L); %更新车辆状态
  time = time + dt;                          %时间过完一周期
% pause(0.1)
  plot(cx,cy,'b',x,y,'r-*')                 %将预瞄点位置用蓝色表示，当前位置用红色表示
  drawnow
  hold on
  
end


function [x, y, yaw, v] = update(x, y, yaw, v, ai, delta,dt,L)   %更新车辆状态的函数
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
    v = v + ai * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp)              %计算加速度的函数
a = Kp * (target_v - current_v);
end

function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,ind,k,Lfc,L,Ld)  %纯追踪控制器前轮转角方程的函数
    tx = cx(ind);                          %距离车辆当前最近的点的位置对应的cx值     ind是距离车辆当前最近的点的位置
    ty = cy(ind);                          %距离车辆当前最近的点的位置对应的cy值
    
    alpha = atan((ty-y)/(tx-x))-yaw;       %该处定义向左转为alpha=beta-Fai，所以向右转就输出-alpha
    
 Ld = k * v + Lfc ;                        % 预瞄距离Ld与车速成线性关系 
 delta = atan(2*L * sin(alpha)/Ld)  ;                %前轮转角
end

function [ind] = calc_target_index(x,y, cx,cy)      %计算找到距车辆当前位置最近点的序号  输出最近点的函数
N =  length(cx);                                     %N = 501
Distance = zeros(N,1);                               % 501行 1列的矩阵

for i = 1:N
Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2);      %每一个预瞄点到初始位置的距离
end

[~, location]= min(Distance);                        %找出最近的距离对应的位置
ind = location;
ind = ind + 10
end