Skip to content
Snippets Groups Projects
Commit 3a040a76 authored by Ferdinand Fischer's avatar Ferdinand Fischer
Browse files

New trajectory planner based on polynomial interpolation

parent 8794737a
Branches Haoyue
No related tags found
No related merge requests found
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
......@@ -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];
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment