基于前轮跟踪的Pure Pursuit原理及实现

艰默

发布于 2024-05-07 16:57:10

1230

发布于 2024-05-07 16:57:10

文章被收录于专栏：iDoitnowiDoitnow

1. 引言

在自动驾驶车辆的路径跟踪技术中，Pure Pursuit算法以其对低速至中速行驶环境的适应性而著称。该算法采用后轴跟踪策略，通过精心选择一个预瞄点，从而精确计算出所需的前轮转向角，以实现对预定路径的稳定跟踪。然而，Pure Pursuit算法的后轴响应特性在面对路径变化时可能稍显迟缓，这是因为它依赖于前轮的转向动作来间接引导后轮的行驶路径。

在对快速响应有较高要求的应用场景，如路况变化较大的区域，车辆的操控性和灵活性变得尤为重要。针对这一需求，我们可以将路径跟踪点前移至前轮，以此优化车辆对路径变化的响应速度。前轴跟踪不仅能够提供更迅捷的操控反应，而且在频繁需要转向的复杂路况中，能够确保车辆保持更高的机动性和适应性。通过这种策略调整，我们旨在提升自动驾驶车辆在多变交通环境中的导航性能，确保其在保障安全的前提下，实现更加流畅和精确的路径跟踪。

2. Pure Pursuit前轮跟踪原理

其中

：当前车辆的预瞄点

l_d

：辆前轴中心点

到

的距离，即预瞄距离

\theta

：

l_d

与车前轮朝向所在的轴的夹角

\varphi

：车辆的航向角

e_y

：预瞄点与车辆横向偏差

：轴距

：纯跟踪的目标转弯半径

\delta_f

：前轮转角

由图中的三角形几何关系可得

\frac{sin(2\theta)}{l_d} = \frac{sin(\frac{\pi}{2} - \theta)}{R} \tag{1}

化简后为

R=\frac{l_d}{2sin\theta} \tag{2}

又因为

\theta=\alpha-\delta_f\tag{3}

代入2式可得

R=\frac{l_d}{2sin(\alpha-\delta_f)}=\frac{l_d}{2sin\alpha cos\delta_f-2cos\alpha sin\delta_f}\tag{4}

在直角

\triangle AOB

中，有

sin\delta_f=\frac{L}{R}\tag{5}

将4代入可得

\begin{align*} sin\delta_f&=\frac{L(2sin\alpha cos\delta_f-2cos\alpha sin\delta_f)}{l_d} \\ l_dsin\delta_f&=2Lsin\alpha cos\delta_f-2Lcos\alpha sin\delta_f\\ l_dtan\delta_f&=2Lsin\alpha-2Lcos\alpha tan\delta_f\\ tan\delta_f(l_d+2Lcos\alpha)&=2Lsin\alpha\\ tan\delta_f&=\frac{2Lsin\alpha}{l_d+2Lcos\alpha}\end{align*}\tag{6}

由此可得

\delta_f=arctan\frac{2Lsin\alpha}{l_d+2Lcos\alpha}\tag{7}

其中

\alpha=arctan\frac{P_y-B_y}{P_x-B_x}-\varphi

。

3. 算法和仿真实现

pure_pursuit_controller.py

import numpy as np
import math

k = 0.1  # 预瞄距离系数
Lfc = 3.0  # 初始预瞄距离


def calc_preparation(vehicle, ref_path):
    """
    计算index、er、rx和ry
    """

    rx, ry, ryaw = ref_path[:, 0], ref_path[:, 1], ref_path[:, 3]
    dx = [vehicle.x - icx for icx in rx]
    dy = [vehicle.y - icy for icy in ry]
    d = np.hypot(dx, dy)
    index = np.argmin(d)

    vec_nr = np.array([math.cos(ryaw[index] + math.pi / 2.0),
                       math.sin(ryaw[index] + math.pi / 2.0)])
    vec_target_2_rear = np.array([vehicle.x - rx[index],
                                  vehicle.y - ry[index]])
    e_lat = np.dot(vec_target_2_rear, vec_nr)

    return index, e_lat, rx, ry


def pure_pursuit_steer_control(vehicle, ref_path, index_old, use_front_wheel=False):
    ind, e_lat, rx, ry = calc_preparation(vehicle, ref_path)
    if index_old is None:
        index_old = ind

    ld = k * abs(vehicle.v) + Lfc  # 根据速度更新预瞄距离

    if use_front_wheel:
        front_wheel_x = vehicle.x + vehicle.L * math.cos(vehicle.yaw)
        front_wheel_y = vehicle.y + vehicle.L * math.sin(vehicle.yaw)
        while ld > math.hypot(rx[ind] - front_wheel_x, ry[ind] - front_wheel_y):
            if (ind + 1) >= len(rx):
                break
            ind += 1
        ind = max(ind, index_old)
        tx = rx[ind]
        ty = ry[ind]

        alpha = math.atan2(ty - front_wheel_y, tx - front_wheel_x) - vehicle.yaw
        delta = math.atan2(2.0 * vehicle.L * math.sin(alpha), ld + 2.0 * vehicle.L * math.cos(alpha))
        return delta, ind, e_lat
    else:
        while ld > math.hypot(rx[ind] - vehicle.x, ry[ind] - vehicle.y):
            if (ind + 1) >= len(rx):
                break
            ind += 1

        ind = max(ind, index_old)
        tx = rx[ind]
        ty = ry[ind]
        alpha = math.atan2(ty - vehicle.y, tx - vehicle.x) - vehicle.yaw
        delta = math.atan2(2.0 * vehicle.L * math.sin(alpha), ld)
        return delta, ind, e_lat

kinematic_bicycle_model.py

import math
import numpy as np

class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

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

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 2.0  # 轴距
    W = 2.0  # 宽度
    LF = 2.8  # 后轴中心到车头距离
    LB = 0.8  # 后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  # 轮距
    LENGTH = LB + LF  # 车辆长度

def draw_vehicle(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2,
                       vehicle_info.TW / 2]])

    rr_wheel = wheel.copy()  # 右后轮
    rl_wheel = wheel.copy()  # 左后轮
    fr_wheel = wheel.copy()  # 右前轮
    fl_wheel = wheel.copy()  # 左前轮
    rr_wheel[1, :] += vehicle_info.WD / 2
    rl_wheel[1, :] -= vehicle_info.WD / 2

    # 方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    # yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    ax.plot(x, y, 'o')

    # 绘制箭头
    ax.arrow(x, y, vehicle_info.L * 0.5 * math.cos(yaw), vehicle_info.L * 0.5 * math.sin(yaw),
              head_width=vehicle_info.L * 0.2, head_length=vehicle_info.L * 0.2, fc=color, ec=color)
    ax.axis('equal')

path_generator.py

"""
路径轨迹生成器
"""
import math
import numpy as np

class Path:
    def __init__(self, isReverse = False):
        self.is_reverse = isReverse
        self.ref_line = self.design_reference_line()
        self.ref_yaw = self.cal_yaw()
        self.ref_s = self.cal_accumulated_s()
        self.ref_kappa = self.cal_kappa()
        if isReverse:
            # 反转数组
            self.ref_line[:, 0] = np.flip(self.ref_line[:, 0], 0)
            self.ref_yaw = np.flip(self.ref_yaw, 0)
            self.ref_kappa = np.flip(self.ref_kappa, 0)  # 确保self.ref_kappa是NumPy数组

    def design_reference_line(self):
        rx = np.linspace(0, 50, 2000) + 5 # x坐标
        ry = 20 * np.sin(rx / 20.0) + 60  # y坐标
        rv = np.full(2000, (-2 if self.is_reverse else 2))
        return np.column_stack((rx, ry, rv))

    def cal_yaw(self):
        yaw = []
        for i in range(len(self.ref_line)):
            if i == 0:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i, 1],
                                 self.ref_line[i + 1, 0] - self.ref_line[i, 0]))
            elif i == len(self.ref_line) - 1:
                yaw.append(math.atan2(self.ref_line[i, 1] - self.ref_line[i - 1, 1],
                                 self.ref_line[i, 0] - self.ref_line[i - 1, 0]))
            else:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i - 1, 1],
                                  self.ref_line[i + 1, 0] - self.ref_line[i - 1, 0]))
        return yaw

    def cal_accumulated_s(self):
        rx = self.ref_line[:, 0]
        ry = self.ref_line[:, 1]
        if self.is_reverse:
            rx = np.flip(rx, 0)
            ry = np.flip(ry, 0)  # 反转x和y坐标

        s = []
        distance = 0
        for i in range(len(self.ref_line)):
            if i == 0:
                s.append(0.0)
            else:
                distance = np.sqrt((rx[i] - rx[i-1]) ** 2 + (ry[i] - ry[i-1]) ** 2)
                # 将新计算的距离累加到累积距离数组中
                s.append(distance + s[i - 1])
        return s

    def cal_kappa(self):
        # 计算曲线各点的切向量
        dp = np.gradient(self.ref_line.T, axis=1)
        # 计算曲线各点的二阶导数
        d2p = np.gradient(dp, axis=1)
        # 计算曲率
        kappa = (d2p[0] * dp[1] - d2p[1] * dp[0]) / ((dp[0] ** 2 + dp[1] ** 2) ** (3 / 2))

        return kappa

    def get_ref_line_info(self):
        return self.ref_line[:, 0], self.ref_line[:, 1], self.ref_line[:, 2], self.ref_yaw, self.ref_s, self.ref_kappa

main.py

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_vehicle
from pure_pursuit_controller import pure_pursuit_steer_control
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageio

MAX_SIMULATION_TIME = 200.0  # 程序最大运行时间200*dt


