matlab兰伯特问题求解器

兰伯特问题求解器，包括从给定的两个位置矢量（和可选的速度、时间）计算轨道根数的功能。

一、理论基础

兰伯特问题（Lambert's Problem）定义为：已知两个位置矢量 $\mathbf{r}_1$ 和 $\mathbf{r}_2$，以及从 $\mathbf{r}_1$ 到 $\mathbf{r}_2$ 的飞行时间 $\Delta t$，求连接这两点的轨道。

基本方程：

对于椭圆轨道，兰伯特定理表示为：

$\Delta t = \sqrt{\frac{a^3}{\mu}} \left[ (E_2 - E_1) - e(\sin E_2 - \sin E_1) \right]$

其中 (a) 是半长轴，(e) 是偏心率，(E) 是偏近点角。

二、MATLAB完整实现

1. 主求解函数

function [orbital_elements, v1, v2] = solve_lambert_problem(r1, r2, dt, mu, varargin)
    % 求解兰伯特问题并返回轨道根数
    %
    % 输入参数：
    %   r1 - 初始位置矢量 (3×1, km)
    %   r2 - 终点位置矢量 (3×1, km)
    %   dt - 飞行时间 (s)
    %   mu - 引力常数 (km^3/s^2, 默认地球: 398600.4418)
    %   varargin - 可选参数：
    %       'direction' - 转移方向 ('prograde' 或 'retrograde', 默认 'prograde')
    %       'solution' - 解的类型 ('short' 短路径, 'long' 长路径, 默认 'short')
    %       'tolerance' - 收敛容差 (默认 1e-10)
    %       'max_iter' - 最大迭代次数 (默认 100)
    %
    % 输出参数：
    %   orbital_elements - 轨道根数结构体
    %   v1 - 初始速度矢量 (km/s)
    %   v2 - 终点速度矢量 (km/s)
    
    %% 1. 参数解析
    % 设置默认值
    default_mu = 398600.4418;  % 地球引力常数 (km^3/s^2)
    default_direction = 'prograde';
    default_solution = 'short';
    default_tol = 1e-10;
    default_max_iter = 100;
    
    % 解析输入参数
    p = inputParser;
    addRequired(p, 'r1', @(x) validateattributes(x, {'numeric'}, {'size', [3,1]}));
    addRequired(p, 'r2', @(x) validateattributes(x, {'numeric'}, {'size', [3,1]}));
    addRequired(p, 'dt', @(x) validateattributes(x, {'numeric'}, {'scalar', 'positive'}));
    addOptional(p, 'mu', default_mu, @(x) validateattributes(x, {'numeric'}, {'scalar', 'positive'}));
    addParameter(p, 'direction', default_direction, @(x) ismember(x, {'prograde', 'retrograde'}));
    addParameter(p, 'solution', default_solution, @(x) ismember(x, {'short', 'long'}));
    addParameter(p, 'tolerance', default_tol, @(x) validateattributes(x, {'numeric'}, {'scalar', 'positive'}));
    addParameter(p, 'max_iter', default_max_iter, @(x) validateattributes(x, {'numeric'}, {'scalar', 'positive'}));
    
    parse(p, r1, r2, dt, mu, varargin{:});
    
    % 提取参数
    mu = p.Results.mu;
    direction = p.Results.direction;
    solution_type = p.Results.solution;
    tol = p.Results.tolerance;
    max_iter = p.Results.max_iter;
    
    %% 2. 计算几何参数
    fprintf('求解兰伯特问题...\n');
    fprintf('飞行时间: %.2f 秒 (%.2f 小时)\n', dt, dt/3600);
    fprintf('引力常数 mu: %.4f km^3/s^2\n', mu);
    
    % 位置矢量的模
    r1_norm = norm(r1);
    r2_norm = norm(r2);
    
    fprintf('初始位置半径: %.2f km\n', r1_norm);
    fprintf('终点位置半径: %.2f km\n', r2_norm);
    
    % 位置矢量的夹角（转移角）
    cos_dtheta = dot(r1, r2) / (r1_norm * r2_norm);
    cos_dtheta = max(min(cos_dtheta, 1), -1);  % 避免数值误差
    dtheta = acos(cos_dtheta);
    
    % 确定转移角（考虑方向）
    if strcmpi(direction, 'retrograde')
        dtheta = 2*pi - dtheta;
    end
    
    fprintf('转移角: %.2f 度\n', rad2deg(dtheta));
    
    %% 3. 计算兰伯特参数
    % 弦长
    c = norm(r2 - r1);
    s = (r1_norm + r2_norm + c) / 2;
    
    fprintf('弦长: %.2f km\n', c);
    fprintf('半周长: %.2f km\n', s);
    
    % 转移角的正负号（用于多值解）
    if dtheta > pi
        long_way = true;
    else
        long_way = false;
    end
    
    % 根据解类型调整
    if strcmpi(solution_type, 'long')
        long_way = ~long_way;
    end
    
    % 计算参数 A
    sin_dtheta = sin(dtheta);
    A = sin_dtheta * sqrt(r1_norm * r2_norm / (1 - cos_dtheta));
    
    if long_way
        A = -A;
    end
    
    %% 4. 求解兰伯特方程（使用普适变量法）
    % 初始猜测
    if A > 0
        y_initial = 0;
    else
        y_initial = -2*A;
    end
    
    % 迭代求解
    [y, converged, iterations] = solve_lambert_equation(y_initial, r1_norm, r2_norm, A, dt, mu, tol, max_iter);
    
    if ~converged
        warning('兰伯特方程迭代未收敛，使用最后一次迭代结果');
    end
    
    fprintf('迭代求解完成，迭代次数: %d\n', iterations);
    
    %% 5. 计算速度矢量
    % 计算轨道参数
    f = 1 - y / r1_norm;
    g = A * sqrt(y / mu);
    g_dot = 1 - y / r2_norm;
    
    % 计算速度矢量
    v1 = (r2 - f * r1) / g;
    v2 = (g_dot * r2 - r1) / g;
    
    fprintf('初始速度大小: %.4f km/s\n', norm(v1));
    fprintf('终点速度大小: %.4f km/s\n', norm(v2));
    
    %% 6. 计算轨道根数
    orbital_elements = rv2oe(r1, v1, mu);
    
    %% 7. 验证结果
    verify_lambert_solution(r1, r2, v1, v2, dt, mu, orbital_elements);
    
    fprintf('兰伯特问题求解完成！\n');
