Psins代码解析一之全局变量&轨迹仿真&

旋转椭球体的4个基本参数：长半轴、扁率(椭圆度)、地心引力常数、自转角速率；

以上内容来自：《车载定位定向系统关键技术研究》付强文

旋转椭球体：

地球自转角速度：

地球重力加速度为：

子午圈和卯酉圈曲率半径为：

 

 以上内容来自：《捷联惯导算法及车载组合导航系统研究（硕士论文）》严恭敏

一、全局变量

global glv
    if ~exist('Re', 'var'),  Re = [];  end
    if ~exist('f', 'var'),   f = [];  end
    if ~exist('wie', 'var'), wie = [];  end
    if isempty(Re),  Re = 6378137;  end
    if isempty(f),   f = 1/298.257;  end
    if isempty(wie), wie = 7.2921151467e-5;  end
    glv.Re = Re;                    % the Earth's semi-major axis
    glv.f = f;                      % flattening
    glv.Rp = (1-glv.f)*glv.Re;      % semi-minor axis
    glv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; % 1st eccentricity
    glv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; % 2nd eccentricity
    glv.wie = wie;                  % the Earth's angular rate
    glv.meru = glv.wie/1000;        % milli earth rate unit
    glv.g0 = 9.7803267714;          % gravitational force
    glv.mg = 1.0e-3*glv.g0;         % milli g
    glv.ug = 1.0e-6*glv.g0;         % micro g 为 微g
    glv.mGal = 1.0e-3*0.01;         % milli Gal = 1cm/s^2 ~= 1.0E-6*g0
    glv.ugpg2 = glv.ug/glv.g0^2;    % ug/g^2
    glv.ws = 1/sqrt(glv.Re/glv.g0); % Schuler frequency
    glv.ppm = 1.0e-6;               % parts per million百万分之一
    glv.deg = pi/180;               % arcdeg 弧度单位 rad
    glv.min = glv.deg/60;           % arcmin
    glv.sec = glv.min/60;           % arcsec
    glv.hur = 3600;                 % time hour (1hur=3600second)
    glv.dps = pi/180/1;             % arcdeg / second
    glv.dph = glv.deg/glv.hur;      % arcdeg / hour 为 rad/hour
    glv.dpss = glv.deg/sqrt(1);     % arcdeg / sqrt(second) 为 rad/sqrt(s)
    glv.dpsh = glv.deg/sqrt(glv.hur);  % arcdeg / sqrt(hour) 为 rad/sqrt(hour)
    glv.dphpsh = glv.dph/sqrt(glv.hur); % (arcdeg/hour) / sqrt(hour) 为 rad/hour/sqrt(hour)
    glv.Hz = 1/1;                   % Hertz
    glv.dphpsHz = glv.dph/glv.Hz;   % (arcdeg/hour) / sqrt(Hz) 为 rad/hour/sqrt(Hz)
    glv.ugpsHz = glv.ug/sqrt(glv.Hz);  % ug / sqrt(Hz)
    glv.ugpsh = glv.ug/sqrt(glv.hur); % ug / sqrt(hour)
    glv.mpsh = 1/sqrt(glv.hur);     % m / sqrt(hour)
    glv.mpspsh = 1/1/sqrt(glv.hur); % (m/s) / sqrt(hour), 1*mpspsh~=1700*ugpsHz
    glv.ppmpsh = glv.ppm/sqrt(glv.hur); % ppm / sqrt(hour)
    glv.mil = 2*pi/6000;            % mil
    glv.nm = 1853;                  % nautical mile 海里
    glv.kn = glv.nm/glv.hur;        % knot 节
    %%
    glv.wm_1 = [0,0,0]; glv.vm_1 = [0,0,0];   % the init of previous gyro & acc sample
    glv.cs = [                      % coning & sculling compensation coefficients
        [2,    0,    0,    0,    0    ]/3
        [9,    27,   0,    0,    0    ]/20
        [54,   92,   214,  0,    0    ]/105
        [250,  525,  650,  1375, 0    ]/504 
        [2315, 4558, 7296, 7834, 15797]/4620 ];
    glv.csmax = size(glv.cs,1)+1;  % max subsample number
    glv.v0 = [0;0;0];    % 3x1 zero-vector
    glv.qI = [1;0;0;0];  % identity quaternion 初始四元数
    glv.I33 = eye(3); glv.o33 = zeros(3);  % identity & zero 3x3 matrices
    glv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380]; % position of INS [e-mail]
    glv.eth = []; glv.eth = earth(glv.pos0);
    %%
    [glv.rootpath, glv.datapath, glv.mytestflag] = psinsenvi;
    glv1 = glv;

二、生成仿真轨迹

俯仰角抬头为正；横滚角右倾斜为正；

其中方位角变化时（转弯时），先将飞机滚转相应的角度；即：先滚转、后转弯；

已知航向角速率、前向速度，利用向量叉乘得到向心力；利用如下公式，计算出滚转角、滚转角速率、

代码实现：

%‘2’为航向角速率(转弯)，2deg/s，转弯时间为45秒;
%协调转弯时间为4秒，即飞机横滚时间，很滚角速率 根据前向速度和航向角速率计算出
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4); 

trjsegment子函数中对应部分：

        case 'coturnleft', % coordinate turn left
            rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting; %rollw为横滚角速率（deg/s）
            seg = trjsegment(seg, 'rollleft',  rolllasting, rollw);
            seg = trjsegment(seg, 'turnleft',  lasting, w);
            seg = trjsegment(seg, 'rollright', rolllasting, rollw);

        case 'turnleft',
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, w*dps,-cf, 0, 0]];
        case 'turnright',
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0,-w*dps, cf, 0, 0]];
        case 'rollleft',
            seg.wat = [seg.wat; [lasting, seg.vel, 0,-w*dps, 0, 0, 0, 0]];
        case 'rollright',
            seg.wat = [seg.wat; [lasting, seg.vel, 0, w*dps, 0, 0, 0, 0]];

描述一段完整的运动轨迹，其中初始姿态为0、初始速度为0、初始位置为【34.246048; 108.909664; 380】

• 初始化
• 静止100秒
• 向北加速飞行10秒（a=1m/s^2）
• 匀速保持100秒
• 左转弯90°(最终方向为正西，包括飞机先滚转、再转弯、最后反向滚转，恢复滚转角为0)
• 匀速保持100秒
• 右转弯450°（最终方向为正北，包括飞机先滚转、再转弯、最后反向滚转，恢复滚转角为0）
• 匀速保持100秒
• climb 抬头(2°/s *10s=20°)-匀速保持-低头(-2°/s *10s=-20°)
• 匀速保持100秒
• descent 低头(-2°/s *10s=-20°) -保持 -抬头(2°/s *10s=20°)
• 匀速保持100秒
• 减速 5秒（a=-2m/s^2），至速度为0
• 静止100秒

通过轨迹可以看出，最终：roll=pitch=0；yaw=0°;飞机朝北飞行；飞机速度矢量为0矢量；