无人车系统（五）：轨迹跟踪Pure Pursuit方法

今天介绍一种基于几何追踪的无人车轨迹跟踪方法——Pure Pursuit（纯跟踪）方法。

1. 阿克曼转向几何模型

在无人车系统（一）：运动学模型及其线性化一文中，里面介绍无人车的运动学模型为阿克曼转向几何模型，并最终可简化为如下图所示的单车模型。

精确描述单车模型中几何关系的核心公式如下：
$$\delta =arctan(\frac{L}{R})\text{\hspace{1em}(1)}$$
其中，$\delta$为前轮转角，$L$为轴距(wheel base)，$R$为给定转向角运动时，无人车形成的轨迹圆的半径。

2. Pure pursuit控制器

我们利用下图【引自无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪】来介绍Pure pursuit控制器的设计。

Pure pursuit方法的依据是使如上图所示的单车模型以合适的前轮转向（$\delta$）运动，并恰好使无人车后轴中心经过当前的路点。 这样一来，我们就可以根据当前的路点以及单车几何模型计算当前的期望前轮转向角($\delta$)。

根据上图，我们有以下几何关系：

$$\sin \alpha =\frac{l_d}{2R}\Rightarrow R=\frac{l_d}{2\sin \alpha }\text{\hspace{1em}(2)}$$

将式(2)代入式(1)可得：

$$\delta =arctan(\frac{2L\sin \alpha }{l_d})\text{\hspace{1em}(3)}$$

其中，$\alpha$为路点与车后轴连成的向量的角度与车航向角的差值，当路点在车的左边时，$\alpha >0$，反之则$\alpha <0$；$l_d$为车后轴离路点的距离，又被称为前视距离。

为了让式(3)在形式上更加统一，我们可以利用无人车相对当前路点的横向偏差$e_y$与前视距离$l_d$表示（如果不能立马了悟，还请看无人车系统（四）：轨迹跟踪PID控制第一张图）：

$$\sin \alpha =\frac{e_y}{l_d}\text{\hspace{1em}(4)}$$

将式(4)代入式(3)得：

$$\delta (k)=arctan(\frac{2L{e}_y(k)}{l_d^2(k)})\text{\hspace{1em}(5)}$$

在pure pursuit方法中，前视距离表示成无人车纵向线速度的形式，即$l_d=\lambda v_x+c$，$c$为一常值，最终的控制器如下：

$$\delta (k)=arctan(\frac{2L{e}_y(k)}{(k{v}_x(k)+c{)}^2})\text{\hspace{1em}(6)}$$

分析式(6)，利用小角度近似，我们有：
$$\delta (k)\approx \frac{2L}{l_d^2}{e}_y\text{\hspace{1em}(7)}$$

我们把$\frac{2L}{l_d^2}$看作比例控制器的参数，$e_y$作为系统误差，那么这就相当于一个以横向跟踪误差CTE作为系统误差的比例控制器。

与无人车系统（四）：轨迹跟踪PID控制不同的是，在pure pursuit 方法中，$l_d$是关于速度的线性函数$l_d=\lambda v_X$，其中参数$\lambda$与$c$是需要调整。

无人车模型的两个控制输入：无人车前轮转向角($\delta$)，有时候也用方向盘转角来表示；无人车线速度$v$。前者是影响无人车横向运动的主要控制输入，因此，设计的轨迹跟踪控制器，都是为了得到合适的$\delta$。

3. python 示例

"""
Pure Pursuit
"""

import numpy as np
import math
import copy
import matplotlib
import matplotlib.pyplot as plt
%matplotlib inline

# set up matplotlib
is_ipython = 'inline' in matplotlib.get_backend()
if is_ipython:
    from IPython import display

plt.ion()
plt.figure(figsize=(18, 3))

class UGV_model:
    def __init__(self, x0, y0, theta0, L, v0, T): # L:wheel base
        self.x = x0 # X
        self.y = y0 # Y
        self.theta = theta0 # headding
        self.l = L  # wheel base
        self.v = v0  # speed
        self.dt = T  # decision time periodic
    def update(self, vt, deltat):  # update ugv's state
        dx = self.v*np.cos(self.theta)
        dy = self.v*np.sin(self.theta)
        dtheta = self.v*np.tan(deltat)/self.l
        self.x += dx*self.dt
        self.y += dy*self.dt
        self.theta += dtheta*self.dt
        
    def plot_duration(self):
        plt.scatter(self.x, self.y, color='r')   
        plt.axis([self.x-9, self.x+9, -3, 3])
#         plt.axis([self.x-9, self.x+9, -10, 10])
        if is_ipython:
            display.clear_output(wait=True)
            display.display(plt.gcf())  
            
            
from scipy.spatial import KDTree

# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 1000, 1000)
# refer_path[:,1] = 5*np.sin(refer_path[:,0]/5.0)

refer_tree = KDTree(refer_path)

plt.plot(refer_path[:,0], refer_path[:,1], '-.b', linewidth=5.0)
L = 2.0
v = 2.0
k = 1.0  #  前视距离ld = k*v+2.0
ugv = UGV_model(0, 1.0, 0, L, v, 0.1)
ind = 0 
ld = k*v+2.0
for i in range(1000):
    robot_state = np.zeros(2)
    robot_state[0] = ugv.x
    robot_state[1] = ugv.y
    for i in range(ind, len(refer_path)):
        dist = np.linalg.norm(robot_state-refer_path[i])
        if dist >= ld:
            ind = i
            break
           
    dist = np.linalg.norm(robot_state-refer_path[ind])
    dx, dy = refer_path[ind] - robot_state
    alpha = math.atan2(dy, dx)
    delta = math.atan2(2.0*L*np.sin(alpha-ugv.theta)/ld, 1)  # pure pursuit controller
    ugv.update(2.0, delta)
    ugv.plot_duration()

我随便选了一个参数:$\lambda =1.0,c=2.0$

• 跟踪直线效果
  

• 跟踪直线效果
  

4. 当前视距离$l_d=1.0\ast v_x+2.0$时，PID的效果

为了进一步说有pure pursuit方法其实等价于无人车系统（四）：轨迹跟踪PID控制中介绍的纯比例控制器。我们采用与pure pursuit同样的前视距离，做一组实验。实验结果如下：

• 纯比例P，$k_p=1.0$
  

• PD控制，$k_p=1.0,k_d=20.0$

• PID控制，$k_p=1.0,k_i=0.001,k_d=20.0$

对比纯P控制的效果与pure pursuit非常接近，加入微分与积分后，控制效果反而变差。（也可能是我PID参数没有调好，这套参数与无人车系统（四）：轨迹跟踪PID控制）中的一样。

总结

本篇主要对用于无车轨迹跟踪控制的pure pursuit方法进行介绍。我们从原理出发，并给出pure pursuit的控制器计算公式，最后给出python示例。结果显示pure pursuit的跟踪效果还不错。文中也说明了，pure pursuit与P控制形式一样，只不过在无人车系统（四）：轨迹跟踪PID控制中，我们只是选择离无人车最近的路点作为当前的目标点，并没有根据前视距离选择适当的路点。