严恭敏老师PSINS工具箱学习笔记-4

已于 2024-03-26 20:39:29 修改
阅读量830 收藏 7
点赞数 22
分类专栏： 惯导 文章标签： 学习 笔记 matlab
于 2024-03-26 11:22:29 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_57967772/article/details/137022689
版权

航位推算（Dead Reckoning，DR）

结合严老师教材7.4.1航位推算算法学习工具箱内test_DR.m，代码如下。

glvs
trj = trjfile('trj10ms.mat');
[nn, ts, nts] = nnts(2, trj.ts);
inst = [3;60;10]*glv.min;  kod = 1;  qe = 0; dT = 0;  % od parameters
trjod = odsimu(trj, inst, kod, qe, dT, 0);
imuerr = imuerrset(0.01, 50, 0.001, 5);
imu = imuadderr(trjod.imu, imuerr);
davp = avperrset([60;0;60], 0, 0);
dinst = [15;0;10]*glv.min; dkod = 0.05;
dr = drinit(avpadderr(trjod.avp0,davp), inst+dinst, kod*(1+dkod), ts); % DR init
len = length(imu); avp = prealloc(fix(len/nn), 10);
ki = timebar(nn, len, 'DR simulation.');
for k=1:nn:len-nn+1
    k1 = k+nn-1;
    wm = imu(k:k1,1:3);  dS = sum(trjod.od(k:k1,1)); t = imu(k1,end);
    dr = drupdate(dr, wm, dS); 
    avp(ki,:) = [dr.avp; t]';
    ki = timebar;
end
dr.distance,
avperr = avpcmp(avp, trjod.avp);
insplot(avp, 'DR', trj.avp);
inserrplot(avperr);

里程仪数据仿真部分

glvs
trj = trjfile('trj10ms.mat');
[nn, ts, nts] = nnts(2, trj.ts);
inst = [3;60;10]*glv.min;  kod = 1;  qe = 0; dT = 0;  % od parameters
trjod = odsimu(trj, inst, kod, qe, dT, 0);
  1. 数据，test_SINS_trj.m中产生trj数据，imu为陀螺仪和加速度计的数据（真实无噪声），avp为姿态速度位置数据（真实无噪声），wat、ts、repeats为产生轨迹时的数据。
    trj
  2. 设置里程仪的参数inst（安装误差角，里程仪坐标系与载体坐标系之间的误差角）、kod（里程仪的刻度误差系数，默认为1）、qe（量化等效系数？？，默认为0）、时间延迟dT。
  3. 使用 odsimu函数 输入真实的imu和avp信息、里程仪的参数，产生里程仪的测量数据。

误差添加部分

imuerr = imuerrset(0.01, 50, 0.001, 5);
imu = imuadderr(trjod.imu, imuerr);
davp = avperrset([60;0;60], 0, 0);
dinst = [15;0;10]*glv.min; dkod = 0.05;
dr = drinit(avpadderr(trjod.avp0,davp), inst+dinst, kod*(1+dkod), ts); % DR init
len = length(imu); avp = prealloc(fix(len/nn), 10);
ki = timebar(nn, len, 'DR simulation.');

添加imu误差，avp误差以及里程仪安装误差、刻度系数误差。
使用drinit函数进行DR初始化。

航位推算过程

for k=1:nn:len-nn+1
    k1 = k+nn-1;
    wm = imu(k:k1,1:3);  dS = sum(trjod.od(k:k1,1)); t = imu(k1,end);
    dr = drupdate(dr, wm, dS); 
    avp(ki,:) = [dr.avp; t]';
    ki = timebar;
end
dr.distance,
avperr = avpcmp(avp, trjod.avp);
insplot(avp, 'DR', trj.avp);
inserrplot(avperr);

结果分析

真实avp信息和轨迹：
原
dr仿真出来的avp和轨迹数据（右下角DR trajectory为仿真轨迹）：

insplot(trjod.avp, 'DR', trj.avp);

在这里插入图片描述
真实轨迹与仿真轨迹的偏差：

simuerr=avpcmp(trj.avp,trjod.avp);
inserrplot(simuerr);

仿真和实际轨迹对比

根据dr仿真数据（姿态、航向和路程增量）推算出来的结果：
dr推算
DR解算和仿真轨迹的误差对比：

avperr = avpcmp(avp, trjod.avp);
inserrplot(avperr)

估计偏差

里程仪数据仿真函数odsimu

使用odsimu输入真实的imu和avp信息、里程仪的参数，产生里程仪的测量数据。

trjod = odsimu(trj, inst, kod, qe, dT, 0);

对里程仪的各个参数进行默认值处理：

function trj = odsimu(trj, inst, kod, qe, dt, ifplot)
if nargin<6,  ifplot=0;   end
if nargin<5,  dt=0;   end
if nargin<4,  qe=0;   end   % default 0 meter/pulse, for no quantization
if nargin<3,  kod=1;   end   % default 1 for no scale factor error
if nargin<2,  inst=0;   end
if length(inst)==1,  inst=[1;1;1]*inst;  end

数据处理分为4步：

  1. imu数据从{b0}旋转到{b1}系，b1为里程仪坐标系，b0为imu坐标系。
  2. 姿态avp从从{b0}旋转到{b1}系。
  3. 产生距离增长值，将dpos（纬度经度高度）转换到dxyz（在导航坐标系下的位置变化），在将xyz三个方向平方和开方求得dS；再进行时间延时处理；（初始dS置零后）叠加求和——得到里程仪的测距信息。
  4. 根据量化等效值对dS进行处理，diff操作又得到增量dS值。
    Cb1b0 = a2mat(inst);  Cb0b1 = Cb1b0';
    % SIMU rotation
    trj.imu(:,1:6) = [trj.imu(:,1:3)*Cb1b0', trj.imu(:,4:6)*Cb1b0'];
    % attitude rotation
    trj.avp0(1:3) = m2att(a2mat(trj.avp0(1:3))*Cb0b1);
    for k=1:length(trj.avp)
        trj.avp(k,1:3) = m2att(a2mat(trj.avp(k,1:3)')*Cb0b1)';
    end
    % distance increments
    pos = [trj.avp0(7:9)'; trj.avp(:,7:9)];
%     dS = pos2dS(pos, 10);
    [RMh, clRNh] = RMRN(pos);
    dpos = diff(pos);
    dxyz = [[RMh(1:end-1), clRNh(1:end-1)].*dpos(:,1:2), dpos(:,3)];
    dS = sqrt(sum(dxyz.^2,2));
    t = trj.avp(:,10);
    dS = interp1([t(1)-1;t;t(end)+1],[dS(1);dS;dS(end)], t+dt); % time delay
    dS = cumsum([0;dS]);
    if qe==0
        dS = diff(dS/kod);
        if ifplot==1,  myfigure; plot(t,dS); xygo('Odometer / m');  end
    else
        dS = fix(diff(dS/kod/qe));
        if ifplot==1,  myfigure; plot(t,dS); xygo('Odometer / pulse');  end
    end

最后得到od信息，包含路程增量信息和相应时间。
使用drpure函数得到含有的avdp值（十一列：avp+dS+t），此时的avp是根据dS（里程仪的输出）解算出来的的avp信息，最后去掉dS列，得到根据里程仪推算出来的avp信息（avpd）。此处涉及到 drpure函数 。


    trj.od = [dS,t];
    avpd = drpure([trj.imu(:,1:6), trj.od], trj.avp0, inst, kod);  % re-calculate INS - IMU & AVP
    avpd = [[trj.avp0',2*avpd(1,end)-avpd(2,end)]; avpd(:,[1:9,end])];
    [trj.imu, trj.avp0, trj.avp] = ap2imu(avpd(:,[1:3,7:end]), trj.ts);
    trj.avp(1,:) = [];

drpure函数

该函数以里程仪坐标系中的imu和od信息，初始avp，安装偏差角，刻度系数误差和时间延迟为输入，输出avpd信息，关于avpd信息又涉及两个函数 drinit 和 drupdate 。 
function avpd = drpure(imuod, avp0, inst, kod, Td)
    if nargin<5, Td = 0; end
    if nargin<4, kod = 1; end
    if nargin<3, inst = [0;0;0]; end
    [nn, ts] = nnts(2, diff(imuod(1:2,end)));
    if length(avp0)<9, avp0=[avp0(1:3);zeros(3,1);avp0(4:end)]; end
    dr = drinit(avp0, inst, kod, ts, Td);
    len = size(imuod,1);
    avpd = zeros(fix(len/nn), 11);
    ki = timebar(nn, len, 'Pure DR navigation processing.');
    for k=1:nn:len-nn+1
        k1 = k+nn-1;
        dr = drupdate(dr, imuod(k:k1,1:6), sum(imuod(k:k1,7:end-1))');
        avpd(ki,:)  = [dr.avp; dr.distance1; imuod(k1,end)]';
        ki = timebar;
    end
    insplot(avpd(:,[1:9,11]));

drinit 初始化

初始化定义了结构体dr的变量。

  • qnb、Cnb、att、avp、vn、kod
  • aos（安装偏差角的y向即车辆行进方向，见里程仪坐标系建立）
  • Cbo（从里程仪坐标系到IMU坐标系的变换矩阵）
  • prj（关于速度的投影 OD到IMU）
  • distance、distance1
  • eth（关于某个具体位置pos的eth结构体）
  • Mpv（根据速度计算dpos的关系矩阵）

for leveing此处不讨论

function dr = drinit(avp0, inst, kod, ts, Td)
	dr = [];
    avp0 = avp0(:);
    if length(avp0)<9, avp0 = [avp0(1:3); zeros(3,1); avp0(4:end)]; end
	dr.qnb = a2qua(avp0(1:3)); dr.vn = zeros(3,1); dr.pos = avp0(7:9);
    [dr.qnb, dr.att, dr.Cnb] = attsyn(dr.qnb);
    dr.avp = [dr.att; dr.vn; dr.pos];
	dr.kod = kod;
    dr.aos = inst(2); inst(2)=0;  % AOS - angle of silde coefficient
    dr.Cbo = a2mat(-inst)*kod;
	dr.prj = dr.Cbo*[0;1;0]; % from OD to SIMU
	dr.ts = ts;
	dr.distance = 0;  dr.distance1 = 0;
	dr.eth = earth(dr.pos); dr.web = [0;0;0];
    dr.Mpv = [0, 1/dr.eth.RMh, 0; 1/dr.eth.clRNh, 0, 0; 0, 0, 1];
    if nargin<5, Td=0; end
    dr.Td = Td;
    if Td>0  % for leveling
        xi=0.707; xi2=xi*xi; ws2=9.8/6738160; sigma=2*pi*xi/(Td*sqrt(1.0-xi2)); sigma2=sigma*sigma;
        dr.gck(1) = 3.0*sigma; 
        dr.gck(2) = sigma2*(2.0+1.0/xi2)/ws2-1.0; 
        dr.gck(3) = sigma2*sigma/(9.8*xi2);
        dr.wnc = zeros(3,1); dr.vni = zeros(3,1); dr.dpos = zeros(3,1);
    end

drupdate 更新

更新函数在初始化的基础上，输入imu和od信息，在初始化后对dr结构体进行更新。 更新变量如下：
  1. distance，distance1，存放dr测的里程信息（dS之和），一个存放值（模值/范数）一个存放向量。
 function dr = drupdate(dr, imu, dS)
    nts = dr.ts*size(imu,1);
    dr.distance = dr.distance + dr.kod*norm(dS);
    dr.distance1 = dr.distance1 + dr.kod*dS;
  1. dSn更新，vn更新，web更新

    [phim, dvbm] = cnscl(imu);  qnb12 = qupdt(dr.qnb, phim/2);
    if length(dS)>1,
        dSn = qmulv(qnb12, dr.Cbo*dS);
    else
        dSn = qmulv(qnb12, dr.prj*dS);
    end
%     dSn = rotv([0;0;-dr.aos*norm(dS)/nts*phim(3)/nts], dSn);
    dSn = rotv([0;0;-dr.aos*phim(3)/nts], dSn);
    dr.vn = dSn/nts;
    dr.eth = earth(dr.pos, dr.vn);
    dr.web = phim/nts-dr.Cnb'*dr.eth.wnie;
    dr.Mpv = [0, 1/dr.eth.RMh, 0; 1/dr.eth.clRNh, 0, 0; 0, 0, 1];
    dr.pos = dr.pos + dr.Mpv*dSn; % see my PhD thesis Eq.(4.1.1)
    dr.qnb = qupdt(dr.qnb, phim-dr.Cnb'*dr.eth.wnin*nts);
    [dr.qnb, dr.att, dr.Cnb] = attsyn(dr.qnb);
    dr.avp = [dr.att; dr.vn; dr.pos];
    if dr.Td>0.01  % leveling
		fn=qmulv(dr.qnb, dvbm/nts);
        dVE = dr.vni(1) - dr.vn(1);     % vni: inertial vel;  vn: ref vel
        dr.vni(1) = dr.vni(1) + (fn(1)-dr.gck(1)*dVE)*nts;
        dr.dpos(1) = dr.dpos(1) + dVE*dr.gck(3)*nts;
        dr.wnc(2) = dVE*(1+dr.gck(2))/6378137 + dr.dpos(1);
        dVN = dr.vni(2) - dr.vn(2);
        dr.vni(2) = dr.vni(2) + (fn(2)-dr.gck(1)*dVN)*nts;
        dr.dpos(2) = dr.dpos(2) + dVN*dr.gck(3)*nts;
        dr.wnc(1) = -dVN*(1+dr.gck(2))/6378137 - dr.dpos(2);
        dr.qnb = qmul(rv2q(-dr.wnc*nts),dr.qnb);
        dr.avp = [dr.att; dr.vni; dr.pos];
    end

航位推算

航位推算误差方程推导

误差方程

嘀嗒zxy
关注
  • 22
    点赞
  • 踩
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值