PSINS工具箱中轨迹生成工具详细解析
PSINS工具箱中轨迹生成工具解析
- 设置轨迹段
- 最终轨迹平面图
- 生成自己的轨迹
设置轨迹段
seg = trjsegment(xxx, 'init', 0);% 表示轨迹结构数组的初始化
初始化语句如下：
case 'init' % trjsegment(***, 'init', initvelocity)
initvelocity = lasting;
seg = [];
seg.vel = initvelocity; seg.wat = [];
seg = trjsegment(seg, 'uniform', 100);%表示保持上一状态100s
此时更新语句为：
case 'uniform', % trjsegment(seg, 'uniform', lasting)
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, 0, 0]];
此时seg.wat数组如下所示：
100 0 0 0 0 0 0 0
此时生成的轨迹avp信息为：
0 0 0 0 0 0 0.597706293396019 1.90083222404074 380 100
seg = trjsegment(seg, 'accelerate', 10, xxx, 1);%表示用1m/s2的速度加速10s
此时更新语句为：
case 'accelerate',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];
seg.vel = seg.vel + lasting*a;
此时seg.wat数组如下所示：
10 0 0 0 0 0 1 0
此时生成的轨迹avp信息为：
0 0 0 0 10 0 0.597714159954192 1.90083222404074 380 110
可以看到，已经利用10s时间加速到10m/s了
seg = trjsegment(seg, 'uniform', 100);%表示保持上一时刻状态100s
此时seg.wat数组如下所示：
100 10 0 0 0 0 0 0
此时生成的轨迹avp信息为：
0 0 0 0 10.0000000000000 0 0.597871490996066 1.90083222404074 380 210
可以看到，速度一直保持在10m/s
seg = trjsegment(seg, 'coturnleft', 45, 2, xxx, 4);%表示协调左转弯，先横滚方向左转4s，再整体转45s,最后横滚方向右转4s.考虑到了航向与横滚
此时更新语句为：
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);
此时seg.wat数组如下所示：
4 10 0 -0.00890097809914488 0 0 0 0
45 10 0 0 0.0349065850398866 -0.349065850398866 0 0
4 10 0 0.00890097809914488 0 0 0 0
此时生成的轨迹avp信息为：
0 -2.81892564846231e-18 1.57079632679490 -10.0174431386039 0.0174634464359821 0 0.597922945846830 1.90077029464407 380 263
seg = trjsegment(seg, 'uniform', 100);%表示保持上一个状态100s
此时seg.wat数组如下所示：
100 10 0 0 0 0 0 0
此时生成的轨迹avp信息为：
0 0.158933466905843 1.57079632679490 -10.0000000000000 -1.93716910436791e-14 0 0.597922947787353 1.90057322881847 380 367
seg = trjsegment(seg, 'coturnright', 10*5, 9, xxx, 4);%表示协调右转弯，先横滚方向右转4s，再整体转50s,最后横滚方向左转4s.考虑到了航向与横滚
此时更新语句为：
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);
此时seg.wat数组如下所示：
4 10 0 0.0397333667264608 0 0 0 0
50 10 0 0 -0.157079632679490 1.57079632679490 0 0
4 10 0 -0.0397333667264608 0 0 0 0
此时生成的轨迹avp信息为：
0 -6.93889390390723e-18 3.31075308887341e-14 -0.000108245811050810 10.0001076805146 0 0.597950893012223 1.90056096813122 380 428
seg = trjsegment(seg, 'uniform', 100);%表示保持上一个状态100s
此时seg.wat数组如下所示：
100 10 0 0 0 0 0 0
此时生成的轨迹avp信息为：
0 -6.93889390390723e-18 3.31075308887341e-14 -3.31075308887341e-13 10.0000000000000 0 0.598097210572095 1.90056096812188 380 521
seg = trjsegment(seg, 'climb', 10, 2, xxx, 50);%表示向上爬坡10s，再向下10s
此时更新语句为：
case 'climb',
uniformlasting = var1;
seg = trjsegment(seg, 'headup', lasting, w);
seg = trjsegment(seg, 'uniform', uniformlasting);
seg = trjsegment(seg, 'headdown', lasting, w);
此时seg.wat数组如下所示：
10 10 0.0349065850398866 0 0 0 0 0.349065850398866
50 10 0 0 0 0 0 0
10 10 -0.0349065850398866 0 0 0 0 -0.349065850398866
此时生成的轨迹avp信息为：
-9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 -3.31077703057269e-13 10.0000723149647 1.27944630294799e-05 0.598205117738701 1.90056096812188 585.575096913505 593
seg = trjsegment(seg, 'uniform', 100);%表示保持上一时刻状态100s
此时seg.wat数组如下所示：
100 10 0 0 0 0 0 0
此时生成的轨迹avp信息为：
-9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 -3.31075308887341e-13 10.0000000000000 -9.54097911787244e-17 0.598359296471714 1.90056096812188 585.575102735199 691
seg = trjsegment(seg, 'climb', 10, 2, xxx, 50);%表示下坡10s，再向上10s
此时更新语句为：
case 'climb',
uniformlasting = var1;
seg = trjsegment(seg, 'headup', lasting, w);
seg = trjsegment(seg, 'uniform', uniformlasting);
seg = trjsegment(seg, 'headdown', lasting, w);
此时seg.wat数组如下所示：
10 10 -0.0349065850398866 0 0 0 0 -0.349065850398866
50 10 0 0 0 0 0 0
10 10 0.0349065850398866 0 0 0 0 0.349065850398866
此时生成的轨迹avp信息为：
9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 -3.31272919967326e-13 10.0059687652531 -0.00105603517436349 0.598464056442422 1.90056096812188 380.000480513597 761
seg = trjsegment(seg, 'uniform', 100);%表示保持上一时刻状态100s
此时seg.wat数组如下所示：
100 10 0 0 0 0 0 0
此时生成的轨迹avp信息为：
9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 -3.31075308887341e-13 10.0000000000000 9.54097911787244e-17 0.598621386806681 1.90056096812188 379.999999999992 861
seg = trjsegment(seg, 'deaccelerate', 5, xxx, 2);%表示减速5s
此时更新语句为：
case 'deaccelerate',
seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0,-a, 0]]; % a>0
seg.vel = seg.vel - lasting*a;
此时seg.wat数组如下所示：
5 10 0 0 0 0 -2 0
此时生成的轨迹avp信息为：
9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 8.04638123325432e-29 5.60662627435704e-15 -1.04770588974666e-31 0.598625320052132 1.90056096812188 379.999999999992 866
seg = trjsegment(seg, 'uniform', 100);%表示保持上一时刻状态100s
此时seg.wat数组如下所示：
100 0 0 0 0 0 0 0
此时生成的轨迹avp信息为：
9.54097911787244e-18 -6.93889390390723e-18 3.31075308887341e-14 1.18245492979398e-124 8.23921050400522e-111 -1.53965485650258e-127 0.598625320052132 1.90056096812188 379.999999999992 966
最终轨迹平面图
生成自己的轨迹
了解了上述代码的含义，就可以完成任意轨迹的生成了，比如生成如下轨迹：
代码如下：
glv.pos0=[43.791*glv.deg;125.443*glv.deg;268.3];
ts = 0.1; % sampling interval
avp0 = [[0;0;0]; [0;0;0]; glv.pos0]; % init avp
% trajectory segment setting
xxx = [];
seg = trjsegment(xxx, 'init', 0);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'accelerate', 10, xxx, 1);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'coturnleft', 10, 2, xxx, 4);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'deaccelerate', 5, xxx, 1);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'coturnright', 10, 2, xxx, 4);
seg = trjsegment(seg, 'uniform', 100);
seg = trjsegment(seg, 'deaccelerate', 5, xxx, 1);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);
trjfile('mytrj0100ms.mat', trj);
insplot(trj.avp);
imuplot(trj.imu);