SimuLink仿真卫星姿态控制

6
o b b b
b
轨道坐标系中表示简单，该值在 m 文件中给出。 利用运动学方程画运动学模块图如下：
建立四元数转姿态矩阵模块： 用 mask 修改模块封面。 （使用 mask 的时候要保证输入输出的个数正确，若因为输入输 出个数出现错误，则修改代码错误后点击左下角的 Unmask）
依照下面的转换公式，使用 Fcn 模块和 Create3*3 Matrix 模块建立四元数 转换模块如下：
9. 观测器 Scope 的添加
本仿真的观测量为误差姿态角和姿态角速度。建立观测模块如下：
Quaternion2Euler 模块是四元数转姿态角模块， 其将卫星的姿态四元数 Qo 转为姿态角。 Euler Error 是误差姿态角模块，其将 M 文件中的目标姿态角与卫星姿态角做差。 四元数转 ZYX 顺序得到的 Euler 姿态角的公式如下：
建立四元数求解模块： 使用 Subsystem 模块建立 Quaternion Solve Model，并 create mask 如下：
8
四元数主部与矢部分开计算积分如下：
根据上式画模块图如下：
9
利用 Integrator、Mux、Gain、Add 模块建立初始模块如下，其中两个 Integrator 的初值 即为初始姿态四元数的初值的主部和矢部。
7
q12 + q0 2 − q32 − q2 2 2( q1q2 + q0 q3 ) 2( q1q3 − q0 q2 ) b T 2 2 2 2 2 Co = qq + ( q0 E − q ) = 2( q1q2 − q0 q3 ) q2 + q0 − q3 − q1 2( q2 q3 + q0 q1 ) 2( q1q3 + q0 q2 ) 2( q2 q3 − q0 q1 ) q32 + q0 2 − q12 − q2 2
动力学模块如下图所示：
2
先引入控制力矩 T。再添加 add 模块。得到 dH，引入 Integrator 模块得到 H。Integrator 模块的初值是积分的初值，即为初始角动量在体坐标系下的值 HB0（由体坐标系相对于惯性 坐标系的初始角速度在在体坐标系下的量决定，在 m 文件中给出。 ） 利用 Gain 模块乘以转动惯量矩阵 I 的逆 invIB 来得到 w。修改 gain 模式为 Matrix(u*K)。 I 的逆最好在 m 文件中计算出来（SimuLink 从 Workspace 中调用） ，这样减少计算量。
其中 Q o 表示星体坐标系 b 相对于轨道坐标系 o 的姿态四元数在体坐标系 b 中的投影 （在星体坐标系中轨道坐标系 o 转到与星体坐标系 b 重合（与后面的姿态坐标转换矩阵对 应）所需的姿态四元数） ； ⊗ 表示四元数乘法； wob 表示星体坐标系 b 相对于轨道坐标系 o 的角速度在 b 坐标系中的投影；w ib 表示体坐标系 b 相对于惯性坐标系 i 的角速度在 b 坐标 系中的投影，由动力学模块给出； Co 表示 o 系到 b 系的坐标转换矩阵，由四元数 Q 计算得 到； w io 表示轨道坐标系 o 相对于惯性坐标系 i 的角速度在 o 坐标系中的投影，该角速度在
其中 InterQ Multiply 模块是四元数乘法模块，四元数乘法的公式如下：
依照上式搭建起四元数乘法模块的内部结构如下：
14
7. 控制模块的建立
控制率为 T = −(Kpdq+ Kd w) 。按照控制率搭建控制模块如下：
控制参数的设置如下：
15
8. 执行机构模块的建立
本仿真假设执行机构为理想的零动量飞轮， 输出力矩无误差且无大小限制。 即执行器仅 是增益为 1 的 gain 模块。
模块是 gain 模块，是为了在白噪声上面乘以一个增益参数，参 数 Noig 由 M 文件提供。
是 constant 模块，是为了提供常值漂移 cong。
：Quantizer 模块是为了体现陀螺精度。当这一时刻的测量值与 上一时刻的测量值的变化值小于陀螺精度时，Quantizer 仍会提供上一时刻的 测量值。精度值 Gyquan 由 M 文件提供。
将 H 与 w 引入 CrossProduct 模块（叉乘模块） 。 右键 Subsystem 模块-“Create Mask”来修改模块的外貌。 （为模块起名 Cross Product， 为输入输出起名，以防止叉乘顺序出错）
3
建立叉乘模块： 在模块里面添加两个 in，一个 out。添加两个 Mux，三个 Fuc，模型如下。Mux 的 Input 设置为[3 3]和[1 1 1]。
SimuLink 仿真卫星姿态控制
孙亚辉，易涛 哈尔滨工业大学卫星技术研究所
syhhit@
2014 年 4 月 2 日
目录
SimuLink 仿真卫星姿态控制 ................................................................................................... 1 1. 姿态控制模块的组成............................................................................................... 1 2. 动力学模块的建立................................................................................................... 2 3. 运动学模块的建立................................................................................................... 6 4. 角速度陀螺模块的建立......................................................................................... 12 5. 星敏感器模块的建立............................................................................................. 14 6. 控制目标模块的建立............................................................................................. 14 7. 控制模块的建立..................................................................................................... 15 8. 执行机构模块的建立............................................................................................. 16 9. 观测器 Scope 的添加 ............................................................................................. 16 10. M 文件的代码 .................................................................................................... 18
16
按照上式，利用 Fcn 模块搭建四元数转姿态角模块如下：
误差姿态角模块如下。该模块将姿态角与目标姿态角做差。
至此我们便建立了完整的卫星姿态控制模型如下图所示：
利用 Demux 模块将 Q 的矢部与主部分开。其中 Demux 模块设置如下（将四 个数按[1 3]分成两部分）：
10
利用 Dot Product 模块求 q0 的导：
利用动力学模块中的叉乘模块来完成 q 的导的求解如下：
这样四元数求解模块即建立，输入 w 输出 Q。
11
建立运动学模块：
引入动力学模块计算的姿态角速度 wib 和 m 文件中的轨道角速度 wio， 引出 Wob 和 Qo。 建立运动学模块如下（注意 product 的 multiplication 应为 Matrix） ：
这样运动学模块即建立。
4. 角速度陀螺模块的建立
陀螺噪声分为两部分，一是白噪声，二是常值漂移。陀螺模块如下图所示。
初值漂移为 M 文件中的 cong。 白噪声由 Random Number 模块提供， 注意白噪声模块的 参数：Mean，Variance，Seed，Sample Time。噪声是三维的。
计算叉乘结果如下：
4
按照计算结果填写 Fcn 模块：
5
叉乘模块即建立完成。将叉乘结果 w*H 引至 add 模块，引出体坐标系相对于
惯性坐标系的角速度在在体坐标系下的量 Wib 即完成动力学模块。
3. 运动学模块的建立
四元数姿态运动学方程： （见《基于星敏感器角速度估计的陀螺故障诊断》 ）
• 1 b 1 b b b b b o Qo wio ) = Qo ⊗ wob = Qo ⊗ ( wib − Co 2 2
= −(K p qe + Kd w)
Actuator 模块是执行机构模块，它输入控制信号，输出控制力矩 T，然后将 T 引入动力 学模块。 卫星姿态控制的 SimuLink 模块即建立完成。
2. 动力学模块的建立
卫星动力学方程如下： （模型中 T 为体坐标系下的力矩，H 为体坐标系下的角动量，w 为体坐标系相对于惯性坐标系的角速度在在体坐标系下的量）
12
Help——Random Number 模块： Seed：The seed must be 0 or a positive integer. Output is repeatable for a given seed. 不能对 Random Number 模块提供的噪声进行积分， 需要积分的话应该使用 Limited White Noise 模块。 （Avoid integrating a random signal, because solvers must integrate relatively smooth signals. Instead, use the Band-Limited White Noise block.） Random Number 模块的参数设置：