【路径跟踪控制:Pure Pursuit控制与车辆运动学模型】
【路径跟踪控制:Pure Pursuit控制与车辆运动学模型】
- 1. 引言
- 2. 车辆运动学模型
- 3. Pure Pursuit 控制原理
- 4. 实现代码
- 5. 结果与分析
- 6. 串联学习
- 6.1. PID 控制器 (PID Controller)
- 6.2. Pure Pursuit 控制器 (Pure Pursuit Controller)
- 6.3. Bang-Bang 控制器 (Bang-Bang Controller)
- 7. 总结
1. 引言
建议系统学习以下博客:
【【自动驾驶】车辆运动学模型】
【路径跟踪控制:Bang-Bang 控制与车辆运动学模型】
【路径跟踪控制:PID控制与车辆运动学模型】
【路径跟踪控制:Pure Pursuit控制与车辆运动学模型】
论文:Implementation of the Pure Pursuit Path Tracking Algorithm
路径跟踪控制是自动驾驶领域的一个重要组成部分,其目的是使车辆沿着预定的路径行驶。本文将介绍一种常用的路径跟踪控制方法——Pure Pursuit 控制,并结合车辆运动学模型进行实现。通过 Python 代码示例,我们将展示如何使用 Pure Pursuit 控制器使车辆沿给定路径行驶。
2. 车辆运动学模型
车辆运动学模型描述了车辆的位置、速度和方向随时间的变化。对于两轮模型(参考【【自动驾驶】车辆运动学模型】),车辆的状态可以用以下四个参数表示:
- x x x:车辆在 x 轴上的位置
- y y y:车辆在 y 轴上的位置
- ψ \psi ψ:车辆的航向角(即车头指向的方向)
-
v
v
v:车辆的速度
车辆的运动可以通过以下方程组描述:
x
˙
=
v
cos
(
ψ
)
y
˙
=
v
sin
(
ψ
)
ψ
˙
=
v
L
tan
(
δ
f
)
v
˙
=
a
(1)
\begin{align*} \dot{x} &= v \cos(\psi) \\ \dot{y} &= v \sin(\psi) \\ \dot{\psi} &= \frac{v}{L} \tan(\delta_f) \\ \dot{v} &= a \end{align*} \tag{1}
x˙y˙ψ˙v˙=vcos(ψ)=vsin(ψ)=Lvtan(δf)=a(1)
其中:
- L L L 是车辆的轴距
- δ f \delta_f δf 是前轮的转向角
- a a a 是车辆的加速度
3. Pure Pursuit 控制原理
Pure Pursuit 控制是一种基于几何的路径跟踪方法。其基本思想是选择路径上的一个前视目标点(lookahead point),并通过调整车辆的转向角使其朝向前视目标点行驶。前视目标点的选择通常基于当前车辆位置和前视距离
l
d
l_d
ld。
在三角形中,运用正弦定理:
s
i
n
(
π
2
−
α
)
R
=
s
i
n
(
2
α
)
l
d
(2)
\frac{sin(\frac{\pi}{2} - \alpha)}{R} =\frac{sin(2 \alpha)}{l_d} \tag{2}
Rsin(2π−α)=ldsin(2α)(2)
化简:
c
o
s
(
α
)
R
=
2
s
i
n
(
α
)
c
o
s
(
α
)
l
d
(3)
\frac{cos(\alpha)}{R} =\frac{2sin( \alpha)cos( \alpha)}{l_d} \tag{3}
Rcos(α)=ld2sin(α)cos(α)(3)
R
=
l
d
2
s
i
n
(
α
)
(4)
R=\frac{l_d}{2sin( \alpha)} \tag{4}
R=2sin(α)ld(4)
以后轴中心为车辆中心的单车模型特点:
t
a
n
(
δ
)
=
L
R
(5)
tan(\delta)=\frac{L}{R} \tag{5}
tan(δ)=RL(5)
联合公式(4)(5),消去
R
R
R,转向角
δ
\delta
δ (和
δ
f
\delta_f
δf一样)的计算公式为:
δ
=
a
r
c
t
a
n
(
2
L
s
i
n
(
α
)
l
d
)
(6)
\delta = arctan\left(\frac{2L sin(\alpha)}{l_d}\right) \tag{6}
δ=arctan(ld2Lsin(α))(6)
其中:
-
α
\alpha
α 是前视目标点与车辆当前位置之间的相对航向角
前视距离 l d l_d ld 可以根据车辆的速度动态调整:
l d = λ v + c (7) l_d = \lambda v + c \tag{7} ld=λv+c(7) - λ \lambda λ 是前视距离系数
- c c c 是前视距离常数,一般取车长长度
4. 实现代码
以下是使用 Python 实现 Pure Pursuit 控制的完整代码:
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation
import numpy as np
import matplotlib.pyplot as plt
import math
# 定义车辆参数
L = 2 # 车辆轴距,单位:m
v = 2 # 初始速度
x_0 = 0 # 初始x
y_0 = -3 # 初始y
psi_0 = 0 # 初始航向角
dt = 0.1 # 时间间隔,单位:s
lam = 0.1 # 前视距离系数
c = 2 # 前视距离常数
# 车辆运动学模型类
class KinematicModel_3:
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 * math.cos(self.psi) * self.dt
self.y += self.v * math.sin(self.psi) * self.dt
self.psi += (self.v / self.L) * math.tan(delta_f) * self.dt
self.v += a * self.dt
def get_state(self):
return self.x, self.y, self.psi, self.v
# 计算前视目标点
def cal_target_index(robot_state, refer_path, l_d):
tree = KDTree(refer_path)
_, min_index = tree.query(robot_state)
while l_d > np.linalg.norm(refer_path[min_index] - robot_state) and (min_index + 1) < len(refer_path):
min_index += 1
return min_index
# Pure Pursuit 控制器
def pure_pursuit_control(robot_state, current_ref_point, l_d, psi):
alpha = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psi
delta = math.atan2(2 * L * math.sin(alpha), l_d)
return delta
# 主函数
def main(interactive=True):
# 设置参考轨迹
refer_path = np.zeros((1000, 2))
refer_path[:, 0] = np.linspace(0, 100, 1000)
refer_path[:, 1] = 2 * np.sin(refer_path[:, 0] / 3.0) + 2.5 * np.cos(refer_path[:, 0] / 2.0)
# 初始化车辆状态
ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)
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")
target_dot, = ax.plot([], [], 'go', label="Target")
ax.set_title("Pure Pursuit Control")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_xlim(-1, 101)
ax.set_ylim(-6, 6)
ax.grid(True)
ax.legend()
def init():
line.set_data([], [])
current_dot.set_data([], [])
target_dot.set_data([], [])
return line, current_dot, target_dot
def update(frame):
nonlocal ugv
robot_state = np.array([ugv.x, ugv.y])
l_d = lam * ugv.v + c
ind = cal_target_index(robot_state, refer_path, l_d)
delta = pure_pursuit_control(robot_state, refer_path[ind], l_d, ugv.psi)
ugv.update_state(0, delta) # 加速度设为0,恒速
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])
target_dot.set_data([refer_path[ind, 0]], [refer_path[ind, 1]])
return line, current_dot, target_dot
# 创建动画
anim = FuncAnimation(fig, update, frames=700, init_func=init, blit=True, interval=20, repeat=False)
if interactive:
plt.show()
else:
import matplotlib
matplotlib.use('Agg') # 非交互模式下,使用非GUI后端
anim.save('Control/Controllers_python/Pure_pursuit/pure_pursuit_trajectory.gif', writer='pillow', fps=20)
print("GIF 已保存")
if __name__ == '__main__':
interactive_mode = False # 设置为 True 以在交互式模式下运行,False 以保存 GIF
main(interactive=interactive_mode)
5. 结果与分析
运行上述代码后,将生成一个动画,展示车辆沿参考路径行驶的过程。动画中可以看到:
- 蓝色虚线表示参考路径
- 红色实线表示车辆的实际轨迹
- 蓝色圆点表示车辆的当前位置
- 绿色圆点表示前视目标点
通过观察动画,可以验证 Pure Pursuit 控制器的有效性。车辆能够沿着预定的路径平稳行驶,且前视目标点的选择合理,确保了路径跟踪的精度。
6. 串联学习
PID、Pure Pursuit 和 Bang-Bang 控制
import numpy as np
import matplotlib.pyplot as plt
import math
from matplotlib.animation import FuncAnimation
# 初始化参考路径
refer_path = np.zeros((1000, 2))
# 车辆初始状态
x0, y0, psi0, v0 = 0, 2, 0, 1
L = 2 # 车轮间距
dt = 0.1 # 时间间隔
delta_limit = np.deg2rad(30) # 最大转向角度限制
# 车辆运动学模型类
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 control(self, robot_state, current_ref_point, psi):
error = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psi
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
# Pure Pursuit 控制器类
class PurePursuitController:
def __init__(self, L, lam, c):
self.L = L
self.lam = lam
self.c = c
def control(self, robot_state, current_ref_point, psi):
alpha = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psi
l_d = self.lam * robot_state[2] + self.c
delta = math.atan2(2 * self.L * math.sin(alpha), l_d)
return delta
# Bang-Bang 控制器类
class BangBangController:
def __init__(self, tolerance, max_delta):
self.tolerance = tolerance
self.max_delta = max_delta
def control(self, robot_state, current_ref_point, psi):
dx, dy = current_ref_point - robot_state[:2]
alpha = math.atan2(dy, dx)
l_d = np.linalg.norm(current_ref_point - robot_state[:2])
theta_e = alpha - psi
error_y = l_d * math.sin(theta_e)
# 根据误差调整最大转向角
max_delta = self.max_delta if abs(theta_e) > np.pi / 2.0 else self.max_delta / 2.0
# Bang-Bang 控制
delta_f = np.sign(error_y) * max_delta if abs(error_y) > self.tolerance else 0
return delta_f
# 公共更新函数
def update_common(ugv, controller, refer_path, iadd, delta_limit, dt, trajectory_x, trajectory_y, line, current_dot, delta_f_line, target_dot, reached_end):
robot_state = np.array([ugv.x, ugv.y, ugv.v])
current_ref_point = refer_path[iadd]
# 计算控制输入
delta_f = controller.control(robot_state, current_ref_point, ugv.psi)
delta_f = np.clip(delta_f, -delta_limit, delta_limit)
l_d = np.linalg.norm(current_ref_point - robot_state[:2])
# 固定速度
a = l_d * np.sin(delta_f)/50 # 可以根据需要调整速度控制
# 更新车辆状态
ugv.update_state(a, delta_f)
# 判断是否到达终点
if iadd == refer_path.shape[0] - 1 and abs(l_d) < 0.2: # 到达终点且距离误差小于 0.2
ugv.v = 0
a = 0
reached_end = True
print(f"{controller.__class__.__name__} 控制到达终点")
else:
# 更新轨迹
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([current_ref_point[0]], [current_ref_point[1]])
return line, current_dot, delta_f_line, target_dot, reached_end
# 主函数
def main(interactive=True):
global refer_path, iadd # 确保这些变量被声明为全局变量
# 初始化车辆状态
ugv_pid = KinematicModel(x0, y0, psi0, v0, L,dt)
ugv_pp = KinematicModel(x0, y0, psi0, v0, L, dt)
ugv_bb = KinematicModel(x0, y0, psi0, v0, L, dt)
trajectory_x_pid, trajectory_y_pid = [], []
trajectory_x_pp, trajectory_y_pp = [], []
trajectory_x_bb, trajectory_y_bb = [], []
# 创建绘图窗口
fig, ax = plt.subplots()
ax.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0, label="Reference Path")
line_pid, = ax.plot([], [], '-r', label="PID Trajectory")
line_pp, = ax.plot([], [], '-g', label="PP Trajectory")
line_bb, = ax.plot([], [], '-m', label="BB Trajectory")
current_dot_pid, = ax.plot([], [], 'bo', label="PID Current")
current_dot_pp, = ax.plot([], [], 'go', label="PP Current")
current_dot_bb, = ax.plot([], [], 'mo', label="BB Current")
delta_f_line_pid, = ax.plot([], [], '-y', label=r"PID $\delta_f$")
delta_f_line_pp, = ax.plot([], [], '-c', label=r"PP $\delta_f$")
delta_f_line_bb, = ax.plot([], [], '-k', label=r"BB $\delta_f$")
target_dot_pid, = ax.plot([], [], 'go', label="Target")
ax.set_title("PID vs Pure Pursuit vs Bang-Bang Trajectory Control")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_xlim(-1, 110)
ax.set_ylim(-5, 6)
ax.grid(True)
ax.legend()
# 控制器参数
Kp = 20 # 航向角PID控制器比例增益
Ki = 0.05 # 航向角PID控制器积分增益
Kd = 0.05 # 航向角PID控制器微分增益
lam = 0.5 # 前视距离系数
c = 0.5 # 车头到参考路径的距离
tolerance = 0.01
max_delta = np.deg2rad(30)
pid_controller = PIDController(Kp, Ki, Kd, dt)
pp_controller = PurePursuitController(L, lam, c)
bb_controller = BangBangController(tolerance, max_delta)
def init():
line_pid.set_data([], [])
line_pp.set_data([], [])
line_bb.set_data([], [])
current_dot_pid.set_data([], [])
current_dot_pp.set_data([], [])
current_dot_bb.set_data([], [])
delta_f_line_pid.set_data([], [])
delta_f_line_pp.set_data([], [])
delta_f_line_bb.set_data([], [])
target_dot_pid.set_data([], [])
return line_pid, line_pp, line_bb, current_dot_pid, current_dot_pp, current_dot_bb, delta_f_line_pid, delta_f_line_pp, delta_f_line_bb, target_dot_pid
def update(frame):
global iadd, refer_path # 确保使用全局变量
# 判断 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 坐标
# PID 控制
_, _, _, _, reached_pid_end = update_common(
ugv_pid, pid_controller, refer_path, iadd, delta_limit, dt,
trajectory_x_pid, trajectory_y_pid, line_pid, current_dot_pid, delta_f_line_pid, target_dot_pid, False
)
# Pure Pursuit 控制
_, _, _, _, reached_pp_end = update_common(
ugv_pp, pp_controller, refer_path, iadd, delta_limit, dt,
trajectory_x_pp, trajectory_y_pp, line_pp, current_dot_pp, delta_f_line_pp, target_dot_pid, False
)
# Bang-Bang 控制
_, _, _, _, reached_bb_end = update_common(
ugv_bb, bb_controller, refer_path, iadd, delta_limit, dt,
trajectory_x_bb, trajectory_y_bb, line_bb, current_dot_bb, delta_f_line_bb, target_dot_pid, False
)
iadd += 1
return line_pid, line_pp, line_bb, current_dot_pid, current_dot_pp, current_dot_bb, delta_f_line_pid, delta_f_line_pp, delta_f_line_bb, target_dot_pid
# 创建动画
anim = FuncAnimation(fig, update, frames=1050, init_func=init, blit=True, interval=20, repeat=False)
if interactive:
plt.show()
else:
anim.save('Control/Controllers_python/Pure_pursuit/PID_vs_PP_vs_BB_trajectory.gif', writer='pillow', fps=20)
print("GIF 已保存")
# 运行主程序
if __name__ == '__main__':
iadd = 0
refer_path[:, 0] = np.linspace(0, 100, 1000)
refer_path[:, 1] = 2 * np.sin(refer_path[:, 0] / 7.0) + 2.5 * np.cos(refer_path[:, 0] / 5.0) # 新的参考路径
interactive_mode = True # 设置为 True 以在交互式模式下运行,False 以保存 GIF
if not interactive_mode:
import matplotlib
matplotlib.use('Agg')
main(interactive=interactive_mode)
效果如下:
6.1. PID 控制器 (PID Controller)
【路径跟踪控制:PID控制与车辆运动学模型】
优点:
- 鲁棒性强:PID 控制器通过比例、积分和微分项的组合,能够有效地应对系统中的扰动和不确定性。
- 易于调参:通过调整 Kp、Ki 和 Kd 参数,可以灵活地控制系统的响应特性。
- 广泛适用:适用于多种类型的控制系统,特别是在线性和非线性系统中都有较好的表现。
缺点:
- 参数敏感:PID 控制器的性能高度依赖于参数的选择,不当的参数可能导致系统振荡或响应过慢。
- 复杂性:对于复杂的非线性系统,PID 控制器可能无法达到最优控制效果。
- 积分饱和:积分项可能导致积分饱和问题,需要额外的措施来防止。
6.2. Pure Pursuit 控制器 (Pure Pursuit Controller)
优点:
- 简单直观:Pure Pursuit 控制器基于几何原理,易于理解和实现。
- 实时性好:计算量小,适合实时控制应用。
- 鲁棒性好:对路径的形状和曲率变化有较好的适应能力。
缺点:
- 路径跟踪精度有限:对于复杂路径,特别是急转弯处,Pure Pursuit 控制器可能无法精确跟踪路径。
- 参数选择:前视距离 ( l_d ) 的选择对控制效果影响较大,需要根据具体应用场景进行调整。
- 速度依赖:控制效果受车辆速度的影响较大,高速时可能难以保持稳定。
6.3. Bang-Bang 控制器 (Bang-Bang Controller)
【路径跟踪控制:Bang-Bang 控制与车辆运动学模型】
优点:
- 响应快:Bang-Bang 控制器通过快速切换控制信号,能够在短时间内使系统接近目标。
- 简单易实现:控制逻辑简单,容易实现。
缺点:
- 过度震荡:由于控制信号的突然变化,系统可能会出现过度震荡,导致不稳定。
- 精度差:Bang-Bang 控制器通常只能将系统控制到目标的近似位置,难以实现高精度控制。
- 对噪声敏感:外部噪声和测量误差可能导致控制信号频繁切换,影响系统性能。
7. 总结
- PID 控制器适用于需要高精度和鲁棒性的应用场景,但需要仔细调参。
- Pure Pursuit 控制器适用于实时性要求高、路径跟踪精度要求适中的场景,特别适合自动驾驶等应用。
- Bang-Bang 控制器适用于对响应速度要求高、但对精度要求不高的场景,如简单的开关控制。
本文介绍了 Pure Pursuit 控制的基本原理及其在车辆路径跟踪中的应用。通过 Python 代码实现了车辆运动学模型和 Pure Pursuit 控制器,并展示了其在实际路径跟踪任务中的表现。Pure Pursuit 控制方法简单有效,适用于多种路径跟踪场景。未来的工作可以进一步优化控制器参数,提高路径跟踪的精度和稳定性。
参考文献:【自动驾驶】PurePursuit实现轨迹跟踪