52matlab技术网站,matlab教程,matlab安装教程,matlab下载

 找回密码
 立即注册
搜索
热搜: 活动 交友 discuz
查看: 886|回复: 2

捷联惯导程序

[复制链接]

92

主题

163

帖子

2043

积分

版主

Rank: 7Rank: 7Rank: 7

积分
2043
发表于 2020-5-15 05:24:51 | 显示全部楼层 |阅读模式
捷联惯导程序,依据加表和陀螺仪的输出数据来求解飞行器的姿态

clc;
clear;
format long;  %设置数据精度为15位小数

Data=importdata('temp.txt'); % 导入实验所采集的数据,以矩阵形式赋给Data变量,temp.txt必须与该M文件在同一个文件夹中
Px=Data(:,3);  % Px,Py,Pz为陀螺仪的输出值
Py=Data(:,4);
Pz=Data(:,5);

Nx=Data(:,6);  % Nx,Ny,Nz为加速度计的输出值
Ny=Data(:,7);
Nz=Data(:,8);
% 陀螺仪模型参数标定如下:
Sx  = -4.085903e-006 ; Sy  = -4.085647e-006 ; Sz  = -4.085170e-006 ;
Mxy = 5.059527e-003  ; Mxz = -1.031103e-003 ; Myx = -3.355451e-003 ;
Myz = 3.508468e-003  ; Mzx = -1.266671e-003 ; Mzy = -2.318244e-004 ;
Dx  = -2.009710e-006 ; Dy  = 8.156346e-007  ; Dz  = -5.749059e-007 ;
GyroCali_A = [ 1 -Mxy -Mxz ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];
% 加速度计模型参数标定如下:
Kx  = 9.272930e-004  ; Ky  = 9.065544e-004  ; Kz  = 9.443748e-004  ;
Ixy = 6.533872e-003  ; Ixz = 9.565992e-004  ; Iyx = -6.319376e-003 ;
Iyz = -6.902339e-004 ; Izx = -1.144549e-003 ; Izy = -3.857963e-004 ;
Bx  = -3.400847e-002 ; By  = -8.916341e-003 ; Bz  = -9.947414e-003 ;
AccCali_A = [1 -Ixy -Ixz ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];

Delta_t = 0.05; %采样时间为0.05秒

Delta_Theta_x = 0;
Delta_Theta_y = 0;
Delta_Theta_z = 0;  %定义陀螺仪输出的角度增量

Delta_Vx = 0;
Delta_Vy = 0;
Delta_Vz = 0;      %定义加速度计输出的速度增量

L = zeros(1,12001);
L(1)= 45.7328*pi/180 ;    %纬度用L表示,纬度的初始值划为弧度形式,因为后面计算位置矩阵更新
L(2)= 45.7328*pi/180 ;    %时需要用到前两次的L值来计算当前L值,所以在此定义2个初始L值
Lamda = 126.6287*pi/180 ; %经度用Lamda表示,经度的初始值划为弧度形式
h = 136 ;                 %高度用h表示
V = [ 0 ; 0 ; 0 ];  %导航坐标系中的东北天初始速度都为0
Vx = 0;             %方便后面的速度计算与速度更新
Vy = 0;
Vz = 0;

Theta = 0;
Gama  = 0;  
Fai   = 0;          %初始姿态角(俯仰角/倾斜角/航向角)都为0,此处均为弧度

Re = 6378254 ;  Rp = 6356803 ;%定义地球的半长轴与半短轴
e  = (Re - Rp)/Re ;           %定义旋转椭球扁率(椭球度)
Wie = 15.04107/180*pi ;       %定义地球自转角速度,地球坐标系相对于惯性坐标系的角速度

