基于MPC的轨迹跟踪控制 CarSim

 import numpy as np

    
 from casadi import *
    
  
    
 # 定义系统参数
    
 L = 2.5  # 车辆轴距
    
 v_ref = 10  # 参考速度
    
 dt = 0.1  # 控制周期
    
  
    
 # 定义状态变量
    
 x = MX.sym('x')
    
 y = MX.sym('y')
    
 psi = MX.sym('psi')
    
 v = MX.sym('v')
    
 states = vertcat(x, y, psi, v)
    
  
    
 # 定义控制变量
    
 delta = MX.sym('delta')  # 前轮转角
    
 a = MX.sym('a')  # 加速度
    
 controls = vertcat(delta, a)
    
  
    
 # 定义系统动力学方程
    
 xdot = v * cos(psi)
    
 ydot = v * sin(psi)
    
 psidot = v / L * tan(delta)
    
 vdot = a
    
  
    
 rhs = vertcat(xdot, ydot, psidot, vdot)
    
 f = Function('f', [states, controls], [rhs])
    
  
    
 # 定义优化问题
    
 N = 20  # 预测时域
    
 X = MX.sym('X', N+1, len(states))
    
 U = MX.sym('U', N, len(controls))
    
 X_next = np.zeros((N+1, len(states)))
    
  
    
 for k in range(N):
    
     X_next[k+1] = f(X[k], U[k])
    
     cost = (X[k, 0] - X_ref[k, 0])**2 + (X[k, 1] - X_ref[k, 1])**2  # 性能指标
    
     constraint = [-5 <= U[k, 0] <= 5, -3 <= U[k, 1] <= 3]  # 约束条件
    
  
    
 # 定义优化求解器
    
 nlp = {'x': vertcat(X.reshape(-1, 1), U.reshape(-1, 1)), 'f': cost, 'g': vertcat(constraint)}
    
 opts = {'ipopt.print_level': 0, 'print_time': 0, 'ipopt.tol': 1e-3, 'ipopt.hessian_approximation': 'limited-memory'}
    
 solver = nlpsol('solver', 'ipopt', nlp, opts)
    
  
    
 # 初始化状态和控制输入
    
 X_init = np.array([0, 0, 0, v_ref]).reshape(-1, 1)
    
 U_init = np.array([0, 0]).reshape(-1, 1)
    
  
    
 # 求解优化问题并得到最优控制序列
    
 res = solver(x0=vertcat(X_init, U_init))
    
 X_opt = res['x'][:N+1].reshape(-1, len(states))
    
 U_opt = res['x'][N+1:].reshape(-1, len(controls))
    
  
    
 # 应用最优控制输入至系统模型
    
 for k in range(N):
    
     X_next[k+1] = f(X_opt[k], U_opt[k])
    
    
    
    
    代码解读