6.5 生成最优控制轨迹
文件cftoc_trajectory_generator.py用于生成约束有限时间最优控制（CFTOC）轨迹，通过有限时间优化生成一个光滑且可行的“超车”轨迹，以绕过障碍（自我）车辆。后续的 mpc_vehicle_controller.ipynb 脚本将使用模型预测控制（MPC）来沿着这个参考轨迹控制车辆。
（1）下面代码的功能是设置一个环境，以便在 Google Colab 中安装所需的库和工具，并导入必要的模块，用于生成直道路段的超车轨迹。它通过安装 Pyomo、GLPK 和 IPOPT 等工具，准备后续的轨迹优化和可视化。
import sys
IN_COLAB = 'google.colab' in sys.modules
if IN_COLAB:
# 安装所需的库和工具
!pip install -q pyomo
!apt-get install -y -qq glpk-utils
!apt-get install -y -qq coinor-cbc
!wget -N -q "https://ampl.com/dl/open/ipopt/ipopt-linux64.zip"
!unzip -o -q ipopt-linux64
!pip install ttictoc
# 超车轨迹生成器，用于直道路段
import matplotlib.pyplot as plt
import numpy as np
import pyomo.environ as pyo
from mpl_toolkits import mplot3d（2）下面代码的功能是定义超车轨迹生成模拟的参数和初始状态，包括时间步长、仿真步数、障碍物状态和控制输入。它初始化了障碍物的状态，并通过循环计算每个时间步的障碍物位置，存储在数组中。
# 模拟参数：
Ts = 0.1 # 时间步长
N = 150 # 仿真步数
Nz = 2 # 状态维度
Nu = 2 # 控制输入维度
# 初始状态：
initEgoState = [0, 15] # 自车初始状态
initEgoControl = [20, 0] # 自车初始控制输入
initObsState = [100, 15] # 障碍物初始状态
initObsControl = [10, 0] # 障碍物初始控制输入
# 定义障碍物状态：
obsState = np.zeros((2, N+1)) # 障碍物状态数组
obsControl = np.zeros((2, N+1)) # 障碍物控制输入数组
for i in range(N + 1):
obsControl[0, i] = initObsControl[0] # 设置障碍物的控制输入
obsControl[1, i] = initObsControl[1]
obsState[0, i] = initObsState[0] + obsControl[0, i] * Ts * i # 计算障碍物位置
obsState[1, i] = initObsState[1]（3）下面代码的功能是使用Pyomo库构建和求解一个超车轨迹生成模型，包括定义模型参数、目标函数、约束条件，并求解优化问题。模型考虑了车辆的状态、控制输入、速度、加速度、障碍物位置以及车道限制，最后输出车辆在每个时间步的轨迹和控制数据。
# 模型初始化：
model = pyo.ConcreteModel()
model.tidx = pyo.Set(initialize=range(0, N + 1))
model.zidx = pyo.Set(initialize=range(0, Nz))
model.uidx = pyo.Set(initialize=range(0, Nu))
# 创建状态和输入变量轨迹：
model.z = pyo.Var(model.zidx, model.tidx) # 状态变量
model.u = pyo.Var(model.uidx, model.tidx) # 控制输入变量
model.o = pyo.Var(model.tidx, initialize=0) # 超车状态变量
# 目标函数：
model.cost = pyo.Objective(expr = 5. * (model.z[1, N] - initEgoState[1])**2 +
5. * (model.u[0, N-1] - initEgoControl[0])**2 +
10. * (model.z[1, N] - model.z[1, N-1])**2 +
0.1 * sum((model.u[1, t+1] - model.u[1, t])**2 for t in model.tidx if t < N-1) +
- 10.0 * sum((model.z[0, t] - obsState[0, t]) for t in model.tidx if t < N) +
# - model.mb, sense=pyo.minimize) # 注释掉的部分
+ 0.2 * sum((model.z[1, t] - obsState[1, t])**2 * (model.o[t]) for t in model.tidx if t < N) +
+ 0.05 * sum((model.z[1, t] - obsState[1, t] - 10)**2 * (1 - model.o[t]) for t in model.tidx if t < N), sense=pyo.minimize)
# 初始约束条件：
model.c1 = pyo.Constraint(model.zidx, rule=lambda model, i: model.z[i, 0] == initEgoState[i]) # 自车初始状态
model.c2 = pyo.Constraint(model.uidx, rule=lambda model, j: model.u[j, 0] == initEgoControl[j]) # 自车初始控制输入
# 模型约束条件：
model.c3 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[0, t+1] == model.z[0, t] + model.u[0, t] * pyo.cos(model.u[1, t]) * Ts if t < N else pyo.Constraint.Skip)
model.c4 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[1, t+1] == model.z[1, t] + model.u[0, t] * pyo.sin(model.u[1, t]) * Ts if t < N else pyo.Constraint.Skip)
# 速度和加速度约束：
model.c5 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t] <= 50 if t < N else pyo.Constraint.Skip) # 速度上限
model.c6 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t] >= initEgoControl[0] if t < N else pyo.Constraint.Skip) # 速度下限
model.c7 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t+1] - model.u[0, t] >= -5 * Ts if t < N-1 else pyo.Constraint.Skip) # 加速限制
model.c8 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t+1] - model.u[0, t] <= 5 * Ts if t < N-1 else pyo.Constraint.Skip)
# 角度和角速度约束：
model.c9 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t] >= -0.5 if t < N else pyo.Constraint.Skip) # 角度下限
model.c10 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t] <= 0.5 if t < N else pyo.Constraint.Skip) # 角度上限
model.c11 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t+1] - model.u[1, t] >= -0.175 * Ts if t < N-1 else pyo.Constraint.Skip) # 角速度限制
model.c12 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t+1] - model.u[1, t] <= 0.175 * Ts if t < N-1 else pyo.Constraint.Skip)
# 障碍物约束：
model.c13（4）下面代码的功能是绘制自车在轨迹生成模型中运行的结果，包括自车的X坐标、Y坐标、速度和角度，并展示自车轨迹与障碍物的相对位置。最后，代码输出自车在最后时间步的位置以及障碍物的位置，并打印超车状态。
# 绘制结果：
plt.figure()
plt.subplot(4,1,1) # 创建4行1列的子图，选择第1个子图
plt.plot(egoXData) # 绘制自车X坐标
plt.ylabel('x') # Y轴标签
plt.subplot(4,1,2) # 选择第2个子图
plt.plot(egoYData) # 绘制自车Y坐标
plt.ylabel('y') # Y轴标签
plt.subplot(4,1,3) # 选择第3个子图
plt.plot(egoVData) # 绘制自车速度
plt.ylabel('speed') # Y轴标签
plt.subplot(4,1,4) # 选择第4个子图
plt.plot(egoPData) # 绘制自车角度
plt.ylabel('psi') # Y轴标签
plt.show() # 显示绘图
# 绘制自车轨迹与障碍物位置：
plt.plot(egoXData, egoYData) # 绘制自车轨迹
plt.plot(obsState[0,:], obsState[1,:], 'r-') # 绘制障碍物轨迹，红色线
plt.axis('scaled') # 设置坐标轴比例相同
plt.show() # 显示绘图
# 输出最后时刻自车位置和障碍物位置：
print(egoXData[N], obsState[0, N]) # 输出自车X坐标和障碍物X坐标
print(egoYData[N], obsState[1, N]) # 输出自车Y坐标和障碍物Y坐标
print(pyo.value(model.o[:])) # 输出超车状态（5）下面代码的功能是为超车轨迹生成动画准备工作，包括设置图形、定义车道位置以及存储自车和障碍物的位置。为后续的动画创建准备了必要的环境和数据结构，设置了车道的Y坐标，并为自车和障碍物的动画展示做了准备。
# 超车轨迹动画
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Rectangle
from random import randint
from matplotlib import rc
rc('animation', html='jshtml') # 设置动画显示格式为HTML
# 车辆尺寸
width = 2.5
length = 10
# 创建图形和坐标轴
fig, ax = plt.subplots()
# 初始化自车和障碍物的坐标列表
egoX = []
egoY = []
obsX = []
obsY = []
# 定义车道位置
lowerLaneX = np.linspace(start=egoXData[0]-5, stop=egoXData[N]+5, num=100) # 下车道X坐标
lowerLaneY = 10 * np.ones(100) # 下车道Y坐标
middleLaneX = np.linspace(start=egoXData[0]-5, stop=egoXData[N]+5, num=100) # 中车道X坐标
middleLaneY = 20 * np.ones(100) # 中车道Y坐标
upperLaneX = np.linspace(start=egoXData[0]-5, stop=egoXData[N]+5, num=100) # 上车道X坐标
upperLaneY = 30 * np.ones(100) # 上车道Y坐标（6）下面代码的功能是创建一个超车轨迹的动画，显示自车和障碍物的运动，并绘制不同车道的线路。它还将自车的状态（位置、速度和角度）整合为一个数组，以便进行后续分析。
def animate(i):
# 更新自车坐标
egoX.append(egoXData[i])
egoY.append(egoYData[i])
# 更新障碍物坐标
obsX.append(obsState[0, i])
obsY.append(obsState[1, i])
# 清除当前图形
ax.clear()
# 绘制障碍物的路径
ax.plot(obsX, obsY, c='r')
# 绘制自车的矩形
ax.add_patch(Rectangle((egoX[-1] - length/2, egoY[-1] - width/2), length, width, angle=egoPData[i]*180/np.pi))
# 绘制自车的路径
ax.plot(egoX, egoY, c='b')
# 绘制障碍物的矩形
ax.add_patch(Rectangle((obsX[-1] - length/2, obsY[-1] - width/2), length, width))
# 绘制车道线
ax.plot(lowerLaneX, lowerLaneY, c='g')
ax.plot(middleLaneX, middleLaneY, c='g')
ax.plot(upperLaneX, upperLaneY, c='g')
# 设置坐标轴比例
plt.gca().set_aspect(2)
# 创建动画
ani = FuncAnimation(fig, animate, frames=N, interval=100, repeat=False)
# 整合自车状态数据
egoXdata = np.array(egoXData)
egoXdata = egoXdata.reshape((1, 151))
egoYdata = np.array(egoYData)
egoYdata = egoYdata.reshape((1, 151))
egoVdata = np.array(egoVData)
egoVdata = np.append(egoVdata, egoVData[-1])
egoVdata = egoVdata.reshape((1, 151))
egoPdata = np.array(egoPData)
egoPdata = np.append(egoPdata, egoPData[-1])
egoPdata = egoPdata.reshape((1, 151))
# 合并状态数据
combined_state_data1 = np.concatenate((egoXdata, egoYdata))
combined_state_data2 = np.concatenate((egoVdata, egoPdata))
combined_state_data = np.concatenate((combined_state_data1, combined_state_data2))
# 绘制合并后的状态数据
plt.plot(combined_state_data[0, :], combined_state_data[1, :])
nz = 4 # 状态数量
nu = 2 # 输入数量
l_r = 1.738 # 车辆后轴到重心的距离
上述代码通过动画展示自车的超车过程，同时记录和整合了自车的状态数据，以便进一步分析。
（7）下面代码的功能是利用优化模型解决一个控制问题，生成自车在给定状态下的轨迹，比较开环和闭环的轨迹，并将生成的轨迹进行可视化。它使用了Pyomo库来构建并求解线性规划模型，同时支持对自车状态和输入约束的定义。
def solve_cftoc(Ts, Nmodel, d, nz, nu, x0, stateData):
model = pyo.ConcreteModel()
model.tidx = pyo.Set(initialize=range(0, Nmodel + 1)) # 有限优化问题的长度
model.zidx = pyo.Set(initialize=range(0, nz))
model.uidx = pyo.Set(initialize=range(0, nu))
# 创建状态和输入变量轨迹：
model.z = pyo.Var(model.zidx, model.tidx)
model.u = pyo.Var(model.uidx, model.tidx)
# 目标函数：
model.cost = pyo.Objective(expr=10 * sum((model.z[i, Nmodel] - stateData[i, -1]) ** 2 for i in model.zidx) +
sum((model.z[0, t] - stateData[0, t]) ** 2 +
(model.z[1, t] - stateData[1, t]) ** 2 +
(model.z[2, t] - stateData[2, t]) ** 2 for t in model.tidx),
sense=pyo.minimize)
# 约束条件：
model.constraint1 = pyo.Constraint(model.zidx, rule=lambda model, i: model.z[i, 0] == x0[i])
model.constraint2 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[0, t + 1] == model.z[0, t] +
Ts * (model.z[2, t] * pyo.cos(model.z[3, t] + model.u[1, t] + 0.5 * d))
if t < Nmodel else pyo.Constraint.Skip)
model.constraint3 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[1, t + 1] == model.z[1, t] +
Ts * (model.z[2, t] * pyo.sin(model.z[3, t] + model.u[1, t] + 0.5 * d))
if t < Nmodel else pyo.Constraint.Skip)
model.constraint4 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[2, t + 1] == model.z[2, t] +
Ts * model.u[0, t]
if t < Nmodel else pyo.Constraint.Skip)
model.constraint5 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[3, t + 1] == model.z[3, t] +
Ts * (model.z[2, t] / l_r * pyo.sin(model.u[1, t]))
if t < Nmodel else pyo.Constraint.Skip)
model.constraint6 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t] <= 4
if t < Nmodel else pyo.Constraint.Skip)
model.constraint7 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[0, t] >= -4
if t < Nmodel else pyo.Constraint.Skip)
model.constraint8 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t + 1] - model.u[1, t] <= 0.2
if t < Nmodel - 1 else pyo.Constraint.Skip)
model.constraint9 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t + 1] - model.u[1, t] >= -0.2
if t < Nmodel - 1 else pyo.Constraint.Skip)
model.constraint10 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t] >= -0.6
if t < Nmodel else pyo.Constraint.Skip)
model.constraint11 = pyo.Constraint(model.tidx, rule=lambda model, t: model.u[1, t] <= 0.6
if t < Nmodel else pyo.Constraint.Skip)
model.constraint12 = pyo.Constraint(model.tidx, rule=lambda model, t: model.z[2, t] <= 50
if t < Nmodel else pyo.Constraint.Skip)
# 现在可以求解：
results = pyo.SolverFactory('ipopt').solve(model)
xOpt = np.asarray([[model.z[i, t]() for i in model.zidx] for t in model.tidx]).T
uOpt = np.asarray([model.u[:, t]() for t in model.tidx]).T
return [model, xOpt, uOpt]
xOpt = combined_state_data[:, 0]
dtest = np.random.random() * 0.1
print(dtest)
[a, b, c] = solve_cftoc(Ts, N, dtest, nz, nu, xOpt, combined_state_data)
plt.plot(b[0, :], b[1, :])
plt.plot(combined_state_data[0, :], combined_state_data[1, :], 'r--')
plt.show()
plt.plot(b[2, :])
plt.plot(combined_state_data[2, :])
plt.show()
# 保存和导出生成的轨迹
M = 50 # 模拟步骤
xOpt = np.zeros((4, N + 1))
xOpt[:, 0] = combined_state_data[:, 0]
fig = plt.figure(figsize=(9, 6))
for t in range(0, M):
print(t)
[model, x, u] = solve_cftoc(Ts, N - t, nz, nu, xOpt[:, t], combined_state_data[:, t:])
# 保存闭环轨迹
# 注意x的第二列表示最优闭环状态
xOpt[:, t + 1] = x[:, 1]
for t in range(M, 2 * M):
print(t)
# print(N-t)
[model, x, u] = solve_cftoc(Ts, N - t, nz, nu, xOpt[:, t], combined_state_data[:, t:])
# plt.plot(x[0,:],x[1,:])
# plt.show()
xOpt[:, t + 1] = x[:, 1]
# 绘制开环轨迹
line1 = plt.plot(combined_state_data[0, :], combined_state_data[1, :], 'r--')
# 绘制闭环轨迹
line2 = plt.plot(xOpt[0, :], xOpt[1, :], 'b-')
plt.legend(['Closed-loop', 'Open Loop'])
plt.xlabel('x')
plt.ylabel('y')
plt.axis('equal')