Theta_Matrix = zeros(1,12000);    %定义姿态角矩阵,供画图用
Gama_Matrix  = zeros(1,12000);
Fai_Matrix   = zeros(1,12000);
L_Matrix     = zeros(1,12001);    %定义经纬度矩阵,供画图用,L的特殊性决定了其数据个数为12001
L_Matrix(1)  = 45.7328;           
Lamda_Matrix = zeros(1,12000);
Ve_Matrix    = zeros(1,12000);    %定义速度矩阵,供画图用
Vn_Matrix    = zeros(1,12000);
Vu_Matrix    = zeros(1,12000);

%以下计算捷联矩阵的初始值,捷联矩阵的初始值仅仅由Theta,Gama,Fai的初始值决定
T = [ cos(Gama)*cos(Fai)-sin(Gama)*sin(Theta)*sin(Fai)  -cos(Theta)*sin(Fai)  sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(Fai) ;
          cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai)   cos(Theta)*cos(Fai)  sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai) ;
                -sin(Gama)*cos(Theta)                             sin(Theta)                   cos(Gama)*cos(Theta)                 ];
    %由捷联矩阵的初始值计算初始四元数值,为捷联矩阵的实时更新做准备
    if(T(3,2)-T(2,3)>0)
        Q1 = 0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));
    else if (T(3,2)-T(2,3)==0)
        Q1 = 0;
     else Q1 = -0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));
        end            %求解Q1
    end

    if(T(1,3)-T(3,1)>0)
        Q2 = 0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));
    else if (T(1,3)-T(3,1)==0)
       Q2 = 0;
     else Q2 = -0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));
        end            %求解Q2
    end

    if(T(2,1)-T(1,2)>0)
        Q3 = 0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));
     else if (T(2,1)-T(1,2)==0)
       Q3 = 0;
        else Q3 = -0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));
         end           %求解Q3
    end
        
    Q0 = 0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3);   %求解Q0
    Q = [Q0 ; Q1 ; Q2 ; Q3];  %四元数初始值
    Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %四元数的初始归一化,为得到最小漂移误差
   
   %以下求位置矩阵的初始值,通过位置矩阵更新后,反过来算运载体所在的经纬度
   %位置矩阵仅仅与经纬度有关系,Ce2n表示把地球坐标系转换为导航坐标系的转换矩阵
    Ce2n = [     -sin(Lamda)             cos(Lamda)                0 ;
             -sin( L(1) )*cos(Lamda)    -sin(L(1))*sin(Lamda)      cos( L(1) );  
              cos( L(1) )*cos(Lamda)   cos( L(1) )*sin(Lamda)   sin( L(1) )     ];

