Psins代码解析之全局变量&轨迹仿真(test_SINS_trj.m)&惯性解算(test_SINS.m)

旋转椭球体的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/s;
%比如:imuerr.eb(1:3) = 1*glv.dph;即为:((1*pi/180)/3600)rad/s
    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);最终单位为:rad/sqrt(s)
%imuerr.web(1:3) = web*glv.dpsh;即为:((1*pi/180)/sqrt(3600))rad/s   
    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 Lab@NWPU
    glv.eth = []; glv.eth = earth(glv.pos0);
    %%
    [glv.rootpath, glv.datapath, glv.mytestflag] = psinsenvi;
    glv1 = glv;

二、生成仿真轨迹 test_SINS_trj.m

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

其中方位角变化时(转弯时),先将飞机滚转相应的角度;即:先滚转、后转弯;

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

代码实现:

%‘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矢量;轨迹仿真时间为:966秒

航向角:

水平姿态角:

存在问题地方:生成角增量和速度增量的公式:

三、纯惯性导航仿真 test_SINS.m

由于纯惯导解算高度通道发散;

1、IMU误差生成函数和IMU数据添加误差函数:imuerrset.m和imuadderr.m

此处主要添加两项误差:常值零偏和随机游走为例:

(1)imuerrset.m

%% constant bias & random walk
imuerr.eb(1:3) = eb*glv.dph;   imuerr.web(1:3) = web*glv.dpsh;
imuerr.db(1:3) = db*glv.ug;    imuerr.wdb(1:3) = wdb*glv.ugpsHz;

最终:

imuerr.eb单位为:rad/s ;  imuerr.web单位为:rad/sqrt(s)

imuerr.db单位为:m/s^2 ;  imuerr.wdb单位为:m/s^2/sqrt(s)

(2)imuadderr.m

    drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...
              ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...
              ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...
              ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...
              ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...
              ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];
imu(:,1:6) = imu(:,1:6) + drift;

2、初始姿态、速度、位置误差

avpseterr.m

function davp = avpseterr(phi, dvn, dpos)
% avp errors setting.
% Inputs: phi - platform misalignment angles. NOTE: leveling errors 
%               phi(1:2) in arcsec, azimuthe error phi(3) in arcmin
%         dvn - velocity errors in m/s
%         dpos - position errors dpos=[dlat;dlon;dhgt], all in m
% Output: davp = [phi; dvn; dpos]

3、利用一阶马尔可夫生成气压高度计仿真信息

bhsimu.m

    t = (trj.avp(1,10):ts:trj.avp(end,10))';
    bh = interp1(trj.avp(:,10), trj.avp(:,9), t, 'linear');
    bh = bh + bias + markov1(var, tau, ts, length(bh),1);

一阶马尔可夫过程生成的数据为:

利用一阶马尔可夫+初始高度偏差+真实高度=气压高度计,结果为;

目的是每次捷联惯导更新时,都将气压高度计送到位置信息中;

ins.vn(3) = href(k1,2);  ins.pos(3) = href(k1,1);

4、纯惯导解算:

(1)首先对地球、载体相关参数进行外推以双子样为例,nts=2*ts,外推ts;

    %% earth & angular rate updating 
    vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2
    ins.eth = ethupdate(ins.eth, pos01, vn01);
    ins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimu
    ins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
%     ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;
    ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30 因为是外推1/2时刻,所以除以2

主要是利用:

  • 前一时刻速度、加速度、前一时刻位置;外推得到nts时刻的速度、位置
  • 利用外推得到的速度、位置去更新地球相关参数,主要为:子午圈、卯酉圈半径;东北天下wnie分量(与纬度有关)、wnen分量(与速度、位置有关)、重力加速度(与纬度、高度有关)、等信息

(2)速度更新:以双子样为例,nts=2*ts

程序中,首先根据子样数进行圆锥补偿,得到dvbm,即速度增量;

    nn = size(imu,1);
    nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;
    [phim, dvbm] = cnscl(imu,0);    % coning & sculling compensation
%     [phim, dvbm] = cnscl0(imu);    % coning & sculling compensation
    phim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration

然后根据速度更新公式:

其中速度更新公式中的一项如下:dvbm/Δt就等于括号中的内容,也就是下面的 ins.fb

也就是ins.fn=qmulv(ins.qnb,ins.fb);然后通过旋转矢量进行变换;

    %% (1)velocity updating
    ins.fn = qmulv(ins.qnb, ins.fb);
%     ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;
    ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;
    vn1 = ins.vn + ins.an*nts;

(3)位置更新:以双子样为例,nts=2*ts

利用上面外推nts时刻得到的地球参数和速度更新得到的速度;利用梯形积分得到位置;

    %% (2)position updating
%     ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];
    ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
%     ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30
    ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;
    ins.pos = ins.pos + ins.Mpvvn*nts;  
    ins.vn = vn1;
    ins.an0 = ins.an;

(4)姿态更新:以双子样为例,nts=2*ts

    %% (3)attitude updating
    ins.Cnb0 = ins.Cnb;
%     ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than next line
    ins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);
    [ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);
    ins.avp = [ins.att; ins.vn; ins.pos];

利用四元数进行姿态更新;

其中用到了三角函数和单位四元数关系:

5、误差图:

 

  • 26
    点赞
  • 123
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
以下是一个简单的IMU SINS(strapdown inertial navigation system)解算代码框架,包含了主要的解算步骤和数据处理流程: 1. 定义初始状态和常数: ```python import numpy as np # IMU常数 g = 9.81 # 重力加速度 w_ie = 7.2921150e-5 # 地球自转角速度 # 初始状态 lat0 = 39.9 * np.pi / 180 # 初始纬度 h0 = 1000 # 初始高度 v0 = np.array([0, 0, 0]) # 初始速度 Rm = 6378137 # 地球半径 Rn = 6356752 # 地球极半径 ``` 2. 定义主函数,包含IMU数据处理和SINS解算: ```python def main(imu_data, dt): # 定义状态和误差 pos = np.array([lat0, 0, -h0]) # 纬度、经度和高度 vel = v0 # 速度 C_b_n = np.eye(3) # 姿态矩阵 phi = 0 # 横滚角 theta = 0 # 俯仰角 psi = 0 # 航向角 dpos = np.zeros(3) # 位置误差 dvel = np.zeros(3) # 速度误差 dC_b_n = np.zeros((3, 3)) # 姿态误差 # 循环处理IMU数据 for i in range(len(imu_data)): # 获取IMU数据 w_ib_b = imu_data[i][0:3] # 角速度 f_ib_b = imu_data[i][3:6] # 加速度 # 转换到地理坐标系 w_ie_n = np.array([0, w_ie * np.cos(pos[0]), w_ie * np.sin(pos[0])]) w_ib_n = np.dot(C_b_n, w_ib_b) f_ib_n = np.dot(C_b_n, f_ib_b) f_ib_n[2] = f_ib_n[2] - g f_ib_n = f_ib_n - np.cross(w_ie_n + w_ib_n, v0 + np.array([0, 0, w_ie * (Rm/Rn)**2 * np.sin(pos[0])])) # 更新状态和误差 pos += dt * (vel / (Rm + h0)) vel += dt * (np.dot(C_b_n, f_ib_b.T) - np.array([0, 0, g])) C_b_n = np.dot(C_b_n, update_attitude(w_ib_b, phi, theta, psi, dt)) phi, theta, psi = attitude_angles(C_b_n) dpos = dpos + dt * (vel / (Rm + h0)) dvel = dvel + dt * (np.dot(C_b_n, f_ib_b.T) - np.array([0, 0, g])) dC_b_n = np.dot(dC_b_n, update_attitude(w_ib_b, phi, theta, psi, dt)) # 输出最终状态 print("Position: ", pos) print("Velocity: ", vel) print("Attitude: ", phi, theta, psi) ``` 3. 定义姿态角更新函数: ```python def update_attitude(w_ib_b, phi, theta, psi, dt): p, q, r = w_ib_b C_b_n = np.array([ [1, np.sin(phi) * np.tan(theta), np.cos(phi) * np.tan(theta)], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi) / np.cos(theta), np.cos(phi) / np.cos(theta)] ]) dC_b_n = np.dot(C_b_n, np.array([ [0, r, -q], [-r, 0, p], [q, -p, 0] ])) return np.eye(3) + dC_b_n * dt ``` 4. 定义姿态角计算函数: ```python def attitude_angles(C_b_n): phi = np.arctan2(C_b_n[1, 2], C_b_n[2, 2]) theta = -np.arcsin(C_b_n[0, 2]) psi = np.arctan2(C_b_n[0, 1], C_b_n[0, 0]) return phi, theta, psi ``` 以上是一个简单的IMU SINS解算代码框架,实际应用中需要根据具体情况进行修改和优化。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值