简介面向卫星姿态控制入门者的 MATLAB 飞轮仿真教学包以飞轮为执行机构实现三轴稳定控制基于经典 PID 控制器动力学建模参考《航天器姿态动力学与控制》标准模型适合理解状态方程推导与控制律编程。资源包共 14 个文件、约 594KB涵盖 6 个 M 文件、1 个 Simulink 模型slx、S 函数及相关结果图片其中 M 文件用于编写动力学模型与 PID 控制器Simulink 模型则通过图形化方式配合 S 函数搭建闭环系统既适合代码调试也适合框图理解。功能覆盖姿态角/角速度动态响应、飞轮转速输出、控制力矩计算并验证典型扰动下的稳定性包含无控状态与 PID 控制效果的对比结果。已有 32 人学习/下载所有模块命名清晰、变量注释规范便于调试和参数调整也方便后续扩展至 LQR 或非线性补偿等进阶控制策略。 我整理这套MATLAB飞轮式卫星姿态控制仿真包其实是被课程设计逼出来的。当时既要交一份能跑出曲线的M文件又要在Simulink里摆出控制回路还要体现S函数这种高级玩法网上零散找的资料总是对不上号。后来我把飞轮式卫星姿态控制整个模型重新推了一遍写成了这套双实现仿真包同一条控制律一边是纯M文件的数值积分一边是SimulinkS函数的模块化仿真两边结果互相验证。如果你正准备用MATLAB做卫星姿态控制仿真或者对S函数怎么写、怎么和模型连起来有疑问这篇文章能帮你省下不少弯路。这套仿真包解决的核心问题其实很具体飞轮式执行机构如何与刚体姿态动力学组成闭环以及同一套算法如何在M文件和Simulink里各跑一遍。它不是要替代专业航天软件而是让你在入门阶段就能直观看到角度误差怎样收敛、飞轮力矩怎样变化并且能快速改参数做对比实验。内容适合课程设计、毕业设计也适合刚开始接触姿态控制算法验证的工程师。1. 项目背景与双实现方案的由来1.1 为什么选飞轮作为执行机构卫星姿态控制执行机构常见的有喷气推力器、磁力矩器和反作用飞轮。喷气推力器控制力矩大但消耗燃料磁力矩器结构简单但力矩很小通常只用于偏置力矩卸载。飞轮的好处是不消耗燃料通过改变飞轮角动量来与卫星本体交换动量长期在轨运行可以用太阳能补充电能因此工程上非常广泛。响应速度也够快适合做三轴稳定控制。仿真时飞轮模型并不复杂核心是它产生的反作用力矩会同时作用在卫星本体和飞轮上。只要把飞轮角动量作为状态变量在动力学方程里加上一项就能把执行机构的行为完整地仿真出来。这个特点让飞轮尤其适合做控制算法验证因为你能很直观地看到控制力矩引起的角速度响应。1.2 双实现方案的取舍M文件仿真和Simulink仿真不是二选一的关系而是互补的。M文件适合做快速原型验证写一个函数、调用ode45、画图整个流程干净利落。批量扫描参数、自动输出报告时脚本优势非常明显。Simulink的优点在于模块化控制回路、被控对象、执行机构各自独立信号流动一目了然方便给别人讲解。配合S函数还能把自定义算法和数据流很好地封装起来为后续用Simulink Coder生成代码做准备。我在这次仿真包里特意把两套实现的命名、参数、输出都保持完全一致这样你在M文件里改一组转动惯量拿到Simulink里用同样值跑曲线基本重合。两套实现互相印证能排查出不少模型公式上的低级错误。比如我曾经在M文件里把控制项符号写反结果Simulink曲线稳定而脚本发散一对比马上就找到问题。1.3 仿真包文件结构整个仿真包按功能拆成几个文件目标是“拿过去就能跑改参数就能用”。推荐结构如下satellite_params.m所有参数的设置脚本包括转动惯量、飞轮参数、控制器增益、初值、仿真时长。attitude_dynamics.mM文件仿真用的被控对象微分方程函数也就是状态导数的计算。reaction_wheel_ctrl.mM文件仿真用的控制器函数输入为姿态误差和角速度输出为飞轮控制力矩。sim_mfile_main.mM文件仿真主程序负责初始化、调用ode45、绘图。attitude_ctrl_sfun.mSimulink中使用的Level-2 MATLAB S-Function控制律和reaction_wheel_ctrl.m完全一致。attitude_control_sim.slxSimulink模型里面用S函数模块加动力学模块搭成闭环。plot_results.m公共绘图脚本两个仿真结果统一使用。这套结构的好处是M文件和Simulink共享同一个参数文件改完参数两边一起生效。我在实际使用时还会把satellite_params.m里的参数打印出来方便留存每次仿真的工况记录。1. 理论模型与控制器设计1.1 姿态运动学模型为了不受欧拉角奇异问题困扰仿真包里用的是四元数。姿态四元数定义为q [q0, q1, q2, q3]其中q0是标量部分[q1, q2, q3]是矢量部分。运动学方程是d(q)/dt 0.5 * [ q0 * I3 skew(qv); -qv ] * omega这里的I3是3x3单位阵skew(qv)是矢量部分构成的反对称阵omega是卫星本体角速度。四元数运动学是线性形式数值上比较好处理。需要特别注意四元数范数不为1会带来姿态误差计算错误所以每次算完必须归一化尤其是长时间仿真时数值积分误差会让四元数慢慢漂移。初始姿态我习惯设置成一个小角度偏差比如欧拉角大约5度、5度、5度转换到四元数后速度很快收敛。如果初始角速度也给一个非零小值能更好地测试控制器的阻尼性能。1.2 刚体卫星动力学与飞轮耦合刚体卫星带反作用飞轮的动力学方程需要把飞轮角动量纳入考虑。这里直接给出用于M文件和Simulink的状态方程形式J * d(omega)/dt - cross(omega, J*omega h_wheel) - d(h_wheel)/dt T_dist其中J是卫星本体转动惯量矩阵omega是本体角速度h_wheel是飞轮角动量T_dist是外部干扰力矩。右侧第一项是陀螺力矩第二项是飞轮对本体反作用力矩第三项是外干扰。这个方程里面最重要也最容易出错的是符号。飞轮角动量增大时卫星本体向相反方向转动体现在方程里就是负号。还需要一个飞轮自身的角动量积分方程d(h_wheel)/dt u_cmd这里的u_cmd是控制器送来的飞轮指令力矩。我把飞轮模型简化成理想积分环节只考虑指令力矩和角动量变化不考虑摩擦、饱和和转速限制。如果你要做更接近工程的分析可以在saturate函数里加上力矩饱和在飞轮模型里加上角动量饱和。1.3 PD控制器设计与参数选取控制律采用经典的PD反馈形式。姿态误差用当前姿态相对于参考姿态的误差四元数矢量部分来表示。控制力矩为u_cmd -Kp * qe_vec - Kd * omega其中qe_vec是误差四元数的矢量部分omega是本体角速度。这里的逻辑很直观姿态偏离目标时Kp项提供回复力矩角速度存在时Kd项提供阻尼力矩。飞轮把u_cmd当作指令力矩最终让姿态误差和角速度同时收敛到零。参数选取上可以先从转动惯量最大的轴入手估算系统带宽。转动惯量J10 kg·m²左右的卫星如果希望闭环带宽在0.1 Hz附近Kp大致在几十量级Kd根据阻尼比取在十倍Kp的平方根左右。仿真包里默认的Kp25Kd12在默认参数下能稳定收敛超调量也比较小。改参数时注意不要只看时域曲线可以画出误差四元数范数能更清楚地看出控制是否真正收敛。3. M文件仿真实现细节3.1 主程序初始化与参数配置M文件仿真主程序的第一件事是调用satellite_params.m把转动惯量、初值、控制器增益全部加载到工作区。然后用一个结构体params把这些参数组装起来方便传给ode求解器。我的习惯用法是run(satellite_params.m); params.J J_sat; params.Kp Kp; params.Kd Kd; params.cmd_limit 0.2; params.T_dist [0.001; 0.001; 0.001];状态向量定义为一列10维向量前4维是四元数接着3维是本体角速度最后3维是飞轮角动量。初值设置时四元数必须归一化角速度和飞轮角动量一般从零开始。这个顺序要和动力学函数严格对应不然错位之后曲线会非常诡异。3.2 动力学函数与ode45求解动力学函数是纯M文件仿真的核心。它的输入是时间t、状态x、参数结构体params输出是状态导数dx。关键代码段如下function dx attitude_dynamics(t, x, params) q x(1:4); q q / norm(q); omega x(5:7); hw x(8:10); qe quat_error(q_ref, q); u_cmd -params.Kp * qe(2:4) - params.Kd * omega; u_cmd max(min(u_cmd, params.cmd_limit), -params.cmd_limit); dhw u_cmd; domega params.J \ (-cross(omega, params.J*omega hw) - dhw params.T_dist); dq 0.5 * quat_kinematics(q, omega); dx [dq; domega; dhw]; endquat_error和quat_kinematics可以写成独立函数让你在不同仿真里复用。需要注意每次计算刚开始就要对四元数归一化但不要把归一化后的q覆盖回x里的原状态因为ode45的状态值本身会保留到下一步只有导数才需要修正。如果直接在导数里用了原始未归一化的q姿态误差会逐步失真。主程序调用ode45时我会选择变步长求解器并设置较高的相对误差容限options odeset(RelTol, 1e-8, AbsTol, 1e-9, MaxStep, 0.1); [t, x] ode45((t,x) attitude_dynamics(t, x, params), tspan, x0, options);如果计算速度很慢或者曲线出现高频振荡可以优先检查参数噪声。飞轮符号错误往往表现为曲线发散而不是简单的振荡。我调试时习惯先在控制器里把u_cmd打印出来看它是否在按预期方向变化这比看姿态角更直观。3.3 仿真结果可视化M文件仿真结束后我把结果直接绘制成两张图。第一张图画出四元数变化曲线第二张图画本体角速度和飞轮指令力矩。用两个子图并排比较能清楚看到角速度先被阻尼抑制姿态误差再慢慢归零。绘制时的关键点是把时间轴统一避免因为维度问题对不上。实际跑完的典型结果是初始偏差大约5度在PD控制下约15秒内误差降到1度以内稳态时因为干扰力矩的存在会有大约0.1度左右的静差。如果你希望消除稳态误差可以在PD控制器基础上加积分项或者引入更高级的姿态控制器。我在仿真包里保留了PID的实现位置只需要在函数里加一个积分状态即可。4. Simulink S函数实现方法4.1 Simulink模型整体框架Simulink模型的主体思路和M文件完全一致但信号流更清晰。我在模型中放了一个S-Function模块名字叫attitude_ctrl_sfun它接收两个输入姿态误差四元数和本体角速度输出一个三维修正力矩。控制器模块之后接的是一个被控对象子系统子系统内部用积分器累加角度导数和角速度导数同时把飞轮角动量作为状态反馈回控制器。搭建时需要注意被控对象里面的状态排列顺序必须和S函数里的输入顺序一致。我通常把模型的顶层端口做成一个大向量方便连线然后在被控对象内部用Selector把四元数、角速度、飞轮角动量拆开。这样看起来更结构化调试时也能单独观察每个信号。4.2 用Level-2 MATLAB S-Function写控制器这里我采用Level-2 MATLAB S-Function它比老的Level-1 S-Function更容易支持数组输入、参数配置和采样时间设定。核心文件attitude_ctrl_sfun.m开头部分是设置阶段function attitude_ctrl_sfun(block) setup(block); function setup(block) block.NumInputPorts 2; block.NumOutputPorts 1; block.InputPort(1).Dimensions 4; block.InputPort(2).Dimensions 3; block.InputPort(1).SamplingMode Sample; block.InputPort(2).SamplingMode Sample; block.OutputPort(1).Dimensions 3; block.OutputPort(1).SamplingMode Sample; block.NumDialogPrms 3; block.DialogPrmsTunable {Nontunable,Nontunable,Nontunable}; block.SampleTimes [0 0]; block.RegBlockMethod(Outputs, Outputs); block.RegBlockMethod(Terminate, Terminate); end关键的设置是block.SampleTimes [0 0]表示连续采样时间这样控制器和连续被控对象位于同一仿真层。如果你希望控制律按固定时间周期更新比如每个0.01秒更新一次那就要改成[0.01 0]同时给动力学模型设置适当采样时间。输出计算部分直接复用M文件里的控制律function Outputs(block) qe block.InputPort(1).Data; omega block.InputPort(2).Data; Kp block.DialogPrm(1).Data; Kd block.DialogPrm(2).Data; cmd_limit block.DialogPrm(3).Data; qe(1:4) qe(1:4) / norm(qe(1:4)); u_cmd -Kp * qe(2:4) - Kd * omega; u_cmd max(min(u_cmd, cmd_limit), -cmd_limit); block.OutputPort(1).Data u_cmd; end把参数放在DialogPrm里而不是硬编码这是S函数规范和常规实践。这样你在Simulink模型里双击S函数模块就能填写Kp、Kd和力矩限幅不需要改代码。如果需要频繁调参还可以打开其“Block Parameters”下的Tunable设置但在Simulink中直接调S函数对话框参数会更方便。要注意DialogPrmsTunable设置为Nontunable这样即使参数改了S函数在快速加速模式下也能正常更新。如果想在仿真过程中动态修改需要选择Tunable但要注意外部模式兼容性。4.3 模型参数封装与仿真配置为了让模型易用我给S函数模块套了一层Mask把Kp、Kd、力矩限制这三个参数做成对话框这样比让用户直接面对S函数参数要好。操作方法是右键S函数模块选择“Mask Create Mask”在Mask Editor里添加三个编辑框然后在“Initialization”里把参数对应到block.DialogPrm。这样做的好处是同事或学生看到模块就知道该填什么参数不用去翻S函数源码。Simulink模型里的被控对象部分我推荐你直接把动力学方程写成MATLAB Function块而不是用一堆Sum和Gain模块。这样做的好处是代码和M文件中的公式完全一致维护成本低。MATLAB Function块内部写一个普通函数输入状态向量和干扰力矩输出状态导数然后连接积分器。如果你更希望完全用模块搭建也完全可以但会多出几条布线和矩阵运算块容易出错。仿真配置上一定要在Simulink“模型设置”里选“变步长”和ode45求解器否则连续时间状态可能不准确。如果S函数是连续采样时间模型会自动采用连续求解器。我还会设置相对误差1e-6和M文件的ode45配置保持一致这样两个仿真的数值精度具有可比性。5. 常见问题与排查技巧5.1 S函数采样时间设置不当导致的异常S函数最常见的坑就是采样时间设置。block.SampleTimes [0 0]表示连续时间[0.01 0]表示离散采样[-1 0]表示继承驱动源。如果你把控制器S函数设置成离散采样而动力学是连续积分器模型会自动唤醒离散事件但只要参数不匹配可能会出现奇怪的振铃。实际操作中我遇到过把采样时间写成[0.01 1]后模型无法进入连续仿真状态的情况那是因为第二项表示偏移量不是随便写的。建议在没有特殊需求时保持连续采样时间或明确离散采样周期。采样时间问题还常常伴随仿真速度过慢。如果你发现模型每个步长都非常小先看看是不是S函数里用了连续采样时间但内部却包含离散变量。连续采样时间如果内部又依赖前一步的值会触发零阶保持问题等效于引入一个极小的时间常数。此时可以把控制律设为离散时间采样或者在内部加一个Memory块作为状态缓冲。5.2 代数环与数值发散在Simulink中如果控制器的输出又直接参与控制器输入的运算却没有积分器或延迟模块隔开就会构成代数环。代数环的求解需要非线性迭代轻则警告重则仿真发散。飞轮动力学中是连续积分一般情况下不太容易产生代数环但如果你加入了反步控制或某种直接前馈路径就要小心。排查代数环的方法是查看诊断信息或者直接给可能的反馈回路插入一个极小的滞后模块。我更推荐的结构是让控制器只依赖状态变量而不是输出变量凡是从积分器出来的状态都可以直接反馈不会形成代数环。如果你需要给控制器输入理想力矩的目标值可以把这个目标值作为状态变量延一拍用Unit Delay模块处理。5.3 四元数归一化与单位一致性我差点被四元数问题坑到底。刚开始跑Simulink模型时误差四元数曲线看起来很正常但是角速度持续小幅漂移。检查发现是积分器输出端没有对四元数归一化误差只在控制器内部归一化长期仿真时状态四元数范数慢慢变成了1.005。虽然每条曲线看起来都差不多但再往下做姿态估计精度分析时误差会被放大。处理办法有两个一是在动力学函数里每次都计算归一化后的导数这适合M文件二是在Simulink中把四元数状态单独接到一个归一化子系统上用四元数范数的倒数乘回去。两种办法我都试过第一种简单第二种更明显能在模型图上看到这个步骤。单位一致性也很重要尤其是控制器增益的单位要和角速度、四元数误差的单位匹配。角速度默认用rad/s如果你拿到一组用度每秒采集的数据必须换算否则Kd完全失效。5.4 仿真结果对不齐怎么办类似M文件和Simulink曲线对不上时我建议先做静态坐标测试设置一个初始姿态误差同时把Kd设为0看看控制器力矩是不是只沿着误差方向恢复。如果一致再打开Kd验证阻尼项。按顺序排查比一次改多个变量快得多。再有就是使用相同的求解器容差M文件ode45的RelTol和Simulink模型设置的相对误差必须一致否则微小数值差异会被时间轴累积放大。曲线粘贴在一起比较时记得对齐时间向量Simulink的变步长输出点可能和M文件不同建议用interp1插值到统一时间网格再比较。6. 实操心得与后续扩展6.1 我踩过的几个坑第一飞轮角动量符号。我最初在动力学方程里把-dhw写成了dhw结果控制器无论怎么调都发散。后来把飞轮角动量单独画出来发现它和卫星角速度在同步增长而不是相反才反应过来符号错了。所以建议你在M文件仿真里加一句断言检查控制力是否与误差方向一致。第二S函数的参数可视化。一开始我直接用字符串把参数填进S函数模块结果修改一次参数就要重新编译一次还不方便对比。后来我把参数全部提取到Mask里统一用结构体传递调试效率高了很多。特别是多个S函数同时存在时结构化参数能显著减少错误。第三阻尼参数过大导致高频振荡。PD控制器中Kd过大会让飞轮指令力矩在仿真初始阶段瞬间饱和反而引起振荡。仿真包里的力矩限幅是必要的但限幅后的饱和效应会让等效增益下降直观表现是响应变慢。如果你想要更好的动态品质可以加入抗饱和积分或做控制分配而不只是简单地截断。6.2 可以继续扩展的方向这套仿真包可以往很多方向发展。给飞轮加上角动量饱和与摩擦力矩就能复现飞轮转速超出限制时的下溢行为。将控制律从PD换成滑模控制或自适应控制时S函数只需修改Outputs部分非常适合做算法对比。如果要用在嵌入式工程中建议把attitude_ctrl_sfun.m改写成C MEX S函数然后结合Simulink Coder生成C代码在硬件在环平台上验证。我在实际项目里遇到的最大价值是把S函数作为算法接口把控制策略和实验平台分离这样每次调算法只需换一个S函数文件模型基本不动。最后再说一个小技巧无论M文件还是Simulink都要在仿真前保存一份参数快照。我在参数脚本头部加了一行disp(参数载入完成)并且在每次仿真后把关键性能指标存成mat文件后面回看大量仿真记录时非常有用。飞轮姿态控制仿真的难点不在某个单独模块而在整个闭环的一致性你只要把公式、符号、参数和采样时间四项对齐剩下的只是时间长短的问题。本文还有配套的精品资源点击获取
飞轮式卫星姿态控制MATLAB仿真：M文件与Simulink双实现详解
简介面向卫星姿态控制入门者的 MATLAB 飞轮仿真教学包以飞轮为执行机构实现三轴稳定控制基于经典 PID 控制器动力学建模参考《航天器姿态动力学与控制》标准模型适合理解状态方程推导与控制律编程。资源包共 14 个文件、约 594KB涵盖 6 个 M 文件、1 个 Simulink 模型slx、S 函数及相关结果图片其中 M 文件用于编写动力学模型与 PID 控制器Simulink 模型则通过图形化方式配合 S 函数搭建闭环系统既适合代码调试也适合框图理解。功能覆盖姿态角/角速度动态响应、飞轮转速输出、控制力矩计算并验证典型扰动下的稳定性包含无控状态与 PID 控制效果的对比结果。已有 32 人学习/下载所有模块命名清晰、变量注释规范便于调试和参数调整也方便后续扩展至 LQR 或非线性补偿等进阶控制策略。 我整理这套MATLAB飞轮式卫星姿态控制仿真包其实是被课程设计逼出来的。当时既要交一份能跑出曲线的M文件又要在Simulink里摆出控制回路还要体现S函数这种高级玩法网上零散找的资料总是对不上号。后来我把飞轮式卫星姿态控制整个模型重新推了一遍写成了这套双实现仿真包同一条控制律一边是纯M文件的数值积分一边是SimulinkS函数的模块化仿真两边结果互相验证。如果你正准备用MATLAB做卫星姿态控制仿真或者对S函数怎么写、怎么和模型连起来有疑问这篇文章能帮你省下不少弯路。这套仿真包解决的核心问题其实很具体飞轮式执行机构如何与刚体姿态动力学组成闭环以及同一套算法如何在M文件和Simulink里各跑一遍。它不是要替代专业航天软件而是让你在入门阶段就能直观看到角度误差怎样收敛、飞轮力矩怎样变化并且能快速改参数做对比实验。内容适合课程设计、毕业设计也适合刚开始接触姿态控制算法验证的工程师。1. 项目背景与双实现方案的由来1.1 为什么选飞轮作为执行机构卫星姿态控制执行机构常见的有喷气推力器、磁力矩器和反作用飞轮。喷气推力器控制力矩大但消耗燃料磁力矩器结构简单但力矩很小通常只用于偏置力矩卸载。飞轮的好处是不消耗燃料通过改变飞轮角动量来与卫星本体交换动量长期在轨运行可以用太阳能补充电能因此工程上非常广泛。响应速度也够快适合做三轴稳定控制。仿真时飞轮模型并不复杂核心是它产生的反作用力矩会同时作用在卫星本体和飞轮上。只要把飞轮角动量作为状态变量在动力学方程里加上一项就能把执行机构的行为完整地仿真出来。这个特点让飞轮尤其适合做控制算法验证因为你能很直观地看到控制力矩引起的角速度响应。1.2 双实现方案的取舍M文件仿真和Simulink仿真不是二选一的关系而是互补的。M文件适合做快速原型验证写一个函数、调用ode45、画图整个流程干净利落。批量扫描参数、自动输出报告时脚本优势非常明显。Simulink的优点在于模块化控制回路、被控对象、执行机构各自独立信号流动一目了然方便给别人讲解。配合S函数还能把自定义算法和数据流很好地封装起来为后续用Simulink Coder生成代码做准备。我在这次仿真包里特意把两套实现的命名、参数、输出都保持完全一致这样你在M文件里改一组转动惯量拿到Simulink里用同样值跑曲线基本重合。两套实现互相印证能排查出不少模型公式上的低级错误。比如我曾经在M文件里把控制项符号写反结果Simulink曲线稳定而脚本发散一对比马上就找到问题。1.3 仿真包文件结构整个仿真包按功能拆成几个文件目标是“拿过去就能跑改参数就能用”。推荐结构如下satellite_params.m所有参数的设置脚本包括转动惯量、飞轮参数、控制器增益、初值、仿真时长。attitude_dynamics.mM文件仿真用的被控对象微分方程函数也就是状态导数的计算。reaction_wheel_ctrl.mM文件仿真用的控制器函数输入为姿态误差和角速度输出为飞轮控制力矩。sim_mfile_main.mM文件仿真主程序负责初始化、调用ode45、绘图。attitude_ctrl_sfun.mSimulink中使用的Level-2 MATLAB S-Function控制律和reaction_wheel_ctrl.m完全一致。attitude_control_sim.slxSimulink模型里面用S函数模块加动力学模块搭成闭环。plot_results.m公共绘图脚本两个仿真结果统一使用。这套结构的好处是M文件和Simulink共享同一个参数文件改完参数两边一起生效。我在实际使用时还会把satellite_params.m里的参数打印出来方便留存每次仿真的工况记录。1. 理论模型与控制器设计1.1 姿态运动学模型为了不受欧拉角奇异问题困扰仿真包里用的是四元数。姿态四元数定义为q [q0, q1, q2, q3]其中q0是标量部分[q1, q2, q3]是矢量部分。运动学方程是d(q)/dt 0.5 * [ q0 * I3 skew(qv); -qv ] * omega这里的I3是3x3单位阵skew(qv)是矢量部分构成的反对称阵omega是卫星本体角速度。四元数运动学是线性形式数值上比较好处理。需要特别注意四元数范数不为1会带来姿态误差计算错误所以每次算完必须归一化尤其是长时间仿真时数值积分误差会让四元数慢慢漂移。初始姿态我习惯设置成一个小角度偏差比如欧拉角大约5度、5度、5度转换到四元数后速度很快收敛。如果初始角速度也给一个非零小值能更好地测试控制器的阻尼性能。1.2 刚体卫星动力学与飞轮耦合刚体卫星带反作用飞轮的动力学方程需要把飞轮角动量纳入考虑。这里直接给出用于M文件和Simulink的状态方程形式J * d(omega)/dt - cross(omega, J*omega h_wheel) - d(h_wheel)/dt T_dist其中J是卫星本体转动惯量矩阵omega是本体角速度h_wheel是飞轮角动量T_dist是外部干扰力矩。右侧第一项是陀螺力矩第二项是飞轮对本体反作用力矩第三项是外干扰。这个方程里面最重要也最容易出错的是符号。飞轮角动量增大时卫星本体向相反方向转动体现在方程里就是负号。还需要一个飞轮自身的角动量积分方程d(h_wheel)/dt u_cmd这里的u_cmd是控制器送来的飞轮指令力矩。我把飞轮模型简化成理想积分环节只考虑指令力矩和角动量变化不考虑摩擦、饱和和转速限制。如果你要做更接近工程的分析可以在saturate函数里加上力矩饱和在飞轮模型里加上角动量饱和。1.3 PD控制器设计与参数选取控制律采用经典的PD反馈形式。姿态误差用当前姿态相对于参考姿态的误差四元数矢量部分来表示。控制力矩为u_cmd -Kp * qe_vec - Kd * omega其中qe_vec是误差四元数的矢量部分omega是本体角速度。这里的逻辑很直观姿态偏离目标时Kp项提供回复力矩角速度存在时Kd项提供阻尼力矩。飞轮把u_cmd当作指令力矩最终让姿态误差和角速度同时收敛到零。参数选取上可以先从转动惯量最大的轴入手估算系统带宽。转动惯量J10 kg·m²左右的卫星如果希望闭环带宽在0.1 Hz附近Kp大致在几十量级Kd根据阻尼比取在十倍Kp的平方根左右。仿真包里默认的Kp25Kd12在默认参数下能稳定收敛超调量也比较小。改参数时注意不要只看时域曲线可以画出误差四元数范数能更清楚地看出控制是否真正收敛。3. M文件仿真实现细节3.1 主程序初始化与参数配置M文件仿真主程序的第一件事是调用satellite_params.m把转动惯量、初值、控制器增益全部加载到工作区。然后用一个结构体params把这些参数组装起来方便传给ode求解器。我的习惯用法是run(satellite_params.m); params.J J_sat; params.Kp Kp; params.Kd Kd; params.cmd_limit 0.2; params.T_dist [0.001; 0.001; 0.001];状态向量定义为一列10维向量前4维是四元数接着3维是本体角速度最后3维是飞轮角动量。初值设置时四元数必须归一化角速度和飞轮角动量一般从零开始。这个顺序要和动力学函数严格对应不然错位之后曲线会非常诡异。3.2 动力学函数与ode45求解动力学函数是纯M文件仿真的核心。它的输入是时间t、状态x、参数结构体params输出是状态导数dx。关键代码段如下function dx attitude_dynamics(t, x, params) q x(1:4); q q / norm(q); omega x(5:7); hw x(8:10); qe quat_error(q_ref, q); u_cmd -params.Kp * qe(2:4) - params.Kd * omega; u_cmd max(min(u_cmd, params.cmd_limit), -params.cmd_limit); dhw u_cmd; domega params.J \ (-cross(omega, params.J*omega hw) - dhw params.T_dist); dq 0.5 * quat_kinematics(q, omega); dx [dq; domega; dhw]; endquat_error和quat_kinematics可以写成独立函数让你在不同仿真里复用。需要注意每次计算刚开始就要对四元数归一化但不要把归一化后的q覆盖回x里的原状态因为ode45的状态值本身会保留到下一步只有导数才需要修正。如果直接在导数里用了原始未归一化的q姿态误差会逐步失真。主程序调用ode45时我会选择变步长求解器并设置较高的相对误差容限options odeset(RelTol, 1e-8, AbsTol, 1e-9, MaxStep, 0.1); [t, x] ode45((t,x) attitude_dynamics(t, x, params), tspan, x0, options);如果计算速度很慢或者曲线出现高频振荡可以优先检查参数噪声。飞轮符号错误往往表现为曲线发散而不是简单的振荡。我调试时习惯先在控制器里把u_cmd打印出来看它是否在按预期方向变化这比看姿态角更直观。3.3 仿真结果可视化M文件仿真结束后我把结果直接绘制成两张图。第一张图画出四元数变化曲线第二张图画本体角速度和飞轮指令力矩。用两个子图并排比较能清楚看到角速度先被阻尼抑制姿态误差再慢慢归零。绘制时的关键点是把时间轴统一避免因为维度问题对不上。实际跑完的典型结果是初始偏差大约5度在PD控制下约15秒内误差降到1度以内稳态时因为干扰力矩的存在会有大约0.1度左右的静差。如果你希望消除稳态误差可以在PD控制器基础上加积分项或者引入更高级的姿态控制器。我在仿真包里保留了PID的实现位置只需要在函数里加一个积分状态即可。4. Simulink S函数实现方法4.1 Simulink模型整体框架Simulink模型的主体思路和M文件完全一致但信号流更清晰。我在模型中放了一个S-Function模块名字叫attitude_ctrl_sfun它接收两个输入姿态误差四元数和本体角速度输出一个三维修正力矩。控制器模块之后接的是一个被控对象子系统子系统内部用积分器累加角度导数和角速度导数同时把飞轮角动量作为状态反馈回控制器。搭建时需要注意被控对象里面的状态排列顺序必须和S函数里的输入顺序一致。我通常把模型的顶层端口做成一个大向量方便连线然后在被控对象内部用Selector把四元数、角速度、飞轮角动量拆开。这样看起来更结构化调试时也能单独观察每个信号。4.2 用Level-2 MATLAB S-Function写控制器这里我采用Level-2 MATLAB S-Function它比老的Level-1 S-Function更容易支持数组输入、参数配置和采样时间设定。核心文件attitude_ctrl_sfun.m开头部分是设置阶段function attitude_ctrl_sfun(block) setup(block); function setup(block) block.NumInputPorts 2; block.NumOutputPorts 1; block.InputPort(1).Dimensions 4; block.InputPort(2).Dimensions 3; block.InputPort(1).SamplingMode Sample; block.InputPort(2).SamplingMode Sample; block.OutputPort(1).Dimensions 3; block.OutputPort(1).SamplingMode Sample; block.NumDialogPrms 3; block.DialogPrmsTunable {Nontunable,Nontunable,Nontunable}; block.SampleTimes [0 0]; block.RegBlockMethod(Outputs, Outputs); block.RegBlockMethod(Terminate, Terminate); end关键的设置是block.SampleTimes [0 0]表示连续采样时间这样控制器和连续被控对象位于同一仿真层。如果你希望控制律按固定时间周期更新比如每个0.01秒更新一次那就要改成[0.01 0]同时给动力学模型设置适当采样时间。输出计算部分直接复用M文件里的控制律function Outputs(block) qe block.InputPort(1).Data; omega block.InputPort(2).Data; Kp block.DialogPrm(1).Data; Kd block.DialogPrm(2).Data; cmd_limit block.DialogPrm(3).Data; qe(1:4) qe(1:4) / norm(qe(1:4)); u_cmd -Kp * qe(2:4) - Kd * omega; u_cmd max(min(u_cmd, cmd_limit), -cmd_limit); block.OutputPort(1).Data u_cmd; end把参数放在DialogPrm里而不是硬编码这是S函数规范和常规实践。这样你在Simulink模型里双击S函数模块就能填写Kp、Kd和力矩限幅不需要改代码。如果需要频繁调参还可以打开其“Block Parameters”下的Tunable设置但在Simulink中直接调S函数对话框参数会更方便。要注意DialogPrmsTunable设置为Nontunable这样即使参数改了S函数在快速加速模式下也能正常更新。如果想在仿真过程中动态修改需要选择Tunable但要注意外部模式兼容性。4.3 模型参数封装与仿真配置为了让模型易用我给S函数模块套了一层Mask把Kp、Kd、力矩限制这三个参数做成对话框这样比让用户直接面对S函数参数要好。操作方法是右键S函数模块选择“Mask Create Mask”在Mask Editor里添加三个编辑框然后在“Initialization”里把参数对应到block.DialogPrm。这样做的好处是同事或学生看到模块就知道该填什么参数不用去翻S函数源码。Simulink模型里的被控对象部分我推荐你直接把动力学方程写成MATLAB Function块而不是用一堆Sum和Gain模块。这样做的好处是代码和M文件中的公式完全一致维护成本低。MATLAB Function块内部写一个普通函数输入状态向量和干扰力矩输出状态导数然后连接积分器。如果你更希望完全用模块搭建也完全可以但会多出几条布线和矩阵运算块容易出错。仿真配置上一定要在Simulink“模型设置”里选“变步长”和ode45求解器否则连续时间状态可能不准确。如果S函数是连续采样时间模型会自动采用连续求解器。我还会设置相对误差1e-6和M文件的ode45配置保持一致这样两个仿真的数值精度具有可比性。5. 常见问题与排查技巧5.1 S函数采样时间设置不当导致的异常S函数最常见的坑就是采样时间设置。block.SampleTimes [0 0]表示连续时间[0.01 0]表示离散采样[-1 0]表示继承驱动源。如果你把控制器S函数设置成离散采样而动力学是连续积分器模型会自动唤醒离散事件但只要参数不匹配可能会出现奇怪的振铃。实际操作中我遇到过把采样时间写成[0.01 1]后模型无法进入连续仿真状态的情况那是因为第二项表示偏移量不是随便写的。建议在没有特殊需求时保持连续采样时间或明确离散采样周期。采样时间问题还常常伴随仿真速度过慢。如果你发现模型每个步长都非常小先看看是不是S函数里用了连续采样时间但内部却包含离散变量。连续采样时间如果内部又依赖前一步的值会触发零阶保持问题等效于引入一个极小的时间常数。此时可以把控制律设为离散时间采样或者在内部加一个Memory块作为状态缓冲。5.2 代数环与数值发散在Simulink中如果控制器的输出又直接参与控制器输入的运算却没有积分器或延迟模块隔开就会构成代数环。代数环的求解需要非线性迭代轻则警告重则仿真发散。飞轮动力学中是连续积分一般情况下不太容易产生代数环但如果你加入了反步控制或某种直接前馈路径就要小心。排查代数环的方法是查看诊断信息或者直接给可能的反馈回路插入一个极小的滞后模块。我更推荐的结构是让控制器只依赖状态变量而不是输出变量凡是从积分器出来的状态都可以直接反馈不会形成代数环。如果你需要给控制器输入理想力矩的目标值可以把这个目标值作为状态变量延一拍用Unit Delay模块处理。5.3 四元数归一化与单位一致性我差点被四元数问题坑到底。刚开始跑Simulink模型时误差四元数曲线看起来很正常但是角速度持续小幅漂移。检查发现是积分器输出端没有对四元数归一化误差只在控制器内部归一化长期仿真时状态四元数范数慢慢变成了1.005。虽然每条曲线看起来都差不多但再往下做姿态估计精度分析时误差会被放大。处理办法有两个一是在动力学函数里每次都计算归一化后的导数这适合M文件二是在Simulink中把四元数状态单独接到一个归一化子系统上用四元数范数的倒数乘回去。两种办法我都试过第一种简单第二种更明显能在模型图上看到这个步骤。单位一致性也很重要尤其是控制器增益的单位要和角速度、四元数误差的单位匹配。角速度默认用rad/s如果你拿到一组用度每秒采集的数据必须换算否则Kd完全失效。5.4 仿真结果对不齐怎么办类似M文件和Simulink曲线对不上时我建议先做静态坐标测试设置一个初始姿态误差同时把Kd设为0看看控制器力矩是不是只沿着误差方向恢复。如果一致再打开Kd验证阻尼项。按顺序排查比一次改多个变量快得多。再有就是使用相同的求解器容差M文件ode45的RelTol和Simulink模型设置的相对误差必须一致否则微小数值差异会被时间轴累积放大。曲线粘贴在一起比较时记得对齐时间向量Simulink的变步长输出点可能和M文件不同建议用interp1插值到统一时间网格再比较。6. 实操心得与后续扩展6.1 我踩过的几个坑第一飞轮角动量符号。我最初在动力学方程里把-dhw写成了dhw结果控制器无论怎么调都发散。后来把飞轮角动量单独画出来发现它和卫星角速度在同步增长而不是相反才反应过来符号错了。所以建议你在M文件仿真里加一句断言检查控制力是否与误差方向一致。第二S函数的参数可视化。一开始我直接用字符串把参数填进S函数模块结果修改一次参数就要重新编译一次还不方便对比。后来我把参数全部提取到Mask里统一用结构体传递调试效率高了很多。特别是多个S函数同时存在时结构化参数能显著减少错误。第三阻尼参数过大导致高频振荡。PD控制器中Kd过大会让飞轮指令力矩在仿真初始阶段瞬间饱和反而引起振荡。仿真包里的力矩限幅是必要的但限幅后的饱和效应会让等效增益下降直观表现是响应变慢。如果你想要更好的动态品质可以加入抗饱和积分或做控制分配而不只是简单地截断。6.2 可以继续扩展的方向这套仿真包可以往很多方向发展。给飞轮加上角动量饱和与摩擦力矩就能复现飞轮转速超出限制时的下溢行为。将控制律从PD换成滑模控制或自适应控制时S函数只需修改Outputs部分非常适合做算法对比。如果要用在嵌入式工程中建议把attitude_ctrl_sfun.m改写成C MEX S函数然后结合Simulink Coder生成C代码在硬件在环平台上验证。我在实际项目里遇到的最大价值是把S函数作为算法接口把控制策略和实验平台分离这样每次调算法只需换一个S函数文件模型基本不动。最后再说一个小技巧无论M文件还是Simulink都要在仿真前保存一份参数快照。我在参数脚本头部加了一行disp(参数载入完成)并且在每次仿真后把关键性能指标存成mat文件后面回看大量仿真记录时非常有用。飞轮姿态控制仿真的难点不在某个单独模块而在整个闭环的一致性你只要把公式、符号、参数和采样时间四项对齐剩下的只是时间长短的问题。本文还有配套的精品资源点击获取