end

function [y, converged, iter] = solve_lambert_equation(y0, r1, r2, A, dt, mu, tol, max_iter)
    % 使用牛顿-拉弗森法求解兰伯特方程
    
    y = y0;
    converged = false;
    
    for iter = 1:max_iter
        % 计算当前y值的函数值和导数值
        [f_val, df_dy] = lambert_f_function(y, r1, r2, A, dt, mu);
        
        % 牛顿更新
        dy = -f_val / df_dy;
        y_new = y + dy;
        
        % 检查收敛性
        if abs(dy) < tol
            y = y_new;
            converged = true;
            break;
        end
        
        % 防止y值超出合理范围
        if y_new < -2*A && A < 0
            y_new = -2*A + 0.1;
        end
        
        y = y_new;
        
        % 每10次迭代显示进度
        if mod(iter, 10) == 0
            fprintf('  迭代 %d: y = %.6e, f(y) = %.6e\n', iter, y, f_val);
        end
    end
    
    if iter >= max_iter
        warning('达到最大迭代次数，可能未收敛');
    end
end

function [f, df_dy] = lambert_f_function(y, r1, r2, A, dt, mu)
    % 兰伯特方程的函数形式及其导数
    
    % 计算x
    if y > 0
        x = sqrt(y);
        sin_sqrt_y = sin(x);
        cos_sqrt_y = cos(x);
        S = (x - sin_sqrt_y) / (x^3);
        C = (1 - cos_sqrt_y) / y;
    elseif y < 0
        x = sqrt(-y);
        sinh_x = sinh(x);
        cosh_x = cosh(x);
        S = (x - sinh_x) / (x^3);
        C = (1 - cosh_x) / y;
    else % y = 0
        S = 1/6;
        C = 1/2;
    end
    
    % 计算函数值
    Q = sqrt(A^2 * C);
    if A > 0
        f = (Q^3 * S + A * sqrt(y)) / sqrt(mu) - dt;
    else
        f = -(Q^3 * S + A * sqrt(y)) / sqrt(mu) - dt;
    end
    
    % 计算导数
    if y ~= 0
        if y > 0
            dS_dy = (3*(sin_sqrt_y - x) + x^3) / (2 * x^5);
            dC_dy = (y*sin_sqrt_y/(2*x) - 2*(1-cos_sqrt_y)) / (y^2);
        else
            dS_dy = (3*(sinh_x - x) + x^3) / (2 * x^5);
            dC_dy = (y*sinh_x/(2*x) - 2*(1-cosh_x)) / (y^2);
        end
        
        dQ_dy = A^2 * dC_dy / (2*Q);
        df_dy = (3*Q^2*dQ_dy*S + Q^3*dS_dy + A/(2*sqrt(y))) / sqrt(mu);
    else
        df_dy = (1/40) * A^3 / sqrt(mu) + A/(2*sqrt(mu));
    end
end

function oe = rv2oe(r, v, mu)
    % 从位置和速度矢量计算经典轨道根数
    % 输入：r, v (3×1 矢量), mu (引力常数)
    % 输出：轨道根数结构体
    
    %% 1. 计算基本矢量
    h = cross(r, v);          % 角动量矢量
    h_norm = norm(h);
    
    % 节点矢量
    n = cross([0; 0; 1], h);
    n_norm = norm(n);
    
    %% 2. 计算轨道平面参数
    % 半长轴 a
    r_norm = norm(r);
    v_norm = norm(v);
    energy = v_norm^2/2 - mu/r_norm;  % 比机械能
    
    if abs(energy) < 1e-10
        a = inf;  % 抛物线
    else
        a = -mu/(2*energy);
    end
    
    %% 3. 计算偏心率矢量
    e_vec = ((v_norm^2 - mu/r_norm)*r - dot(r, v)*v)/mu;
    e = norm(e_vec);  % 偏心率大小
    
    %% 4. 计算轨道倾角 i
    i = acos(h(3)/h_norm);
    
    %% 5. 计算升交点赤经 Ω
    if n_norm > 0
        Omega = acos(n(1)/n_norm);
        if n(2) < 0
            Omega = 2*pi - Omega;
        end
    else
        Omega = 0;  % 赤道轨道
    end
    
    %% 6. 计算近地点幅角 ω
    if n_norm > 0 && e > 0
        omega = acos(dot(n, e_vec)/(n_norm*e));
        if e_vec(3) < 0
            omega = 2*pi - omega;
        end
    else
        omega = 0;
    end
    
    %% 7. 计算真近点角 ν
    if e > 0
        nu = acos(dot(e_vec, r)/(e*r_norm));
        if dot(r, v) < 0
            nu = 2*pi - nu;
        end
    else
        % 圆轨道，近地点未定义
        nu = acos(dot(n, r)/(n_norm*r_norm));
        if r(3) < 0
            nu = 2*pi - nu;
        end
    end
    
    %% 8. 计算平近点角 M 和偏近点角 E
    if e < 1 && ~isinf(a)  % 椭圆轨道
        % 计算偏近点角 E
        cosE = (e + cos(nu))/(1 + e*cos(nu));
        sinE = sqrt(1 - e^2)*sin(nu)/(1 + e*cos(nu));
        E = atan2(sinE, cosE);
        
        % 计算平近点角 M
        M = E - e*sinE;
        
        % 周期
        T = 2*pi*sqrt(a^3/mu);
    elseif e > 1 && ~isinf(a)  % 双曲线轨道
        % 双曲线偏近点角 F
        coshF = (e + cos(nu))/(1 + e*cos(nu));
        F = acosh(coshF);
        if nu < 0
            F = -F;
        end
        
        % 双曲线平近点角 N
        N = e*sinh(F) - F;
        M = N;  % 对于双曲线，常用 N 表示
        
        % 周期概念不适用于双曲线
        T = inf;
    else  % 抛物线
        E = nan;
        M = nan;
        T = inf;
    end
    
    %% 9. 返回轨道根数结构体
    oe = struct();
    oe.a = a;                     % 半长轴 (km)
    oe.e = e;                     % 偏心率
    oe.i = rad2deg(i);            % 轨道倾角 (度)
    oe.Omega = rad2deg(Omega);    % 升交点赤经 (度)
    oe.omega = rad2deg(omega);    % 近地点幅角 (度)
    oe.nu = rad2deg(nu);          % 真近点角 (度)
    oe.E = rad2deg(E);            % 偏近点角 (度)
    oe.M = rad2deg(M);            % 平近点角 (度)
    oe.T = T;                     % 轨道周期 (s)
    oe.h = h_norm;                % 角动量大小
    oe.energy = energy;           % 比机械能
    oe.type = get_orbit_type(e, a);  % 轨道类型
    
    % 额外参数
    oe.r_periapsis = a*(1 - e);   % 近地点半径 (km)
    oe.r_apoapsis = a*(1 + e);    % 远地点半径 (km)
    if ~isinf(a)
        oe.period = T/3600;       % 周期 (小时)
    else
        oe.period = inf;
    end
end

function orbit_type = get_orbit_type(e, a)
    % 判断轨道类型
    if e < 1e-10
        orbit_type = 'Circular';
    elseif e < 1
        orbit_type = 'Elliptical';
    elseif abs(e - 1) < 1e-6
        orbit_type = 'Parabolic';
    else
        orbit_type = 'Hyperbolic';
    end
    
    % 特殊地球轨道
    if strcmp(orbit_type, 'Elliptical')
        if a * (1 - e) < 6578  % 低于200km
            orbit_type = 'Low Earth Orbit (LEO)';
        elseif a * (1 - e) < 42164 && a * (1 + e) > 42164  % 穿越GEO
            orbit_type = 'Geostationary Transfer Orbit (GTO)';
        elseif abs(a * (1 - e) - 42164) < 100  % GEO附近
            orbit_type = 'Geostationary Orbit (GEO)';
        end
    end
end

function verify_lambert_solution(r1, r2, v1, v2, dt, mu, oe)
    % 验证兰伯特问题求解结果
    
    fprintf('\n=== 验证求解结果 ===\n');
    
    % 1. 验证位置矢量
    fprintf('1. 位置矢量验证:\n');
    fprintf('   初始位置给定: [%.2f, %.2f, %.2f] km\n', r1);
    fprintf('   终点位置给定: [%.2f, %.2f, %.2f] km\n', r2);
    
    % 2. 验证能量守恒
    energy1 = norm(v1)^2/2 - mu/norm(r1);
    energy2 = norm(v2)^2/2 - mu/norm(r2);
    energy_diff = abs(energy1 - energy2);
    
    fprintf('2. 能量守恒验证:\n');
    fprintf('   初始比机械能: %.6f km^2/s^2\n', energy1);
    fprintf('   终点比机械能: %.6f km^2/s^2\n', energy2);
    fprintf('   能量差异: %.2e km^2/s^2\n', energy_diff);
    
    if energy_diff < 1e-6
        fprintf('   ✓ 能量守恒验证通过\n');
    else
        fprintf('   ⚠ 能量差异较大\n');
    end
    
    % 3. 验证轨道根数的一致性
    fprintf('3. 轨道根数验证:\n');
    fprintf('   轨道类型: %s\n', oe.type);
    fprintf('   半长轴 a: %.2f km\n', oe.a);
    fprintf('   偏心率 e: %.6f\n', oe.e);
    
    if oe.e < 1 && ~isinf(oe.a)
        fprintf('   近地点半径: %.2f km\n', oe.r_periapsis);
        fprintf('   远地点半径: %.2f km\n', oe.r_apoapsis);
        fprintf('   轨道周期: %.2f 小时\n', oe.period);
    end
    
    % 4. 验证飞行时间
    fprintf('4. 飞行时间验证:\n');
    fprintf('   给定飞行时间: %.2f 秒\n', dt);
    
    % 计算预测飞行时间（对于椭圆轨道）
    if oe.e < 1 && ~isinf(oe.a)
        % 计算初始和终点的偏近点角
        nu1 = deg2rad(oe.nu);
        nu2 = deg2rad(mod(oe.nu + rad2deg(acos(dot(r1, r2)/(norm(r1)*norm(r2)))), 360));
        
        E1 = 2 * atan(sqrt((1 - oe.e)/(1 + oe.e)) * tan(nu1/2));
        E2 = 2 * atan(sqrt((1 - oe.e)/(1 + oe.e)) * tan(nu2/2));
        
        dt_calc = sqrt(oe.a^3/mu) * ((E2 - E1) - oe.e*(sin(E2) - sin(E1)));
        fprintf('   计算飞行时间: %.2f 秒\n', dt_calc);
        fprintf('   时间差异: %.2e 秒\n', abs(dt - dt_calc));
    end
    
    fprintf('=== 验证完成 ===\n\n');
end

2. 可视化函数

