無人車系統（五）：軌跡跟蹤Pure Pursuit方法

今天介紹一種基於幾何追蹤的無人車軌跡跟蹤方法——Pure Pursuit（純跟蹤）方法。

1. 阿克曼轉向幾何模型

在無人車系統（一）：運動學模型及其線性化一文中，裏面介紹無人車的運動學模型爲阿克曼轉向幾何模型，並最終可簡化爲如下圖所示的單車模型。

精確描述單車模型中幾何關係的核心公式如下：
$\delta=arctan(\frac{L}{R}) \tag{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} \tag{2}$

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

$\delta=arctan(\frac{2L \sin \alpha}{l_d}) \tag{3}$

其中，$\alpha$爲路點與車後軸連成的向量的角度與車航向角的差值，當路點在車的左邊時，$\alpha>0$，反之則$\alpha<0$；$l_d$爲車後軸離路點的距離，又被稱爲前視距離。

爲了讓式(3)在形式上更加統一，我們可以利用無人車相對當前路點的橫向偏差$e_y$與前視距離$l_d$表示（如果不能立馬了悟，還請看無人車系統（四）：軌跡跟蹤PID控制第一張圖）：

$\sin \alpha = \frac{e_y}{l_d} \tag{4}$

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

$\delta(k) = arctan(\frac{2Le_y(k)}{l_d^2(k)}) \tag{5}$

在pure pursuit方法中，前視距離表示成無人車縱向線速度的形式，即$l_d=\lambda v_x+c$，$c$爲一常值，最終的控制器如下：

$\delta(k) = arctan(\frac{2Le_y(k)}{(kv_x(k)+c)^2}) \tag{6}$

分析式(6)，利用小角度近似，我們有：
$\delta(k)\approx \frac{2L}{l_d^2} e_y \tag{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*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控制中，我們只是選擇離無人車最近的路點作爲當前的目標點，並沒有根據前視距離選擇適當的路點。