1、程序运行效果

选中三维动画窗口，切换英文输入法，WADSER控制卫星三轴力矩。右图为实时动力学数值仿真结果，左图根据仿真结果进行绘图。

运行截图

运行动画

2、仿真说明

卫星的动力学模型如下

$$\{\begin{array}{rl}& \dot{w_x}=[M_x-(I_z-I_y)\cdot w_y\cdot w_z]/I_x\\ & \dot{w_y}=[M_y-(I_x-I_z)\cdot w_x\cdot w_z]/I_y\\ & \dot{w_z}=[M_z-(I_y-I_x)\cdot w_y\cdot w_x]/I_z\end{array}$$

姿态角变化很大，欧拉角、方向余弦矩阵有奇异。这里采用四元数、欧拉轴角参数。运动学模型如下，

$$\left[\begin{array}{c}\dot{q_0}\\ \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\end{array}\right]=\frac{1}{2}\left[\begin{array}{ccccc}& q_0& -q_1& -q_2& -q_3\\ & q_1& q_0& -q_3& q_2\\ & q_2& q_3& q_0& -q_1\\ & q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{cc}& 0\\ & w_x+2(q_1{q}_2+q_3{q}_0)w_0\\ & w_y+(q_0^2-q_1^2+q_2^2-q_3^2)w_0\\ & w_z+2(q_2{q}_3-q_1{q}_0)w_0\end{array}\right]$$

圆轨道卫星绕地球运转的角速度

$$w_0=\sqrt{\frac{{\mu }_e}{(R+H{)}^3}}$$

式中：

$w_0$，卫星绕地球公转的角速度

$H$，卫星轨道高度

$R$，地球半径

$Q=[q_0,q_1,q_2,q_3{]}^T$，卫星本体坐标系相对于地心惯性坐标系的姿态四元数；

$I_x,I_y,I_z$，卫星的三个主轴转动惯量；

$M_x,M_y,M_z$，卫星受到的沿着三个体坐标轴的力矩。

数值积分方法：四阶龙格库塔

绘图方法：OPENGL、欧拉轴/角参数

3、exe程序

卫星姿态动力学仿真-基于四元数-欧拉轴角v3.0.zip 381.61KB ZIP 170次下载

4、源码

动力学模型部分

weixing.cpp

C++
开启自动换行复制
#include"weixing.h"
#include <math.h>

//微分方程结构体
ST_ROCKET rocket={0.0f,0.0f,0.0f,0.0f,{0,0,0,1.0f,0.0f,0.0f,0.0f},{0.0f}};
float SIM_DT = 0.01f; //仿真步长时间s

/******************************************************************************************************************************************************************************************
内容：动力学微分方程(四元数)
输入：Y-状态，t-时间
输出：dy-状态变化率
备注：Y=[0 WX rad/s, 1 WY rad/s, 2 WZ rad/s, 3 q0, 4 q1, 5 q2 ,6 q3] ,注意初始值q0=1
**************************************************************************************************************************************************************************************************/
void daodan(ST_ROCKET *pst)
{
    //模型参数
    const float Ix=0.005f*1.0f;//x轴转动惯量kg*m^2
    const float Iy=0.005f*2.0f;//y轴转动惯量kg*m^2
    const float Iz=0.005f*2.0f;//z轴转动惯量kg*m^2
    const float H=600E3;//卫星圆轨道高度m
    const float R=6373e3;//地球赤道半径m
    const float mu=3.986e14;//地球引力常数
    const float w0=sqrt(mu/((R+H)*(R+H)*(R+H)));//卫星轨道角速度rad/s
    //double w0=1.08474e-3;
    //w0= sqrt(2.0f);//卫星轨道角速度rad/s
    //控制参数
    float Mx=0;
    float My=0;
    float Mz=0;
    Mx=pst->Mx;
    My=pst->My;
    Mz=pst->Mz;
//    if(pst->t<1.0f)
//    {
//        Mx=0.01f;
//        My=0.01f;
//        Mz=0.01f;
//    }
//    else
//    {
//        Mx=0;
//        My=0;
//        Mz=0;
//    }

    //动力学模型

    float temp = sqrt( pst->Y[3]*pst->Y[3] + pst->Y[4]*pst->Y[4] + pst->Y[5]*pst->Y[5] + pst->Y[6]*pst->Y[6] ); //四元数的归一化
    pst->Y[3]=pst->Y[3]/temp;
    pst->Y[4]=pst->Y[4]/temp;
    pst->Y[5]=pst->Y[5]/temp;
    pst->Y[6]=pst->Y[6]/temp;

    double A[4][4]={ {pst->Y[3], -pst->Y[4], -pst->Y[5], -pst->Y[6]},
                     {pst->Y[4],  pst->Y[3], -pst->Y[6],  pst->Y[5]},
                     {pst->Y[5],  pst->Y[6],  pst->Y[3], -pst->Y[4]},
                     {pst->Y[6], -pst->Y[5],  pst->Y[4],  pst->Y[3]} }; //四元数描述的运动学矩阵

    double B[4]={  0,
                   pst->Y[0] + w0 * 2.0F * ( pst->Y[4] *pst->Y[5] + pst->Y[6]* pst->Y[3] ),
                   pst->Y[1] + w0 * ( pst->Y[3]*pst->Y[3] - pst->Y[4]*pst->Y[4] + pst->Y[5]*pst->Y[5] - pst->Y[6]*pst->Y[6] ),
                   pst->Y[2] - w0 * 2.0F * ( pst->Y[5] *pst->Y[6] - pst->Y[4]* pst->Y[3] ) }; //卫星相对惯性系的角速度

    pst->dy[0] = ( Mx - ( Iz - Iy ) * pst->Y[1] * pst->Y[2] ) / Ix;
    pst->dy[1] = ( My - ( Ix - Iz ) * pst->Y[2] * pst->Y[0] ) / Iy;
    pst->dy[2] = ( Mz - ( Iy - Ix ) * pst->Y[0] * pst->Y[1] ) / Iz;      //外力引起的星体坐标系下的角加速度

    pst->dy[3] = A[0][0] * B[0] + A[0][1] * B[1] + A[0][2] * B[2] + A[0][3] * B[3];
    pst->dy[4] = A[1][0] * B[0] + A[1][1] * B[1] + A[1][2] * B[2] + A[1][3] * B[3];
    pst->dy[5] = A[2][0] * B[0] + A[2][1] * B[1] + A[2][2] * B[2] + A[2][3] * B[3];
    pst->dy[6] = A[3][0] * B[0] + A[3][1] * B[1] + A[3][2] * B[2] + A[3][3] * B[3];  //轨道坐标系下的姿态角速度（四元数描述）
}



/**************************************************************************************************************************************************************************
内容：四阶龙格库塔积分函数
输入：t-当前时间,Y[]-当前帧状态参数
输出：重新计算下一帧的状态参数，写入Y[]中
备注：每调用一次，更新一次(t,Y[])。使用四阶龙格库塔积分进行一步递推，根据当前状态，计算出下一刻的状态，更新t和Y[]。
**************************************************************************************************************************************************************************************************/
void my_ode45(ST_ROCKET* pst)
{
    UCHAR8 i;
    ST_ROCKET rocket1=*pst;
    ST_ROCKET rocket2=*pst;
    ST_ROCKET rocket3=*pst;
    ST_ROCKET rocket4=*pst;

    //k1=f(t,yn)
    daodan(&rocket1);

    //k2=f(t+0.5*h,yn+0.5*h*k1)
    for(i=0;i<ROCKET_DEMENTION;i++)
    {
        rocket2.Y[i]+=0.5f*SIM_DT*rocket1.dy[i];
    }
    rocket2.t+=0.5f*SIM_DT;
    daodan(&rocket2);

    //k3=f(t+0.5*h,yn+0.5*h*k2)
    for(i=0;i<ROCKET_DEMENTION;i++)
    {
        rocket3.Y[i]+=0.5f*SIM_DT*rocket2.dy[i];
    }
    rocket3.t+=0.5f*SIM_DT;
    daodan(&rocket3);

    //k4=f(t+h,yn+h*k3)
    for(i=0;i<ROCKET_DEMENTION;i++)
    {
        rocket4.Y[i]+=SIM_DT*rocket3.dy[i];
    }
    rocket4.t+=SIM_DT;
    daodan(&rocket4);

    //y(n+1)=1/6*(k1+2k2+2k3+k4)
    for(i=0;i<ROCKET_DEMENTION;i++)
    {
        pst->Y[i] += SIM_DT * ( rocket1.dy[i] + 2.0f*rocket2.dy[i] + 2.0f*rocket3.dy[i] + rocket4.dy[i] ) / 6.0f;
    }
    pst->t+=SIM_DT;
}

/******************************************************************************************************************************************************************************************
内容：符号函数
输入：FP32 x
输出：UCHAR8 +1 or -1
备注：大于零输出1，小于零输出-1
**************************************************************************************************************************************************************************************************/
UCHAR8 sign(FP32 x)
{
    if(x>0)
        return 1;
    else
        return 0;
}

weixing.h

C++
开启自动换行复制
#ifndef _WEIXING_H_
#define _WEIXING_H_

#include <math.h>
#include "main.h"


//#define SIM_DT              0.021F            //仿真步长s
#define ROCKET_DEMENTION    7                 //微分方程的维数


extern float SIM_DT;
//微分方程结构体-火箭微分方程当前的参数
typedef struct{
    //控制输入
    float Mx;//x轴力矩N*m
    float My;//y轴力矩N*m
    float Mz;//z轴力矩N*m
    //状态输入
    FP32 t;//当前时间s
    FP32 Y[ROCKET_DEMENTION];//当前状态Y=[0 WX rad/s, 1 WY rad/s, 2 WZ rad/s, 3滚转角rad, 4偏航角rad, 5俯仰角rad]
    //状态输出
    FP32 dy[ROCKET_DEMENTION];//当前状态变化率dy=dY/dt
}ST_ROCKET;

//函数声明
extern void daodan(ST_ROCKET *pst);
extern void my_ode45(ST_ROCKET* pst);
extern UCHAR8 sign(FP32 x);

//全局变量声明
extern ST_ROCKET rocket;
#endif // _WEIXING_H_

主函数

main.cpp

C++
开启自动换行复制
#include"main.h"

double a=0.0f;
double b=0.0f;
double c=5.0f;
UINT32 gl_cnt,gl_fps;
UINT32 systime;
UINT32 my_cnt;


//定时器中断100Hz
VOID CALLBACK myTimerProc1(HWND hwnd, UINT uMsg, UINT idEvent, DWORD dwTime )
{
    //卫星动力学模型
    my_ode45(&rocket);

    gl_cnt++;
    systime++;
    if(systime>50)
    {
        systime=0;
        gl_fps=gl_cnt;
        gl_cnt=0;
        my_cnt++;
        printf("t= %.1f s | wx=%.1f deg/s | wy=%.1f deg/s | wz=%.1f deg/s | dt=%.1f ms\n",
               my_cnt*50*(float)SIM_DT, rocket.Y[0]*57.3f, rocket.Y[1]*57.3f, rocket.Y[2]*57.3f,SIM_DT*1000);
        rocket.Mx=0;
        rocket.My=0;
        rocket.Mz=0;
    }
}

/****************************************************************************************************************************************************************************
*函数名：main
*功能：主函数
*备注：
*****************************************************************************************************************************************************************************************************************/
int main(int argc, char *argv[])
{
    printf("*******************************************************************************************************************************\n");
    printf("【1】鼠标选中卫星画面，切换英文输入法，通过按键施加力矩控制卫星\n");
    printf("【2】施加力矩：W-A-D-S-E-R\n");
    printf("【3】仿真步长：+ -\n");
    printf("【4】复位姿态：N\n");
    printf("【5】退出程序：Q\n");
    printf("【6】惯量主轴与体坐标系重合，参数Ix=0.005kg*m^2,Iy=6*Ix,Iz=4*Ix，基于四元数解算运动学模型。w=[Wx,Wy,Wz]'为体坐标系下角速度。2020/02\n");
    printf("*******************************************************************************************************************************\n");

    //开定时器中断，数值仿真
    SetTimer(NULL,0,(UINT32)(1000*SIM_DT),myTimerProc1);//间隔时间SIM_DT ms

    //openGL绘图
    my_opengl_task(argc, argv);

    return EXIT_SUCCESS;
}

opengl可视化部分

opengl.cpp

C++
开启自动换行复制
#include "main.h"

int slices=16;
int stacks=16;

const GLfloat light_ambient[]  = { 0.0f, 0.0f, 0.0f, 1.0f };
const GLfloat light_diffuse[]  = { 1.0f, 1.0f, 1.0f, 1.0f };
const GLfloat light_specular[] = { 1.0f, 1.0f, 1.0f, 1.0f };
const GLfloat light_position[] = { 2.0f, 5.0f, 5.0f, 0.0f };

const GLfloat mat_ambient[]    = { 0.7f, 0.7f, 0.7f, 1.0f };
const GLfloat mat_diffuse[]    = { 0.8f, 0.8f, 0.8f, 1.0f };
const GLfloat mat_specular[]   = { 1.0f, 1.0f, 1.0f, 1.0f };
const GLfloat high_shininess[] = { 100.0f };

/****************************************************************************************************************************************************************************
*函数名：my_opengl_task
*功能：penGL程序的入口
*备注：
*****************************************************************************************************************************************************************************************************************/
void my_opengl_task(int argc, char *argv[])
{
    glutInit(&argc, argv);

    //创建窗口
    glutInitWindowSize(640,480);//大小
    glutInitWindowPosition(10,10);//左上角的坐标

    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
    glutCreateWindow("卫星姿态动力学仿真");

    //penGL中断服务函数
    glutReshapeFunc(resize);
    glutDisplayFunc(display);
    glutKeyboardFunc(key);
    glutIdleFunc(idle);

    //场景光照等设置
    glClearColor(0,1,1,1);
    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);

    glEnable(GL_DEPTH_TEST);
    glDepthFunc(GL_LESS);

    glEnable(GL_LIGHT0);
    glEnable(GL_NORMALIZE);
    glEnable(GL_COLOR_MATERIAL);
    glEnable(GL_LIGHTING);

    glLightfv(GL_LIGHT0, GL_AMBIENT,  light_ambient);
    glLightfv(GL_LIGHT0, GL_DIFFUSE,  light_diffuse);
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);

    glMaterialfv(GL_FRONT, GL_AMBIENT,   mat_ambient);
    glMaterialfv(GL_FRONT, GL_DIFFUSE,   mat_diffuse);
    glMaterialfv(GL_FRONT, GL_SPECULAR,  mat_specular);
    glMaterialfv(GL_FRONT, GL_SHININESS, high_shininess);

    //penGL绘图主循环，直到窗口关闭时退出
    glutMainLoop();
}

