企业官网建设流程全解析

简介：CW方程是天体力学中描述两体相对运动的经典模型，在航天器相对导航控制与轨道规划中扮演关键角色。这份MATLAB程序包聚焦CW方程的数值求解，面向航天工程、天体力学及数值计算学习者，可帮助解决相对导航控制中精确预测航天器间相对位置、速度与加速度的问题，并为轨道规划提供可扩展的数值求解框架。包内仅包含1个m文件，压缩后约2KB，程序围绕CW方程构建，集成trappa4四次多项式插值算法进行数值积分，并设计了初始条件输入、相对运动参数输出及结果可视化等环节，方便直接运行或修改参数复现不同场景。该程序可模拟卫星交会对接、太空垃圾清除、深空探测器路径规划等典型任务，通过调整质量参数与初始状态，能够观察它们对相对运动轨迹的影响，进而辅助优化导航策略，是理论分析与工程实践结合的良好入门资源。目前已有392人学习/下载。

1. 交会对接中的CW方程，一个步长算错就撞车

当两个航天器相距几十公里时，相对运动可以用一个线性化的常微分方程来描述，这就是CW方程，也叫Hill方程。它假设目标星在圆轨道、两星距离远小于轨道半径，把复杂的星间相对运动变成了一个6维线性时不变系统。正因为线性，相对导航与轨道规划里的状态预测和协方差递推都变得非常快；在MATLAB里解这个方程，常见做法是直接调用ode45，但如果需要固定步长控制、或想把轨迹分段约束，trappa4这种四次多项式插值积分器更顺手。接下来从CW方程的线性模型说起，实现trappa4，再走一遍LQR相对导航控制闭环，最后用解析解校验数值积分。

2. CW方程的线性化模型与无量纲化

2.1 从非线性相对运动到CW方程

实际任务中，伴随星和目标星都在绕地球运动，严格来说是求解相对两体问题。当两星距离足够近时，可以在目标星的圆轨道附近做一阶泰勒展开，丢掉二阶以上的小量。把坐标原点放在目标星质心，建立Hill坐标系：x轴沿地心矢量方向向外，y轴沿飞行方向，z轴垂直于轨道面。设相对位置为（x, y, z），相对速度为（x', y', z'），在不施加推力时，CW方程可以写成：

x'' = 3 n^2 x + 2 n y'

y'' = -2 n x'

z'' = -n^2 z

这里n是目标星的平均轨道角速度，单位为rad/s。3 n^2 x来自重力梯度效应，两个交叉速度项来自旋转坐标系的科里奥利加速度。z方向完全解耦，意味着只要初始z向位置和速度均为零，运动就始终保持在轨道面内。编队控制任务中常把平面内和平面外分开设计，正是利用了这一点。

注意这个方程有两个前提：目标星轨道偏心率接近0，且两星相对距离远小于地心到目标星的距离。如果目标星在椭圆轨道上，CW方程会变成周期系数方程，需要改用Tschauner-Hempel方程。实际工程里，交会末端一般都在近圆轨道上，所以CW方程在几百公里内的相对运动计算中已经足够精确。

2.2 状态矩阵与MATLAB构建

把方程改写为状态空间形式。取状态向量 X = [x, y, z, vx, vy, vz]^T，其中 vx = x', vy = y', vz = z'。那么闭环方程写作 X' = A X + B U，A是常数矩阵，B是控制输入矩阵。直接写A矩阵容易出错，我习惯用分块方式在MATLAB里拼：

% 地球引力常数与目标轨道参数 mu = 398600.4418; % km^3/s^2 a = 6878.0; % km，对应高度约500 km n = sqrt(mu / a^3); % rad/s，约0.00108 % 状态矩阵 A，6阶 A = [0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; 3*n^2 0 0 0 2*n 0; 0 0 0 -2*n 0 0; 0 0 -n^2 0 0 0]; % 控制输入矩阵 B，三轴推力加速度 B = [zeros(3); eye(3)];

A矩阵的右上角是三阶单位阵，表示速度是位置的导数；第四行第一列的元素3 n^2是重力梯度项，第四行第五列2 n是y向速度对x向加速度的贡献；第五行第四列-2 n是x向速度对y向加速度的贡献；第六行第三列-n^2对应z方向的恢复力。B矩阵把三个控制加速度直接加到速度导数上，这样单位才统一。

状态量的单位需要全程一致。推荐位移用km、速度用km/s，时间用s。此时A的量纲是1/s，B是无量纲矩阵。用lqr、kalman等函数时可以少做一次坐标变换。下表给出典型参数：

2.3 无量纲化处理

求解CW方程前，建议先做无量纲化。令无量纲时间 tau = n t，长度用初始相对距离 L0，速度用 n L0。归一化后方程变为：

x'' = 3 x + 2 y'

y'' = -2 x'

z'' = -z

