Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
C
conI
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Terms and privacy
Keyboard shortcuts
?
Snippets
Groups
Projects
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
LRT_infinite_dimensional_systems
conI
Commits
3a040a76
Commit
3a040a76
authored
4 years ago
by
Ferdinand Fischer
Browse files
Options
Downloads
Patches
Plain Diff
New trajectory planner based on polynomial interpolation
parent
8794737a
Branches
Haoyue
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
+misc/+ss/planPolynomialTrajectory.m
+150
-0
150 additions, 0 deletions
+misc/+ss/planPolynomialTrajectory.m
+unittests/+misc/testSs.m
+31
-0
31 additions, 0 deletions
+unittests/+misc/testSs.m
with
181 additions
and
0 deletions
+misc/+ss/planPolynomialTrajectory.m
0 → 100644
+
150
−
0
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 i-th 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 (n-1)
% derivatives must be 0 for tau = 0, this is included in EQN1 and that
% y_schlange and its (n-1) 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
,
1e-12
)
*
x0
;
c1
=
pinv
(
PI
,
1e-12
)
*
x1
;
% An auxiliary matrix Y containing each y(i) and its (n-1) 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 (n-1)
% 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 (n-1) derivatives. The first element of a column in
% Y_schlange corresponds y_schlange_i and the last element corresponds to
% the n-1 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 n-th 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
This diff is collapsed.
Click to expand it.
+unittests/+misc/testSs.m
+
31
−
0
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'
,
2e-2
);
tc
.
verifyEqual
(
trj
.
y
.
on
(),
y
,
'AbsTol'
,
1e-2
);
end
function
testPlanTrajectory
(
tc
)
%%
A
=
[
-
30
0
;
0
-
50
];
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment