【路径跟踪控制：PID控制与车辆运动学模型】

2345VOR

已于 2024-10-24 15:29:39 修改

阅读量1.6k

点赞数 29

分类专栏：自动驾驶文章标签：路径跟踪 PID 无人驾驶运动学模型

于 2024-10-23 16:45:00 首次发布

本文链接：https://blog.csdn.net/vor234/article/details/143175074

版权

自动驾驶专栏收录该内容

5 篇文章

订阅专栏

路径跟踪控制：PID控制与车辆运动学模型

1. 概述

在这里插入图片描述

路径跟踪控制是自动驾驶技术中的一个重要组成部分，它涉及到如何使车辆沿着预定的路径行驶。本文将详细介绍路径跟踪控制的基本原理，特别是使用PID控制器和车辆运动学模型的方法。我们将通过Python代码示例来展示如何实现这一控制策略。

2. 理论基础

2.1 车辆运动学模型

车辆运动学模型描述了车辆的位置和方向随时间的变化。常见的车辆运动学模型假设车辆是一个质点，并且忽略了动力学效应（如惯性和摩擦）。这种模型适用于低速和中速的路径跟踪控制。

2.1.1 运动学方程

假设车辆的状态由位置 $(x, y)$ 、航向角 $\psi$ 和速度 $v$ 组成，车轮基距为 $L$ ，时间步长为 $\Delta t$ 。则车辆的运动学方程可以表示为：
$\begin{aligned} x_{k+1} &= x_k + v_k \cos(\psi_k) \Delta t \\ y_{k+1} &= y_k + v_k \sin(\psi_k) \Delta t \\ \psi_{k+1} &= \psi_k + \frac{v_k}{L} \tan(\delta_f) \Delta t \\ v_{k+1} &= v_k + a \Delta t \end{aligned}$
其中：

$x_k$ 和 $y_k$ 是车辆在第 $k$ 步的位置。
$\psi_k$ 是车辆在第 $k$ 步的航向角。
$v_k$ 是车辆在第 $k$ 步的速度。
$\delta_f$ 是前轮转角。
$a$ 是加速度。

2.2 PID控制器

PID控制器是一种常用的反馈控制算法，广泛应用于工业控制领域。PID控制器通过比例、积分和微分三个部分来调整控制量，使得系统能够快速准确地跟踪目标值。

2.2.1 PID控制公式

PID控制器的输出 (u(t)) 可以表示为：
$K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{d e(t)}{dt}$
其中：

$K_p$ 是比例增益。
$K_i$ 是积分增益。
$K_d$ 是微分增益。
$e (t)$ 是误差信号，即目标值与实际值之差。

2.3 增量式PID与位置式PID

2.3.1 位置式PID

位置式PID控制器的输出是当前时刻的控制量 $u (t)$ ，它是误差 $e (t)$ 的函数。控制量 $u (t)$ 直接决定了执行机构的位置。

2.3.1.1 公式

$K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{d e(t)}{dt}$

2.3.1.2 特点

积分项累积误差：位置式PID的积分项会不断累积误差，这有助于消除稳态误差，但可能导致积分饱和问题。
直接输出控制量：输出 $u (t)$ 是当前时刻的控制量，适合需要直接控制执行机构位置的场景。
抗积分饱和：为了防止积分饱和，通常需要采用抗积分饱和算法，如积分分离法或限幅法。

2.3.2 增量式PID

增量式PID控制器的输出是控制量的增量 $\Delta u(t)$ ，它是误差 $e (t)$ 的函数。控制量 $u (t)$ 是上一时刻控制量 $u (t - 1)$ 和增量 $\Delta u(t)$ 的和。

2.3.2.1 公式

$\Delta u(t) = K_p \left( e(t) - e(t-1) \right) + K_i e(t) + K_d \left( e(t) - 2e(t-1) + e(t-2) \right)$
控制量 $u (t)$ 可以表示为：
$\Delta u(t)$

2.3.2.2 特点

增量输出：输出 $\Delta u(t)$ 是控制量的增量，适合需要逐步调整控制量的场景。
避免积分饱和：增量式PID不需要积分项的累积，因此不容易发生积分饱和问题。
计算简单：只需要当前和前几次的误差值，计算量较小。
动态响应好：增量式PID对系统动态变化的响应较好，适合需要快速调整的系统。

2.4 比较

特点	位置式PID	增量式PID
输出类型	当前控制量 $u (t)$	控制量增量 $\Delta u(t)$
积分项	累积误差，容易饱和	不累积误差，避免饱和
计算复杂度	较高，需要积分和微分	较低，只需当前和前几次误差
动态响应	较慢	较快
抗干扰能力	较强	较弱
适用场景	位置控制、静态误差小	速度控制、动态响应快

3. Python代码示例

3.1 基础版本

以下是路径跟踪控制的基础版本代码：

import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation

# 初始化参考路径
refer_path = np.zeros((1000, 2))

# 车辆运动学模型类
class KinematicModel:
    def __init__(self, x, y, psi, v, L, dt):
        self.x = x
        self.y = y
        self.psi = psi
        self.v = v
        self.L = L
        self.dt = dt

    def update_state(self, a, delta_f):
        self.x += self.v * np.cos(self.psi) * self.dt
        self.y += self.v * np.sin(self.psi) * self.dt
        self.psi += (self.v / self.L) * np.tan(delta_f) * self.dt
        self.v += a * self.dt

    def get_state(self):
        return self.x, self.y, self.psi, self.v

# PID 控制器类
class PIDController:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.prev_error = 0
        self.integral = 0

    def update(self, error):
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

def main(interactive=True):
    global refer_path, iadd  # 确保这些变量被声明为全局变量
    
    # 初始化车辆状态
    ugv = KinematicModel(0, 2, 0, 1, 2, 0.1)  # 初始速度设为2
    trajectory_x, trajectory_y = [], []

    # 创建绘图窗口
    fig, ax = plt.subplots()
    ax.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0, label="Reference Path")
    line, = ax.plot([], [], '-r', label="UGV Trajectory")
    target_dot, = ax.plot([], [], 'go', label="Target")
    ax.set_title("PID Trajectory Control (Heading Only with Limit)")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_xlim(-1, 110)
    ax.set_ylim(-3, 3)
    ax.grid(True)
    ax.legend()

    # PID 控制器参数
    Kp = 10  # 航向角PID控制器比例增益
    Ki = 0.01  # 航向角PID控制器积分增益
    Kd = 0.02  # 航向角PID控制器微分增益

    pid_controller_heading = PIDController(Kp, Ki, Kd, 0.01)

    def init():
        line.set_data([], [])
        target_dot.set_data([], [])
        return line, target_dot
        
    def update(frame):
        global iadd, refer_path  # 确保使用全局变量
        robot_state = np.array([ugv.x, ugv.y])

        # 判断 iadd 是否超出 refer_path 的长度，适当添加路径点
        if iadd+1 > refer_path.shape[0]:
            refer_path = np.resize(refer_path, (refer_path.shape[0] + 1, 2))  # 增加一行
            refer_path[iadd, 0] = refer_path[iadd-1, 0]  # 增加新路径点
            refer_path[iadd, 1] = refer_path[iadd-1, 1]  # 增加新路径点的 y 坐标

        dx, dy = refer_path[iadd] - robot_state
        alpha = math.atan2(dy, dx)
        l_d = np.linalg.norm(refer_path[iadd] - robot_state)
        theta_e = alpha - ugv.psi
        error_y = l_d * math.sin(theta_e)

        # 使用PID控制器计算转向角
        delta_f = pid_controller_heading.update(theta_e)

        # 对转向角进行限幅处理
        delta_f = np.clip(delta_f, -1, 1)

        # 固定速度
        a = error_y/100

        # 更新车辆状态
        ugv.update_state(a, delta_f)

        # 更新轨迹
        trajectory_x.append(ugv.x)
        trajectory_y.append(ugv.y)
        line.set_data(trajectory_x, trajectory_y)

        # 更新目标点
        target_dot.set_data([refer_path[iadd, 0]], [refer_path[iadd, 1]])
        iadd += 1  # 增加 iadd
        return line, target_dot

    # 创建动画
    anim = FuncAnimation(fig, update, frames=1030, init_func=init, blit=True, interval=20, repeat=False)

    if interactive:
        plt.show()
    else:
        anim.save('Control/Controllers_python/PID/PID_trajectory.gif', writer='pillow', fps=20)
        print("GIF 已保存")