这里的导数是对 tau 求导。好处是方程不再依赖具体轨道高度，所有特征频率变成1，积分步长可以直接按“每周期多少步”设置。比如仿真一个轨道周期，无量纲周期是 2*pi，如果步长取0.01，就是约628步，容易推断计算量。

在依赖推算时，先用无量纲形式做粗扫描，找到合适的控制参数后，再用有量纲形式做高保真仿真。例如无量纲位置是2.5，对应实际距离2.5L0 km，速度是2.5n*L0 km/s，时间除以n即可恢复真实值。这个换算关系在后续轨道规划里会反复用到。

3. trappa4积分器原理与MATLAB实现

3.1 为什么固定步长积分器在相对导航里不可替代

很多人一开始用ode45解CW方程，会发现精度足够，但变步长带来的问题是采样点不均匀。相对导航控制中，导航滤波器通常以固定节拍输出量测值，控制律也必须在固定周期内更新；如果积分器在推力开关附近将步长缩小到毫秒级，下一个控制周期前可能来不及算完。另一种做法是给ode45指定固定的输出时间向量，但内部步长仍然变化，每个输出节点上的状态实际上经历了插值处理，对碰撞检测这种对时间严格要求的场景并不稳妥。

trappa4这个名字看起来陌生，实质是采用四次多项式插值思路的固定步长格式。在[t, t+h]区间内取三个中间点处的导数，用四次多项式逼近真实运动，再积分得到下一状态。从系数看，它与Kutta 3/8法则等价，属于经典四阶Runge-Kutta族。比RK4的稳定域略宽，函数评估次数相同，适合CW方程这种非刚性但需要固定时间节点的问题。

3.2 核心实现：trappa4函数

下面是我在相对导航仿真里使用的trappa4实现，保存在trappa4.m中。输入是导数函数句柄、时间区间、初始状态和固定步长，输出是时间序列和状态矩阵：

function [T, Y] = trappa4(odefun, tspan, y0, h) % odefun - 状态导数函数句柄，形式 @(t,y) % tspan - 积分区间 [t0 tf] % y0 - 初始状态列向量 % h - 固定步长，单位与时间轴一致 t0 = tspan(1); tf = tspan(2); N = floor((tf - t0) / h); T = linspace(t0, t0 + N*h, N+1)'; Y = zeros(N+1, numel(y0)); Y(1,:) = y0(:)'; for i = 1:N t = T(i); y = Y(i,:)'; k1 = odefun(t, y); k2 = odefun(t + h/3, y + h*k1/3); k3 = odefun(t + 2*h/3, y - h*k1/3 + h*k2); k4 = odefun(t + h, y + h*(k1 - k2 + k3)); Y(i+1,:) = y' + h*(k1' + 3*k2' + 3*k3' + k4') / 8; end end

k1、k2、k3、k4分别是四个节点上的瞬时斜率。k3的表达式可以写成 y + h*(-k1/3 + k2)，代码里展开成 y - hk1/3 + hk2，避免括号过多。最后一步是四阶加权和，权重为1/8、3/8、3/8、1/8，和Simpson 3/8法则一致。如果直接复制到旧版MATLAB，要注意行向量和列向量的转换；这里统一用列向量保存状态，输出时转存成行矩阵。

调用时只需要两步：

odefun = @(t,y) cw_rhs(t, y, n); [T, Y] = trappa4(odefun, [0, 5827], y0, 10);

其中cw_rhs是CW方程的导数函数，定义如下：

function dydt = cw_rhs(t, y, n) dydt = zeros(6,1); dydt(1:3) = y(4:6); dydt(4) = 3*n^2*y(1) + 2*n*y(5); dydt(5) = -2*n*y(4); dydt(6) = -n^2*y(3); end

这样trappa4和cw_rhs是两个独立函数，后续替换成带控制力的cw_rhs_ctrl时，积分器本身不用改。

3.3 在CW方程上做精度对比

用解析解作为基准，测试不同步长下的最大误差。初始状态取位置[1; 0.5; 0.2] km，速度[0.001; -0.0005; 0] km/s，仿真时长6000秒，得到如下数据：

步长减半后误差约降到原来的1/16，符合四阶格式的收敛特性。ode45在高容差下略优，但优势有限。遇到推力器开关导致的强不连续时，固定步长会显得僵硬；此时可以把trappa4步长设小，或者切换到ode15s处理事件。工程上我习惯用trappa4做标称仿真，用ode15s做推力切换段的复核，两者结果一致再进入下一阶段。

4. 相对导航控制闭环仿真与参数调节

4.1 LQR控制器设计与CW状态矩阵的配合

相对导航控制通常分为任务规划层和跟踪控制层。在CW线性模型下，跟踪控制最简单的选择是LQR。状态量仍为6维，控制量是三个方向的推力加速度。LQR代价函数为 J = ∫(X'QX + U'RU) dt，Q惩罚状态误差，R惩罚控制量，MATLAB中直接调用lqr函数：

% 控制器加权矩阵 Q = diag([1e-2, 1e-2, 1e-2, 1e-4, 1e-4, 1e-4]); R = diag([1e-4, 1e-4, 1e-4]); K = lqr(A, B, Q, R);

