卫星轨道提升轨迹优化模型不收敛问题排查求助

我正在开展以燃料最小化为目标的卫星轨道提升轨迹优化研究，基于Casadi框架编写了采用高斯变分方程（GVE）的求解代码，但模型始终无法收敛。已尝试调整节点数量、传播时长等参数，仍未解决问题。作为Casadi新手，恳请专业人士协助排查问题。

代码实现 #

import casadi as ca
import numpy as np
from poliastro.bodies import Earth
from poliastro.twobody import Orbit
from poliastro.core.propagation import func_twobody

from astropy import units as u
from astropy.time import TimeDelta
from poliastro.twobody.propagation import CowellPropagator
from poliastro.twobody.sampling import EpochsArray
from poliastro.core.elements import rv2coe

k = Earth.k.to(u.km**3 / u.s**2).value

def gve(r_norm, a, e, inc, Omega, omega, theta, dv_R, dv_T, dv_N, dt):
    p = a * (1 - e**2)  # Semi-latus rectum
    h = np.sqrt(k * p)  # Specific angular momentum

    da = 2 * a**2 / h * (e * ca.sin(theta) * dv_R + p/r_norm *  dv_T )
    de = p/h * (ca.sin(theta) *  dv_R + (ca.cos(theta) + (e + ca.cos(theta))/(1 + e*ca.cos(theta)))* dv_T)
    di = (r_norm / h) * ca.cos(theta + omega) * dv_N
    dOmega = (r_norm / (h * ca.sin(inc))) * ca.sin(theta + omega) * dv_N
    # 处理e=0的奇异情况，添加小偏移量
    e_safe = ca.if_else(ca.fabs(e) < 1e-6, 1e-6, e)
    domega = p/(h*e_safe)*(-ca.cos(theta)* dv_R + ca.sin(theta)*(1+r_norm/p)*dv_T) - r_norm/(h*ca.tan(inc))*ca.sin(theta + omega)*dv_N
    # GVE只计算推力引起的变化，无推力演化单独处理
    dtheta = (1 / (e_safe * h)) * (p * ca.cos(theta) * dv_R - (p + r_norm) * ca.sin(theta) * dv_T)
    # 乘以时间步长得到要素变化量
    return da*dt, de*dt, di*dt, dOmega*dt, domega*dt, dtheta*dt

def f(t0, state, k):
    du_kep = func_twobody(t0, state, k)
    return du_kep

def traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass):
    # Define initial orbit
    orb = Orbit.from_classical(Earth, init_coes[0] * u.km, init_coes[1] * u.one, init_coes[2] * u.rad, init_coes[3] * u.rad, init_coes[4] * u.rad, init_coes[5] * u.rad)

    # Constants
    N = int((raise_prop_duration.to(u.s)/raise_dt).value)  # Number of discrete time steps
    print(f"Number of Nodes: {N}")

    tofs = TimeDelta(np.linspace(0 * u.h, raise_prop_duration, num=N))
    rr, vv = orb.to_ephem(EpochsArray(orb.epoch + tofs, method=CowellPropagator(f=f))).rv()

    coe_final = init_coes.copy()
    coe_final[0] = coe_final[0] + 100  # 增大提升幅度，避免优化目标过弱

    coes_init_sol = np.zeros((N, 6))
    for i in range(N):
        coes_init_sol[i, :] = rv2coe(k, rr[i, :].value, vv[i, :].value)

    dv_max = maxThrust*raise_dt.value/mass  # 单位m/s
    print(f"Max Δv per step: {dv_max:.2f} m/s")

    # OPTIMIZATION PROCESS
    opti = ca.Opti()

    coe = opti.variable(6, N)  # 轨道要素序列
    DV = opti.variable(3, N-1) # 每个时间步的推力Δv（RTN坐标系）

    print(f"Optimization Variables: {coe.shape}, {DV.shape}")

    # 轨道演化约束：先无推力传播，再叠加GVE的推力变化
    for i in range(N-1):
        # 当前时刻轨道要素
        a = coe[0, i]
        e = coe[1, i]
        inc = coe[2, i]
        Omega = coe[3, i]
        omega = coe[4, i]
        theta = coe[5, i]
        
        r_norm = np.linalg.norm(rr[i, :].value)
        
        # 计算推力引起的轨道要素变化
        da, de, di, dOmega, domega, dtheta = gve(r_norm, a, e, inc, Omega, omega, theta, DV[0, i], DV[1, i], DV[2, i], raise_dt.value)

        # 无推力下的轨道要素演化（直接使用预计算的coes_init_sol[i+1]作为基准）
        coe_next = ca.vertcat(
            coes_init_sol[i+1,0] + da,
            coes_init_sol[i+1,1] + de,
            coes_init_sol[i+1,2] + di,
            coes_init_sol[i+1,3] + dOmega,
            coes_init_sol[i+1,4] + domega,
            coes_init_sol[i+1,5] + dtheta
        )
        opti.subject_to(coe[:, i+1] == coe_next)
        
        # 推力约束
        opti.subject_to(ca.norm_2(DV[:, i]) <= dv_max)
        # 轨道要素合理性约束
        opti.subject_to(coe[0, i] >= 6400.0)
        opti.subject_to(coe[1, i] >= 0.0)
        opti.subject_to(coe[1, i] <= 0.9)

    # 初始和终态条件
    for j in range(6):
        opti.subject_to(ca.fabs(coe[j, 0] - coes_init_sol[0, j]) <= 1e-5)
    opti.subject_to(ca.fabs(coe[0, N-1] - coe_final[0]) <= 1e-3)

    # 设置初始猜测
    for i in range(N):
        opti.set_initial(coe[:, i], coes_init_sol[i, :])
    opti.set_initial(DV, np.zeros((3, N-1)))  # 初始猜测为无推力

    # 目标：最小化总Δv模长
    total_dv = ca.sum([ca.norm_2(DV[:, i]) for i in range(N-1)])
    opti.minimize(total_dv)

    # 配置求解器
    opti.solver('ipopt', {
        'print_time': True, 
        'ipopt': {
            'tol': 1e-6,
            'max_iter': 300,
            'mu_strategy': 'adaptive',  
            'print_level': 3,
            'linear_solver': 'mumps'
        }
    })

    try:
        sol = opti.solve()
        # 提取结果
        a_solution = sol.value(coe[0, :])
        e_solution = sol.value(coe[1, :])
        dv_sol = sol.value(DV)

        print("Final SMA:", sol.value(coe[0, N-1]))
        print("Final eccentricity:", sol.value(coe[1, N-1]))
        print(f"Total Δv used: {np.sum(np.linalg.norm(dv_sol, axis=0)):.2f} m/s")
    except Exception as e:
        print("求解失败，错误信息:", str(e))
        # 打印当前迭代的中间结果用于调试
        print("当前初始条件满足情况:", opti.debug.value(ca.fabs(coe[:, 0] - coes_init_sol[0, :])))
        print("当前终态SMA误差:", opti.debug.value(ca.fabs(coe[0, N-1] - coe_final[0])))


if __name__ == "__main__":
    # INPUTS
    mass = 12           # kg
    maxThrust = 200     # N
    init_coes = np.array([7000, 1e-6, 98, 0, 270, 0])  # 添加小偏心率避免奇异
    raise_prop_duration = 3600*u.s  # 1小时提升时长
    raise_dt = 60*u.s   # 60秒时间步长

    init_coes[2] = np.radians(init_coes[2])
    init_coes[3] = np.radians(init_coes[3])
    init_coes[4] = np.radians(init_coes[4])
    init_coes[5] = np.radians(init_coes[5])

    traj_optim(init_coes, raise_prop_duration, raise_dt, maxThrust, mass)

核心问题修正说明 #

• 变量作用域修复：将时间步长dt作为参数传入GVE函数，解决外部变量无法访问的问题。
• GVE公式修正：
  • 移除无推力角速度项，GVE仅计算推力引起的轨道要素变化，无推力演化使用预计算的开普勒轨道作为基准。
  • 添加e_safe处理偏心率为0的奇异情况，避免除以0错误。
• 初始条件约束修复：修正初始约束的维度匹配问题，确保单个要素与初始值对应。
• 目标函数修正：将目标改为最小化Δv模长之和，符合燃料最小化的物理意义。
• 数值参数调整：增大轨道提升幅度和时长，避免优化目标过弱导致收敛困难；修正Δv单位显示错误。
• 积分逻辑优化：采用"开普勒基准+推力修正"的方式，提升数值稳定性和物理合理性。
• 错误处理添加：捕获求解异常并打印调试信息，便于定位问题。

内容的提问来源于stack exchange，提问作者NIKITA SEHRAWAT