其中执行机构为零动量反作用飞轮,此处略去;传感器测量部分也简化了,后续会补充基于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