QUESTION

Text
Image

# Slalom Manoeuvre For term project 2, find the guidance command that solves the following optimisation, $\begin{array}{c} u^{*}=\arg \min _{u} J(u)=\int_{4_{0}}^{t_{f}}|| x_{d}(t)-x(t) \| d t \\ \text { s.t. } \\ |u|<<0 \end{array}$ Figure TP2: Helicopter slalom manoeuvre. This exercise illustrates the two complementary functions of the guidance algorithm- finding a feasible path and calculating the correct commands for the flight controller. Step 1 - Define the trajectory The maximum lateral extent $(h)$ is $20 \mathrm{~m}$. You should define the desired lateral motion of each segment using a polynomial for $y$. $y(\tau)=a_{0}+a_{1} \tau+a_{2} \tau^{2}+a_{3} r^{3}$ To calculate the polynomial coefficients $\left(a_{0}, a_{1}, a_{2}, a_{3}\right)$ of each trajectory segment, express the equations in vector/matrix form i.e., $\Phi a=b$ where, $\Phi=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & t_{f} & t_{f}^{2} & t_{f}^{3} \\ 0 & 1 & 2 t_{f} & 3 t_{f}^{2} \end{array}\right], a=\left[\begin{array}{c} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \end{array}\right], b=\left[\begin{array}{c} y(0) \\ y(0) \\ y\left(t_{f}\right) \\ \dot{y}\left(t_{f}\right) \end{array}\right]$$\Phi=\left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & t_{f} & t_{f}^{2} & t_{f}^{3} \\ 0 & 1 & 2 t_{f} & 3 t_{f} \end{array}\right], a=\left[\begin{array}{c} a_{0} \\ a_{1} \\ a_{2} \\ a_{3} \end{array}\right], b=\left[\begin{array}{c} y(0) \\ y(0) \\ y\left(t_{f}\right) \\ y\left(t_{f}\right) \end{array}\right]$ Then $a=\Phi^{-1} b$ Remember you need to perform this calculation for each segment! Once you have all the coefficients, the last step is computing the desired trajectories for both $x$ and $y$ axes. The polynomials only define the position and velocity of desired motion in the $y$-axis. To obtain the desired position in the $x$-axis, use the constraint of constant airspeed, $\dot{x}_{d}=\sqrt{V^{2}-\dot{y}_{d}^{2}}$ Step 2 - Find the Controls This problem is best formulated using a double integrator model. Recall from the notes that the difference equations for this type of model are (for constant $V_{f}$ ), $\begin{array}{l} x_{k+1}=x_{k}+v_{x} \Delta t \\ y_{k+1}=y_{k}+v_{y} \Delta t \\ v_{k+1}=v_{k}+a_{x} \Delta t \\ v_{k+1}=v_{k}+a_{y} \Delta t \end{array}$ calculating the error at each time step ( $k k$ - see cost function loop below) using the inner product, i.e. $J_{\mu}=\left(X_{d_{\mu}}-X_{\mu}\right)^{\mathrm{T}} C^{\mathrm{T}} C\left(X_{d_{\mu}}-X_{\mu}\right.$ ), while $X$ is the state vector and $C$ is the output selection matrix. Using the MATLAB function fmincon, solve the optimisation problem ( $i$ ). Once your code have passed all of the MATLAB Grader tests, you MUST press SUBMIT for marking. Script $\odot$SKELETON CODE%% Define the problem% ConstantsVf = 5; % UAV flight speedt0 = 0; tseg = 5; % Initial and segment timedt = 0.1; % sampling interval%--------------------------------------% Double Integrator model (ENG5017)%--------------------------------------A = [1 dt 0 0;0 1 0 0;0 0 1 dt;0 0 0 1]; % state transition matrixB = [0;dt;0;dt]; % control transition matrixC = [0 0 1 0]; % output selection matrix% Discretisation limitsnpts(1) = floor((tseg-t0)/dt); % number of optimisation points, seg1npts(2) = floor(1.5*npts(1)); % number of optimisation points, seg2npts(3) = npts(1); % number of optimisation points, seg3%% Trajectory generationxd = [0;0;0;0]; % placeholder for desired trajectorylim = 20; % maximum lateral offset% Calculate the array of boundary conditions 'bca' defining trajectory polynomials for each segment% ------ INSERT YOUR CODE HERE ------nseg = 3; % number of polynomial segments% Loop over each trajectory segmentfor jj=1:nsegtf = floor(npts(jj))*dt; % segment duration (s)bc = bca(jj,:);% Calculate the polynomial coefficient matrix 'Phi'% Calculate the boundary condition vector 'b'% ------ INSERT YOUR CODE HERE ------a = Phi\b;for ii=1:floor(npts(jj))tau = (ii-1)* dt;% Now calculate the trajectory coordinates using polynomials% The variables to be calculated include position (xp and yp) and velocity (xpd and ypd)% ------ INSERT YOUR CODE HERE ------% append the trajectory arrayxd = [xd [xp;xpd;yp;ypd]];endend%% Solve the optimisation% Define the initial conditionsx0 = [0;Vf;0;0];CC = C'*C;tp = sum(npts);tend = tp*dt;t = t0:dt:tend;U0 = zeros(tp,1);Ulower = -50*ones(tp,1);Uupper = 50*ones(tp,1);% Overwrite some of the default optimisation propertiesoptions = optimset('TolFun',1e-3,...'Display','iter',...'MaxFunEvals',50000);%---------------------------------------% Run the optimisation!%---------------------------------------% Call fmincon using an anonymous function to pass the extra parameters defined in the cost function: costFun(U,A,B,x0,CC,dt,tp,Xd).% Return the optimal values in 'U_opt'.% Remember to include the upper and lower constraints.% ------ INSERT YOUR CODE HERE ------%% display the results% Calculate the complete trajectory using the optimal control vectorxv = x0;x = x0;for jj=1:tpx = A*x + B * U_opt(jj,1);xv = [xv x];end% Now plot the UAV position time-seriesfigure(1)plot(t,xv(3,:)); hold on;plot(t(1:length(xd(1,:))),xd(3,:),'--g');legend('Planned optimal trajectory','Polynomial generated trajectory');xlabel('t (s)');ylabel('y (m)');grid on; hold off;% and the control inputsfigure(2)stairs(t,[U_opt;U_opt(end)],'r'); grid on;xlabel('t (s)');ylabel('u (N)');%% Calculate the cost functionfunction J = costFun(U,A,B,x0,CC,dt,tp,xd)J = 0.0;x = x0;for kk=1:tp-1x = A*x + B*U(kk,1);diff = (xd(:,kk+1) - x)'*CC*(xd(:,kk+1) - x)*dt;J = J + diff;endend  