Detailed analysis of trajectory generation tool in psins toolbox

Set track segment

seg = trjsegment(xxx, 'init',         0);%  Represents the initialization of the track structure array 

The initialization statement is as follows ：

case 'init' % trjsegment(***, 'init', initvelocity)
            initvelocity = lasting;
            seg = [];
            seg.vel = initvelocity;  seg.wat = [];

seg = trjsegment(seg, 'uniform',      100);% It means to keep the previous state 100s

At this time, the update statement is ：

 case 'uniform', % trjsegment(seg, 'uniform', lasting)
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, 0, 0]];

here seg.wat The array looks like this ：

100	0	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

0	0	0	0	0	0	0.597706293396019	1.90083222404074	380	100

seg = trjsegment(seg, 'accelerate',   10, xxx, 1);% To express with 1m/s2 Speed up 10s

At this time, the update statement is ：

 case 'accelerate',
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];
            seg.vel = seg.vel + lasting*a;

here seg.wat The array looks like this ：

10	0	0	0	0	0	1	0

The trajectory generated at this time avp The message is ：

0	0	0	0	10	0	0.597714159954192	1.90083222404074	380	110

You can see , Used 10s Time accelerates to 10m/s 了

seg = trjsegment(seg, 'uniform',      100);% It means to keep the state of the last moment 100s

here seg.wat The array looks like this ：

100	10	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

0	0	0	0	10.0000000000000	0	0.597871490996066	1.90083222404074	380	210

You can see , The speed has been kept at 10m/s

seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);% Indicates coordinated left turn , First turn left in the rolling direction 4s, Then turn to the whole 45s, Finally, turn right in the rolling direction 4s. Considering heading and rolling 

At this time, the update statement is ：

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);

here seg.wat The array looks like this ：

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

The trajectory generated at this time avp The message is ：

0	-2.81892564846231e-18	1.57079632679490	-10.0174431386039	0.0174634464359821	0	0.597922945846830	1.90077029464407	380	263

seg = trjsegment(seg, 'uniform',      100);% It means to keep the previous state 100s

here seg.wat The array looks like this ：

100	10	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

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);% Indicates coordinated right turn , First turn right in the rolling direction 4s, Then turn to the whole 50s, Finally, turn left in the rolling direction 4s. Considering heading and rolling 

At this time, the update statement is ：

 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);

here seg.wat The array looks like this ：

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

The trajectory generated at this time avp The message is ：

0	-6.93889390390723e-18	3.31075308887341e-14	-0.000108245811050810	10.0001076805146	0	0.597950893012223	1.90056096813122	380	428

seg = trjsegment(seg, 'uniform',      100);% It means to keep the previous state 100s

here seg.wat The array looks like this ：

100	10	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

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);% It means climbing up 10s, Down again 10s

At this time, the update statement is ：

        case 'climb',
            uniformlasting = var1;
            seg = trjsegment(seg, 'headup',   lasting, w);
            seg = trjsegment(seg, 'uniform',  uniformlasting);
            seg = trjsegment(seg, 'headdown', lasting, w);

here seg.wat The array looks like this ：

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

The trajectory generated at this time avp The message is ：

-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);% It means to keep the state of the last moment 100s

here seg.wat The array looks like this ：

100	10	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

-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);% It means downhill 10s, Up again 10s

At this time, the update statement is ：

        case 'climb',
            uniformlasting = var1;
            seg = trjsegment(seg, 'headup',   lasting, w);
            seg = trjsegment(seg, 'uniform',  uniformlasting);
            seg = trjsegment(seg, 'headdown', lasting, w);

here seg.wat The array looks like this ：

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

The trajectory generated at this time avp The message is ：

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);% It means to keep the state of the last moment 100s

here seg.wat The array looks like this ：

100	10	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

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);% Indicates deceleration 5s

At this time, the update statement is ：

       case 'deaccelerate',
            seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0,-a, 0]];  % a>0
            seg.vel = seg.vel - lasting*a;

here seg.wat The array looks like this ：

5	10	0	0	0	0	-2	0

The trajectory generated at this time avp The message is ：

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);% It means to keep the state of the last moment 100s

here seg.wat The array looks like this ：

100	0	0	0	0	0	0	0

The trajectory generated at this time avp The message is ：

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

Final trajectory plan

Generate your own trajectory

Understand the meaning of the above code , You can complete the generation of any trajectory , For example, generate the following trajectory ：

The code is as follows ：

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);