[C++知识库]卫星姿态控制 C++ 可扩展仿真

#include <iostream>
#include <fstream>
#include <string>
using namespace std;

// 引入控制目标模块
#include "ControlTarget.h"

// 引入控制器模块
#include "PID_Controler.h"

// 引入敏感器模块，此部分也受空间环境影响
//#include "StarSensor.h"       
//#include "Gyroscope.h"

// 引入执行机构模块
//#include "FlyWheel.h"
//#include "Jet_Thruster.h"
//#include "MagneticTorqueConverter.h"

// 引入卫星动力学模块
#include "SatelliteDynamics.h"

// 工具模块
//#include "OrbitDescribe.hpp"
//#include "CoordinateTrans.hpp"
//#include "VecterMatrix.hpp"
#include "AttitudeDescribe.h"
#include "Constant.h"
using namespace m_Constant;

// 引入数据保存模块
//#include "PointDataList.h"

/* 卫星姿态控制测试！*/
int main()
{
    // 姿态描述及转换工具
    AttitudeDescribe m_AttitudeDescribe;
    array<double, 4> m_q_start;
    m_AttitudeDescribe.Euler_to_q(10 * d2r, 10 * d2r, 10 * d2r, m_q_start);

    // 卫星初始运动状态
    array<double, 14> yn = { 0, 0, 0, 0, 0, 0, 0 * d2r,0 * d2r,0 * d2r, m_q_start[0], m_q_start[1], m_q_start[2], m_q_start[3], 0 };

    // 卫星动力学
    SatelliteDynamics *m_pSatelliteDynamics = new SatelliteDynamics(yn);

    // PID控制器参数设定
    double wnx = 5, wny = 5, wnz = 5;
    array<double, 3> kp = { 2 * 1.25 / 0.515 * wnx * wnx * 0.6, 2 * 9.65 / 3.18 * wny * wny * 0.6, 2 * 9.65 / 1.59 * wnz * wnz * 0.6 };
    array<double, 3> ki = { 0, 0, 0 };
    array<double, 3> kd = { sqrt(0.6) * 2 * wnx * 1.25 / 0.515 * 2, sqrt(0.6) * 2 * wny * 9.65 / 3.18, sqrt(0.6) * 2 * wnz * 9.65 / 1.59 * 2 };
    
    // PID控制器
    PID_Controler *m_pPID = new PID_Controler(kp, ki, kd, h);

    // 控制目标指令
    ControlTarget *m_pControlTarget = new ControlTarget(0 * d2r, 0 * d2r, 0 * d2r);

    // 陀螺仪测量
    array<double, 3> wob_b_mea;

    // 星敏感器测量
    array<double, 4> q_mea;

    // 姿态误差四元数
    array<double, 4> q_error;

    // 执行机构控制力矩
    array<double, 3> Mc = { 0, 0, 0 };  

    // 执行机构控制力
    array<double, 3> Fc = { 0, 0, 0 }; 

    // 数据保存到文件
    string FileName("Euler_Angle.txt");
    ofstream outFile(FileName, ios::out);
    if (outFile.fail()) cerr << "打开失败！" << endl;

    // 开始！
    for (; m_pSatelliteDynamics->Get_Time() < 9.99;)   // 仿真10秒
    {   
        // 卫星动力学推演
        m_pSatelliteDynamics->RungeKuttaGill(yn, Mc, Fc);

        // 陀螺仪的测量值 wob_b_mea, 控制器输入
        wob_b_mea = m_pSatelliteDynamics->Get_wob_b();

        // 星敏感器测量值 q_mea, 控制目标指令模块输入
        q_mea = m_pSatelliteDynamics->Get_Quaternion();

        // 控制目标指令模块输入 q_mea 输出 q_error，控制器输入
        m_pControlTarget->ControlCommand(q_mea, q_error);

        // 控制器输入 q_error、陀螺仪的测量值 wob_b_mea, 输出控制力矩 Mc, 输入到卫星动力学模块中
        m_pPID->AttitudeControl(q_error, wob_b_mea, Mc);

        // 保存数据到文件
        outFile << m_pSatelliteDynamics->Get_Time() << ",  "
                << m_pSatelliteDynamics->Get_EulerAngle()[0] * r2d << ",  " << m_pSatelliteDynamics->Get_EulerAngle()[1] * r2d << ",  " << m_pSatelliteDynamics->Get_EulerAngle()[2] * r2d << ",  "
                << m_pSatelliteDynamics->Get_wob_b()[0] * r2d << ",  " << m_pSatelliteDynamics->Get_wob_b()[1] * r2d << ",  " << m_pSatelliteDynamics->Get_wob_b()[2] * r2d << ",  "
                << Mc[0] << ",  " << Mc[1] << ",  " << Mc[2] << endl;
    }

    // 关闭文件     
    outFile.close();

    // 释放内存
    delete m_pSatelliteDynamics;
    delete m_pPID;
    delete m_pControlTarget;

    return 0;
}

测试数据：

各个模块后续补充ing…