%大循环,共执行12000次,实时更新捷联矩阵,速度矩阵,位置矩阵,保存作图所需数据
for k = 1:12000;
   
    GyroCali_B = [Sx*Px(k)-Dx*Delta_t ; Sy*Py(k)-Dy*Delta_t ; Sz*Pz(k)-Dz*Delta_t ];
    Delta_Theta = GyroCali_A * GyroCali_B ; %计算陀螺仪输出的角度增量
    Delta_Theta_x = Delta_Theta(1);
    Delta_Theta_y = Delta_Theta(2);
    Delta_Theta_z = Delta_Theta(3);
    Delta_Theta_Module = sqrt( Delta_Theta_x * Delta_Theta_x + Delta_Theta_y * Delta_Theta_y + Delta_Theta_z * Delta_Theta_z );

    AccCali_B = [Kx*Nx(k)-Bx*Delta_t ; Ky*Ny(k)-By*Delta_t ; Kz*Nz(k)-Bz*Delta_t ];
    Delta_V = AccCali_A * AccCali_B ;       %计算加速度计输出的速度增量
    Delta_Vx = Delta_V(1);
    Delta_Vy = Delta_V(2);
    Delta_Vz = Delta_V(3);
    Delta_V_Module = sqrt( Delta_Vx * Delta_Vx + Delta_Vy * Delta_Vy + Delta_Vz * Delta_Vz );

    %使用毕卡法求解四元数更新矩阵,即捷联矩阵
    Bika = zeros(4);
    Bika(1,1) = cos(0.5 * Delta_Theta_Module);
    Bika(1,2) = -Delta_Theta_x / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);
    Bika(1,3) = -Delta_Theta_y / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);
    Bika(1,4) = -Delta_Theta_z / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);
    Bika(2,1) = -Bika(1,2);
    Bika(2,2) =  Bika(1,1);
    Bika(2,3) = -Bika(1,4);
    Bika(2,4) =  Bika(1,3);
    Bika(3,1) = -Bika(1,3);
    Bika(3,2) = -Bika(2,3);
    Bika(3,3) =  Bika(1,1);
    Bika(3,4) = -Bika(1,2);
    Bika(4,1) = -Bika(1,4);
    Bika(4,2) = -Bika(2,4);
    Bika(4,3) = -Bika(3,4);
    Bika(4,4) =  Bika(1,1);
    Q = Bika * Q;    % 每循环一次,更新一次四元素Q值,为求捷联矩阵
    Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);  %四元数的归一化,为得到最小漂移误差
    Q0 = Q(1);
    Q1 = Q(2);
    Q2 = Q(3);
    Q3 = Q(4);
   
    %捷联矩阵的四元数表达式
    T = [ Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q3      2*(Q1*Q2-Q0*Q3)       2*(Q1*Q3+Q0*Q2)
            2*(Q1*Q2+Q0*Q3)       Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3    2*(Q2*Q3-Q0*Q1)
             2*(Q1*Q3-Q0*Q2)       2*(Q2*Q3+Q0*Q1)     Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3 ];
    %*********************************************************************
    %********************求三个姿态角Theta,Gama和 Fai ********************
    %*********************************************************************
    Theta_Main = asin( T(3,2) );
    Gama_Main = atan( -T(3,1) / T(3,3));
    Fai_Main  = atan( -T(1,2) / T(2,2));
    Theta = Theta_Main;
   
    if (T(3,3)>0)
        Gama = Gama_Main ;
    else if (T(3,3)<0 && Gama_Main > 0)
            Gama = Gama_Main + pi;
        else  Gama = Gama_Main - pi;  %此处用else实为不妥,不过为了程序的完善性,只能这样了
        end
    end
   
    if ( T(2,2)<0 )
        Fai = Fai_Main + pi ;
    else if (T(2,2)==0)
            Fai = pi/2;
        else if ( Fai_Main>0)
            Fai = Fai_Main;
            else Fai = Fai_Main + 2*pi ;
            end
        end
    end
   
    %以下存储姿态角到三个矩阵里面,为画图做准备
    Theta_Matrix(k) = Theta*180/pi;   %作图用矩阵,以角度表示
    Gama_Matrix(k) = Gama*180/pi;     %作图用矩阵,以角度表示
    if (Fai<2*pi)
        Fai_Matrix(k) = Fai*180/pi;
    else  Fai_Matrix(k) = Fai*180/pi-360;  %作图用矩阵,以角度表示
    end
    %到此为止,姿态角的求解完毕,以下先求速度
    %*********************************************************************
    %********************求飞行器相对于东北天的速度*************************
    %*********************************************************************
    Rm = Re*( 1-2*e+3*e*sin( L(k+1) )^2 );
    Rn = Re*( 1+e*sin(L(k+1))^2 );
    LL = 3/2*L(k+1) - 1/2*L(k) ;
    F = [ 0  -1/(Rm + h)  0 ; 1/(Rn + h) 0  0 ; tan( LL )/(Rn + h)  0  0];
    g = 9.7803+0.051799*sin(L(k+1))^2-0.94114e-006*h;
    G = [0;0;-g];
    Wen2n = F*V;
    Wie2n = [0 ; Wie*cos(L(k+1)); Wie*sin(L(k+1))];
    W = 2*Wie2n + Wen2n;
    W_X = [ 0 -W(3) W(2) ; W(3)  0  -W(1) ; -W(2)  W(1) 0 ]; %此式中W_X为 2*Wie+Wen2n 的反对称矩阵
    V = V + T*( Delta_V+ [ 0 -Delta_Theta_z Delta_Theta_y ; Delta_Theta_z  0  -Delta_Theta_x ; -Delta_Theta_y  Delta_Theta_x 0 ]*Delta_V ) + Delta_t*(G-W_X*V);

    Vx = V(1);
    Vy = V(2);
    Vz = V(3);
    Ve_Matrix(k) = V(1);
    Vn_Matrix(k) = V(2);
    Vu_Matrix(k) = V(3);
   
    %*********************************************************************
    %********************求飞行器所在的经纬度******************************
    %*********************************************************************
   
    Epsilon = F*V*Delta_t;
    Ce2n =( eye(3) - [ 0 -Epsilon(3) Epsilon(2) ; Epsilon(3)  0  -Epsilon(1) ; -Epsilon(2)  Epsilon(1) 0 ] )*Ce2n ; %位置矩阵实时更新
    %下面通过位置矩阵来实时更新经纬度
    L(k+2) = asin( Ce2n(3,3)); %由于L本来就是以矩阵形式定义的,下面定义一个把L用角度表示的矩阵
    L_Matrix(k+1) = L(k+2)*180/pi;
    Lamda_Main = atan( Ce2n(3,2)/Ce2n(3,1) ); %计算出来的L和Lamda都是弧度制的
    if (Ce2n(3,1)>0)
        Lamda = Lamda_Main;
    else if (Lamda_Main<0)  
             Lamda = Lamda_Main + pi;
        else Lamda = Lamda_Main - pi;
        end
    end
    Lamda_Matrix(k) = Lamda*180/pi;  %作图用矩阵,以角度表示
   