function visualize_lambert_solution(r1, r2, v1, v2, oe, mu, dt)
    % 可视化兰伯特问题求解结果
    
    fprintf('生成可视化...\n');
    
    % 创建图形窗口
    figure('Position', [100, 100, 1400, 900]);
    
    %% 子图1: 三维轨道图
    subplot(2, 3, [1, 2, 4, 5]);
    hold on;
    grid on;
    axis equal;
    
    % 绘制地球
    [X, Y, Z] = sphere(50);
    R_earth = 6378.137;  % 地球半径 (km)
    surf(R_earth*X, R_earth*Y, R_earth*Z, 'FaceAlpha', 0.3, ...
        'EdgeColor', 'none', 'FaceColor', [0.1, 0.5, 0.8]);
    
    % 绘制坐标系
    plot3([0, 2*R_earth], [0, 0], [0, 0], 'r-', 'LineWidth', 2);  % X轴
    plot3([0, 0], [0, 2*R_earth], [0, 0], 'g-', 'LineWidth', 2);  % Y轴
    plot3([0, 0], [0, 0], [0, 2*R_earth], 'b-', 'LineWidth', 2);  % Z轴
    text(2*R_earth, 0, 0, 'X', 'FontSize', 12);
    text(0, 2*R_earth, 0, 'Y', 'FontSize', 12);
    text(0, 0, 2*R_earth, 'Z', 'FontSize', 12);
    
    % 绘制完整的轨道
    if oe.e < 1 && ~isinf(oe.a)  % 椭圆轨道
        % 生成轨道点
        nu_points = linspace(0, 2*pi, 200);
        [orbit_points, ~] = oe2rv(oe, nu_points, mu);
        plot3(orbit_points(1,:), orbit_points(2,:), orbit_points(3,:), ...
            'b-', 'LineWidth', 2);
        
        % 标记近地点和远地点
        [r_peri, ~] = oe2rv(oe, 0, mu);
        [r_apo, ~] = oe2rv(oe, pi, mu);
        plot3(r_peri(1), r_peri(2), r_peri(3), 'go', 'MarkerSize', 10, ...
            'MarkerFaceColor', 'g', 'DisplayName', '近地点');
        plot3(r_apo(1), r_apo(2), r_apo(3), 'ro', 'MarkerSize', 10, ...
            'MarkerFaceColor', 'r', 'DisplayName', '远地点');
    else
        % 对于非椭圆轨道，只绘制转移段
        fprintf('   非椭圆轨道，仅绘制转移段\n');
    end
    
    % 绘制转移弧段
    t_points = linspace(0, dt, 100);
    [transfer_points, ~] = propagate_orbit(r1, v1, t_points, mu);
    plot3(transfer_points(1,:), transfer_points(2,:), transfer_points(3,:), ...
        'r-', 'LineWidth', 3, 'DisplayName', '转移轨道');
    
    % 标记初始和终点位置
    plot3(r1(1), r1(2), r1(3), 'ko', 'MarkerSize', 12, ...
        'MarkerFaceColor', 'k', 'DisplayName', '初始位置');
    plot3(r2(1), r2(2), r2(3), 'mo', 'MarkerSize', 12, ...
        'MarkerFaceColor', 'm', 'DisplayName', '终点位置');
    
    % 绘制速度矢量
    scale = 500;  % 速度矢量缩放因子
    quiver3(r1(1), r1(2), r1(3), v1(1)*scale, v1(2)*scale, v1(3)*scale, ...
        'r', 'LineWidth', 2, 'MaxHeadSize', 0.5, 'DisplayName', '初始速度');
    quiver3(r2(1), r2(2), r2(3), v2(1)*scale, v2(2)*scale, v2(3)*scale, ...
        'm', 'LineWidth', 2, 'MaxHeadSize', 0.5, 'DisplayName', '终点速度');
    
    % 设置图形属性
    xlabel('X (km)', 'FontSize', 12);
    ylabel('Y (km)', 'FontSize', 12);
    zlabel('Z (km)', 'FontSize', 12);
    title('兰伯特问题三维轨道图', 'FontSize', 14);
    legend('Location', 'best');
    view(45, 30);  % 设置视角
    
    %% 子图2: 轨道根数摘要
    subplot(2, 3, 3);
    axis off;
    
    % 显示轨道参数
    text_str = {
        sprintf('轨道类型: %s', oe.type);
        sprintf('半长轴 a: %.2f km', oe.a);
        sprintf('偏心率 e: %.6f', oe.e);
        sprintf('轨道倾角 i: %.2f°', oe.i);
        sprintf('升交点赤经 Ω: %.2f°', oe.Omega);
        sprintf('近地点幅角 ω: %.2f°', oe.omega);
        sprintf('真近点角 ν: %.2f°', oe.nu);
        sprintf('飞行时间: %.1f s', dt);
        sprintf('初始速度: %.4f km/s', norm(v1));
        sprintf('终点速度: %.4f km/s', norm(v2));
    };
    
    if oe.e < 1 && ~isinf(oe.a)
        text_str = [text_str; {
            sprintf('近地点半径: %.2f km', oe.r_periapsis);
            sprintf('远地点半径: %.2f km', oe.r_apoapsis);
            sprintf('轨道周期: %.2f h', oe.period);
        }];
    end
    
    text(0.1, 0.9, text_str, 'FontSize', 10, 'VerticalAlignment', 'top');
    title('轨道参数摘要', 'FontSize', 12);
    
    %% 子图3: 速度与能量图
    subplot(2, 3, 6);
    hold on;
    grid on;
    
    % 计算轨道上的速度分布
    if oe.e < 1 && ~isinf(oe.a)
        nu_vals = linspace(0, 2*pi, 100);
        v_vals = zeros(size(nu_vals));
        
        for i = 1:length(nu_vals)
            [~, v_temp] = oe2rv(oe, nu_vals(i), mu);
            v_vals(i) = norm(v_temp);
        end
        
        % 绘制速度分布
        plot(rad2deg(nu_vals), v_vals, 'b-', 'LineWidth', 2);
        
        % 标记转移段的起点和终点
        nu1 = deg2rad(oe.nu);
        nu2 = mod(nu1 + acos(dot(r1, r2)/(norm(r1)*norm(r2))), 2*pi);
        
        [~, v1_calc] = oe2rv(oe, nu1, mu);
        [~, v2_calc] = oe2rv(oe, nu2, mu);
        
        plot(rad2deg([nu1, nu2]), [norm(v1_calc), norm(v2_calc)], 'ro', ...
            'MarkerSize', 10, 'MarkerFaceColor', 'r');
        
        xlabel('真近点角 ν (°)', 'FontSize', 12);
        ylabel('速度大小 (km/s)', 'FontSize', 12);
        title('轨道速度分布', 'FontSize', 12);
        legend('轨道速度', '转移段端点', 'Location', 'best');
    else
        text(0.5, 0.5, '非椭圆轨道，速度图不可用', ...
            'HorizontalAlignment', 'center', 'FontSize', 12);
        axis off;
    end
    
    %% 添加总标题
    sgtitle(sprintf('兰伯特问题求解结果 (飞行时间: %.1f 秒)', dt), ...
        'FontSize', 16, 'FontWeight', 'bold');
    
    fprintf('可视化生成完成！\n');
end