# 运行主程序
if __name__ == '__main__':
    iadd = 0
    refer_path[:, 0] = np.linspace(0, 100, 1000)
    refer_path[:, 1] = np.sin(refer_path[:, 0] / 10) * 2  # sine 轨迹
    interactive_mode = True  # 设置为 True 以在交互式模式下运行，False 以保存 GIF
    
    if not interactive_mode:
        import matplotlib
        matplotlib.use('Agg')

    main(interactive=interactive_mode)

实验效果
在这里插入图片描述

3.2 优化改进版本

以下是路径跟踪控制的优化改进版本代码：

import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation

# 初始化参考路径
refer_path = np.zeros((1000, 2))

# 车辆运动学模型类
class KinematicModel:
    def __init__(self, x, y, psi, v, L, dt):
        self.x = x
        self.y = y
        self.psi = psi
        self.v = v
        self.L = L
        self.dt = dt

    def update_state(self, a, delta_f):
        self.x += self.v * np.cos(self.psi) * self.dt
        self.y += self.v * np.sin(self.psi) * self.dt
        self.psi += (self.v / self.L) * np.tan(delta_f) * self.dt
        self.v += a * self.dt

    def get_state(self):
        return self.x, self.y, self.psi, self.v

# PID 控制器类
class PIDController:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.prev_error = 0
        self.integral = 0

    def update(self, error):
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

def main(interactive=True):
    global refer_path, iadd  # 确保这些变量被声明为全局变量
    
    # 初始化车辆状态
    ugv = KinematicModel(0, 2, 0, 1, 2, 0.1)  # 初始速度设为2
    trajectory_x, trajectory_y = [], []

    # 创建绘图窗口
    fig, ax = plt.subplots()
    ax.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0, label="Reference Path")
    line, = ax.plot([], [], '-r', label="UGV Trajectory")
    current_dot, = ax.plot([], [], 'bo', label="Current")
    delta_f_line, = ax.plot([], [], '-y', label=r"$\delta_f$")
    target_dot, = ax.plot([], [], 'go', label="Target")
    ax.set_title("PID Trajectory Control (Heading Only with Limit)")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_xlim(-1, 110)
    ax.set_ylim(-3, 3)
    ax.grid(True)
    ax.legend()

    # PID 控制器参数
    Kp = 10  # 航向角PID控制器比例增益
    Ki = 0.01  # 航向角PID控制器积分增益
    Kd = 0.02  # 航向角PID控制器微分增益

    pid_controller_heading = PIDController(Kp, Ki, Kd, 0.01)

    def init():
        line.set_data([], [])
        current_dot.set_data([], [])
        delta_f_line.set_data([], [])
        target_dot.set_data([], [])
        return line, current_dot, delta_f_line, target_dot
        
    def update(frame):
        global iadd, refer_path  # 确保使用全局变量
        robot_state = np.array([ugv.x, ugv.y])

        # 判断 iadd 是否超出 refer_path 的长度，适当添加路径点
        if iadd+1 > refer_path.shape[0]:
            refer_path = np.resize(refer_path, (refer_path.shape[0] + 1, 2))  # 增加一行
            refer_path[iadd, 0] = refer_path[iadd-1, 0]  # 增加新路径点
            refer_path[iadd, 1] = refer_path[iadd-1, 1]  # 增加新路径点的 y 坐标

        dx, dy = refer_path[iadd] - robot_state
        alpha = math.atan2(dy, dx)
        l_d = np.linalg.norm(refer_path[iadd] - robot_state)
        theta_e = alpha - ugv.psi
        error_y = l_d * math.sin(theta_e)

        # 使用PID控制器计算转向角
        delta_f = pid_controller_heading.update(theta_e)

        # 对转向角进行限幅处理
        delta_f = np.clip(delta_f, -1, 1)
        

        # 固定速度
        a = error_y/100

        # 更新车辆状态
        ugv.update_state(a, delta_f)
        # 判断是否到达终点
        if iadd == refer_path.shape[0]-1 and abs(l_d) < 0.2 and abs(theta_e) < 0.2:  # 到达终点且距离误差小于 0.01
            ugv.v = 0  # 停止车辆
            print("Arrived!")
            return line, current_dot, delta_f_line, target_dot

        # 更新轨迹
        trajectory_x.append(ugv.x)
        trajectory_y.append(ugv.y)
        line.set_data(trajectory_x, trajectory_y)
        # 更新当前点
        current_dot.set_data([ugv.x], [ugv.y])
        # 更新转向角曲线
        delta_f_line.set_data([ugv.x, ugv.x + 1 * np.cos(ugv.psi + delta_f)], [ugv.y, ugv.y + 1 * np.sin(ugv.psi + delta_f)])

        # 更新目标点
        target_dot.set_data([refer_path[iadd, 0]], [refer_path[iadd, 1]])
        iadd += 1  # 增加 iadd
        return line, current_dot, delta_f_line, target_dot

    # 创建动画
    anim = FuncAnimation(fig, update, frames=1040, init_func=init, blit=True, interval=20, repeat=False)

    if interactive:
        plt.show()
    else:
        anim.save('Control/Controllers_python/PID/PID_trajectory.gif', writer='pillow', fps=20)
        print("GIF 已保存")

