SimuLink仿真卫星姿态控制

1SimuLink仿真卫星姿态控制孙亚辉，易涛哈尔滨工业大学卫星技术研究所[e-mail]2014年4月2日目录SimuLink仿真卫星姿态控制...................................................................................................11.姿态控制模块的组成...............................................................................................12.动力学模块的建立...................................................................................................23.运动学模块的建立...................................................................................................64.角速度陀螺模块的建立.........................................................................................125.星敏感器模块的建立.............................................................................................146.控制目标模块的建立.............................................................................................147.控制模块的建立.....................................................................................................158.执行机构模块的建立.............................................................................................169.观测器Scope的添加.............................................................................................1610.M文件的代码....................................................................................................181.姿态控制模块的组成卫星姿态控制的SimuLink模块如下图所示。其中：Dynamics模块是动力学模块，它的输入是控制力矩T，输出是卫星在惯性系下的角速度Wib。Kinematics模块是运动学模块，它引入Wib然后输出体坐标系下的姿态四元数Qo和卫2星本体在体坐标系下的角速度Wob。StarSensor模块是星敏感器模块，它引入姿态四元数Qo，加入星敏感器噪声后输出测量姿态四元数Qo_m。Gyro模块是陀螺模块，它引入Wob，然后加入陀螺噪声得到测量的角速度Wob_m。Target模块是控制目标模块，它输入测量四元数Qo_m，输出姿态误差四元数的矢部eq。Controller模块是控制模块，它输入姿态误差四元数的矢部eq和姿态角速度的测量值Wob_m，输出控制信号T_Signal。控制率为PD控制率：(KKw)pedTq=-+Actuator模块是执行机构模块，它输入控制信号，输出控制力矩T，然后将T引入动力学模块。卫星姿态控制的SimuLink模块即建立完成。2.动力学模块的建立卫星动力学方程如下：（模型中T为体坐标系下的力矩，H为体坐标系下的角动量，w为体坐标系相对于惯性坐标系的角速度在在体坐标系下的量）动力学模块如下图所示：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，三个Fuc，模型如下。Mux的Input设置为[33]和[111]。计算叉乘结果如下：5按照计算结果填写Fcn模块：6叉乘模块即建立完成。将叉乘结果w*H引至add模块，引出体坐标系相对于惯性坐标系的角速度在在体坐标系下的量Wib即完成动力学模块。3.运动学模块的建立四元数姿态运动学方程：（见《基于星敏感器角速度估计的陀螺故障诊断》）()bbbbbbooooboiboio11C22•=⊗=⊗-QQwQww其中boQ表示星体坐标系b相对于轨道坐标系o的姿态四元数在体坐标系b中的投影（在星体坐标系中轨道坐标系o转到与星体坐标系b重合（与后面的姿态坐标转换矩阵对应）所需的姿态四元数）；⊗表示四元数乘法；bobw表示星体坐标系b相对于轨道坐标系o的角速度在b坐标系中的投影；bibw表示体坐标系b相对于惯性坐标系i的角速度在b坐标系中的投影，由动力学模块给出；boC表示o系到b系的坐标转换矩阵，由四元数Q计算得到；oiow表示轨道坐标系o相对于惯性坐标系i的角速度在o坐标系中的投影，该角速度在7轨道坐标系中表示简单，该值在m文件中给出。利用运动学方程画运动学模块图如下：建立四元数转姿态矩阵模块：用mask修改模块封面。（使用mask的时候要保证输入输出的个数正确，若因为输入输出个数出现错误，则修改代码错误后点击左下角的Unmask）依照下面的转换公式，使用Fcn模块和Create3*3Matrix模块建立四元数转换模块如下：82222103212031302bT22222o01203203123012222130223013012qqqq2(qqqq)2(qqqq)C(q)2(qqqq)qqqq2(qqqq)2(qqqq)2(qqqq)qqqq⎡⎤+--+-⎢⎥=+-=-+--+⎢⎥⎢⎥+-+--⎣⎦qqEq建立四元数求解模块：使用Subsystem模块建立QuaternionSolveModel，并createmask如下：9四元数主部与矢部分开计算积分如下：根据上式画模块图如下：10利用Integrator、Mux、Gain、Add模块建立初始模块如下，其中两个Integrator的初值即为初始姿态四元数的初值的主部和矢部。利用Demux模块将Q的矢部与主部分开。其中Demux模块设置如下（将四个数按[13]分成两部分）：11利用DotProduct模块求q0的导：利用动力学模块中的叉乘模块来完成q的导的求解如下：这样四元数求解模块即建立，输入w输出Q。12建立运动学模块：引入动力学模块计算的姿态角速度wib和m文件中的轨道角速度wio，引出Wob和Qo。建立运动学模块如下（注意product的multiplication应为Matrix）：这样运动学模块即建立。4.角速度陀螺模块的建立陀螺噪声分为两部分，一是白噪声，二是常值漂移。陀螺模块如下图所示。初值漂移为M文件中的cong。白噪声由RandomNumber模块提供，注意白噪声模块的参数：Mean，Variance，Seed，SampleTime。噪声是三维的。13Help——RandomNumber模块：Seed：Theseedmustbe0orapositiveinteger.Outputisrepeatableforagivenseed.不能对RandomNumber模块提供的噪声进行积分，需要积分的话应该使用LimitedWhiteNoise模块。（Avoidintegratingarandomsignal,becausesolversmustintegraterelativelysmoothsignals.Instead,usetheBand-LimitedWhiteNoiseblock.）RandomNumber模块的参数设置：模块是gain模块，是为了在白噪声上面乘以一个增益参数，参数Noig由M文件提供。是constant模块，是为了提供常值漂移cong。：Quantizer模块是为了体现陀螺精度。当这一时刻的测量值与上一时刻的测量值的变化值小于陀螺精度时，Quantizer仍会提供上一时刻的测量值。精度值Gyquan由M文件提供。145.星敏感器模块的建立星敏感器用于测量姿态四元数，当不考虑星敏感器误差时可以忽视该模块。6.控制目标模块的建立该模块是为了由测量姿态四元数得到姿态误差四元数的矢部eq。令目标姿态四元数为TQ，姿态误差四元数为eQ，误差四元数不是由两四元数做差得到的。误差四元数的求取公式：依照上式建立起来控制目标模块如下，输入测量量Qo_m，利用M文件中的目标四元数Qt来输出eq。其中InterQMultiply模块是四元数乘法模块，四元数乘法的公式如下：依照上式搭建起四元数乘法模块的内部结构如下：157.控制模块的建立控制率为(KdqKw)pdT=-+。按照控制率搭建控制模块如下：控制参数的设置如下：168.执行机构模块的建立本仿真假设执行机构为理想的零动量飞轮，输出力矩无误差且无大小限制。即执行器仅是增益为1的gain模块。9.观测器Scope的添加本仿真的观测量为误差姿态角和姿态角速度。建立观测模块如下：Quaternion2Euler模块是四元数转姿态角模块，其将卫星的姿态四元数Qo转为姿态角。EulerError是误差姿态角模块，其将M文件中的目标姿态角与卫星姿态角做差。四元数转ZYX顺序得到的Euler姿态角的公式如下：17按照上式，利用Fcn模块搭建四元数转姿态角模块如下：误差姿态角模块如下。该模块将姿态角与目标姿态角做差。至此我们便建立了完整的卫星姿态控制模型如下图所示：1810.M文件的代码%%%SimuLink仿真参数。（该M文件不能与SimuLink文件重名）对应SimuLink文件：AttitudeControl.mdlclear;clc;d2r=pi/180;%%degree2radian%%%转动惯量IBIB=[1.25,0.0,0.00.0,9.65,0.00.0,0.0,9.65];%%%转动惯量IB的逆invIBinvIB=inv(IB);%%%初始角速度WwoWw0=[0.10.10.1]'*d2r;%%%初始姿态角(按照ZYX转动得到的Euler姿态角)fei0=[PhiThetaPsi]fei0=[0,42.7,5.6]'*d2r;%%%初始姿态四元数[Q00Q0]19Q00=cos(fei0(1)/2)*cos(fei0(2)/2)*cos(fei0(3)/2)...+sin(fei0(1)/2)*sin(fei0(2)/2)*sin(fei0(3)/2);Q1=sin(fei0(1)/2)*cos(fei0(2)/2)*cos(fei0(3)/2)...-cos(fei0(1)/2)*sin(fei0(2)/2)*sin(fei0(3)/2);Q2=cos(fei0(1)/2)*sin(fei0