function [r, v] = oe2rv(oe, nu, mu)
    % 从轨道根数计算位置和速度矢量
    % 输入：oe - 轨道根数结构体
    %       nu - 真近点角 (弧度)
    %       mu - 引力常数
    
    % 提取轨道参数
    a = oe.a;
    e = oe.e;
    i = deg2rad(oe.i);
    Omega = deg2rad(oe.Omega);
    omega = deg2rad(oe.omega);
    
    % 计算轨道半径
    r_mag = a * (1 - e^2) / (1 + e * cos(nu));
    
    % 在轨道平面内的坐标
    r_pf = [r_mag * cos(nu); r_mag * sin(nu); 0];
    v_pf = sqrt(mu/(a*(1-e^2))) * [-sin(nu); e + cos(nu); 0];
    
    % 旋转矩阵：从轨道平面到地心惯性系
    R3_Omega = [cos(Omega), sin(Omega), 0;
               -sin(Omega), cos(Omega), 0;
                0, 0, 1];
    
    R1_i = [1, 0, 0;
            0, cos(i), sin(i);
            0, -sin(i), cos(i)];
    
    R3_omega = [cos(omega), sin(omega), 0;
               -sin(omega), cos(omega), 0;
                0, 0, 1];
    
    % 组合旋转矩阵
    R = R3_Omega' * R1_i' * R3_omega';
    
    % 转换到地心惯性系
    r = R * r_pf;
    v = R * v_pf;
end

function [positions, velocities] = propagate_orbit(r0, v0, t_vec, mu)
    % 使用开普勒方程传播轨道
    % 输入：r0, v0 - 初始状态
    %       t_vec - 时间向量
    %       mu - 引力常数
    
    % 计算初始轨道根数
    oe0 = rv2oe(r0, v0, mu);
    
    positions = zeros(3, length(t_vec));
    velocities = zeros(3, length(t_vec));
    
    % 对于每个时间点，计算相应的真近点角
    for k = 1:length(t_vec)
        t = t_vec(k);
        
        % 计算平近点角
        if oe0.e < 1  % 椭圆轨道
            n = sqrt(mu/oe0.a^3);  % 平均角速度
            M = mod(deg2rad(oe0.M) + n*t, 2*pi);
            
            % 解开普勒方程求偏近点角
            E = solve_kepler_equation(M, oe0.e);
            
            % 计算真近点角
            nu = 2 * atan(sqrt((1+oe0.e)/(1-oe0.e)) * tan(E/2));
        else  % 双曲线轨道（简化处理）
            % 这里只返回初始状态
            nu = deg2rad(oe0.nu);
        end
        
        % 计算位置和速度
        [r, v] = oe2rv(oe0, nu, mu);
        positions(:, k) = r;
        velocities(:, k) = v;
    end
end

function E = solve_kepler_equation(M, e)
    % 解开普勒方程 E - e*sin(E) = M
    % 使用牛顿-拉弗森法
    
    if M < 0
        M = M + 2*pi;
    end
    
    % 初始猜测
    if e < 0.8
        E = M;
    else
        E = pi;
    end
    
    % 迭代求解
    max_iter = 50;
    tol = 1e-12;
    
    for iter = 1:max_iter
        f = E - e*sin(E) - M;
        f_prime = 1 - e*cos(E);
        
        dE = -f / f_prime;
        E = E + dE;
        
        if abs(dE) < tol
            break;
        end
    end
end

3. 测试和验证脚本

%% 兰伯特问题测试脚本
function test_lambert_solver()
    % 测试兰伯特问题求解器
    
    fprintf('=== 兰伯特问题求解器测试 ===\n\n');
    
    % 地球引力常数 (km^3/s^2)
    mu = 398600.4418;
    
    %% 测试1: 地球同步转移轨道 (GTO)
    fprintf('测试1: 地球同步转移轨道 (GTO)\n');
    
    % 初始位置：近地点 200 km
    r1 = [6578; 0; 0];  % 200 km 高度，赤道上
    % 终点位置：地球同步轨道 35786 km 高度
    r2 = [0; 42164; 0];  % GEO，赤道上空
    
    % 飞行时间：约 5.3 小时（霍曼转移）
    dt = 5.3 * 3600;
    
    % 求解兰伯特问题
    [oe1, v1, v2] = solve_lambert_problem(r1, r2, dt, mu, ...
        'direction', 'prograde', 'solution', 'short');
    
    % 可视化
    visualize_lambert_solution(r1, r2, v1, v2, oe1, mu, dt);
    
    %% 测试2: 月球转移轨道
    fprintf('\n测试2: 月球转移轨道\n');
    
    % 初始位置：低地球轨道 400 km
    r1 = [6778; 0; 0];  % 400 km 高度
    
    % 终点位置：月球距离 (平均距离 384400 km)
    % 假设在黄道面上
    r2 = [384400; 0; 0];
    
    % 飞行时间：约 3 天
    dt = 3 * 24 * 3600;
    
    % 求解兰伯特问题
    [oe2, v1_moon, v2_moon] = solve_lambert_problem(r1, r2, dt, mu, ...
        'direction', 'prograde', 'solution', 'short');
    
    figure;
    visualize_lambert_solution(r1, r2, v1_moon, v2_moon, oe2, mu, dt);
    
    %% 测试3: 星际转移（简化）
    fprintf('\n测试3: 火星转移轨道（简化）\n');
    
    % 使用简化模型：地球和火星的日心轨道
    mu_sun = 1.32712440018e11;  % 太阳引力常数 (km^3/s^2)
    
    % 地球位置（假设在近日点）
    r_earth = [1.4710e8; 0; 0];  % km
    
    % 火星位置（平均距离）
    r_mars = [2.2794e8; 0; 0];  % km
    
    % 飞行时间：约 8 个月
    dt_mars = 8 * 30 * 24 * 3600;  % 秒
    
    % 求解兰伯特问题
    [oe3, v1_mars, v2_mars] = solve_lambert_problem(r_earth, r_mars, dt_mars, mu_sun, ...
        'direction', 'prograde', 'solution', 'short');
    
    figure;
    visualize_lambert_solution(r_earth, r_mars, v1_mars, v2_mars, oe3, mu_sun, dt_mars);
    
    %% 测试4: 多解验证（短路径 vs 长路径）
    fprintf('\n测试4: 多解验证\n');
    
    % 相同的位置，不同的飞行时间
    r1_test = [7000; 0; 0];
    r2_test = [0; 7000; 0];
    
    % 飞行时间：1 小时
    dt_test = 3600;
    
    % 短路径解
    [oe_short, v1_short, v2_short] = solve_lambert_problem(r1_test, r2_test, dt_test, mu, ...
        'solution', 'short');
    
    % 长路径解
    [oe_long, v1_long, v2_long] = solve_lambert_problem(r1_test, r2_test, dt_test, mu, ...
        'solution', 'long');
    
    % 比较结果
    fprintf('\n短路径解:\n');
    fprintf('  初始速度: [%.4f, %.4f, %.4f] km/s\n', v1_short);
    fprintf('  终点速度: [%.4f, %.4f, %.4f] km/s\n', v2_short);
    fprintf('  半长轴: %.2f km\n', oe_short.a);
    
    fprintf('\n长路径解:\n');
    fprintf('  初始速度: [%.4f, %.4f, %.4f] km/s\n', v1_long);
    fprintf('  终点速度: [%.4f, %.4f, %.4f] km/s\n', v2_long);
    fprintf('  半长轴: %.2f km\n', oe_long.a);
    
    fprintf('\n=== 所有测试完成 ===\n');
