学习总结(11.10)

车辆主动安全实验室涂志明

一.用matlab实现Pure_Pursuit算法，基于车辆几何模型（如图）

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

$$
\frac{\ell_{d}}{\sin (2 \alpha)}=\frac{R}{\sin \left(\frac{\pi}{2}-\alpha\right)}
$$

$$
\frac{\ell_{d}}{2 \sin (\alpha) \cos (\alpha)}=\frac{R}{\cos (\alpha)}
$$

$$
\frac{\ell_{d}}{\sin (\alpha)}=2 R
$$

$$
\kappa=\frac{2 \sin (\alpha)}{\ell_{d}}
$$

$$
\delta=\tan ^{-1}(\kappa L)
$$

$$
\delta(t)=\tan ^{-1}\left(\frac{2 L \sin (\alpha(t))}{\ell_{d}}\right)
$$

$$
\sin (\alpha)=\frac{e_{\ell_{d}}}{\ell_{d}}
$$

$$
\kappa=\frac{2}{\ell_{d}^{2}} e_{\ell_{d}}
$$

$$
\delta(t)=\tan ^{-1}\left(\frac{2 L \sin (\alpha)}{k v_{x}(t)}\right)
$$

可以得知，影响纯追踪控制算法的参数主要为预瞄距离，预瞄距离与速度又有关系，这种方法在低速情况下几乎不受路径形状的影响。但是，选择最佳的前视距离的方法并不明确。将前视距离表示为速度的函数是一种常见的方法，但是，前视距离也可能是路径曲率的函数，甚至可能和纵向速度有关。在此假设为速度的一次函数，故调参时只需要调整参数K值即可，此处举出k=0.05和k=0.5的运行结果：

clear;
clc;
k = 0.05;   % look forward gain         %速度对预瞄距离的影响因数
Lfc = 2.0;  % look-ahead distance       %固定的预瞄距离
Kp = 1.0 ;  % speed propotional gain    %由目标速度和当前速度的差值，乘以kp系数，得到加速度
dt = 0.1  ; % [s]                       %时间间隔
L = 2.9  ;  % [m] wheel base of vehicle %汽车的轴距
cx = 0:0.1:50;                          %给定路径的横坐标cx取值，共501个点

for i = 1:length(cx)                   
    cy(i) = sin(cx(i)/5)*cx(i)/2;       %通过循环，得到给定路径的501个路点
end

i = 1;                                    %从第一个路点开始，i=1
target_speed = 10/3.6;                  %期望的汽车行驶速度
T = 100;                                %行驶时间不超过100s
lastIndex = length(cx);                 %cx表示路点横坐标的行向量，将最后一个路点赋给LastIndex
x = 0; y = -3; yaw = 0; v = 0;          %汽车初始状态，坐标为（0，-3），横摆角yaw=0，初速度v=0
time = 0;                               %初始时间为0 
Lf = k * v + Lfc;                       %由固定预瞄距离和k*v得到最终的预瞄距离Lf
figure

while T > time                      
    target_ind= calc_target_index(x,y,cx,cy,Lf); %得到在最小路径路点基础上，得到预瞄点的顺序数
    if(target_ind>=lastIndex)                    %判断预瞄点是否到达最后路点，到达则跳出循环结束
        return;
    end
    ai = PIDcontrol(target_speed, v,Kp);         %输入为期望速度、当前速度、P控制的kp系数，a
    di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_ind,k,Lfc,L,Lf);%得到车辆期望转角
    [x,y,yaw,v] = update(x,y,yaw,v, ai, di,dt,L);                    %更新车辆t+dt时刻的状态
    time = time + dt;
    %pause(0.1)
    plot(cx,cy,'b',x,y,'r-*');
    drawnow;
    hold on;
end
    % plot(cx,cy,x,y,'*')
    % hold on

%根据根据当前的坐标（x,y),横摆角yaw，速度v，加速度a，前轮转角delta,轴距L以及时间间隔dt，
%得到t+dt时刻的汽车实时状态，用于下一次循环
function [x, y, yaw, v] = update(x, y, yaw, v, a, delta,dt,L) 
    x = x + v * cos(yaw) * dt;                        %t+dt时刻的横坐标x
    y = y + v * sin(yaw) * dt;                        %t+dt时刻的纵坐标y
    yaw = yaw + v / L * tan(delta) * dt;              %t+dt时刻的车辆横摆角
    v = v + a * dt;                                   %t+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,Lf)
    tx = cx(ind);                                     %得到预瞄点的横坐标
    ty = cy(ind);                                     %得到预瞄点的纵坐标
    alpha = atan2((ty-y),(tx-x))-yaw;                 %预瞄点与后轴中心连线与车纵轴的夹角
    Lf = k * v + Lfc;                                 %车的实时预瞄距离
    delta = atan2((2*L * sin(alpha)/Lf),1.0) ;        %车辆期望转角
end

%输入为汽车当前坐标（x,y), 给定路径（cx，cy），预瞄距离Lf
function [ind] = calc_target_index(x,y, cx,cy,Lf)     
    N =  length(cx);                                  %取cx的个数，作为N进行循环
    Distance = zeros(N,1);                            %Distance为Nx1的列向量，每个元素均为0
    for i = 1:N                                         
        Distance(i) = sqrt((cx(i)-x)^2 + (cy(i)-y)^2);%对应每个路点与汽车当前坐标（x，y）的距离
    end
    [~, location]= min(Distance);                     %得到最小距离路点的位置，即第location个
    ind = location;                                   %ind即为此时最小距离路点在给定路径路点序号
    LL = 0;                                              
    while Lf > LL && (ind + 1) < length(cx)%第一种方法，用循环来计算达到预瞄距离Lf时的路点位置
        dx = cx(ind + 1) - cx(ind);
        dy = cx(ind + 1) - cx(ind);
        LL = LL + sqrt(dx * dx + dy * dy);
        ind  = ind + 1;
    end
    %ind = ind + 10;                       %第二种方法在最小距离路点的基础上，向前预瞄到第ind+10
end

运行结果如图（k=0.05)：

运行结果如图（k=0.5):

因此纯追踪控制器的前视距离调整应注意，很短的前视距离(k小)会造成车辆控制的不稳定甚至震荡，为了确保车辆稳定而设置较长的前视距离(k大)又会出现车辆在大转角处转向不足的问题。

二.用Python实现Pure_Pursuit算法：

import numpy as np
import math
import matplotlib.pyplot as plt

k = 0.1    # 速度对预瞄距离的影响因数,当起步时速度慢，则预瞄距离小来更快到达路径上；速度快则预瞄距离长，变化更平滑
Lfc = 2.0  # 固定预瞄距离
Kp = 1.0   # 速度P控制器系数
dt = 0.1   # 时间间隔，单位：s
L = 2.9    # 车辆轴距，单位：m


# 车辆状态类
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 PControl(target, current):
    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:  # back
        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):
    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      #当前位置的实际预瞄距离
    while Lf > L and (ind + 1) < len(cx):
        dx = cx[ind + 1] - cx[ind]
        dy = cy[ind + 1] - cy[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1
    return ind

def main():
    # 设置目标路点
    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  # 最大模拟时间
    # 设置车辆的初始状态
    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 = PControl(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)
        plt.cla()
        plt.plot(cx, cy, ".r", label="course")
        plt.plot(x, y, "-b", label="trajectory")
        plt.plot(cx[target_ind], cy[target_ind], "go", 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__':
    main()

# plt.cla()清除轴，当前活动轴在当前图中。 它保持其他轴不变。
# plt.clf()清除整个当前数字。与所有的轴，但离开窗口打开，这样它就可以再用在其他的 plots上了。
# plt.close()关上窗户，如果未另指定，则该窗口将是当前窗口。