推导过程代码实现import numpy as np import random import matplotlib.pyplot as plt from cvxopt import matrix, solvers # Simulation parameters dt 0.02 # 采样间隔 T 30 # 总时间 length int(np.ceil(T / dt)) # 总步长 print(---------------------------) print(length) print(---------------------------) # System initialization p np.zeros( (length 1, 1) ) # 定义全的一个一维数组存储每一个步长运行后的位置 p代表后车距离坐标原点的距离 print(p) v np.zeros((length 1, 1)) # 后车速度 print(v) z np.zeros((length 1, 1)) # 前车后车距离 u np.zeros((length, 1)) # wheel force sys { m: 1650, # 小车的质量 g: 9.81, # 加速度 v0: 14, # 初始速度 vd: 24, # 目标速度 f0: 0.1, # f1: 5, # f2: 0.25, # ca: 0.3, # 输入上限系数 cd: 0.3, # 输入下限系数 T: 1.8, # 前瞻时间反应时间 clf_rate: 5, # clf超参数 cbf_rate: 5, # cbf超参数 weight_input: 2 / 1650**2, # weight_slack: 2e-2, # 松弛变量 } sys[u_max] sys[ca] * sys[m] * sys[g] # 输入上限 sys[u_min] -sys[cd] * sys[m] * sys[g] # 输入下线 # Initial conditions p[0] 0 v[0] 10 z[0] 100 # Simulation loop for i in range(length): current_p p[i, 0] current_v v[i, 0] current_z z[i, 0] x np.array( [current_p, current_v, current_z] ) # 系统的状态量后车位置后车速度距离前车速度 F_r sys[f0] sys[f1] * current_v sys[f2] * current_v**2 # 动力学方程f(x) f np.array( [current_v, -F_r / sys[m], sys[v0] - current_v] ) # 仿射函数中的动力学部分 g np.array([0, 1 / sys[m], 0]) # 仿射函数中的控制向量场部分g(x) # clf V (current_v - sys[vd]) ** 2 # 平方,能量函数Vx dV np.array([0, 2 * (current_v - sys[vd]), 0]) # 对能量函数求梯度 LfV np.dot(dV, f) # 利用李导算子化简dVfLfV np.dot做点积 LgV np.dot(dV, g) # dVgLgV # cbf B ( current_z - sys[T] * current_v - 0.5 * (current_v - sys[v0]) ** 2 / (sys[cd] * sys[g]) ) dB np.array([0, -sys[T] - (current_v - sys[v0]) / (sys[cd] * sys[g]), 0]) LfB np.dot(dB, f) LgB np.dot(dB, g) # Quadratic program H_ np.array([[sys[weight_input], 0], [0, sys[weight_slack]]]) # 转化2×2 f_ np.array([-sys[weight_input] * F_r, 0]) # 滚动摩擦 A_ np.array([[LgV, -1], [-LgB, 0], [1, 0], [-1, 0]]) # -LgB考虑实际意义此项为正 b_ np.array( [ -LfV - sys[clf_rate] * V, LfB sys[cbf_rate] * B, sys[u_max], -sys[u_min], ] ) # Convert to cvxopt format P matrix(H_) # 将数组转换成矩阵 q matrix(f_) G matrix(A_) h matrix(b_) # Solve QP problem using cvxopt sol solvers.qp(P, q, G, h) print(----------) print(sol) print(----------) u_opt sol[x][0] # Second term is the slack variable dx f g * u_opt x_n x dx * dt print(---------------------------) print(x_n) print(---------------------------) # Save data u[i, 0] u_opt if i length: p[i 1, 0] x_n[0] v[i 1, 0] x_n[1] z[i 1, 0] x_n[2] # Plotting time np.arange(0, T dt, dt) plt.figure(figsize(10, 8)) plt.subplot(4, 1, 1) plt.plot(time, p) plt.ylabel(p) plt.subplot(4, 1, 2) plt.plot(time, v) plt.ylabel(v) plt.subplot(4, 1, 3) plt.plot(time, z) plt.ylabel(z) plt.subplot(4, 1, 4) plt.plot(time[:-1], u) plt.ylabel(u) plt.xlabel(Time (s)) plt.tight_layout() plt.show()仿真结果