def main():
    # 设置跟踪轨迹
    rx, ry, rv, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info()
    ref_path = np.column_stack((rx, ry, rv, ref_yaw, ref_s, ref_kappa))
    # 假设车辆初始位置为（5，60），航向角yaw=0.0，速度为2m/s，时间周期dt为0.1秒
    vehicle1 = Vehicle(x=5.0,
                       y=60.0,
                       yaw=0.0,
                       v=2.0,
                       dt=0.1,
                       l=VehicleInfo.L)
    vehicle2 = Vehicle(x=5.0,
                       y=60.0,
                       yaw=0.0,
                       v=2.0,
                       dt=0.1,
                       l=VehicleInfo.L)
    time = 0.0  # 初始时间
    target_ind1 = 0
    target_ind2 = 0
    # 记录车辆轨迹
    trajectory_x1 = []
    trajectory_y1 = []
    trajectory_x2 = []
    trajectory_y2 = []
    lat_err1 = []  # 记录横向误差
    lat_err2 = []  # 记录横向误差

    i = 0
    image_list = []  # 存储图片
    plt.figure(1, dpi=100)

    old_index1 = None
    old_index2 = None
    last_idx = ref_path.shape[0] - 1  # 跟踪轨迹的最后一个点的索引
    while MAX_SIMULATION_TIME >= time and (last_idx > target_ind1 or last_idx > target_ind2):
        time += vehicle1.dt  # 累加一次时间周期

        if last_idx > target_ind1:
            delta_f1, target_ind1, e_y1 = pure_pursuit_steer_control(vehicle1, ref_path, old_index1, False)
            old_index1 = target_ind1
            # 横向误差
            lat_err1.append(e_y1)
            # 更新车辆状态
            vehicle1.update(0.0, delta_f1, np.pi/10)  # 由于假设纵向匀速运动，所以加速度a=0.0
            trajectory_x1.append(vehicle1.x)
            trajectory_y1.append(vehicle1.y)

        if last_idx > target_ind2:
            delta_f2, target_ind2, e_y2 = pure_pursuit_steer_control(vehicle2, ref_path, old_index2, True)
            old_index2 = target_ind2
            lat_err2.append(e_y2)
            vehicle2.update(0.0, delta_f2, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
            trajectory_x2.append(vehicle2.x)
            trajectory_y2.append(vehicle2.y)

        # 显示动图
        plt.subplots_adjust(hspace=0.5, wspace=0.5)  # 调整垂直和水平间距
        plt.subplot(3, 1, 1)
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
        draw_vehicle(vehicle1.x, vehicle1.y, vehicle1.yaw, vehicle1.steer, plt, color='blue')
        draw_vehicle(vehicle2.x, vehicle2.y, vehicle2.yaw, vehicle2.steer, plt, color='green')


        plt.plot(trajectory_x1, trajectory_y1, "-b", label="RWPP-trajectory")
        plt.plot(trajectory_x2, trajectory_y2, "-g", label="FWPP-trajectory")
        plt.plot(ref_path[target_ind1, 0], ref_path[target_ind1, 1], "b-o", label="RWPP-target")
        plt.plot(ref_path[target_ind2, 0], ref_path[target_ind2, 1], "g-o", label="FWPP-target")
        plt.xlim(min(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0]) - 3,
                 max(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0]) + 3)
        plt.ylim(min(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1]) - 3,
                 max(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1]) + 3)
        plt.legend()
        plt.grid(True)

        plt.subplot(3, 1, 2)
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
        plt.plot(trajectory_x1, trajectory_y1, 'b', label="RWPP-trajectory")
        plt.plot(trajectory_x2, trajectory_y2, 'g', label="FWPP-trajectory")
        plt.title("actual tracking effect")
        plt.xlim(min(trajectory_x1[-1], trajectory_x2[-1]) - 1, max(trajectory_x1[-1], trajectory_x2[-1]) + 1)
        plt.ylim(min(trajectory_y1[-1], trajectory_y2[-1]) - 0.5, max(trajectory_y1[-1], trajectory_y2[-1]) + 0.5)
        plt.legend()
        plt.grid(True)

        plt.subplot(3, 1, 3)
        plt.cla()
        plt.plot(lat_err1, 'b', label="RWPP")
        plt.plot(lat_err2, 'g', label="FWPP")
        plt.title("lateral error")
        plt.legend()
        plt.xlim((len(trajectory_x1) + len(trajectory_x2)) / 2 - 35, (len(trajectory_x1) + len(trajectory_x2)) / 2 + 35)
        plt.ylim(min(lat_err1[-1], lat_err2[-1]) - 0.1, max(lat_err1[-1], lat_err2[-1]) + 0.1)

        plt.grid(True)
        plt.pause(0.001)
        plt.savefig("temp.png")
        i += 1
        if (i % 5) == 0:
            image_list.append(imageio.imread("temp.png"))

    imageio.mimsave("display.gif", image_list, duration=0.1)

    plt.figure(2)
    plt.subplots_adjust(hspace=0.5, wspace=0.5)  # 调整垂直和水平间距
    plt.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
    plt.plot(trajectory_x1, trajectory_y1, 'b', label="RWPP-trajectory")
    plt.plot(trajectory_x2, trajectory_y2, 'g', label="FWPP-trajectory")
    plt.title("actual tracking effect")
    plt.legend()
    plt.grid(True)

    plt.subplot(2, 1, 2)
    plt.plot(lat_err1, 'b', label="Rear Wheel Pure Pursuit")
    plt.plot(lat_err2, 'g', label="Front Wheel Pure Pursuit")
    plt.title("lateral error")
    plt.legend()
    plt.grid(True)
    plt.show()


if __name__ == '__main__':
    main()

运行效果：

控制效果和横向误差：

在分析运行结果后，我们发现，采用前轮作为跟踪点的小型车辆在应对具有较大道路曲率变化的路况时，展现出更快的响应速度和更高的灵活性。然而，这种设计也导致了相对于使用后轮作为跟踪点的车辆，其控制过程中的误差有所增加。另一方面，在较为平坦的道路条件下，前轮跟踪点的车辆能够实现较小的跟踪误差，从而保持较高的路径跟踪精度。

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2024-05-06，如有侵权请联系 cloudcommunity@tencent.com 删除

算法