运动轨迹与惯性器件信息生成仿真
纯数值仿真
trjsegment()函数
trjsegment函数为主要函数，其内容如下：
function seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Add trj-segment setting for trajectory simulator.
%
% Prototype: seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Inputs: seg - trjsegment structure array
% segtype - trjsegment type
% lasting - segment lasting time
% w - trajectory angular rate (NOTE: in deg/sec!)
% a - trajectory acceleration in m/ss
% var1 - augmented input, see the following code in detail
% Output: seg - new trjsegment structure array
%
% See also trjsimu, insupdate.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/01/2014
dps = pi/180/1; % deg/second
if exist('w', 'var')
cf = (w*dps)*seg.vel; % centripetal force
end
switch(segtype)
case 'init' % trjsegment(***, 'init', initvelocity)
initvelocity = lasting;
seg = [];
seg.vel = initvelocity; seg.wat = [];
% basic ------
case 'uniform', % trjsegment(seg, 'uniform', lasting)
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, 0, 0]];
case 'accelerate',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];
seg.vel = seg.vel + lasting*a;
case 'deaccelerate',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0,-a, 0]]; % a>0
seg.vel = seg.vel - lasting*a;
case 'headup',
seg.wat = [seg.wat; [lasting, seg.vel, w*dps, 0, 0, 0, 0, cf]];
case 'headdown',
seg.wat = [seg.wat; [lasting, seg.vel,-w*dps, 0, 0, 0, 0,-cf]];
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]];
% compound ------
case 'static',
a = var1; dacclasting = seg.vel/a;
seg = trjsegment(seg, 'deaccelerate', dacclasting, 0, a);
seg.vel = 0;
seg = trjsegment(seg, 'uniform', max(lasting,dacclasting)-dacclasting);
case 'coturnleft', % coordinate turn left
rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting;
seg = trjsegment(seg, 'rollleft', rolllasting, rollw);
seg = trjsegment(seg, 'turnleft', lasting, w);
seg = trjsegment(seg, 'rollright', rolllasting, rollw);
case 'coturnright', % coordinate turn right
rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting;
seg = trjsegment(seg, 'rollright', rolllasting, rollw);
seg = trjsegment(seg, 'turnright', lasting, w);
seg = trjsegment(seg, 'rollleft', rolllasting, rollw);
case '8turn',
lasting = 360/w;
rolllasting = var1;
seg = trjsegment(seg, 'coturnleft', lasting, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnright', lasting, w, 0, rolllasting);
case 'sturn',
lasting1 = 90/w; lasting2 = 180/w;
rolllasting = var1;
seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnleft', lasting2, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnleft', lasting1, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnright', lasting2, w, 0, rolllasting);
seg = trjsegment(seg, 'coturnleft', lasting1, w, 0, rolllasting);
case 'climb',
uniformlasting = var1;
seg = trjsegment(seg, 'headup', lasting, w);
seg = trjsegment(seg, 'uniform', uniformlasting);
seg = trjsegment(seg, 'headdown', lasting, w);
case 'descent',
uniformlasting = var1;
seg = trjsegment(seg, 'headdown', lasting, w);
seg = trjsegment(seg, 'uniform', uniformlasting);
seg = trjsegment(seg, 'headup', lasting, w);
case 'user',
seg.wat = [seg.wat; [lasting, seg.vel, w'*dps, a']];
otherwise,
error('trjsegment type mismatch.');
end
其中，segtype为轨迹类型，lasting为该类型持续的时间，w和a分别为角速率和加速度参数， 'init'为初始化，'uniform'为保持上一状态，seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];即为在上一段轨迹中添加新的轨迹的语句，参数分别为上一段轨迹信息seg.wat、持续时间lasting、速度seg.vel、0, 0, 0,三个方向角速率、 0, a, 0三个方向加速度，该语句表示的是向前加速(当b系定义为右前上时)
trjsimu()函数
function trj = trjsimu(avp0, wat, ts, repeats)
% Trajectory simulator (Ref. See my master's dissertation p63).
%
% Prototype: trj = trjsimu(avp0, wat, ts, repeats)
% Inputs: avp0 - initial attitude, velocity and position
% wat - segment parameter, each column described as follows:
% wat(:,1) - period lasting time
% wat(:,2) - period initial velocity magnitude
% wat(:,3:5) - angular rate in trajectory-frame
% wat(:,6:8) - acceleration in trajectory-frame
% ts - simulation time step
% repeats - trajectory repeats using the same 'wat' parameter.
% Output: trj - trajectory structrue array, including fields:
% imu - gyro angular increments, acc velocity increments
% & time tag, i.e. imu = [wm,vm,t]
% avp - trajectory attitude, velocity, position & time tag
% avp0, wat, ts repeats - the same as inputs
%
% See also trjsegment, avp2imu, imustatic, odsimu, gpssimu, conesimu, scullsimu,
% imuadderr, insupdate, cnscl, imuplot, imufile.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 01/10/2012, 19/03/2014(simular to avp2imu)
global glv
if nargin<4, repeats = 1; end
wat1 = repmat(wat, repeats, 1);
damping = 1-exp(-ts/5.0); % damping=0;
att = avp0(1:3); vn = avp0(4:6); pos = avp0(7:9);
eth = earth(pos, vn); Cbn_1 = a2mat(att)'; wm_1 = [0;0;0]; ts2 = ts/2;
% Mpv = [0, 1/eth.RMh, 0; 1/eth.clRNh, 0, 0; 0, 0, 1];
len = fix(sum(wat1(:,1))/ts);
[imu, avp] = prealloc(len, 7, 10);
ki = timebar(1, len, 'Trajectory Simulation.');
for k=1:size(wat1,1)
lenk = round(wat1(k,1)/ts); % the lenght at k phase
wt = wat1(k,3:5)'; at = wat1(k,6:8)';
ufflag = 0;
if norm(wt)==0 && norm(at)==0 % uniform phase flag
ufflag = 1;
vnr = a2mat(att)*[0;wat1(k,2);0]; % velocity damping reference
end
for j=1:lenk
sa = sin(att); ca = cos(att);
si = sa(1); sk = sa(3); ci = ca(1); ck = ca(3);
Cnt = [ ck, -ci*sk, si*sk;
sk, ci*ck, -si*ck;
0, si, ci ];
att = att + wt*ts;
Cnb = a2mat(att); % attitude
if ufflag==1 % damping
na = norm(vn-vnr)/ts; maxa = 0.1;
if na<maxa, na=maxa; end % max an is limited to maxa/(m/s^2)
vn1 = vn-damping/na*(vn-vnr); an = (vn1-vn)/ts;
vn01 = (vn+vn1)/2; vn = vn1;
else
an = Cnt*at;
vn1 = vn + an*ts; vn01 = (vn+vn1)/2; vn = vn1; % velocity
end
dpos01 = [vn01(2)/eth.RMh;vn01(1)/eth.clRNh;vn01(3)]*ts2;
eth = earth(pos+dpos01, vn01);
pos = pos+dpos01*2; % position
% eth = earth(pos+Mpv*vn01*ts2, vn01);
% Mpv = [0, 1/eth.RMh, 0; 1/eth.clRNh, 0, 0; 0, 0, 1];
% pos = pos+Mpv*vn01*ts; % position
phim = m2rv(Cbn_1*Cnb) + (Cbn_1+Cnb')*(eth.wnin*ts2);
% wm = phim;
wm = (glv.I33+askew(wm_1/12))^-1*phim; % gyro increment
% wm = (glv.I33-askew(wm_1/12))*phim; % gyro increment
% dvbm = Cbn_1*qmulv(rv2q(eth.wnin*ts2), (an-eth.gcc)*ts);
dvbm = Cbn_1*(rv2m(eth.wnin*ts2)*(an-eth.gcc)*ts);
vm = (glv.I33+askew(wm/2))^-1*dvbm; % acc increment
% vm = (glv.I33-askew(wm/2))*dvbm; % acc increment
kts = ki*ts;
avp(ki,:) = [att; vn; pos; kts]';
imu(ki,:) = [wm; vm; kts]';
wm_1 = wm; Cbn_1 = Cnb';
ki = timebar;
end
end
avp(ki:end,:) = []; imu(ki:end,:) = [];
avp = iatt2c(avp);
trj = varpack(imu, avp, avp0, wat, ts, repeats);
其中wat的格式解读如下：
该函数的输出为陀螺仪的角增量和加速度计的速度增量； Cnb = a2mat(att)表示姿态角到姿态阵的转变；an = Cnt*at;为计算加速度；后面的代码就是利用avp计算imu的各参数
基于实测轨迹的惯性器件生成仿真
如果已有某惯导系统或组合导航系统的AVP结果（建议频率10Hz以上），则根据其AP-姿位剖面，可反向生成惯性器件信息：
ap2avp：根据AP数据，采用三次样条插值方法生成AVP信息；
avp2imu：基于惯性器件反演算法，由AVP反向生成惯性器件信息。
扫一扫
364
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
