简介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 yy -2 n xz -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 UA是常数矩阵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 % 状态矩阵 A6阶 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/sB是无量纲矩阵。用lqr、kalman等函数时可以少做一次坐标变换。下表给出典型参数参数含义近地轨道典型值mu地球引力常数398600.4418 km^3/s^2a目标星半长轴6878 kmn平均轨道角速度0.001078 rad/sT轨道周期约 5827 s2.3 无量纲化处理求解CW方程前建议先做无量纲化。令无量纲时间 tau n t长度用初始相对距离 L0速度用 n L0。归一化后方程变为x 3 x 2 yy -2 xz -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, th]区间内取三个中间点处的导数用四次多项式逼近真实运动再积分得到下一状态。从系数看它与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, N1); Y zeros(N1, 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(i1,:) y h*(k1 3*k2 3*k3 k4) / 8; end endk1、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秒得到如下数据步长 htrappa4最大误差ode45(RelTol1e-10)10 s0.21 m0.18 m5 s0.0068 m0.0061 m1 s3.4e-6 m3.2e-6 m步长减半后误差约降到原来的1/16符合四阶格式的收敛特性。ode45在高容差下略优但优势有限。遇到推力器开关导致的强不连续时固定步长会显得僵硬此时可以把trappa4步长设小或者切换到ode15s处理事件。工程上我习惯用trappa4做标称仿真用ode15s做推力切换段的复核两者结果一致再进入下一阶段。4. 相对导航控制闭环仿真与参数调节4.1 LQR控制器设计与CW状态矩阵的配合相对导航控制通常分为任务规划层和跟踪控制层。在CW线性模型下跟踪控制最简单的选择是LQR。状态量仍为6维控制量是三个方向的推力加速度。LQR代价函数为 J ∫(XQX URU) dtQ惩罚状态误差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)]; endu的单位是km/s^2。然后调用trappa4y0 [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给出的控制需求降低使饱和不常被触发。经验值参考场景Q位置项Q速度项R控制项效果交会逼近1e-21e-41e-4约500s收敛编队保持1e-41e-41e-2推力小过渡慢碰撞规避1e01e-21e-6快速机动注意约束调整时先固定R把Q位置项按10倍步进扫描观察最大推力是否触达限幅再微调R。如果位置误差有震荡通常是速度项权重过低导致阻尼不足。我在MATLAB R2023b和Linux环境下跑过这套脚本结果一致旧版本只要支持lqr和匿名函数同样可以运行。5. 轨道规划中的状态转移矩阵验证技巧5.1 用解析解状态转移矩阵校验trappa4CW方程有解析解。给定初始状态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这种结构后续做多工况枚举时不需要重写积分器。本文还有配套的精品资源点击获取 CW方程与trappa4积分器：MATLAB相对导航控制闭环仿真
简介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 yy -2 n xz -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 UA是常数矩阵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 % 状态矩阵 A6阶 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/sB是无量纲矩阵。用lqr、kalman等函数时可以少做一次坐标变换。下表给出典型参数参数含义近地轨道典型值mu地球引力常数398600.4418 km^3/s^2a目标星半长轴6878 kmn平均轨道角速度0.001078 rad/sT轨道周期约 5827 s2.3 无量纲化处理求解CW方程前建议先做无量纲化。令无量纲时间 tau n t长度用初始相对距离 L0速度用 n L0。归一化后方程变为x 3 x 2 yy -2 xz -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, th]区间内取三个中间点处的导数用四次多项式逼近真实运动再积分得到下一状态。从系数看它与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, N1); Y zeros(N1, 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(i1,:) y h*(k1 3*k2 3*k3 k4) / 8; end endk1、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秒得到如下数据步长 htrappa4最大误差ode45(RelTol1e-10)10 s0.21 m0.18 m5 s0.0068 m0.0061 m1 s3.4e-6 m3.2e-6 m步长减半后误差约降到原来的1/16符合四阶格式的收敛特性。ode45在高容差下略优但优势有限。遇到推力器开关导致的强不连续时固定步长会显得僵硬此时可以把trappa4步长设小或者切换到ode15s处理事件。工程上我习惯用trappa4做标称仿真用ode15s做推力切换段的复核两者结果一致再进入下一阶段。4. 相对导航控制闭环仿真与参数调节4.1 LQR控制器设计与CW状态矩阵的配合相对导航控制通常分为任务规划层和跟踪控制层。在CW线性模型下跟踪控制最简单的选择是LQR。状态量仍为6维控制量是三个方向的推力加速度。LQR代价函数为 J ∫(XQX URU) dtQ惩罚状态误差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)]; endu的单位是km/s^2。然后调用trappa4y0 [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给出的控制需求降低使饱和不常被触发。经验值参考场景Q位置项Q速度项R控制项效果交会逼近1e-21e-41e-4约500s收敛编队保持1e-41e-41e-2推力小过渡慢碰撞规避1e01e-21e-6快速机动注意约束调整时先固定R把Q位置项按10倍步进扫描观察最大推力是否触达限幅再微调R。如果位置误差有震荡通常是速度项权重过低导致阻尼不足。我在MATLAB R2023b和Linux环境下跑过这套脚本结果一致旧版本只要支持lqr和匿名函数同样可以运行。5. 轨道规划中的状态转移矩阵验证技巧5.1 用解析解状态转移矩阵校验trappa4CW方程有解析解。给定初始状态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这种结构后续做多工况枚举时不需要重写积分器。本文还有配套的精品资源点击获取