### 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 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]; ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!