end

%% 性能测试和精度验证
function accuracy_test()
    % 测试兰伯特问题求解器的精度
    
    fprintf('=== 精度测试 ===\n');
    
    mu = 398600.4418;
    
    % 测试用例1: 圆轨道
    fprintf('\n测试用例1: 圆轨道\n');
    r1 = [7000; 0; 0];
    r2 = [0; 7000; 0];
    
    % 对于圆轨道，90度转移需要的时间
    a = 7000;
    dt_expected = (pi/2) * sqrt(a^3/mu);  % 四分之一周期
    
    [oe, v1, v2] = solve_lambert_problem(r1, r2, dt_expected, mu);
    
    % 验证速度
    v_circular = sqrt(mu/a);
    error_v1 = norm(v1) - v_circular;
    error_v2 = norm(v2) - v_circular;
    
    fprintf('  圆轨道速度理论值: %.6f km/s\n', v_circular);
    fprintf('  初始速度误差: %.6e km/s\n', error_v1);
    fprintf('  终点速度误差: %.6e km/s\n', error_v2);
    
    % 测试用例2: 椭圆轨道
    fprintf('\n测试用例2: 椭圆轨道\n');
    
    % 定义椭圆轨道参数
    a_test = 10000;
    e_test = 0.5;
    i_test = 30;
    Omega_test = 45;
    omega_test = 60;
    nu1_test = 0;
    nu2_test = 90;
    
    % 计算两个位置
    oe_test = struct();
    oe_test.a = a_test;
    oe_test.e = e_test;
    oe_test.i = i_test;
    oe_test.Omega = Omega_test;
    oe_test.omega = omega_test;
    
    [r1_test, v1_test] = oe2rv(oe_test, deg2rad(nu1_test), mu);
    [r2_test, v2_expected] = oe2rv(oe_test, deg2rad(nu2_test), mu);
    
    % 计算飞行时间
    E1 = 2 * atan(sqrt((1-e_test)/(1+e_test)) * tan(deg2rad(nu1_test)/2));
    E2 = 2 * atan(sqrt((1-e_test)/(1+e_test)) * tan(deg2rad(nu2_test)/2));
    dt_test = sqrt(a_test^3/mu) * ((E2 - E1) - e_test*(sin(E2) - sin(E1)));
    
    fprintf('  理论飞行时间: %.2f 秒\n', dt_test);
    
    % 求解兰伯特问题
    [oe_calc, v1_calc, v2_calc] = solve_lambert_problem(r1_test, r2_test, dt_test, mu);
    
    % 验证结果
    error_v2_norm = norm(v2_calc - v2_expected);
    fprintf('  终点速度误差范数: %.6e km/s\n', error_v2_norm);
    fprintf('  半长轴误差: %.6e km\n', oe_calc.a - a_test);
    fprintf('  偏心率误差: %.6e\n', oe_calc.e - e_test);
    
    % 测试用例3: 极端情况（大偏心率）
    fprintf('\n测试用例3: 大偏心率椭圆轨道\n');
    
    a_extreme = 50000;
    e_extreme = 0.9;
    nu1_extreme = 10;
    nu2_extreme = 170;
    
    oe_extreme = struct();
    oe_extreme.a = a_extreme;
    oe_extreme.e = e_extreme;
    oe_extreme.i = 0;
    oe_extreme.Omega = 0;
    oe_extreme.omega = 0;
    
    [r1_extreme, ~] = oe2rv(oe_extreme, deg2rad(nu1_extreme), mu);
    [r2_extreme, v2_extreme_expected] = oe2rv(oe_extreme, deg2rad(nu2_extreme), mu);
    
    E1_extreme = 2 * atan(sqrt((1-e_extreme)/(1+e_extreme)) * tan(deg2rad(nu1_extreme)/2));
    E2_extreme = 2 * atan(sqrt((1-e_extreme)/(1+e_extreme)) * tan(deg2rad(nu2_extreme)/2));
    dt_extreme = sqrt(a_extreme^3/mu) * ((E2_extreme - E1_extreme) - ...
        e_extreme*(sin(E2_extreme) - sin(E1_extreme)));
    
    [oe_extreme_calc, ~, v2_extreme_calc] = solve_lambert_problem(...
        r1_extreme, r2_extreme, dt_extreme, mu);
    
    error_v2_extreme = norm(v2_extreme_calc - v2_extreme_expected);
    fprintf('  大偏心率轨道终点速度误差: %.6e km/s\n', error_v2_extreme);
    
    fprintf('\n=== 精度测试完成 ===\n');
end

4. 实用工具函数

