简介这套MATLAB代码面向编队飞行控制中的C-W方程求解需求可应用于圆轨道编队保持、飞机间相对运动建模等场景适合航空工程、自动化与控制科学方向的师生和工程师学习。资源包共8个文件包含4个.m源程序与4个.asv自动保存备份整体体积仅3KB代码精简、结构清晰易于阅读和二次修改。作者通过主程序与函数文件实现C-W方程定义、初始条件设置、数值求解以及圆形轨道控制策略仿真使读者能直观看到从方程建模到控制验证的完整链路。已有600人学习浏览。借助该示例可快速掌握MATLAB求解微分方程的基本流程理解编队飞行中相对运动建模的关键思路也可基于此代码扩展为多机编队、非圆轨道等更复杂的控制仿真同时可作为课程设计或科研入门的基础模板具有较高的参考价值。1. C-W方程与圆轨道编队飞行控制的数学起点C-W方程是圆轨道近距离编队飞行控制里最常用的线性化相对运动模型。参考航天器绕地球做圆轨道运动时编队卫星与它之间的相对运动可以用一组常系数线性微分方程描述也就是 Clohessy-Wiltshire 方程。之所以值得单独整理一套 MATLAB 实现是因为其状态矩阵恒定工程上能直接调用 lqr、ctrb、ode45 等工具箱函数完成从构型设计到闭环验证的全过程同时它又有封闭解析解方便判断数值误差是来自模型假设还是求解算法本身。这篇文章按“状态空间建模 → 数值积分 → 构型设计 → 脉冲控制 → 解析校验”的顺序给出一套可以在 MATLAB 里直接跑通的圆轨道编队相对运动仿真方案适合做小卫星编队、在轨服务接近段的工程师和航天控制方向的研究生。2. 用MATLAB建立C-W方程的状态空间模型与可控性检查2.1 圆轨道C-W方程的坐标系与线性化边界以参考航天器质心为原点建立 LVLH 坐标系当地垂直当地水平系x 轴沿径向背离地心y 轴沿运动方向z 轴沿轨道面法线方向。对于相对位置向量[x; y; z]和相对速度向量[vx; vy; vz]C-W方程的标准形式为d²x/dt² 3n²x 2n·vy fx/m d²y/dt² -2n·vx fy/m d²z/dt² -n²z fz/m其中 n 是参考轨道角速度m 是编队卫星质量fx/fy/fz 是三轴控制力。需要先确认这个方程的适用前提参考轨道必须是圆轨道轨道偏心率 e 0.001相对距离 rho 必须远小于轨道半径 a工程上一般要求 rho a/1000。满足这两条时n 在积分时间内可视为常数方程才是真正意义上的线性时不变系统否则你应该转向非线性相对轨道动力学C-W方程的结果只能作为一阶近似参考。参数符号说明地球引力常数mu3.986004418e14 m³/s²参考轨道半径aR_e hR_e 取 6371 km轨道角速度nsqrt(mu/a³)单位 rad/s轨道周期T2*pi/n单位 s编队半径rho期望编队空间尺度单位 m控制力f三轴推力单位 N2.2 面向MATLAB的6状态动力学矩阵取状态向量X [x; y; z; vx; vy; vz]把上述二阶方程组改写成dX A*X B*u的形式。直接按 C-W 方程的系数填入矩阵即可mu 3.986004418e14; % 地球引力常数m^3/s^2 Re 6371000; % 地球平均半径m h 500e3; % 轨道高度 500 km a Re h; % 圆轨道半径 n sqrt(mu / a^3); % 轨道角速度rad/s m 12; % 卫星质量kg 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 [zeros(3,3); eye(3) / m];这段代码里最关键的是 A 矩阵第 4 行的3*n^2和2*n前者来自径向重力梯度项后者是 x-y 平面内的科里奥利耦合。第 5 行的-2*n是切向方程的反向耦合位置放错会导致相对轨迹在数值上稳定但几何形状错误。第 6 行的-n^2表明 z 方向退化为独立谐振子这是圆轨道假设的直接结果。建好 A、B 后可以顺手看一下开环系统的特征值分布理解为什么编队会漂移fprintf(特征值\n); disp(eig(A));特征值中包含零特征值对应沿迹向的积分漂移另外两对共轭虚根对应相对运动的周期振荡。零特征值意味着如果没有控制或初始条件不满足约束编队构型会随时间缓慢散开。2.3 可控性和可观测性这两个控制前提编队飞行不是把每个坐标当成独立通道去控制因为 x 和 y 方向存在耦合。做 LQR 或极点配置之前先用控制矩阵检查系统是否可控Co ctrb(A, B); if rank(Co) 6 disp(系统可控可以进行 LQR 设计与极点配置); else warning(系统不可控请检查 A、B 矩阵的符号和维度); end对应的观测问题在实际任务里同样常见。多数小卫星只能通过相对测距或测角得到位置信息没有直接测速。假设只能测量三轴相对位置用位置输出矩阵检查可观测性C [eye(3) zeros(3,3)]; % 只测位置 Ob obsv(A, C); rankOb rank(Ob); disp([位置观测条件下的系统秩为 , num2str(rankOb)]);如果秩不足 6意味着仅靠相对位置无法完全重构 C-W 方程的 6 个状态。实际工程中通常需要增加星间测速或把绝对轨道差分纳入滤波方程否则控制反馈会缺失状态。提示MATLAB 的 ctrb 和 obsv 对连续系统给出的只是代数判据。C-W 方程的耦合结构会影响状态观测器的收敛速度建议后续用仿真确认观测误差能在半个轨道周期内收敛。3. 用ode45解C-W方程圆轨道绕飞算例与误差容限3.1 ODE右侧函数的写法与轨道角速度换算有了状态矩阵 A理论上可以直接用X expm(A*t)*X0求解但后续要接入 J2 摄动、大气阻力、推力饱和等非线性项时最通用的还是把方程写成 ODE 函数喂给 ode45function dX cwRHS(t, X, n, m, uVec) % C-W方程右端项 dX zeros(6,1); dX(1) X(4); dX(2) X(5); dX(3) X(6); dX(4) 3*n^2*X(1) 2*n*X(6) uVec(1)/m; dX(5) -2*n*X(4) uVec(2)/m; dX(6) -n^2*X(3) uVec(3)/m; enduVec 是控制力向量单位先统一成牛顿。这里有个常见错误把2*n*X(6)和-2*n*X(4)的符号写反。写反时轨迹看上去也在振荡但绕飞椭圆的长轴方向会差 90 度且轨迹不再闭合。主脚本调用rho 200; % 编队半径m T 2*pi/n; % 参考轨道周期s tspan [0, 2*T]; % 仿真两个轨道周期 % 使用无漂移初始条件对应相位角 0 X0 [rho; 0; 0; 0; -2*n*rho; n*rho]; opts odeset(RelTol, 1e-9, AbsTol, 1e-11, Stats, on); [tx, Xx] ode45((t, X) cwRHS(t, X, n, m, zeros(3,1)), ... tspan, X0, opts);odeset 参数建议值作用RelTol1e-9控制相对误差编队尺度在百米量级时够用AbsTol1e-11防止接近零的状态分量误差被放大MaxStepT/200限制最大步长避免曲线弯曲处被跨过Statson输出积分步数和函数调用次数用于评估效率轨道角速度 n 必须和长度单位配套使用。上面代码用米作为长度单位n 的单位是 rad/s。如果 a 写成 6878 而不是 6878000那么 n 会凭空放大 1000 倍仿真出来的编队轨迹半径完全失真。3.2 无漂移初始条件为什么速度约束比位置更重要C-W 方程的解里沿迹向 y 会包含(-3*vy0 - 6*n*x0)*t这样的长期项。只要该项系数不为零编队相对距离就会随时间线性增长这不是数值误差而是线性模型本身给出的物理结果。要维持固定编队初始速度必须满足vy0 -2*n*x0在 3.1 节的代码里X0(5) -2*n*rho而X0(1) rho恰好满足这个关系。这个约束意味着径向位置为正编队星在参考星外侧时切向速度必须是负的编队星会略微落后靠轨道角速度差来抵消径向偏移带来的漂移倾向。验证轨迹是否闭合drift norm(Xx(end, 1:3) - Xx(1, 1:3)); if drift 1e-6 disp(相对轨迹闭合初始条件满足无漂移约束); else fprintf(存在漂移位移量 %.4f m\n, drift); end如果位移量在毫米量级说明 ode45 容差设置正常如果位移量达到米级先检查vy0的符号再看n是否算成了度数每秒。3.3 三维相对轨迹的绘图与构型观察figure; plot3(Xx(:,1), Xx(:,2), Xx(:,3), LineWidth, 1.2); xlabel(径向 x (m)); ylabel(沿迹向 y (m)); zlabel(法向 z (m)); axis equal; grid on;axis equal是关键不加的话MATLAB 会自动拉伸坐标轴圆轨道编队的椭圆外形会被误判成直线或畸形。看到的结果应该是一个绕参考点旋转的椭圆柱面上的封闭曲线。坐标系线数量级相同的情况下可以再用view函数从径向和法向两个角度观察投影形状。4. 圆轨道编队构型设计与LQR反馈控制的参数整定4.1 常用编队构型与初始状态表达式实际编队任务中需要根据任务需求反推初始状态。把三类最常用的圆轨道构型整理成表格直接可复现构型位置表达式t0速度表达式t0特点串行编队x0, yd, z0vx0, vy0, vz0沿迹向保持距离最省推进剂xy椭圆编队xA, y0, z0vx0, vy-2nA, vz0在 x-y 平面内形成椭圆绕飞PCO圆编队xρcosθ, y-2ρsinθ, zρsinθvx-nρsinθ, vy-2nρcosθ, vznρcosθ空间三维圆编队常用于卫星伴随飞行介绍 PCOProjected Circular Orbit构型时特别注意它在 x-y 平面的投影本身就接近圆轨道高度差和沿迹向间距通过初始相位角 θ 关联。把构型生成逻辑封装成函数后续 LQR 仿真可以直接复用function X0 initCwFormation(type, rho, n, theta0) s sin(theta0); c cos(theta0); switch type case pco X0 [rho*c; -2*rho*s; rho*s; ... -rho*n*s; -2*rho*n*c; rho*n*c]; case ellipse X0 [rho*c; -2*rho*s; 0; ... -rho*n*s; -2*rho*n*c; 0]; case leader X0 [0; rho; 0; 0; 0; 0]; otherwise error(未定义的编队构型类型); end end串行编队中y 方向保持距离 d 时速度全为零是合理的因为vy0 0 -2*n*x0在 x00 时自动满足。4.2 LQR反馈控制与轨迹跟踪闭环C-W 方程的状态矩阵 A 不含时变项控制矩阵 B 是常数阵这正是 LQR 的理想应用对象。设计目标不是把卫星控制到某个固定点而是跟踪期望的 PCO 周期轨迹。期望状态Xd(t)直接由构型解析表达式给出function Xd cwRefTraj(t, rho, n, theta0) phi n*t theta0; s sin(phi); c cos(phi); Xd [rho*c; -2*rho*s; rho*s; ... -rho*n*s; -2*rho*n*c; rho*n*c]; endLQR 增益计算Q diag([1e-3, 1e-3, 1e-3, 1, 1, 1]); R eye(3) * 0.1; K lqr(A, B, Q, R); fprintf(反馈增益矩阵 K\n); disp(K);权重 Q 中位置项取 1e-3速度项取 1含义是更侧重抑制速度偏差避免轨道重构时出现大幅位置过冲R 取对角 0.1表示三轴控制力成本相同。Q 和 R 的相对比例直接决定闭环极点位置R 越小极点越远离虚轴收敛越快推力峰值越高。闭环微分方程function dX cwClosedLoop(t, X, A, B, K, rho, n, theta0) Xd cwRefTraj(t, rho, n, theta0); u -K * (X - Xd); dX A*X B*u; end仿真时把初始状态故意偏离期望构型观察控制器拉回能力X0_init initCwFormation(pco, rho, n, 0); X0_err X0_init [10; -15; 5; 0.01; 0.02; -0.01]; [tx, Xx] ode45((t, X) cwClosedLoop(t, X, A, B, K, rho, n, 0), ... [0, 3*T], X0_err, opts); err_norm vecnorm(Xx(:, 1:3) - Xx(:, 1:3), 2, 2); figure; plot(tx/T, err_norm); xlabel(时间 / 轨道周期); ylabel(位置偏差范数 (m));参数调节方向位置偏差收敛表现推力峰值表现Q 位置权重调大收敛加快、稳态误差减小推力峰值明显增大Q 速度权重调大超调减少、轨迹更平滑收敛过程变慢R 调大输出推力更小、执行器压力小恢复时间变长R 调小响应更快、能处理大初始偏差容易超出推力饱和边界4.3 推力约束和控制带宽的取舍实际星载推力器有上下限。直接在控制律输出后加饱和函数是最常用的处理方式uMax 0.1; % 单轴最大推力 N u -K * (X - Xd); u max(min(u, uMax), -uMax);饱和限幅后LQR 的线性反馈性质只在小偏差范围内成立。大偏差时控制器处于饱和状态不能指望闭环极点分析结果仍然成立。所以工程上通常把 LQR 设计成“小偏差修正器”先靠脉冲推力完成粗调再用连续 LQR 修细差而不是让 LQR 单独承担整个编队重构任务。5. 脉冲控制中C-W方程的MATLAB实现与单位一致性5.1 单位制陷阱先统一再积分ODE45 不知道你的物理量用的什么单位它只按数值积分。单位不一致是最难排查的故障来源尤其是在从论文复现代码时错误表现可能原因修正方法仿真一圈的时长明显不对n 用了 deg/s 而不是 rad/s确认 n sqrt(mu/a^3)单位 rad/s编队轨迹尺度异常轨道半径 a 用 km而 rho 用 m全部统一为米闭环响应曲线出现高频振荡角度用度数传入 cos/sincos/sin 只接受弧度相对轨迹闭合但方向反了2n 和 -2n 位置互换按 C-W 方程标准矩阵逐项核对推荐的统一策略是所有长度用米、时间用秒、质量用千克、角度用弧度。轨道周期用T 2*pi/n表达不用分钟数换算避免多余的乘除因子。5.2 脉冲推力的仿真流程脉冲控制不能直接写成 ODE 右侧的连续函数因为瞬时速度跳变会让变步长积分器把步长压到极小计算代价成倍增长。稳妥做法是把仿真切成若干段在脉冲时刻停表、改状态、重新启动积分。以编队重构为例t_pulse 0.5 * T; % 半个轨道周期后执行推进 theta_target pi/3; % 目标构型相位角 % 第一段从初始时刻到脉冲时刻 [tSeg1, XSeg1] ode45((t, X) cwRHS(t, X, n, m, zeros(3,1)), ... [0, t_pulse], X0, opts); Xafter1 XSeg1(end, :); % 第二段计算需要的速度增量 Xd_target cwRefTraj(t_pulse, rho, n, theta_target); dV Xd_target(4:6) - Xafter1(4:6); % 施加冲量 Xafter1(4:6) Xafter1(4:6) dV; % 第三段从脉冲后继续积分到仿真终点 [tSeg2, XSeg2] ode45((t, X) cwRHS(t, X, n, m, zeros(3,1)), ... [t_pulse, 2*T], Xafter1, opts);这里只对速度状态做了修改位置状态保持不变符合脉冲推力瞬时施加的物理近似。冲量大小norm(dV)可以直接折算推进剂消耗。注意Xd_target必须在脉冲时刻用 cwRefTraj 求值不能拿初始构型的相位角直接代入否则速度增量会算错。5.3 用事件函数处理编队超界修正编队飞行中最常见的维持策略是死区控制当编队参数偏离到某个边界时施加一次修正冲量。这个“何时触发”的判断最适合交给 ode45 的事件函数而不是在循环里频繁判断状态量function [value, isterminal, direction] driftEvent(t, X, boundary) value abs(X(2)) - boundary; % 沿迹向超界 isterminal 1; % 触发后停止当前段积分 direction 0; % 双向触发 end调用时将事件函数加入 odesetboundary 250; % 沿迹向允许范围m optsEvent odeset(RelTol, 1e-9, AbsTol, 1e-11, ... Events, (t, X) driftEvent(t, X, boundary));事件触发后ode45 返回时te给出触发时刻Xe给出触发时刻的状态。你需要基于当前状态计算修正冲量然后用和 5.2 节同样的“切段重初始化”方式继续积分。事件函数返回的direction0表示正向和负向超界都要检测适合沿迹向正负漂移都需要修正的场景。6. 用解析状态转移矩阵校验ode45结果与精度控制6.1 C-W方程的解析状态转移矩阵C-W 方程是线性时不变系统状态转移矩阵Phi(t, t0) expm(A*(t-t0))有解析闭式。闭式解比任何数值积分都快也完全没有步长误差。实现它只需要 n 和 tfunction Phi cwSTM(t, n) c cos(n*t); s sin(n*t); Phi [4-3*c 0 0 s/n 2*(1-c)/n 0; 6*(s-n*t) 1 0 -2*(1-c)/n (4*s-3*n*t)/n 0; 0 0 c 0 0 s/n; 3*n*s 0 0 c 2*s 0; -6*n*(1-c) 0 0 -2*s 4*c-3 0; 0 0 -n*s 0 0 c]; end这个矩阵中的6*(s-n*t)和(4*s-3*n*t)/n两项包含n*t的线性项正好对应沿迹向的长期漂移项。用它推进任意初始状态X_t cwSTM(deltaT, n) * X0;6.2 数值解与解析解的误差对比用 ode45 跑完全程后把数值终端状态和解析终端状态做差t_final 2*T; X_num Xx(end, :); X_stm cwSTM(t_final, n) * X0; err norm(X_num - X_stm); fprintf(ode45 与解析解终端状态偏差%.4e\n, err); if err 1e-6 disp(数值积分一致); elseif err 1e-3 disp(存在小幅偏差检查容差设置); else disp(偏差过大优先排查单位和符号问题); end如果偏差在 1e-6 量级以下说明 ode45 的容差设置和单位处理都正确。如果偏差达到米的量级不要急着调低容差先核对X0是否满足无漂移约束、n是否按 rad/s 计算。ode45 的误差在通常容差下远小于物理模型误差真正的模型误差来自线性化和圆轨道假设而不是积分器。6.3 后续扩展时的参考基准选择实际项目里线性 C-W 方程的价值在于提供“参考真值”而非替代高精度轨道预报。建议把cwSTM函数保留在工程目录里作为每次改动的回归测试基准把线性模型里新加入的摄动力削弱到零时仿真结果必须能回到 STM 的解。比如加入 J2 摄动后应在代码里保留一个开关半长轴偏差设为零时恢复 C-W 方程这样测试失败时能判断新模块有没有污染原有状态更新逻辑。真正的编队飞行仿真会把 C-W 方程作为制导律设计基线再外接 STK 或高精度轨道动力学做最终确认而不是直接让 ode45 在没有解析解的模型上盲目增大计算量。本文还有配套的精品资源点击获取