def solve(vessel_profile, vessel_initial, vessel_final, solver_options=None, params_super=None, use_c=False, verbose=False): # default params if (params_super == None): params_super = SC_params.SuperParams() if (solver_options == None): solver_options = SC_params.SolverOptions() #normalize Ut = vessel_profile.time_guess / 5. Ul = vessel_initial.pos[0] / 20. Um = vessel_initial.mass / 1.5 vessel_profile = SC_params.normalize(vessel_profile, Ut, Ul, Um) vessel_initial = SC_params.normalize(vessel_initial, Ut, Ul, Um) vessel_final = SC_params.normalize(vessel_final, Ut, Ul, Um) # solver if (use_c): import SC_subproblem_gen as solver else: import SC_subproblem as solver import GFOLD_solver, GFOLD_params # super params K = params_super.K dt = 1 / (K - 1) iterations = solver_options.iterations if (verbose): print('K=%s, iterations=%s' % (K, iterations)) # 参数的转换 将飞行器参数计算转化后填入求解器参数 params = SC_params.Params(K) #sparse params.m_dry = vessel_profile.m_dry params.tan_gamma_gs = math.tan(vessel_profile.gamma_gs) params.cos_theta_max = np.cos(vessel_profile.theta_max) params.omega_max = vessel_profile.omega_max params.cos_delta_max = math.cos(vessel_profile.delta_max) params.T_min = vessel_profile.T_min params.T_max = vessel_profile.T_max #initial params.x_initial[0:1, 0] = vessel_initial.mass params.x_initial[1:4, 0] = vessel_initial.pos params.x_initial[4:7, 0] = vessel_initial.vel params.x_initial[7:11, 0] = vessel_initial.rotation params.x_initial[11:14, 0] = vessel_initial.omega #final params.x_final[0:1, 0] = vessel_profile.m_dry params.x_final[1:4, 0] = vessel_final.pos params.x_final[4:7, 0] = vessel_final.vel params.x_final[7:11, 0] = vessel_final.rotation params.x_final[11:14, 0] = vessel_final.omega # time of flight guess params.s_last = vessel_profile.time_guess # solver options 凸优化目标函数里各部分权重 #params.w_delta = solver_options.w_delta params.w_nu = solver_options.w_nu params.w_delta_s = solver_options.w_delta_s integrator = Integrator(vessel_profile, params_super, use_c=False) #use_c is WIP don't use # initial guess with GFOLD if (verbose): print('solving GFOLD for initial guess') x_guess, u_guess, m_guess = None, None, None for i in range(10): # gfold until feasible x_guess, u_guess, m_guess = GFOLD_solver.solve(vessel_profile, vessel_initial, vessel_final, use_c=use_c, verbose=verbose) if type(x_guess) == type(None): vessel_profile.time_guess *= 1.2 if (verbose): print('GFOLD retry %s, tf=%s' % (i + 1, vessel_profile.time_guess)) else: break k_space = np.linspace( 0, K - 1, GFOLD_params.SuperParams().N) # for interpolation in case N != K params.s_last = vessel_profile.time_guess for k in range(K): # alpha1 = (K - k) / K # alpha2 = (k / K) #xk = params.x_initial * alpha1 + params.x_final * alpha2 xk = np.zeros((14, 1)) xk[7:11, 0] = np.array([1.0, 0.0, 0.0, 0.0]) xk[0, 0] = np.interp(k, k_space, m_guess[0, :]) #m_guess[0, k] for l_i in range(1, 7): xk[l_i, 0] = np.interp(k, k_space, x_guess[l_i - 1, :]) uk = np.zeros((3, 1)) for l_i in range(3): uk[l_i, 0] = np.linalg.norm(np.interp(k, k_space, u_guess[l_i, :])) params.x_last[:, k] = xk[:, 0] #params.u_last[:, k] = xk[0, 0] * -vessel_profile.g # hover #params.u_last_dir[:, k] = -vessel_profile.g / np.linalg.norm(vessel_profile.g) # thrust dir down params.u_last[:, k] = np.array([np.linalg.norm(uk), 0, 0]) params.u_last_dir[:, k] = params.u_last[:, k] / np.linalg.norm( params.u_last[:, k]) x_res, u_res, s_res = params.x_last, params.u_last, params.s_last #迭代求解 for iteration in range(iterations): if (verbose): print("Iteration", iteration + 1) #(1)积分 准备每个时间步的矩阵A B C S z 过程中需要计算积分,耗时较长 start_time = time() for k in range(0, K - 1): params.A[k][:, :], params.B[k][:, :], params.C[k][:, :], params.S[k][:, 0], params.z[k][:, 0] = \ integrator.solve(params.x_last[:, k], params.u_last[:, k], params.u_last[:, k+1], params.s_last) #积分耗时 if (verbose): print(time() - start_time, "sec to integrate") # 令w_delta随着迭代数改变 if isinstance(solver_options.w_delta, type(lambda: 0)): params.w_delta = solver_options.w_delta(iteration) else: params.w_delta = solver_options.w_delta # if solver_options.force_converge: # if iteration < solver_options.force_converge_start: # params.w_delta = solver_options.w_delta # else: # params.w_delta = solver_options.w_delta * solver_options.force_converge_amount # w_delta变大以促进delta收敛 # else: # params.w_delta = solver_options.w_delta #(2)求解凸优化子问题 start_time = time() res, x_res, u_res, s_res, nu_res, delta_res, delta_s_res = solver.solve( params, params_super) #凸优化耗时 if (verbose): print(time() - start_time, "sec to solve") # 这次迭代的结果作为下一次迭代的输入 params.x_last = x_res params.u_last = u_res params.s_last = s_res for k in range(K): #计算u的方向的单位向量 params.u_last_dir[:, k] = params.u_last[:, k] / np.linalg.norm( params.u_last[:, k]) # 收敛情况的评价指标 delta_norm = np.linalg.norm(delta_res) nu_norm = np.linalg.norm(nu_res, ord=1) if (verbose): print("Flight time:", s_res, end=' | ') print("Delta_norm:", delta_norm, end=' | ') print("Nu_norm:", nu_norm) #终止条件 if delta_norm < solver_options.delta_tol and nu_norm < solver_options.nu_tol: if (verbose): print("Converged after", iteration + 1, "iterations!") break #inv normalization x_res[0, :] *= Um x_res[1:4, :] *= Ul x_res[4:7, :] *= Ul / Ut x_res[11:14, 0] *= 1. / Ut u_res *= Um * Ul / Ut**2 s_res *= Ut #返回 状态,控制,tf return x_res, u_res, s_res
solver_options = SC_params.SolverOptions() #solver_options.w_nu = 1e5 #solver_options.w_delta = lambda i:(1e-3 if i < 4 else (1 if i < 8 else 10)) solver_options.w_delta = lambda i: (1e-3 * (2**i)) #solver_options.w_delta_s = 1e-1 #solver_options.nu_tol = 1e-8 #solver_options.delta_tol = 1e-3 #solver_options.force_converge = True #solver_options.force_converge_start = 4 #solver_options.force_converge_amount = 1e3 Ut = vessel.time_guess / 5. Ul = state_initial.pos[0] / 20. Um = state_initial.mass / 1.5 vessel = SC_params.normalize(vessel, Ut, Ul, Um) state_initial = SC_params.normalize(state_initial, Ut, Ul, Um) state_final = SC_params.normalize(state_final, Ut, Ul, Um) print(vessel.__dict__) print(state_initial.__dict__) x, u, tf = SC_solver.solve(vessel, state_initial, state_final, solver_options=solver_options, use_c=True, verbose=True) plot.plot_3D(x.T, u.T) #plot.plot_X(x.T, u.T)