# 运行主程序
if __name__ == '__main__':
    iadd = 0
    refer_path[:, 0] = np.linspace(0, 100, 1000)
    refer_path[:, 1] = np.sin(refer_path[:, 0] / 10) * 2  # sine 轨迹
    interactive_mode = True  # 设置为 True 以在交互式模式下运行，False 以保存 GIF
    
    if not interactive_mode:
        import matplotlib
        matplotlib.use('Agg')

    main(interactive=interactive_mode)

实验效果
在这里插入图片描述

4. 代码对比与优势分析

4.1 基础版本 vs 优化改进版本

为了更清晰地对比基础版本和优化改进版本的特点及优势，我们可以将相关信息整理成表格形式。以下是整理后的表格：

特性/优势	基础版本	优化改进版本
动画效果
轨迹绘制	简单的轨迹绘制，仅包含参考路径和车辆轨迹	增加了当前点和目标点的标记，使得轨迹跟踪过程更加直观
PID控制	实现了基本的PID控制，用于调整车辆的航向角	同样实现了PID控制，用于调整车辆的航向角
速度控制	车辆的速度固定不变	车辆的速度固定不变
当前点和目标点标记	无	增加了当前点和目标点的标记
显示转向角	无	增加了转向角的显示，帮助理解PID控制器的工作原理
终点检测	无	增加了终点检测功能，当车辆接近终点且误差小于阈值时，停止车辆
绘图详细程度	较简单，仅包含参考路径和车辆轨迹	更详细，包括当前点、目标点和转向角曲线
可视化效果	较简单，信息较少	更好，增加了当前点和目标点的标记以及转向角的显示，使得轨迹跟踪过程更加直观和易于理解
控制精度	一般，没有终点检测功能	更精确，增加了终点检测功能，避免了过冲和不必要的运动
信息展示	较少，只有参考路径和车辆轨迹	更丰富，提供了更多的信息，有助于分析和优化控制策略

5. 结论

本文详细介绍了路径跟踪控制的基本原理，包括车辆运动学模型和PID控制器的理论知识。通过Python代码示例，展示了如何实现路径跟踪控制，并对比了两种不同的实现方法。优化改进版本在可视化效果、控制精度和信息展示方面具有明显的优势，我们可以观察到控制策略的效果。后期会有更多的路径跟踪（PID，LQR，MPC等）算法分享，希望本文对你理解和实现无人车轨迹跟踪有所帮助。

如果你有任何问题或建议，欢迎加QQ群留言交流！😘😘😘
参考博客：【自动驾驶】PID实现轨迹跟踪