SimuLink仿真星敏感器像平面误差下的卫星姿态控制

1SimuLink仿真星敏感器像平面误差下的卫星姿态控制孙亚辉，易涛哈尔滨工业大学卫星技术研究所[e-mail]2014年4月14日目录SimuLink仿真星敏感器像平面误差下的卫星姿态控制......................................................11.姿态控制模块的组成.............................................................................................12.动力学模块的建立................................................................................................23.运动学模块的建立................................................................................................64.角速度陀螺模块的建立.......................................................................................105.星敏感器模块的建立...........................................................................................116.控制目标模块的建立...........................................................................................177.控制模块的建立..................................................................................................198.执行机构模块的建立...........................................................................................209.观测器Scope的添加...........................................................................................2010.M文件的代码..................................................................................................221.姿态控制模块的组成卫星姿态控制的SimuLink模块如下图所示。其中：Dynamics模块是动力学模块，它的输入是控制力矩T，输出是卫星的本体坐标系相对2惯性系的角速度在本体坐标系下的值Wbi。Kinematics模块是运动学模块，它引入Wbi然后输出体坐标系下的姿态四元数Qbo和卫星本体在体坐标系下的角速度Wbo。StarSensor模块是星敏感器模块，它引入姿态四元数Qbo，加入星敏感器噪声后输出测量姿态四元数Qbo_m。Gyro模块是陀螺模块，它引入Wbo，然后加入陀螺噪声得到测量的角速度Wbo_m。Target模块是控制目标模块，它输入测量四元数Qbo_m，输出姿态误差四元数的矢部eq。Controller模块是控制模块，它输入姿态误差四元数的矢部eq和姿态角速度的测量值Wbo_m，输出控制信号T_Signal。控制率为PD控制率：(KKw)pedTq=-+Actuator模块是执行机构模块，它输入控制信号，输出控制力矩T，然后将T引入动力学模块。卫星姿态控制的SimuLink模块即建立完成。2.动力学模块的建立卫星动力学方程如下：（模型中T为体坐标系下的力矩，H为体坐标系下的角动量，w为体坐标系相对于惯性坐标系的角速度在在体坐标系下的量bbiw）动力学模块如下图所示：3先引入控制力矩T。再添加add模块。得到dH，引入Integrator模块得到H。Integrator模块的初值是积分的初值，即为初始角动量在体坐标系下的值HB0（由体坐标系相对于惯性坐标系的初始角速度在在体坐标系下的量决定，在m文件中给出。）利用Gain模块乘以转动惯量矩阵I的逆invIB来得到w。修改gain模式为Matrix(u*K)。I的逆最好在m文件中计算出来（SimuLink从Workspace中调用），这样减少计算量。将H与w引入CrossProduct模块（叉乘模块）。右键Subsystem模块-“CreateMask”来修改模块的外貌。（为模块起名CrossProduct，为输入输出起名，以防止叉乘顺序出错）4建立叉乘模块：在模块里面添加两个in，一个out。添加两个Mux，三个Fcn，模型如下。Mux的Input设置为[33]和[111]。计算叉乘结果如下：5按照计算结果填写Fcn模块：叉乘模块即建立完成。将叉乘结果w*H引至add模块，引出体坐标系相对于惯性坐标系的角速度在在体坐标系下的量Wbi即完成动力学模块。63.运动学模块的建立四元数姿态运动学方程：（见《基于星敏感器角速度估计的陀螺故障诊断》）()bbbbbboooboobiooi11C22•=⊗=⊗-QQwQww其中boQ表示轨道坐标系o转到星体坐标系b的姿态四元数；⊗表示四元数乘法；bbow表示星体坐标系b相对于轨道坐标系o（从o系到b系）的角速度在b坐标系中的投影；bbiw表示体坐标系b相对于惯性坐标系i（从i到b）的角速度在b坐标系中的投影，由动力学模块给出；boC表示o系到b系的坐标转换矩阵，由四元数Q计算得到；ooiw表示轨道坐标系o相对于惯性坐标系i（从i到o）的角速度在o坐标系中的投影，该角速度在轨道坐标系中表示简单，该值在m文件中给出。利用运动学方程画运动学模块图如下：建立四元数转姿态矩阵模块：用mask修改模块封面。（使用mask的时候要保证输入输出的个数正确，若因为输入输出个数出现错误，则修改代码错误后点击左下角的Unmask）7依照下面的转换公式，使用Fcn模块和Create3*3Matrix模块建立四元数转姿态矩阵模块如下：2222103212031302bT22222o01203203123012222130223013012qqqq2(qqqq)2(qqqq)C(q)2(qqqq)qqqq2(qqqq)2(qqqq)2(qqqq)qqqq⎡⎤+--+-⎢⎥=+-=-+--+⎢⎥⎢⎥+-+--⎣⎦qqEq建立四元数求解模块：使用Subsystem模块建立QuaternionSolveModel，并createmask如下：四元数主部与矢部分开计算积分如下：8根据上式画模块图如下：利用Integrator、Mux、Gain、Add模块建立初始模块如下，其中两个Integrator的初值即为初始姿态四元数的初值的主部和矢部。利用Demux模块将Q的矢部与主部分开。其中Demux模块的输出设置为[13]。9利用DotProduct模块求q0的导：利用动力学模块中的叉乘模块来完成q的导的求解如下：这样四元数求解模块即建立，输入w输出Q。建立运动学模块：10引入动力学模块计算的姿态角速度bbiw和m文件中的轨道角速度ooiw，引出bbow和boQ。建立运动学模块如下（注意product的multiplication应为Matrix）：4.角速度陀螺模块的建立陀螺噪声分为两部分，一是白噪声，二是常值漂移。陀螺模块如下图所示。常值漂移为M文件中的cong。白噪声由RandomNumber模块提供，注意白噪声模块的参数：Mean，Variance，Seed，SampleTime。噪声是三维的。Help——RandomNumber模块：Seed：Theseedmustbe0orapositiveinteger.Outputisrepeatableforagivenseed.不能对RandomNumber模块提供的噪声进行积分，需要积分的话应该使用LimitedWhiteNoise模块。（Avoidintegratingarandomsignal,becausesolversmustintegraterelativelysmooth11signals.Instead,usetheBand-LimitedWhiteNoiseblock.）RandomNumber模块的参数设置：fix(abs(10000*rand([3,1])))：rand([3,1])产生3个(0,1)内的均匀分布的随机数，abs不必要，但注意seed的值必须是0或正的整数，所以用fix函数取整。Seed是种子文件，模块每次允许时都采用seed中的数来做计算机随机数列的计算初值（其他地方一般用电脑的时间），（注意：seed的值不是模块输出的初值，仅仅是模块计算随机数所需要的一个值）。模块是gain模块，是为了在白噪声上面乘以一个增益参数，参数Noig由M文件提供。是constant模块，是为了提供常值漂移cong。：Quantizer模块是为了体现陀螺精度。当这一时刻的测量值与上一时刻的测量值的变化值小于陀螺精度时，Quantizer仍会提供上一时刻的测量值。精度值Gyquan由M文件提供。5.星敏感器模块的建立该模块的作用：输入真实的卫星本体坐标系b相对于轨道坐标系o（从o到b）的姿态12四元数boQ，得到测量的姿态四元数_boMQ。理论推导：星敏感器坐标系s到本体系b的姿态转换矩阵（安装矩阵）bsC（TsbbsCC=，其他矩阵类似）轨道坐标系o相对于惯性坐标系i（i到o）的姿态矩阵oiC体坐标系b到轨道坐标系o的姿态矩阵bToboCC=（boC由输入值boQ得到）星敏感器坐标系s到惯性系i的姿态矩阵isioobbsCCCC=假设卫星上面有一个星敏感器，星敏感器能始终测量到姿态。生成两组测量的星点坐标值(x,y)，利用误差校正模型生成对应的(x,y)newnew，再利用无误差模型求出目标星矢量在星敏感器坐标系下的方向矢量newsnew222newnewx1Vyxyff-⎡⎤⎢⎥=-⎢⎥++⎢⎥⎣⎦。目标星矢量在惯性坐标系下的方向矢量isisVCV=，12312,,iiiiiVVVVV=×，123iiiBVVV⎡⎤=⎣⎦直接使用两组测量的星点坐标值(x,y)通过无误差模型生成目标星矢量在星敏感器坐标系下的方向矢量的测量值s222x1Wyxyff-⎡⎤⎢⎥=-⎢⎥++⎢⎥⎣⎦，123sssA⎡⎤=⎣⎦。双矢量定姿原理测量得到星敏感器坐标系到惯性系的姿态矩阵的测量值1_isMCBA-=。则__obMoiisMsbCCCC=，__bToMobMCC=。将_boMC转换为四元数即得到测量的姿态四元数_boMQ。求解轨道坐标系相对于惯性坐标系的姿态矩阵oiC：惯性坐标系按照ZYX顺序转换到轨道坐标系，而惯性坐标系转轨道系的姿态角为'''jqy⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦，则惯性系相对于轨道系的角速度在轨道系中的表示如下：130'0'010sin'0(')'(')C(')00cos'cos'sin''0000sin'cos'cos'''ooiyxxywwCCjjqjqjqjqjqjqjyy••••••⎡⎤⎡⎤⎡⎤⎡⎤⎢⎥-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥==++=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦⎣⎦1'10sin'0'0cos'cos'sin'0sin'cos'cos'0'ywjqqjqjjqjy•-••⎡⎤⎢⎥-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥-⎣⎦⎣⎦⎢⎥⎣⎦按照下图搭建模型可以得到'''jqy⎡⎤⎢⎥⎢⎥⎢⎥⎣⎦，然后既可以得到惯性坐标