Psins代码解析之粗对准(test_align_methods_compare.m)&精对准

• 背景：

导航坐标系：东-北-天

载体坐标系：右-前-上

欧拉角定义：3-1-2旋转，（航向角-俯仰角-滚转角），其中航向角北偏西为正，范围【-pi pi】

地球自转引起的导航系旋转：

因地球表面弯曲，载体在地球表面运动，导致导航系旋转：

重力矢量在地理坐标系的投影为：

• 对准条件：

初始对准一般是在运载体对地静止的环境下进行的， 即运载体相对地面既没有明显的线运动也没有角运动，且对准地点处的地理位置准确已知， 也就是说重力矢量 g 和地球自转角速度矢量ωie 在地理坐标系（初始对准参考坐标系）的分量准确已知，分别如下：

一、解析粗对准 alignsb.m

1、捷联惯导姿态微分方程和比力方程为：

又因为：

一般实际情况中，选择主矢量的原则通常是选择两个矢量中的重要性较大者（或者测量误差较小者），在对准中，一般加速度计测量误差优于陀螺仪，选择重力矢量和加速度计测量值作为主矢量；同时在计算矩阵之前，为了防止出现不正交现象，一般先将矢量进行正交化：

最终计算公式如下：

对准的极限精度为：

即：水平失准角的对准误差取决于加速度计的等效水平测量误差

方位失准角的对准误差取决于陀螺的等效东向测量误差

[-imuerr.db(2);imuerr.db(1)]/glv.g0;-imuerr.eb(1)/(cos(avp(7))*glv.wie)]

• 仿真数据实现：

glvs
ts = 0.1;   % sampling interval
T = 1000; %总时长1000秒、即16分钟.40秒
avp0 = avpset([0;0;0], [0;0;0], [30;108;380]); %初始姿态为0
imuerr = imuerrset(0.01, 100, 0.001, 1);
% imuerr = imuerrset(0.01, 100,0,0);
imu = imustatic(avp0, ts, T, imuerr);   % IMU simulation
davp = avpseterr([-30;30;30], [0.01;0.01;0.01]*0, [1;1;1]*0); %只存在初始姿态误差；速度、位置误差为0
avp = avpadderr(avp0, davp);

• 解析粗对准实现代码;

%% coarse align
[attsb, qnb] = alignsb(imu, avp(7:9));
phi = [aa2phi(attsb,[0;0;0]), [[-imuerr.db(2);imuerr.db(1)]/glv.g0;-imuerr.eb(1)/(cos(avp(7))*glv.wie)]]

• 其中初始姿态角为[0 0 0 ]，结果如下：

二、间接粗对准： aligni0.m

• 初始时刻载体惯性系（b0 ）： 与初始对准开始瞬时的载体坐标系（ b 系）重合， 随后相对于惯性空间无转动；
• 初始时刻导航惯性系（n0 ）： 与初始对准开始瞬时的导航坐标系（ n 系，即地理坐标系）重合， 随后相对于惯性空间无转动；

间接初始对准方法的关键是求解b0 系与 n0 系的方位关系，即Cnobo。

1、重力矢量在n0系上的投影为：

因为：

 

2、加速度计的比力输出在b0系投影为：

因为 Cb0为惯性系，而且IMU的输出是基于惯性系，所以Wbb0b=Wbib;

根据前述微分方程，如下：

将方程7.1-21b两边同时左乘Cn0n，得：

最终计算如下：

 

 

 

 

 

 

 

 

 

 

 

三、Kalman精对准：alignvn.m& alignfn.m

1、alignvn.m 以速度为量测的Kalman精对准

静基座下，捷联惯导速度更新解算即为速度误差，将其作为观测量，同时也是测量误差量；利用kalman量测方程完成对失准角的估计。

（1）简化的姿态、速度更新算法：

（2）简化的姿态、速度误差方程：

（3）初始对准状态空间模型：

将陀螺仪和加速度计的常值零偏扩展为状态量，状态空间模型如下：

系统的状态量为：[失准角、速度误差、陀螺仪逐次启动零偏稳定性、加速度计零偏]

（4）代码实现：

将粗对准得到的姿态角当作精对准的初始角度；

        wvm = imu(k:k+nn-1,1:6);
        [phim, dvbm] = cnscl(wvm);
        Cnb = q2mat(qnb);
        dvn = Cnn*Cnb*dvbm;
        vn = vn + dvn + eth.gn*nts;
        %qnb = qupdt(qnb, phim-Cnb'*eth.wnin*nts);
        qnb = qupdt2(qnb, phim, eth.wnin*nts);
        Cnbts = Cnb*nts;
        kf.Phikk_1(4:6,1:3) = askew(dvn);
        kf.Phikk_1(1:3,7:9) = -Cnbts; kf.Phikk_1(4:6,10:12) = Cnbts;
        kf = kfupdate(kf, vn);
        tag=0.4;tag1=1-tag;
        qnb = qdelphi(qnb, tag*kf.xk(1:3)); kf.xk(1:3) = tag1*kf.xk(1:3);
        vn = vn-tag*kf.xk(4:6);  kf.xk(4:6) = tag1*kf.xk(4:6);
        attk(ki,:) = [q2att(qnb)',imu(k+nn-1,end)];
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk)];

function kf = avnkfinit(nts, pos, phi0, imuerr, wvn)
    eth = earth(pos); wnie = eth.wnie;
    kf = []; kf.s = 1; kf.nts = nts;
	kf.Qk = diag([3*imuerr.web; 3*imuerr.wdb; zeros(6,1)])^2*nts;
    kf.Gammak = 1;
	kf.Rk = diag(wvn)^2;
	kf.Pxk = diag([phi0; [1;1;1]; imuerr.eb; imuerr.db])^2;
	Ft = zeros(12); Ft(1:3,1:3) = askew(-wnie); kf.Phikk_1 = eye(12)+Ft*nts;
	kf.Hk = [zeros(3),eye(3),zeros(3,6)];
    [kf.m, kf.n] = size(kf.Hk);
    kf.I = eye(kf.n);
    kf.xk = zeros(kf.n, 1);
    kf.adaptive = 0;
    kf.xconstrain = 0; kf.pconstrain = 0;
    kf.fading = 1;

（5）结果实现：

粗对准的结果（°）	精对准的结果（速度为观测量）（°）
0.005750544250598	0.005782319297423
-0.005835008798230	-0.005683278866238
0.038523957584313	0.045178296080427

2、alignvfn.m 以比力为量测的Kalman精对准

静基座下，加速度计输出比力作为观测量；测量误差量为：比力 - 速度误差方程；

（1）加速度计输出比力和捷联惯导更新的速度作为量测，其主要区别在于转移矩阵和量测矩阵的建立；

加速度计输出比力中，状态量为 [phiE, phiN, phiU, eby, ebz];

通常，静基座下，东、北向加速度计、东向陀螺仪是不可观的；因此，此处没有选择东向陀螺仪作为状态量；

• 转移矩阵为：

    Ft = [  0   wU -wN   0   0
           -wU  0   0   -1   0
            wN  0   0    0  -1
            zeros(2,5)          ];

• 量测矩阵为：

    kf.Hk = [ 0  -g  0  0  0
              g   0  0  0  0 ];

量测方程的建立，根据速度误差方程和静基座下(近似)，fn=-gn；

将上式误差状态方程展开如下：

（2）代码实现：

• 状态转移矩阵和量测矩阵：

function kf = afnkfinit(nts, pos, phi0, imuerr)
    eth = earth(pos);
    kf = []; kf.s = 1; kf.nts = nts;
    kf.Qk = diag([imuerr.web; 0;0])^2*nts;
	kf.Rk = diag(imuerr.wdb(1:2)/sqrt(nts))^2;
	kf.Pxk = diag([phi0; imuerr.eb(1:2)])^2;
	wN = eth.wnie(2); wU = eth.wnie(3); g = -eth.gn(3);
	Ft = [  0   wU -wN   0   0 
           -wU  0   0   -1   0 
            wN  0   0    0  -1 
            zeros(2,5)          ];
    kf.Phikk_1 = eye(5)+Ft*nts;
    kf.Hk = [ 0  -g  0  0  0
              g   0  0  0  0 ];
    [kf.m, kf.n] = size(kf.Hk);
    kf.I = eye(kf.n);
    kf.xk = zeros(kf.n, 1);
    kf.adaptive = 0;
    kf.fading = 1;
    kf.Gammak = 1;
    kf.xconstrain = 0;
    kf.pconstrain = 0;

• Kalman滤波器时间和量测更新：

        wvm = imu(k:k+nn-1, 1:6);
        [phim, dvbm] = cnscl(wvm);
        fn = Cnn*qmulv(qnb, dvbm/nts);
        qnb = qupdt(qnb, phim-qmulv(qconj(qnb),eth.wnie)*nts);  % att updating
        kf = kfupdate(kf, fn(1:2));
        tag=0.1;tag1=1-tag;
        qnb = qdelphi(qnb, tag*kf.xk(1:3)); kf.xk(1:3) = tag1*kf.xk(1:3); % feedback
        attk(ki,:) = q2att(qnb)';
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk)];

（3）结果实现：

粗对准的结果（°）	精对准的结果（速度为观测量）（°）
0.005722167719818	0.005687376038254
-0.005724676591646	-0.005754972245868
0.029101368855361	0.030922843858521

 

 

 

参考：

《捷联惯导系统静基座初始对准精度分析及仿真》