Q中的前三个元素是位置误差权重，后三个是速度误差权重。位置项设1e-2，速度项设1e-4，说明优先消除位置偏差，收敛过程会比较干脆。R设1e-4意味着允许较大的控制加速度，适合交会逼近段。如果是编队保持，可以把R提高到1e-2，减少推进剂消耗，代价是过渡时间变长。

4.2 闭环动力学与trappa4集成

加入控制器后，环路方程变为：

X' = A X - B K (X - Xd)

Xd是期望状态，对保持任务取零向量。在MATLAB中定义新的导数函数：

function dydt = cw_rhs_ctrl(t, y, n, K, yd) u = -K * (y - yd); dydt = [y(4:6); 3*n^2*y(1) + 2*n*y(5) + u(1); -2*n*y(4) + u(2); -n^2*y(3) + u(3)]; end

u的单位是km/s^2。然后调用trappa4：

y0 = [1; 0.5; 0.2; 0.001; -0.0005; 0]; % km, km/s yd = zeros(6,1); h = 10; [T, Y] = trappa4(@(t,y) cw_rhs_ctrl(t, y, n, K, yd), [0, 3600], y0, h);

T是时间列向量，Y每一行是一个状态快照。取出前三维位置，可以直接画三维轨迹：

figure; plot3(Y(:,1), Y(:,2), Y(:,3), 'b-', 'LineWidth', 1.5); xlabel('x km'); ylabel('y km'); zlabel('z km'); grid on; axis equal;

这条曲线会直观显示伴随星从初始偏差回到期望点的路径。如果路径出现明显抖动，优先检查Q速度项和R的比值是否合理。

4.3 推力器限幅与参数调节经验

LQR给出的控制量可能远大于实际推力，需要加饱和限制。在cw_rhs_ctrl中加入限幅：

umax = 0.0001; % 0.1 m/s^2 u_raw = -K * (y - yd); u = max(min(u_raw, umax), -umax);

加上限幅后系统变成分段线性，trappa4仍然能处理，但固定步长要小于推力变化的时间尺度。如果umax太小，位置误差会收敛缓慢，甚至出现极限环。此时调大Q中位置项的权重，或调小R，让LQR给出的控制需求降低，使饱和不常被触发。经验值参考：

调整时先固定R，把Q位置项按10倍步进扫描，观察最大推力是否触达限幅；再微调R。如果位置误差有震荡，通常是速度项权重过低导致阻尼不足。我在MATLAB R2023b和Linux环境下跑过这套脚本，结果一致；旧版本只要支持lqr和匿名函数，同样可以运行。

5. 轨道规划中的状态转移矩阵验证技巧

5.1 用解析解状态转移矩阵校验trappa4

CW方程有解析解。给定初始状态X0，任意时刻状态为 X(t) = Φ(t) X0，其中Φ(t)是6x6状态转移矩阵。实现如下：

function Phi = cw_stm(n, t) ct = cos(n*t); st = sin(n*t); Phi = [4-3*ct 0 0 st/n 2*(1-ct)/n 0; 6*(st-n*t) 1 0 -2*(1-ct)/n (4*st-3*n*t)/n 0; 0 0 ct 0 0 st/n; 3*n*st 0 0 ct 2*st 0; 6*n*(ct-1) 0 0 -2*st 4*ct-3 0; 0 0 -n*st 0 0 ct]; end

这个矩阵可以直接验证trappa4的数值结果。积分完成后逐点比较：

for i = 1:length(T) Phi = cw_stm(n, T(i)); err(i) = max(abs(Y(i,:)' - Phi * y0)); end max_err = max(err);

如果max_err超过任务容许值，最简单是把步长h减半重跑。若减半后误差没有明显下降，检查cw_rhs和cw_stm中坐标系符号是否一致。常见错误是把y方向解析解的(4st-3nt)/n写成(4st-3*t)/n，导致误差随时间线性增长。

5.2 从验证到轨道规划的多约束筛选

轨道规划里，CW方程通常用于粗筛：先用解析解生成标称轨迹，在每个节点检查相对距离是否在安全走廊内、推力是否饱和，把通过粗筛的备选轨迹交给trappa4做高精度仿真，再用状态转移矩阵复核末端状态误差。trappa4输出固定步长节点，每个节点可以直接和Φ(t)的结果对齐，不需要插值。这套流程在MATLAB里就是几十行循环，用来做蒙特卡洛打靶也很方便。

如果规划中还要加入控制输入，可以写成 X(t) = Φ(t) X0 + ∫_0^t Φ(t-τ) B U(τ) dτ。这个卷积积分用梯形公式离散后，时间节点和trappa4完全一致。实际项目里，我把trappa4、cw_stm、LQR三个函数放在同一个目录下，仿真脚本只改y0、h和Q，这种结构后续做多工况枚举时不需要重写积分器。

本文还有配套的精品资源，点击获取