Commit 3a040a76 authored by Ferdinand Fischer's avatar Ferdinand Fischer
Browse files

New trajectory planner based on polynomial interpolation

parent 8794737a
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
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";
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;
y0(i) = poly2sym(d(i,:),tau);
PI(i) = {PI_i};
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);
% 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;
%% 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);
% 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);
......@@ -4,6 +4,37 @@ function [tests] = testSs()
tests = functiontests(localfunctions());
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] =, t, 'x0', x0, 'x1', x1);
[y, t, x] = lsim(S, trj.u.on(), t, x0);
plot(t, trj.u.on());
plot(t, y - trj.y.on());
plot(t, x - trj.x.on());
tc.verifyEqual(trj.x.on(), x, 'AbsTol', 2e-2);
tc.verifyEqual(trj.y.on(), y, 'AbsTol', 1e-2);
function testPlanTrajectory(tc)
A = [-30 0; 0 -50];
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment