一、卫星姿态控制模块组成
其中执行机构为零动量反作用飞轮，此处略去；传感器测量部分也简化了，后续会补充基于EKF/UKF的星敏感器和陀螺组合的卫星姿态确定算法来代替图中的测量部分。
二、转动惯量和地球自转角速度
static const double Jx = 1.25; static const double Jy = 9.65; static const double Jz = 9.65; static const double d2r = 3.1415926 / 180.0; static const double we = 2 * 3.1415926 / 5431.184;
三、初始姿态和目标姿态
double wx0 = 0.1 * d2r; // 角速度
double wy0 = 0.1 * d2r; // 角速度
double wz0 = 0.1 * d2r; // 角速度
double roll0 = 10 * d2r; // 滚转角
double pitch0 = 42.7 * d2r; // 俯仰角
double yaw0 = 5.6 * d2r; // 偏航角
double roll1 = 5 * d2r; // 滚转角
double pitch1 = 5 * d2r; // 俯仰角
double yaw1 = 5 * d2r; // 偏航角
四、欧拉角转四元数及四元数转欧拉角
// 顺序：Z-Y-X yaw pitch roll
double q0 = cos(0.5 * roll0) * cos(0.5 * pitch0) * cos(0.5 * yaw0) + sin(0.5 * roll0) * sin(0.5 * pitch0) * sin(0.5 * yaw0);
double q1 = sin(0.5 * roll0) * cos(0.5 * pitch0) * cos(0.5 * yaw0) - cos(0.5 * roll0) * sin(0.5 * pitch0) * sin(0.5 * yaw0);
double q2 = cos(0.5 * roll0) * sin(0.5 * pitch0) * cos(0.5 * yaw0) + sin(0.5 * roll0) * cos(0.5 * pitch0) * sin(0.5 * yaw0);
double q3 = cos(0.5 * roll0) * cos(0.5 * pitch0) * sin(0.5 * yaw0) - sin(0.5 * roll0) * sin(0.5 * pitch0) * cos(0.5 * yaw0);
// 顺序：Z-Y-X yaw pitch roll
double roll = atan2(2.0 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3);
double pitch = asin(-2.0 * (q1 * q3 - q0 * q2));
double yaw = atan2(2.0 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3);
五、仿真
int main()
{
Satellite satellite;
satellite.Initialize(8, x0);
satellite.SetTargetAttitude(roll1, pitch1, yaw1);
while (satellite.gettime() <= 10) satellite.OneStep(8, 0.01); // 卫星仿真一步长
return 0;
}
六、绘图分析
可以看到 C++ 和 Siimulink 仿真结果基本相同；
星体系相对于惯性系的角速度在星体系投影 wb_bi
星体系相对于轨道系的卫星姿态角
控制器指令控制力矩 Mc
