卫星姿态动力学实时可视化仿真【exe程序+说明+源码】
忆昔长别 2020-7-27原创 航天技术数学
中文摘要
不久前写的卫星仿真代码,被老师拿去给本科生上课教学了,拿出来分享一下。
关键词
卫星航天动力学仿真

1、程序运行效果


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


运行截图

QQ截图20200423175132.jpg QQ截图20200423175124.jpg QQ截图20200423175107.jpg


运行动画

20200727-011742 00_00_00-00_00_10.gif


2、仿真说明


卫星的动力学模型如下

\[\left \{ \begin{aligned} &\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{aligned} \right.\]

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

\[\begin{bmatrix}\dot{q_0} \\ \dot{q_1} \\ \dot{q_2}\\ \dot{q_3}\end{bmatrix} =\frac{1}{2}\begin{bmatrix} &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{bmatrix} \begin{bmatrix} &0 \\ &w_x+2(q_1q_2+q_3q_0)w_0 \\ &w_y+(q_0^2-q_1^2+q_2^2-q_3^2)w_0\\ &w_z+2(q_2q_3-q_1q_0)w_0 \end{bmatrix}\]

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

$$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程序


QQ截图20200727021629.jpg

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


4、源码


动力学模型部分

weixing.cpp

#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

#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

#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

#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

#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_


[修改于 1 年前 - 2020-07-27 17:30:08]

来自:航天航空 / 航天技术数理化 / 数学
 
7
忆昔长别 作者
1年3个月前
1楼

公式挂了 sticker sticker

回复
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
忆昔长别作者
1年3个月前
2楼

后续的改进方向

(1)加上干扰力矩模型。

(2)加上地球磁场模型、磁力矩器模型、飞轮模型,编写三轴姿态控制算法、飞轮卸载算法。

(3)转移到ubuntu平台开发。使用QT开发上位机,方便调参,类似MIT Mini Cheetah开源代码。


回复
评论
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
该用户不需要名字
1年3个月前
3楼

sticker 坎巴拉(专业版)


回复
评论(1)
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论
苯宝宝正辛酸
1年3个月前
4楼

手机版有简单火箭2(业余版) sticker sticker

回复
评论(1)
1
加载评论中,请稍候...
200字以内,仅用于支线交流,主线讨论请采用回复功能。
折叠评论

想参与大家的讨论?现在就 登录 或者 注册

所属专业
上级专业
同级专业
忆昔长别
进士 学者 机友 笔友
文章
15
回复
92
学术分
2
2015/12/24注册,2 个月前活动
暂无简介
%7B%22isDisplay%22%3Atrue%7D
视频暂不能访问,请登录试试
仅供内部学术交流或培训使用,请先保存到本地。本内容不代表科创观点,未经原作者同意,请勿转载。
音频暂不能访问,请登录试试
插入资源
全部
图片
视频
音频
附件
全部
未使用
已使用
正在上传
空空如也~
上传中..{{f.progress}}%
处理中..
上传失败,点击重试
等待中...
{{f.name}}
空空如也~
(视频){{r.oname}}
{{selectedResourcesId.indexOf(r.rid) + 1}}
处理中..
处理失败
插入表情
我的表情
共享表情
Emoji
上传
注意事项
最大尺寸100px,超过会被压缩。为保证效果,建议上传前自行处理。
建议上传自己DIY的表情,严禁上传侵权内容。
点击重试等待上传{{s.progress}}%处理中...已上传
空空如也~
草稿箱
加载中...
此处只插入正文,如果要使用草稿中的其余内容,请点击继续创作。
{{fromNow(d.toc)}}
{{getDraftInfo(d)}}
标题:{{d.t}}
内容:{{d.c}}
继续创作
删除插入插入
{{forum.displayName}}
{{forum.countThreads}}
篇文章,
{{forum.countPosts}}
条回复
{{forum.description || "暂无简介"}}
ID: {{user.uid}}
学术分隐藏
投诉或举报
加载中...
{{tip}}
请选择违规类型:
{{reason.type}}

空空如也

支持的图片格式:jpg, jpeg, png
插入公式
分享回复:{{shareId}}
加载中...
评论控制
加载中...
文号:{{pid}}
投诉或举报
加载中...
{{tip}}
请选择违规类型:
{{reason.type}}

空空如也

加载中...
详情
详情
推送到专栏从专栏移除
设为匿名取消匿名
查看作者
回复
只看作者
加入收藏取消收藏
加入关注取消关注
折叠回复
置顶取消置顶
评学术分
鼓励
设为精选取消精选
建议修改
编辑
通过审核
评论控制
退修或删除
历史版本
违规记录
投诉或举报
加入黑名单移除黑名单
查看IP
{{format('YYYY/MM/DD HH:mm:ss', toc)}}
投诉或举报
加载中...
{{tip}}
请选择违规类型:
{{reason.type}}

空空如也

下载资料
{{fileName}}
大小:{{size}}
下载当前附件将花费 {{costMessage}}
{{description}}
你当前剩余 {{holdMessage}}
{{fileName}}
大小:{{size}}
当前附件免费。
你已购买过此附件,下载当前附件不需要花费积分。
加载中...
{{errorInfo}}
附件已丢失
当前账号的附件下载数量限制如下:
时段 个数
{{f.startingTime}}点 - {{f.endTime}}点 {{f.fileCount}}