%% 实用工具函数
function [r1, r2, dt] = get_input_from_user()
    % 从用户获取输入
    
    fprintf('=== 兰伯特问题求解器输入 ===\n\n');
    
    % 引力常数选择
    fprintf('选择引力常数:\n');
    fprintf('1. 地球 (398600.4418 km^3/s^2)\n');
    fprintf('2. 月球 (4902.8 km^3/s^2)\n');
    fprintf('3. 火星 (42828 km^3/s^2)\n');
    fprintf('4. 太阳 (1.3271244e11 km^3/s^2)\n');
    fprintf('5. 自定义\n');
    
    choice = input('请输入选择 (1-5): ');
    
    switch choice
        case 1
            mu = 398600.4418;
            body = '地球';
        case 2
            mu = 4902.8;
            body = '月球';
        case 3
            mu = 42828;
            body = '火星';
        case 4
            mu = 1.3271244e11;
            body = '太阳';
        case 5
            mu = input('请输入引力常数 (km^3/s^2): ');
            body = '自定义';
        otherwise
            mu = 398600.4418;
            body = '地球';
    end
    
    fprintf('\n引力常数: %.4e km^3/s^2 (%s)\n', mu, body);
    
    % 输入初始位置
    fprintf('\n输入初始位置 (km):\n');
    r1_x = input('  X坐标: ');
    r1_y = input('  Y坐标: ');
    r1_z = input('  Z坐标: ');
    r1 = [r1_x; r1_y; r1_z];
    
    % 输入终点位置
    fprintf('\n输入终点位置 (km):\n');
    r2_x = input('  X坐标: ');
    r2_y = input('  Y坐标: ');
    r2_z = input('  Z坐标: ');
    r2 = [r2_x; r2_y; r2_z];
    
    % 输入飞行时间
    fprintf('\n输入飞行时间:\n');
    dt_unit = input('  单位 (1=秒, 2=分, 3=小时, 4=天): ');
    dt_value = input('  数值: ');
    
    switch dt_unit
        case 1
            dt = dt_value;
        case 2
            dt = dt_value * 60;
        case 3
            dt = dt_value * 3600;
        case 4
            dt = dt_value * 24 * 3600;
        otherwise
            dt = dt_value;
    end
    
    fprintf('\n飞行时间: %.1f 秒 (%.2f 小时)\n', dt, dt/3600);
    
    % 输入转移方向
    fprintf('\n选择转移方向:\n');
    fprintf('1. 顺行 (prograde)\n');
    fprintf('2. 逆行 (retrograde)\n');
    direction_choice = input('请输入选择 (1-2): ');
    
    if direction_choice == 2
        direction = 'retrograde';
    else
        direction = 'prograde';
    end
    
    % 输入解类型
    fprintf('\n选择解类型:\n');
    fprintf('1. 短路径 (short)\n');
    fprintf('2. 长路径 (long)\n');
    solution_choice = input('请输入选择 (1-2): ');
    
    if solution_choice == 2
        solution = 'long';
    else
        solution = 'short';
    end
    
    % 返回结果
    fprintf('\n=== 输入完成 ===\n');
end

function export_results(oe, v1, v2, r1, r2, dt, mu, filename)
    % 导出结果到文件
    
    if nargin < 8
        timestamp = datestr(now, 'yyyymmdd_HHMMSS');
        filename = sprintf('lambert_results_%s.txt', timestamp);
    end
    
    fid = fopen(filename, 'w');
    
    fprintf(fid, '=== 兰伯特问题求解结果 ===\n\n');
    fprintf(fid, '生成时间: %s\n\n', datestr(now));
    
    fprintf(fid, '输入参数:\n');
    fprintf(fid, '  引力常数 mu: %.4f km^3/s^2\n', mu);
    fprintf(fid, '  飞行时间: %.1f 秒 (%.2f 小时)\n\n', dt, dt/3600);
    
    fprintf(fid, '初始位置矢量 (km):\n');
    fprintf(fid, '  [%.4f, %.4f, %.4f]\n', r1);
    fprintf(fid, '  半径: %.2f km\n\n', norm(r1));
    
    fprintf(fid, '终点位置矢量 (km):\n');
    fprintf(fid, '  [%.4f, %.4f, %.4f]\n', r2);
    fprintf(fid, '  半径: %.2f km\n\n', norm(r2));
    
    fprintf(fid, '计算结果:\n');
    fprintf(fid, '  初始速度矢量 (km/s):\n');
    fprintf(fid, '    [%.6f, %.6f, %.6f]\n', v1);
    fprintf(fid, '    大小: %.6f km/s\n\n', norm(v1));
    
    fprintf(fid, '  终点速度矢量 (km/s):\n');
    fprintf(fid, '    [%.6f, %.6f, %.6f]\n', v2);
    fprintf(fid, '    大小: %.6f km/s\n\n', norm(v2));
    
    fprintf(fid, '轨道根数:\n');
    fprintf(fid, '  轨道类型: %s\n', oe.type);
    fprintf(fid, '  半长轴 a: %.4f km\n', oe.a);
    fprintf(fid, '  偏心率 e: %.8f\n', oe.e);
    fprintf(fid, '  轨道倾角 i: %.4f°\n', oe.i);
    fprintf(fid, '  升交点赤经 Ω: %.4f°\n', oe.Omega);
    fprintf(fid, '  近地点幅角 ω: %.4f°\n', oe.omega);
    fprintf(fid, '  真近点角 ν: %.4f°\n', oe.nu);
    
    if oe.e < 1 && ~isinf(oe.a)
        fprintf(fid, '  近地点半径: %.2f km\n', oe.r_periapsis);
        fprintf(fid, '  远地点半径: %.2f km\n', oe.r_apoapsis);
        fprintf(fid, '  轨道周期: %.2f 小时\n', oe.period);
    end
    
    fprintf(fid, '\n  比机械能: %.6f km^2/s^2\n', oe.energy);
    fprintf(fid, '  角动量大小: %.6f km^2/s\n', oe.h);
    
    fclose(fid);
    
    fprintf('结果已导出到文件: %s\n', filename);
end

5. 主函数和快速开始

%% 主函数 - 兰伯特问题求解器
function lambert_solver_gui()
    % 兰伯特问题求解器 GUI 版本
    
    fprintf('========================================\n');
    fprintf('       兰伯特问题求解器\n');
    fprintf('========================================\n\n');
    
    while true
        fprintf('主菜单:\n');
        fprintf('  1. 求解兰伯特问题\n');
        fprintf('  2. 运行测试用例\n');
        fprintf('  3. 精度测试\n');
        fprintf('  4. 显示帮助\n');
        fprintf('  5. 退出\n');
        
        choice = input('\n请选择 (1-5): ');
        
        switch choice
            case 1
                % 获取用户输入
                [r1, r2, dt] = get_input_from_user();
                
                % 设置参数
                mu = 398600.4418;  % 地球
                
                % 求解
                [oe, v1, v2] = solve_lambert_problem(r1, r2, dt, mu);
                
                % 可视化
                visualize_lambert_solution(r1, r2, v1, v2, oe, mu, dt);
                
                % 导出结果
                export_choice = input('\n导出结果到文件? (y/n): ', 's');
                if strcmpi(export_choice, 'y')
                    export_results(oe, v1, v2, r1, r2, dt, mu);
                end
                
            case 2
                % 运行测试用例
                test_lambert_solver();
                
            case 3
                % 精度测试
                accuracy_test();
                
            case 4
                % 显示帮助
                display_help();
                
            case 5
                % 退出
                fprintf('\n感谢使用兰伯特问题求解器！\n');
                break;
                
            otherwise
                fprintf('无效选择，请重新输入\n');
        end
        
        fprintf('\n');
    end
