Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
LRT_infinite_dimensional_systems
conI
Commits
3a040a76
Commit
3a040a76
authored
May 13, 2020
by
Ferdinand Fischer
Browse files
New trajectory planner based on polynomial interpolation
parent
8794737a
Changes
2
Hide whitespace changes
Inline
Sidebyside
+misc/+ss/planPolynomialTrajectory.m
0 → 100644
View file @
3a040a76
function
[
u
,
y
,
x
]
=
planPolynomialTrajectory
(
sys
,
t
,
optArgs
)
%FLATKERNELCOMPUTATION solves the transition x(0) > x(T) for a given
%state space based on a flat output
%
% [u,x,Cx] = FLATKERNELCOMPUTATION(system, xT, T) calculates
% the input u, the states x and the output Cx for a given state space
% system symbolically. The calculation is based on the flat output based approach
% for trajectory planning. The system for which the
% transition is done is of the following form.
%
% \dot(x(t)) = A * x(t) + B * u(t)
% y(t) = C * x(t)
% The transition of the states for time T
% x(0) = 0 > x(T) = xT
%
% The Laplace transformation of the input n is calculated with the formula
% u(s) = det(sI  A) * y_schlange
% where y_schlange is a polynomial of (2*n)th degree and
% x(s) = adj(sI  A) * B * y_schlange
%
% The transition is done by solving linear systems of equations for the
% coefficients of the polynomial y_schlange, such that all the
% requierments are fullfilled. For more Information see the derivation in
% the Documentation
%
% INPUT PARAMETERS: system: The LTI state space system defined as an ss
% object in MatLab
% xT: the end State defined as a column vector
% from real numbers
% T: the time for which the transition should occur
arguments
sys
ss
;
t
double
;
optArgs
.
x0
double
=
zeros
(
size
(
sys
.
A
,
1
),
1
);
optArgs
.
x1
double
=
zeros
(
size
(
sys
.
A
,
1
),
1
);
optArgs
.
domainName
string
=
"t"
;
end
x0
=
optArgs
.
x0
;
x1
=
optArgs
.
x1
;
t0
=
t
(
1
);
t1
=
t
(
end
);
%% Definitions
A
=
sys
.
A
;
B
=
sys
.
B
;
% number of states
n
=
size
(
A
,
1
);
% number of inputs
n_u
=
size
(
B
,
2
);
% coefficients for the polynomials
d
=
sym
(
'd'
,[
n_u
,(
2
*
n
)]);
% variables
s
=
sym
(
's'
);
tau
=
sym
(
optArgs
.
domainName
);
%% Computation of the PI matrix
% The PI matrix is a matrix that contains each coefficient of the PI_s
% matrix, the matrix PI_s is given as PI_s = adj(sI  A)*B
% PI_s = [PI_1,PI_2,...,PI_{n_u}] where PI_i is the ith column of the
% PI_s matrix. The computation of the PI matrix is done by taking each
% element of PI_s and with the help of the coeffs function from MatLab
% extracting each coefficient from the complex polynomial. The result is
% stored in a cell structure and then transformed into a matrix PI.
% Additionaly utilizing poly2sym function from MatLab each vector of
% coefficients d is transformed into a polynomial with tau as a variable.
% Each polynomial is then stored in y0. The coefficients are still unknown
PI_s
=
adjoint
(
s
*
eye
(
n
)

A
)
*
B
;
for
i
=
1
:
n_u
PI_i
=
zeros
(
n
,
n
);
for
j
=
1
:
n
coef
=
coeffs
(
PI_s
(
j
,
i
),
s
,
'All'
);
coef
=
flip
(
coef
);
temp
=
size
(
coef
,
2
);
PI_i
(
j
,
1
:
size
(
coef
,
2
))
=
coef
;
end
y0
(
i
)
=
poly2sym
(
d
(
i
,:),
tau
);
PI
(
i
)
=
{
PI_i
};
end
PI
=
cell2mat
(
PI
);
%% Computing y_schlange
% The coefficients of y_schlange are computed as follows:
% It is known from the derivation that each element in y_schlange must fulfill
% certain conditions. These conditions are that y_schlange and its (n1)
% derivatives must be 0 for tau = 0, this is included in EQN1 and that
% y_schlange and its (n1) derivatives for tau = T must be equal to
% PI^{+}xT where PI^{+} is the pseudo inverse of PI. This is included in EQN2
% A linear system of equations for the coefficients d is solved for every
% element in each y_schlange and the result is stored in y_schlange
% PI^{+}xT
c0
=
pinv
(
PI
,
1e12
)
*
x0
;
c1
=
pinv
(
PI
,
1e12
)
*
x1
;
% An auxiliary matrix Y containing each y(i) and its (n1) derivatives
% so that the equations for the coefficients d can be formed
Y
=
sym
(
'y'
,[
n
,
n_u
]);
Y
(
1
,:)
=
y0
;
for
j
=
1
:(
n

1
)
Y
(
j
+
1
,:)
=
diff
(
y0
,
tau
,
j
);
end
% The actual computation of the coefficients d done for each element of
% y_schlange.
i_temp
=
0
;
for
i
=
1
:
n_u
EQN1
=
subs
(
Y
(:,
i
),
tau
,
t0
)
==
c0
((
i_temp
+
1
):(
i_temp
+
n
));
EQN1
=
EQN1
.'
;
EQN2
=
subs
(
Y
(:,
i
),
tau
,
t1
)
==
c1
((
i_temp
+
1
):(
i_temp
+
n
));
EQN2
=
EQN2
.'
;
[
Ai
,
Bi
]
=
equationsToMatrix
([
EQN1
,
EQN2
],
d
(
i
,:));
coefpoly
=
linsolve
(
Ai
,
Bi
);
y_schlange
(
i
)
=
poly2sym
(
coefpoly
,
tau
);
i_temp
=
i_temp
+
n
;
end
%% Computation of the input u, the states x and the output y = Cx
% Each input u_i is computed as the multiplication of the charateristic
% polynomial of the A matrix with the corresponding flat output
% y_schlange_i and its n derivatives. The states x can be calculated
% directly by multiplying the PI matrix and y_schlange and all its (n1)
% derivatives. The calculation of the Output is trivial with y = Cx.
% Y_schlange is an auxiliary matrix that stores all the elements of
% y_schlange and its (n1) derivatives. The first element of a column in
% Y_schlange corresponds y_schlange_i and the last element corresponds to
% the n1 derivative of y_schlange_i.
Y_schlange
=
sym
(
'y'
,[
n
,
n_u
]);
Y_schlange
(
1
,:)
=
y_schlange
;
for
j
=
1
:(
n

1
)
Y_schlange
(
j
+
1
,:)
=
diff
(
y_schlange
,
tau
,
j
);
end
% The computation of the input. Y_schlange must be extended to have one
% more row that containts the nth derivative of y_schlange
u
=
flip
(
charpoly
(
A
))
*
[
Y_schlange
;
diff
(
y_schlange
,
tau
,
n
)];
u
=
quantity
.
Symbolic
(
u
.
', '
grid
', {t}, '
gridName
'
,
optArgs
.
domainName
);
% Y_schlange is now reshaped so that it corresponds to the size of the PI
% matrix and the calculation of the states follows directly.
Y_schlange
=
reshape
(
Y_schlange
,
n_u
*
n
,
1
);
x
=
quantity
.
Symbolic
(
PI
*
Y_schlange
,
'grid'
,
{
t
},
'gridName'
,
optArgs
.
domainName
);
y
=
quantity
.
Symbolic
(
sys
.
C
*
x
.
sym
,
'grid'
,
{
t
},
'gridName'
,
optArgs
.
domainName
);
end
+unittests/+misc/testSs.m
View file @
3a040a76
...
...
@@ 4,6 +4,37 @@ function [tests] = testSs()
tests
=
functiontests
(
localfunctions
());
end
function
planPolynomialTrajectory
(
tc
)
%%
A
=
[

30
0
;
0

50
];
B
=
[
6
;

5
];
C
=
[
1
,
1
];
D
=
[];
S
=
ss
(
A
,
B
,
C
,
D
);
t
=
linspace
(
1
,
2
,
1e3
)
'
;
x0
=
[
10
;
12
];
x1
=
[

5
;

5
];
[
trj
.
u
,
trj
.
y
,
trj
.
x
]
=
misc
.
ss
.
planPolynomialTrajectory
(
S
,
t
,
'x0'
,
x0
,
'x1'
,
x1
);
[
y
,
t
,
x
]
=
lsim
(
S
,
trj
.
u
.
on
(),
t
,
x0
);
subplot
(
3
,
1
,
1
);
plot
(
t
,
trj
.
u
.
on
());
subplot
(
3
,
1
,
2
);
plot
(
t
,
y

trj
.
y
.
on
());
subplot
(
313
);
plot
(
t
,
x

trj
.
x
.
on
());
disp
(
x
(
end
,:))
tc
.
verifyEqual
(
trj
.
x
.
on
(),
x
,
'AbsTol'
,
2e2
);
tc
.
verifyEqual
(
trj
.
y
.
on
(),
y
,
'AbsTol'
,
1e2
);
end
function
testPlanTrajectory
(
tc
)
%%
A
=
[

30
0
;
0

50
];
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment