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!
评论(0)
您还未登录,请登录后发表或查看评论