简介这是一份基于MATLAB的CW方程数值仿真程序适用于学习天体力学、相对导航控制与轨道规划的航天工程或控制类学习者。程序采用trappa4四次多项式插值方法求解Chandrasekhar-Wolf方程能够模拟两个天体间的相对运动输出位置、速度、加速度时间序列并绘制轨迹图便于分析交会对接、太空垃圾清除等场景中的相对轨迹。压缩包共1个文件为单一m脚本大小仅2KB结构精炼适合快速阅读与二次开发。目前已有392人学习下载。通过该程序可掌握CW方程建模流程、ode系列求解器与trappa4方法的区别并能调整初始条件验证不同参数对相对运动的影响是理论结合实践的轻量级参考。1. CW 方程在相对导航与轨道规划里的位置做卫星编队、交会对接或者空间碎片抵近观测时真正难的不是轨道动力学本身而是两个航天器之间的相对运动描述。直接对绝对轨道做差当然可以但非线性强、计算重控制律设计也不直观。CW 方程的价值在于在近圆参考轨道上把相对运动线性化成一个时不变的状态空间方程六个状态量就能描述三个方向的位置与速度这个模型从 20 世纪 60 年代用到现在仍然是相对导航控制、轨道规划算法里最常用的基础层。这个标题里带着 matlab 和 trappa4说明落地形态是一套基于 Matlab 的仿真或半物理验证代码trappa4 大概率是代码里负责轨道传播的数值积分函数名。它解决的是 CW 方程从纸面到可运行的一段距离方程本身是连续线性系统但在三维机动、非球形摄动和导航估计的混合场景里需要离散化的状态转移矩阵、数值积分器、滤波器更新和控制量生成。适合看这篇文章的人是正在做编队控制、交会对接或相对导航仿真手里有 CW 方程推导但缺一个能直接跑起来并验证精度的 Matlab 工程框架。2. 在 Matlab 里建立 CW 方程状态空间模型与解析解2.1 LVLH 坐标系与线性化条件CW 方程的基础是 Local-Vertical-Local-Horizontal 坐标系原点在参考星质心x 轴沿参考星径向向外y 轴沿飞行方向z 轴垂直于轨道面。参考星运行在圆轨道上目标星相对参考星的距离远小于轨道半径才能在展开引力势时只保留一阶项。三个方向的相对运动方程可以写成x - 2*n*y - 3*n^2*x u_x y 2*n*x u_y z n^2*z u_z其中 n 是参考星轨道角速度。这是 CW 方程最常用的形式左边是相对运动自然动力学右边是控制加速度。建立状态向量X [x, y, z, vx, vy, vz]状态矩阵 A 是常数矩阵。注意 z 方向与轨道面内方向完全解耦这是一个经常被忽略但很有用的性质轨道面内的 x-y 运动与轨道面外的 z 运动可以分开设计仿真时三个方向的步长和滤波器参数也就可以独立调整。2.2 状态空间矩阵与 Matlab 表示在 Matlab 中生成 CW 状态矩阵最直接的方式是写一个函数输入轨道角速度 n输出 6x6 的 A 矩阵和 6x3 的 B 矩阵。B 矩阵对应三个方向的控制输入通常情况下是单位阵对应的三列表示控制加速度直接作用在速度变化率上。function [A, B] cw_state_matrix(n) % CW 方程线性时不变状态矩阵 % 输入 n: 参考轨道角速度, rad/s % 输出 A: 6x6 状态矩阵, B: 6x3 控制输入矩阵 A zeros(6, 6); A(1, 4) 1; A(2, 5) 1; A(3, 6) 1; A(4, 1) 3 * n^2; A(4, 2) 2 * n; A(5, 1) -2 * n; A(6, 3) -n^2; B [zeros(3, 3); eye(3)]; end状态矩阵里每一项的物理含义很明确A(4,2) 2n是 Coriolis 耦合项决定了 x 方向的加速度会受到 y 方向速度的影响A(5,1) -2n是另一个方向上的 Coriolis 项A(4,1) 3n^2来自重力梯度与离心力的差。写成状态空间形式之后连续系统的所有工具就都能用了矩阵指数求状态转移矩阵、LQR 设计反馈控制、极点配置分析稳定性。2.3 状态转移矩阵解析解与 Matlab 传播函数CW 方程是线性定常系统状态转移矩阵可以用矩阵指数解析表达不用做数值积分就能把任意时刻的相对状态从初始状态推算出来。这也是 CW 方程相对导航里最常用的底层工具卡尔曼滤波的预测步直接调用状态转移矩阵避免了在线积分非线性微分方程。function Phi cw_stm(n, dt) % CW 方程状态转移矩阵, dt 为传播时长, 单位秒 nt n * dt; c cos(nt); s sin(nt); Phi zeros(6, 6); Phi(1,1) 4 - 3*c; Phi(1,2) 0; Phi(1,3) 0; Phi(1,4) s / n; Phi(1,5) 2*(1 - c) / n; Phi(1,6) 0; Phi(2,1) 6*(s - nt); Phi(2,2) 1; Phi(2,3) 0; Phi(2,4) 2*(c - 1) / n; Phi(2,5) (4*s - 3*nt) / n; Phi(2,6) 0; Phi(3,3) c; Phi(3,6) s / n; Phi(4,1) 3*n*s; Phi(4,2) 0; Phi(4,3) 0; Phi(4,4) c; Phi(4,5) 2*s; Phi(5,1) 6*n*(c - 1); Phi(5,2) 0; Phi(5,3) 0; Phi(5,4) -2*s; Phi(5,5) 4*c - 3; Phi(6,3) -n*s; Phi(6,6) c; end这个矩阵写出来之后建议做一次正确性检查把dt设为参考轨道周期也就是2*pi/n此时c1、s0矩阵应该退化为单位阵因为一个轨道周期之后相对状态完全回归。这是一个很简单的数学检查能直接暴露符号方向或者三角函数参数写错的问题。轨道高度 (km)轨道周期 (min)角速度 n (rad/s)400约 92.6约 0.00113700约 98.8约 0.001061200约 109.1约 0.00096表格里的角速度可以验证一个经验值低轨场景下 n 的量级是 0.001 rad/s所以 CW 矩阵里3*n^2的量级是 3e-6。这个量级会影响后面滤波器的噪声参数设置如果 Q 矩阵里位置噪声设到 1e-3就比系统动力学的量级大了三个数量级滤波结果会明显偏向测量而非模型预测。2.4 解析解与数值解的边界解析解很好用但它只在圆轨道、无摄动、相对距离很小的前提下成立。实际空间环境里 J2 摄动会造成参考轨道面的长期漂移大气阻力在 400 公里高度也不能忽略。工程上常见的做法是把 CW 方程当作主模型用于控制设计同时在仿真里加入动力学摄动用数值积分验证 CW 制导律的鲁棒性。trappa4 这类积分器在这里的价值就是提供一个比解析 STM 更真实的传播基准。一个比较容易踩的坑CW 方程解析解是精确的线性解数值积分器如果用一阶欧拉法反而会产生比解析解更大的误差。所以工程仿真的习惯是线性模型内部用 STM非线性验证用高阶积分器两部分的结果差异本身就说明了线性化模型的适用范围。3. trappa4 数值积分Matlab 的 RK4 实现与步长控制3.1 为什么已经有了解析解还要数值积分CW 方程的状态转移矩阵确实能精确传播线性系统但轨道规划任务里经常遇到这样的需求初始状态不知道精确值而是来自导航滤波器的估计结果控制输入不是理想脉冲而是有限推力动力学模型里加入了 J2、阻力等摄动。这些情况都没有封闭解析解只能靠数值积分。trappa4 这个命名在很多 Matlab 轨道仿真工程里指的就是一个四阶龙格库塔积分器函数。四阶 Runge-Kutta 是数值积分里精度和计算量的一个平衡点单步误差是 h 的五次方量级比欧拉法的二次方精度高得多又比变步长自适应积分器简单稳定适合在循环里对每一条轨道反复调用。3.2 trappa4 的 Matlab 函数实现与参数说明function [t, X] trappa4(odefun, tspan, x0, h) % trappa4: 四阶龙格库塔固定步长积分器 % 输入: % odefun - 函数句柄, dX odefun(t, X) % tspan - [t0, tf] 积分起止时间 % x0 - 初始状态向量, 6x1 % h - 固定步长, 秒 % 输出: % t - 时间序列 % X - 状态序列, length(t) x 6 t0 tspan(1); tf tspan(2); t t0:h:tf; if t(end) ~ tf t [t, tf]; end N numel(t); n numel(x0); X zeros(N, n); X(1, :) x0(:); for k 1:N-1 hk t(k1) - t(k); xk X(k, :); k1 odefun(t(k), xk); k2 odefun(t(k) hk/2, xk hk/2 * k1); k3 odefun(t(k) hk/2, xk hk/2 * k2); k4 odefun(t(k) hk, xk hk * k3); X(k1, :) xk hk/6 * (k1 2*k2 2*k3 k4); end end这段代码的核心思想来自经典的 RK4 龙格库塔法在同一积分步内取四个斜率样本给中间点更高的权重使得单步截断误差降到 O(h^5)。k1 是在当前时刻的斜率k2 和 k3 是半步处的两个斜率估计k4 是全步末端的斜率估计最后做加权平均。固定步长的好处在这个应用场景里很明显轨道规划的循环里每个机动段的积分时长是确定的固定步长可以精确控制调用次数也方便把结果与 STM 解析解做逐点对比。t t0:h:tf这行有一个隐含风险如果(tf - t0)不能被 h 整除最后一个步长会小于 h精度会略降。代码里用t [t, tf]补了一个末点此时最后一步实际步长hk小于 h。对于 CW 方程这种线性系统这种微小的步长波动不会破坏数值稳定性但如果把同一个积分器用于高动态系统就得考虑在末步用自适应步长修正。3.3 步长选择与轨道周期的匹配规则RK4 的误差与 h 的四次方成正比但 h 越小计算量和内存占用越高。CW 方程的相对运动周期与参考轨道周期一致在 400 公里轨道高度大约是 92 分钟。仿真经验上一个轨道周期内取 600 到 1200 个积分点也就是步长 5 到 10 秒就足以让 RK4 的数值误差低于 CW 线性化本身带来的模型误差。继续缩小步长到 1 秒以下不会带来明显收益反而会让仿真矩阵占用变大。仿真时长建议步长积分点数1 个轨道周期 (约 5556 秒)10 秒55610 个轨道周期20 秒27781 天连续仿真30 秒2880表格里的步长是线性 CW 模型下的参考值。如果动力学里加入了 J2 摄动或大气阻力建议先跑一个轨道周期对比两步长结果把 h 减半如果最大位移差异小于 1 毫米说明当前步长已经收敛到足够精度。3.4 用 trappa4 验证 CW 解析解的数值验证流程CW 解析解和 RK4 数值解之间可以互相检验这是一个非常实用的测试思路。给定同一个初始相对状态用 STM 传播到 T再用 trappa4 积分同样时长两者的差应当随步长减小而趋向于零。这个差值的量级反映了积分器误差而两者的共同部分则反映了 CW 模型的线性化精度。n 0.00113; % 400 km 轨道角速度 x0 [100; 0; -50; 0; 0.2; 0]; % 初始相对位置与速度, 单位 m, m/s T 600; % 600 秒, 约十分之一轨道周期 [A, B] cw_state_matrix(n); Phi cw_stm(n, T); x_stm Phi * x0; % 解析解 fun (t, x) A * x; % CW 线性系统 [t, x_rk4] trappa4(fun, [0 T], x0, 1); disp(STM 与 RK4 的最终状态差:); disp((x_stm - x_rk4(end, :)));这段代码的输出应该是一个接近零的六维向量。实际跑出来位置差通常在毫米级速度差在微米每秒级。如果差值的量级达到米级说明 CW 矩阵里的耦合项符号写错了这个测试正好能定位。注意fun (t, x) A * x里 t 虽然没用到但必须保留这个输入参数因为 trappa4 内部会以两个入参调用 odefun。4. 用 CW 方程做相对导航控制与轨道规划4.1 相对导航滤波器设计STM 预测与测量更新相对导航的任务是从测量数据中估计两个航天器之间的相对位置与速度常用的方案是线性卡尔曼滤波器。CW 方程在这里的优势立刻体现出来状态转移矩阵可以直接作为滤波器的预测矩阵过程噪声协方差的传播也可以由 STM 完成。function [x_est, P] cw_kalman_update(x_pred, P_pred, z, H, R) % 卡尔曼滤波测量更新 % z: 测量向量, H: 测量矩阵, R: 测量噪声协方差 S H * P_pred * H R; K P_pred * H / S; x_est x_pred K * (z - H * x_pred); P (eye(6) - K * H) * P_pred; end这里的核心参数是测量矩阵 H。如果测量源是星间测距H 是 1x6 的行向量表示相对距离对位置分量的偏导如果测量源是光学相机提供的视线角H 会变成 2x6 或 3x6且会随相对几何变化。CW 滤波器和一般卡尔曼滤波没有结构区别差别在 H 的构建和 R 的量级。R 参数可以直接从测距传感器的噪声指标读出来但过程噪声 Q 的物理意义更抽象它是线性化误差、未建模摄动和控制噪声的混合体一般用仿真调参确定。滤波器发散的一个典型表现是状态估计误差不收敛P 矩阵持续下降但新息序列明显有偏。这时候先不要调 Q 和 R而是检查 STM 预测与真实动力学是否匹配。如果仿真里加入了 J2 摄动而滤波器仍在使用纯 CW 的 STM就相当于模型错误滤波结果必然发散。4.2 CW 方程控制律设计LQR 与极点配置控制层面最稳妥的方案是线性二次型调节器。CW 系统是线性时不变的只要设计 Q 和 R 矩阵Matlab 的 lqr 函数可以直接解出增益矩阵。n 0.00113; [A, B] cw_state_matrix(n); % 状态权重: 位置误差权重较高, 速度误差次之 Q diag([1e-2, 1e-2, 1e-2, 1, 1, 1]); % 控制权重: 加速度越小越好, 意味着燃料最省 R diag([1e-6, 1e-6, 1e-6]); K lqr(A, B, Q, R);LQR 增益矩阵 K 解出来是 3x6 的矩阵控制指令u -K * x。参数调整的经验是先固定 R 不变只调 Q 中对位置误差的权重。位置权重提高到一定程度后收敛时间会缩短但初始控制加速度峰值会明显增大。实际任务里控制加速度上限由推力器能力决定所以通常先设定max_u再反推 Q 与 R 的比例关系。控制律设计完之后用 trappa4 闭环仿真验证是必要步骤。把u -K * x放入动力学函数积分一个轨道周期观察相对位置是否收敛到零、过程中是否有超调、控制加速度是否超过推力上限。如果不用 STM 而用数值积分闭环CW 模型的线性误差会被控制器自身抑制一部分但如果初始相对距离超过了线性化适用范围反馈控制也会失效。4.3 轨道规划两脉冲机动参数计算轨道规划的目标是设计一组速度增量让追踪星在指定时刻到达目标相对位置。两脉冲机动是最常见的工程方案t0 时刻施加第一个速度增量改变相对运动轨迹tf 时刻施加第二个速度增量消除残余速度。CW 方程让这个规划变成了一个矩阵求逆问题。function [dv1, dv2] plan_two_burn(n, x0, xf, tf) % 两脉冲 CW 轨道规划 % x0, xf: 起止相对状态, 都是 6x1 % 返回两个速度增量, 单位 m/s Phi cw_stm(n, tf); Phi_rr Phi(1:3, 1:3); Phi_rv Phi(1:3, 4:6); Phi_vr Phi(4:6, 1:3); Phi_vv Phi(4:6, 4:6); r0 x0(1:3); v0 x0(4:6); rf xf(1:3); vf xf(4:6); % 第一个脉冲调整初始速度, 使 tf 时刻位置达到目标 v0_new Phi_rv \ (rf - Phi_rr * r0); dv1 v0_new - v0; % 用新初始速度前瞻 tf 时刻速度 vf_new Phi_vr * r0 Phi_vv * v0_new; dv2 vf - vf_new; end这段代码里的关键运算是对Phi_rv求逆它是状态转移矩阵中位置对初始速度的敏感度矩阵。条件数决定了规划对速度精度的敏感程度如果 tf 接近一个轨道周期的整数倍Phi_rv会接近零矩阵求逆结果会很大此时微小导航误差会导致巨大的速度增量。所以两脉冲规划要避开转移时间等于轨道周期整数倍的情况或者改用三脉冲方案在中间时刻修正。转移时长 (s)第一个 dv (m/s)第二个 dv (m/s)6000.280.2215000.190.1728000.110.10表格里的数值对应一组典型的 100 米量级交会场景。转移时间越长速度增量需求越小但误差累积时间也越长这是一个需要权衡的规划参数。4.4 用 trappa4 验证规划结果规划完成之后要用数值积分做一遍开环验证把 dv1 加到初始速度上用 trappa4 积分到 tf终点状态与规划目标 xf 的偏差就是规划误差。这个误差的来源是规划本身用了解析 STM而验证用的是数值积分两者的差别体现了数值传播和线性模型之间的偏差。x0_plan x0; x0_plan(4:6) x0_plan(4:6) dv1; [t, X_sim] trappa4((t, x) A * x, [0 tf], x0_plan, 1); x_end X_sim(end, :); err x_end - xf; if norm(err(1:3)) 0.01 disp(规划验证通过: 终点位置误差小于 1 cm); else disp([规划误差: , num2str(norm(err(1:3)))]); end误差小于 1 厘米说明 STM 的计算精度足够。如果误差偏大先检查步长是否过粗再说是不是需要加 J2 修正。很多轨道规划代码的问题出在单位混乱上位置用公里、速度用米每秒、n 用度每秒混在一起算出的 dv 误差会非常大。规划代码里统一采用国际单位制所有输入参数从外部读入时强制转换一次。5. 验证与排错CW 模型落地的 4 个关键检查CW 方程的理论并不复杂但工程落地时错误往往出在一些看起来不起眼的地方。这里列几个我在 Matlab 仿真里排查过的实际问题照着检查一遍能省不少时间。第一个是单位一致性。n 的单位是 rad/sdt 是秒位置和速度分别用米和米每秒。如果轨道高度数据来自某个表格里以公里为单位的字段进入 CW 矩阵之前必须乘 1000。一个常见的低级错误是把轨道周期的分钟数直接拿去算n 2*pi/T_min算出来的 n 比真实值小 60 倍整个相对运动周期都会错。第二个是状态向量顺序。CW 方程的状态排列习惯有[x, y, z, vx, vy, vz]和[r; v]两种写法矩阵的索引要跟着状态向量走。如果你的滤波器输出顺序是[x, vx, y, vy, z, vz]那么直接用标题里的 STM 矩阵必然错位。建议在仿真入口处加一个状态向量的打印检查把第一个时刻的数值和手算值对比一眼就能看出顺序对不对。第三个是 STM 验证。检查cw_stm(n, dt)是否正确最有效的方法是让 dt 等于四分之一轨道周期。此时 x 方向和 y 方向的位置应该发生明显的交换或转化z 方向则独立振荡。再让 dt 等于整个周期矩阵应当回到单位阵。如果这两个测试都通过STM 基本没有问题。第四个值得检查的是滤波器的模型一致性。纯 CW 滤波器和含 J2 仿真器联调时如果新息序列出现稳定的非零偏移说明滤波器模型与真实动力学不匹配。一个实用的调参顺序是先把过程噪声 Q 设到足够大让滤波结果跟得上仿真器的输出确认滤波逻辑无误后再逐步减少 Q 到合理范围。如果一开始就用很小的 Q滤波发散时很难判断是模型错误还是参数问题。最后的验证技巧把 trappa4 的步长减半再跑一次同一场景两次结果的差异就是数值误差的下界估计。做相对导航仿真时每次修改完代码都跑一次这个对比能避免把积分器误差误判成控制律误差。本文还有配套的精品资源点击获取