/****************************************************************************************************************************************************************************
*函数名：resize
*功能：窗口改变的中断服务函数
*备注：在openGL程序入口，把他的指针传给中断服务函数
*****************************************************************************************************************************************************************************************************************/
void resize(int width, int height)
{
    const float ar = (float) width / (float) height;

    glViewport(0, 0, width, height);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glFrustum(-ar, ar, -1.0, 1.0, 2.0, 100.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity() ;
}


/****************************************************************************************************************************************************************************
*函数名：display
*功能：penGL绘图中断服务函数，主要的绘图函数
*备注：在openGL程序入口，把他的指针传给中断服务函数
*****************************************************************************************************************************************************************************************************************/
void display(void)
{
    //根据四元数计算欧拉轴角参数
    float phi2=acos(rocket.Y[3]);
    float phi=2.0f*phi2*57.3f;  //欧拉轴角-角度
    float temp=sin(phi2);
    float ex=rocket.Y[4]/temp;  //欧拉轴-轴方向
    float ey=rocket.Y[5]/temp;
    float ez=rocket.Y[6]/temp;

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3d(1,0,0);

    //绘制立方体
    glPushMatrix();
        glTranslated(0,0,-c);//平移
        glRotated(phi,ex,ey,ez);//旋转

        //绘制卫星
        if(1)//实体
        {
            glutSolidCube(1.0f);//立方体
            glTranslated(0,0,0.75f);//画镜筒
            glutSolidCube(0.5f);
            glTranslated(1.0f,0,-0.75f);//画太阳帆
            glutSolidCube(0.5f);
            glTranslated(-2.0f,0,0);//画太阳帆
            glutSolidCube(0.5f);
        }
        else//线框
        {
            glutWireCube(1.0f);
            glTranslated(0,0,0.75f);//画镜筒
            glutWireCube(0.5f);
            glTranslated(1.0f,0,-0.75f);//画太阳帆
            glutWireCube(0.5f);
            glTranslated(-2.0f,0,0);//画太阳帆
            glutWireCube(0.5f);
        }

        //glutSolidCube(1.0f);//立方体
        //glutSolidCone(1.0f, 1.0f,slices,stacks);//圆锥
        //glutSolidTorus(0.2,0.8,slices,stacks);//圆环
    glPopMatrix();

	

    //输入缓冲区
    glutSwapBuffers();
}


/****************************************************************************************************************************************************************************
*函数名：key
*功能：penGL按键中断服务函数，主要的按键响应
*备注：在openGL程序入口，把他的指针传给中断服务函数
*****************************************************************************************************************************************************************************************************************/
void key(unsigned char key, int x, int y)
{
    switch (key)
    {
        case 27 :
        case 'q':
            exit(0);
            break;

        case '+':
            //c+=0.01f;
            SIM_DT+=0.001f;
            break;

        case '-':
            //c-=0.01f;
            SIM_DT-=0.001f;
            break;
        //a
        case  'e':
            rocket.Mx=0.001f;
            printf("Mx = 1 mN*m\n");
            break;
        case  'r':
            rocket.Mx=-0.001f;
            printf("Mx = -1 mN*m\n");
            break;

        //b
        case  'a':
            rocket.My=0.001f;
            printf("           My = 1 mN*m\n");
            break;
        case  'd':
            rocket.My=-0.001f;
            printf("           My = -1 mN*m\n");
            break;

        //c
        case  'w':
            rocket.Mz=0.001f;
            printf("                          Mz = 1 mN*m\n");
            break;
        case  's':
           rocket.Mz=-0.001f;
           printf("                          Mz = -1 mN*m\n");
            break;

        case 'n':
            for(UCHAR8 i=0;i<ROCKET_DEMENTION;i++)
            {
                rocket.dy[i]=0;
                rocket.Y[i]=0;
            }
            rocket.Y[3]=1.0f;
            SIM_DT=0.021f;
            break;

        default:
            break;
    }

    //printf("Mx=%.1f mN*m| My=%.1f mN*m | Mz=%.1f mN*m\n",rocket.Mx*1000,rocket.My*1000,rocket.Mz*1000);

    glutPostRedisplay();
}


/****************************************************************************************************************************************************************************
*函数名：idle
*功能：penGL按键中断服务函数，主要的按键响应
*备注：在openGL程序入口，把他的指针传给中断服务函数
*****************************************************************************************************************************************************************************************************************/
void idle(void)
{
    glutPostRedisplay();
}

opengl.h

C++
开启自动换行复制
#ifndef _OPENGL_H_
#define _OPENGL_H_


#include <GL/glut.h>
#include <GL/gl.h>
#include <GL/glu.h>

extern void my_opengl_task(int argc, char *argv[]);
extern void resize(int width, int height);
extern void sysTick(void);
extern void display(void);
extern void key(unsigned char key, int x, int y);
extern void idle(void);

extern int slices;
extern int stacks;

#endif // _OPENGL_H_