end
%**************************************************************************
%********************以下是画图程序*****************************************
%**************************************************************************
k=1:1:12000;              %绘制三轴姿态变化图线-绿色
figure(1);
plot(k/20,Theta_Matrix(k),'g');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Theta(俯仰角)');
grid on;
figure(2);
plot(k/20,Gama_Matrix(k),'g');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Gama(滚转角)');
grid on;
figure(3);
plot(k/20,Fai_Matrix(k),'m');
xlabel('Time(second)');
ylabel('Angle(degree)');
title('Fai(偏航角)');
grid on;
                          %绘制东北天个方向的速度变化曲线-红色
figure(4);
plot(k/20,Ve_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Ve(东向速度)');
grid on;
figure(5);
plot(k/20,Vn_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Vn(北向速度)');
grid on;
figure(6);
plot(k/20,Vu_Matrix(k),'r');
xlabel('Time(second)');
ylabel('Velocity(m/s)');
title('Vu(天向速度)');
grid on;
                          %绘制飞行器所在经纬度曲线-蓝色
figure(7);
plot(k/20,L_Matrix(k),'b');
xlabel('Time(second)');
ylabel('Degree');
title('Latitude L(纬度)');
grid on;

figure(8);
plot(k/20,Lamda_Matrix(k),'b');
xlabel('Time(second)');
ylabel('Degree');
title('Longitude Lamda(经度)');
grid on;
回复

使用道具 举报

92

主题

163

帖子

2043

积分

版主

Rank: 7Rank: 7Rank: 7

积分
2043
 楼主| 发表于 2020-5-15 05:25:06 | 显示全部楼层
捷联惯导程序,依据加表和陀螺仪的输出数据来求解飞行器的姿态
回复 支持 反对

使用道具 举报

0

主题

10

帖子

12

积分

新手上路

Rank: 1

积分
12
发表于 2020-6-7 07:23:51 | 显示全部楼层
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|52matlab技术网站 ( 粤ICP备14005920号-5 )

GMT+8, 2020-8-9 13:48 , Processed in 0.062851 second(s), 19 queries .

Powered by Discuz! X3.2 Licensed

© 2001-2013 Comsenz Inc.

快速回复 返回顶部 返回列表