acados-mpc建模(二)

引言

在前一节中,我们介绍了如果使用casadi进行车辆运动学建模,同时对acados进行简单介绍,在这一节中,我们将以mpc应用为例,介绍如果使用acados建模优化问题,并进行求解。

mpc优化建模

其中代码以注释的形式进行讲解

  • 初始化
ocp = AcadosOcp()# 创建最优化问题
ocp.code_export_directory = EXPORT_DIR
ocp.dims.N = N# 共N个点
ocp.solver_options.tf = Tf# 总长度
ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N + 1]# 每个点对应的时刻
  • 系统模型
ocp.model = gen_car_model()  # 接入系统模型
ocp.parameter_values = np.zeros((P_DIM,))
  • 优化器设置
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = ACADOS_SOLVER_TYPE
ocp.solver_options.qp_solver_iter_max = 10
ocp.solver_options.qp_solver_cond_N = 1
ocp.solver_options.print_level = 0
  • cost设置
###########################################损失定义
######损失优化类型
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'
######基本状态提取
x_ego, y_ego, psi_ego, alpha_ego  = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2], ocp.model.x[3]
alpha_rate = ocp.model.u[0]
v_ego = ocp.model.p[0]
L = ocp.model.p[1]
######cost设置
###cost矩阵设置
QR = np.diag(np.zeros(COST_DIM)) #5维
Q = np.diag(np.zeros(COST_E_DIM)) #3维
ocp.cost.W = QR# Q和R组成的对角阵
ocp.cost.W_e = Q# Q矩阵
###中间和结束cost目标值设置,在mpc中进行赋值
ocp.cost.yref = np.zeros((COST_DIM, ))
ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
###中间和结束cost预测值设置
ocp.model.cost_y_expr = vertcat(
                                x_ego,
                                y_ego,
                                v_ego_offset * psi_ego,
                                alpha_rate,
                                )
ocp.model.cost_y_expr_e = vertcat(
                                 x_ego,
                                 y_ego,
                                 v_ego_offset * psi_ego,
                                 x_ego,
                                 y_ego
  • 约束设置

    这里需要注意的是:

    1、第一项有时候会不起作用,所以建议使用BGP进行尝试

    2、转速的范围需要根据进行调试

###########################################约束设置
ocp.constraints.constr_type = 'BGH'#如果是非线性约束就设置BGP
ocp.constraints.idxbx = np.array([2])
ocp.constraints.lbx = np.array([-np.radians(90)])# 状态的下限
ocp.constraints.ubx = np.array([np.radians(90)])# 状态的上限
ocp.constraints.idxbu = np.array([0])
u_limit_unit = 1
ocp.constraints.lbu = np.array([-u_limit_unit])
ocp.constraints.ubu = np.array([u_limit_unit])
x0 = np.zeros((X_DIM,))
ocp.constraints.x0 = x0
return ocp

mpc优化求解

上面构建了优化模型,接下来我们要使用这个模型进行实际优化

  • 需要首先init
ocp = gen_lat_normal_ocp()
self.solver = AcadosOcpSolver(ocp)

然后reset初始化各个ocp变量

self.x_sol = np.zeros((N + 1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.yref = np.zeros((N + 1, COST_E_DIM))
for i in range(N):
    self.solver.cost_set(i, "yref", self.yref[i][:COST_DIM])
self.solver.cost_set(N, "yref", self.yref[N])
for i in range(N + 1):
    self.solver.set(i, 'x', np.zeros(X_DIM))
    self.solver.set(i, 'p', np.zeros(P_DIM))
self.solver.constraints_set(0, "lbx", x0)
self.solver.constraints_set(0, "ubx", x0)
self.solver.solve()

接下来就可以进行mpc迭代求解了,在每次求解时,需要依次设置cost权重,设置cost对应的ref值,设置状态初始值

W1 = np.asfortranarray(np.diag([path_x_weight,path_y_weight, heading_weight, alpha_weight]))
W2 = np.asfortranarray(np.diag([path_x_weight,path_y_weight, heading_weight, ex,ey]))
for i in range(N):
    self.solver.cost_set(i, 'W', W1)
self.solver.cost_set(N, 'W', W2)

self.yref[:, 0] = x_pts
self.yref[:, 1] = y_pts
self.yref[:, 2] = heading_pts * (v_ego[0] + SPEED_OFFSET)

for i in range(N):
    ego_pos = self.solver.get(i, 'x')[:2]
    self.solver.cost_set(i, "yref", self.yref[i][:COST_DIM])
    self.solver.set(i, "p", p_cp[i])  
self.solver.cost_set(N, "yref", np.array([x_pts[-1],y_pts[-1],heading_pts[-1] * (v_ego[0] + SPEED_OFFSET),x_pts[-1],y_pts[-1]]).reshape(5,))
self.solver.set(N, "p", p_cp[-1])# 设置参数

self.solver.constraints_set(0, "lbx", x0)
self.solver.constraints_set(0, "ubx", x0)

最后调用求解函数进行求解

self.solution_status = self.solver.solve()

结语

经过上面的代码和文字,我们学习了如何使用acados进行mpc优化问题建模以及求解,希望同行们多多批评指正!

May the force be with you!