end

function display_help()
    % 显示帮助信息
    
    fprintf('\n=== 兰伯特问题求解器帮助 ===\n\n');
    
    fprintf('兰伯特问题定义:\n');
    fprintf('  已知两个位置矢量 r1 和 r2，以及从 r1 到 r2 的飞行时间 Δt，\n');
    fprintf('  求连接这两点的轨道。\n\n');
    
    fprintf('输入要求:\n');
    fprintf('  1. 位置矢量: 三维直角坐标 (km)\n');
    fprintf('  2. 飞行时间: 秒 (也可输入分钟、小时或天)\n');
    fprintf('  3. 引力常数: 取决于中心天体\n\n');
    
    fprintf('输出结果:\n');
    fprintf('  1. 初始速度矢量 v1 (km/s)\n');
    fprintf('  2. 终点速度矢量 v2 (km/s)\n');
    fprintf('  3. 轨道根数: a, e, i, Ω, ω, ν\n');
    fprintf('  4. 三维轨道可视化\n\n');
    
    fprintf('算法说明:\n');
    fprintf('  使用普适变量法求解兰伯特方程，\n');
    fprintf('  牛顿-拉弗森迭代法确保精度。\n\n');
    
    fprintf('注意事项:\n');
    fprintf('  1. 对于给定的输入，通常有两个解（短路径和长路径）\n');
    fprintf('  2. 飞行时间必须大于最小转移时间\n');
    fprintf('  3. 对于某些极端情况，算法可能不收敛\n\n');
    
    fprintf('参考文献:\n');
    fprintf('  [1] Curtis, H.D. (2014). Orbital Mechanics for Engineering Students\n');
    fprintf('  [2] Bate, R.R., Mueller, D.D., & White, J.E. (1971). Fundamentals of Astrodynamics\n');
    fprintf('  [3] Vallado, D.A. (2013). Fundamentals of Astrodynamics and Applications\n\n');
end

%% 快速开始示例
function quick_start_example()
    % 快速开始示例
    
    fprintf('=== 快速开始示例 ===\n\n');
    
    % 设置参数
    mu = 398600.4418;  % 地球引力常数
    
    % 示例：从 200 km 圆轨道到 GEO 的转移
    fprintf('示例：地球同步转移轨道 (GTO)\n\n');
    
    % 初始位置：200 km 高度，赤道上
    r1 = [6578; 0; 0];
    
    % 终点位置：GEO (35786 km 高度)
    r2 = [0; 42164; 0];
    
    % 飞行时间：约 5.3 小时（霍曼转移）
    dt = 5.3 * 3600;
    
    fprintf('初始位置: [%.0f, %.0f, %.0f] km\n', r1);
    fprintf('终点位置: [%.0f, %.0f, %.0f] km\n', r2);
    fprintf('飞行时间: %.1f 小时\n\n', dt/3600);
    
    % 求解兰伯特问题
    [oe, v1, v2] = solve_lambert_problem(r1, r2, dt, mu);
    
    % 显示结果
    fprintf('计算结果:\n');
    fprintf('  初始速度: [%.4f, %.4f, %.4f] km/s\n', v1);
    fprintf('  终点速度: [%.4f, %.4f, %.4f] km/s\n', v2);
    fprintf('  轨道类型: %s\n', oe.type);
    fprintf('  半长轴: %.2f km\n', oe.a);
    fprintf('  偏心率: %.6f\n', oe.e);
    fprintf('  近地点半径: %.2f km\n', oe.r_periapsis);
    fprintf('  远地点半径: %.2f km\n', oe.r_apoapsis);
    
    % 可视化
    visualize_lambert_solution(r1, r2, v1, v2, oe, mu, dt);
    
    fprintf('\n示例完成！\n');
end

%% 运行快速开始示例
% 取消注释下一行以运行快速开始示例
% quick_start_example();

参考代码 计算兰伯特问题 www.youwenfan.com/cna/96985.html

三、使用说明

1. 基本用法

% 设置参数
mu = 398600.4418;  % 地球引力常数

% 定义位置矢量和飞行时间
r1 = [7000; 0; 0];    % 初始位置 (km)
r2 = [0; 7000; 0];    % 终点位置 (km)
dt = 1800;            % 飞行时间 (s)

% 求解兰伯特问题
[orbital_elements, v1, v2] = solve_lambert_problem(r1, r2, dt, mu);

% 显示结果
disp('轨道根数:');
disp(orbital_elements);

% 可视化
visualize_lambert_solution(r1, r2, v1, v2, orbital_elements, mu, dt);

2. 高级选项

% 使用所有可选参数
[oe, v1, v2] = solve_lambert_problem(r1, r2, dt, mu, ...
    'direction', 'retrograde', ...  % 逆行转移
    'solution', 'long', ...         % 长路径解
    'tolerance', 1e-12, ...         % 收敛容差
    'max_iter', 200);               % 最大迭代次数

四、关键特性

1. 多种求解方法：

   • 普适变量法求解兰伯特方程
   • 牛顿-拉弗森迭代确保精度
   • 支持短路径和长路径解

2. 完整轨道分析：

   • 计算经典轨道根数 (a, e, i, Ω, ω, ν)
   • 自动识别轨道类型
   • 计算轨道能量和角动量

3. 可视化功能：

   • 三维轨道可视化
   • 速度分布图
   • 参数摘要显示

4. 验证和测试：

   • 精度验证测试
   • 多种测试用例
   • 结果验证功能

5. 实用工具：

   • 用户友好的输入界面
   • 结果导出功能
   • 全面的帮助文档