无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪

对于无人车辆来说，在规划好路径以后（这个路径我们通常称为全局路径），全局路径由一系列路径点构成，这些路径点只要包含空间位置信息即可，也可以包含姿态信息，但是不需要与时间相关，这些路径点被称为全局路径点（Global Waypoint），路径（Path）和轨迹（Trajectory）的区别就在于，轨迹还包含了时间信息，轨迹点也是一种路径点，它在路径点的基础上加入了时间约束，通产我们将这些轨迹点称为局部路径点（Local Waypoints）。

如何让我们的无人车追踪这个轨迹？目前的主流方法分为两类：基于几何追踪的方法和基于模型预测的方法，在本节我们重点介绍一种广泛使用的基于几何追踪的方法——纯追踪法（Pure Pursuit）。

自行车模型和阿克曼转向几何

在论述纯追踪算法之前，首先我们回顾一下自行车模型，下图是一个几何学自行车模型:

这里写图片描述

自行车模型实际上是对阿克曼转向几何的一个简化，我们知道，自行车模型将4轮车辆简化成2轮的模型，并且假定车龄只在平面上行驶，采用自行车模型的一大好处就在于它简化了前轮转向角与后轴将遵循的曲率之间的几何关系，其关系如下式所示：

这里写图片描述

其中 $\delta$ 表示前轮的转角，L为轴距（Wheelbase），R则为在给定的转向角下后轴遵循着的圆的半径。这个公式能够在较低速度的场景下对车辆运动做估计。

纯追踪算法

从自行车模型出发，纯跟踪算法以车后轴为切点, 车辆纵向车身为切线, 通过控制前轮转角，使车辆可以沿着一条经过目标路点（goal point）的圆弧行驶，如下图所示：

这里写图片描述

图中是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆是的车辆的后轴经过该路点，表示车辆当前位置（即后轴位置）到目标路点的距离，表示目前车身姿态和目标路点的夹角，那么更具正弦定理我们可以推导出如下转换式：

这里写图片描述

上式也可以表示为：

这里写图片描述

其中 $\kappa$ 是计算出来的圆弧的曲率，那么前轮的转角 $\delta$ 的表达式为：

这里写图片描述

结合以上两式，我们可以得出纯追踪算法控制量的的最终表达式：

这里写图片描述

这里我们把时间考虑进来，在知道t时刻车身和目标路点的夹角 $\alpha(t)$ 和距离目标路点的前视距离 $l_d$ 的情况下，由于车辆轴距 $L$ 固定，我们可以利用上式估计出应该作出的前轮转角 $\delta$ ，为了更好的理解纯追踪控制器的原理，我们定义一个新的量： $e_l$ —— 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

这里写图片描述

圆弧的弧度就可重写为：

这里写图片描述

考虑到本质是横向上的CTE，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为 $\frac{2}{l_d^2}$ ，这个P控制器受到参数 $l_d$ （即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说， $l_d$ 被认为是车速的函数，在不同的车速下需要选择不同的前视距离。

一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即 $l_d = kv_x$ ，那么前轮的转角公式就变成了：

这里写图片描述

那么纯追踪控制器的调整就变成了调整系数k，通常来说，会使用最大，最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡），下面我们使用Python实现一个简单的纯追踪控制器。

基于Python的纯追踪算法实践

在这个实践中，我们纯追踪控制控制转向角度，使用一个简单的P控制器控制速度，首先我们定义参数数值如下：

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

在这里我们将最小前视距离设置为2，前视距离关于车速的系数k设置为0.1 ，速度P控制器的比例系数Kp设置为1.0，时间间隔为0.1 秒，车的轴距我们定为2.9米。

定义车辆状态类，在简单的自行车模型中，我们只考虑车辆的当前位置 $(x, y)$ ,车辆的偏航角度yaw以及车辆的速度v，为了在软件上模拟，我们定义车辆的状态更新函数来模拟真实车辆的状态更新：

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

在这个实践中，我们纵向控制使用一个简单的P控制器，横向控制（即转角控制）我们使用纯追踪控制器，这两个控制器定义如下：

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 = cx[ind + 1] - cx[ind]
        L += math.sqrt(dx ** 2 + dy ** 2)
        ind += 1

    return ind

主函数：

def main():
    #  设置目标路点
    cx = np.arange(0, 50, 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)

if __name__ == '__main__':
    main()

运行效果：

这里写图片描述

我们看这个运行图，首先，红点表示我们实现规划好的路点，蓝线则表示我们的车辆实际运行的轨迹，前面的绿点表示我们的当前前视距离，在这段代码中，我们设置了最小前视距离为2m，同学们还可以进一步去实验，比如说将前视距离设置得更大些，那么我们的纯追踪控制器就会表现得更加“平滑”，更加平滑的结果就是在某些急剧的转角处会存在转向不足的情况。

小结

以纯追踪控制器为代表的几何路径跟踪器很容易理解和实现。本文介绍的几何方法达到了基本的路径跟踪性能，但是当存在显著的速度改变时会遇到瓶颈，纯追踪方法使用前视距离考虑路径信息，这种方法在低速情况下几乎不受路径形状的影响。但是，选择最佳的前视距离的方法并不明确。将前视距离表示为速度的函数是一种常见的方法，但是，前视距离也可能是路径曲率的函数，甚至可能和纵向速度以外的CTE有关。所以纯追踪控制器的前视距离调整应该额外注意，很短的前视距离会造成车辆控制的不稳定甚至震荡，为了确保车辆稳定而设置较长的前视距离又会出现车辆在大转角处转向不足的问题。