Trajectory control for an autonomous quadcopter Mauriello Tommaso Guidage et pilotage des drones 2A - 2021/2022 Abstract In the last years drone technology has seen a sharp increase in commercial applications, and more and more drones will start providing daily services: goods delivery, medical assistance, safety and rescue are just some of the applications that can be imagined. This brief work shows how it’s possible to design a high level control of the trajectory of an autonomous quadcopter. Contents 1 Context 2 2 Vertical control 2 3 Horizontal control 4 3.1 Control along ˆ y . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 3.2 Control along ˆ x . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 4 Trajectory control 7 4.1 Objective and score computation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 4.2 Trajectory reference . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 4.3 Simulated trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 4.4 Real trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 4.4.1 T side = 2 s, no velocity tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 4.4.2 T side = 2 s, with velocity tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 4.4.3 T side = 2.2 s, with velocity tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10 5 Conclusions and perspectives 11 1 1 Context The drone under use is a Bebp 2.0 by Parrot. A low level control low is already embedded into the drone, providing a fast and robust attitude control, while a higher level control loop (control, guidance, navigation) will be the objective of this study. A Matlab/Simulink environment has been used to perform both the flight simulation and the actual real time control of the drone inside the quadrotor facility system of ISAE-SUPAERO. The reference frame used for all the simulations and experimental analyses has the ˆ x and ˆ y axes laying on the horizontal plane and the ˆ z axis oriented downwards. That’s why an ascending drone will see a decrease in the values of its z position (that will become more and more negative). 2 Vertical control The vertical control takes as input the reference vertical speed ( V z ref ), so the dynamics after the controller can be modelled as shown in Fig. 1. Figure 1: Simulink scheme for the vertical dynamics, with unknown V z dynamics. The dynamics of ̇ z could be estimated by studying the simulated step response of this system. Such an analysis showed that it could be approximated to a first order system with static gain equal 1 and time constant τ = 0.3 (the presence of an overshoot indicated that it was actually a second order system, but this was small enough for the system to be considered as a first order). τ has been computed as the time at which the system response reached 63.2% of the static gain. Figure 2: Simulink scheme for the vertical dynamics, with approximated V z dynamics. 2 A simple PD control has been put in place for this corrector. A trial and error procedure allowed to find satisfactory values of the gains as K p = 6 and K v = 0.8, which respected the following specifications: • zero static error ε • response time at 5%: 1 second • overshoot < 20% • phase margin > 45 ° Figure 3: Simulink scheme of the vertical control. (a) Simulated trajectory (b) Real trajectory Figure 4: Comparison of simulated and real vertical trajectory. (a) Simulated trajectory (b) Real trajectory Figure 5: Comparison of the z position for the simulated and real vertical trajectory. As it can be observed in Fig. 4b the real vertical trajectory was not really precise in this experimental analysis. Yet from the plot in Fig. 5b it can be observed that the reference tracking of the z position is performed correctly, while an offset of the 0 position is present. Also, an horizontal drift can be observed in Fig. 4b, but the drone still returns to the original position, even if no horizontal control is present. These two effects lead to believe that there may have been an error in the optitrack tracking during this trial. The following simulations (for the horizontal control and the for the complete trajectory) show in fact a much smoother vertical track. 3 3 Horizontal control 3.1 Control along ˆ y As for the vertical control, the first step to do is to perform a system identification. The control takes place using as input the roll angle φ (around ˆ x ). Considering a small angles approximation the product φ · g is equal to ̈ y , the acceleration along ˆ y , such that the y position can be obtained by double integration. An identification of the dynamics between the reference roll angle φ ref and the real one φ is still needed, as shown in Fig. 6. Figure 6: Simulink scheme for the horizontal dynamics along ˆ y , with unknown φ dynamics. As it was done previously the unknown dynamics can be identified by analysing the (simulated) step response of this system. It is obtained that it can be well approximated by a first order system with static gain = 1 and time constant τ = 0.12, as shown in Fig. 7. Figure 7: Simulink scheme for the horizontal dynamics along ˆ y , with approximated φ dynamics. Once the system has been identified the control has been put in place by a pole placement technique. Since three inputs will be employed ( y , ̇ y and φ ), three poles needs to be placed. The most reasonable choice is to put two complex conjugate stable poles and one parasite pole at a larger (but not too large) frequency. The two complex conjugate poles has been computed by imposing a damping of 0.72 and a frequency of 4 rad/s. These values were selected as they allowed to respect the assigned specification (as shown in Table 1), namely: • static error ε = 0 • response time at 5%: 2 seconds • rise time: 0.6 seconds • overshoot < 15% Then the vector of the gains has been computed using the Matlab function place applied to the open loop system (with the dynamics approximated as illustrated previously). Also, the DC gain of the system has been computed to define the pr ́ ecommande N (as the inverse of the DC gain) that will be placed after the position reference in order to set the static gain to one. Note that both y and ̇ y are subtracted from the reference values to be then used as inputs for the control law. 4 δ = 0 78 δ = 0 78 δ = 0 78 δ = 0 75 ω = 2 5 ω = 3 ω = 3 5 ω = 4 ω 3 = − 8 ω 3 = − 8 ω 3 = − 8 ω − 8 = − 8 ε = 0 ✓ ✓ ✓ ✓ T 5% = 2 s ✓ ✓ ✓ ✓ T 10 − → 90% = 0 6 s ✓ overshoot < 15% ✓ ✓ ✓ ✓ Table 1: Choice of the closed system poles. Pole Damping Frequency [rad/s] Time constant [s] − 2 88 + 2 78 i 7.2 4 0.347 − 2 88 − 2 78 i 7.2 4 0.347 − 8 1 8 0.125 Table 2: Imposed closed system poles. Figure 8: Poles-zeros map of the closed system. The computed gain vector is: k = 1 5664 0 7597 0 6512 (1) The obtained Simulink scheme for the control law is presented in Fig. 10. 3.2 Control along ˆ x The dynamics and hence the control along the ˆ x direction is analogous, except for the input angle, that is the pitch angle θ now. Also, it is assumed that the inertial properties of the drone are the same in the ˆ x and ˆ y direction, hence it is possible to use the same values of k and N computed for the ˆ y control. The only things that changes is the sign of some signals in the control law, as shown in Fig. 9. The simulated and real results for the horizontal control are shown in Fig. 11 for a step of 0.2 m in both directions. From the plots in Figs. 12 it can be observed that the simulated reference tracking is well performing, as required, while the experimental trajectory shows more higher frequencies 5 Figure 9: Simulink scheme for the horizontal control along ˆ y Figure 10: Simulink scheme for the horizontal control along ˆ x components, some overshoot and also a non-zero static error. This static error does not show in the experimental trajectory tracking, which will be shown further on, so it may be due to an optitrack error in this case. It also has to be noted that in this case the control was not fed any reference in velocity. 6 (a) Simulated trajectory (b) Real trajectory Figure 11: Comparison of simulated and real horizontal trajectory. (a) Simulated trajectory (b) Real trajectory Figure 12: Comparison of the x position for the simulated and real horizontal trajectory. 4 Trajectory control 4.1 Objective and score computation Once the drone can be controlled in the three spatial dimension it is possible to code a trajectory that it should follow (basically adding the fourth dimension - time - to the equation). The goal is to travel a square path of side 3.2 m centered in (0, 0, -1), so at 1 m from the ground. The tolerance is of ± 0 1 m in each of the three directions, as shown in Fig. 13. Figure 13: The desired trajectory (red) and the tolerance region (blue). A score on the trajectory is computed as the time required to travel it completely. Exiting the tolerance region invalidates the score. 7 4.2 Trajectory reference It has been decided to perform the trajectory starting from the middle point of one of the sides of the square. So initially the vertical control brings the drone to -1 m, then the x control brings it to the start point, and then the drone starts the trajectory. A reference for x and y must be fed to the horizontal control in order follow the desired trajectory. It has been decided to define the reference position by smoothing a step input over a certain time (which is the time to travel one side). Such a function has been defined as a sinus with half-period equal to the time to travel one side. Fig. 14 shows how a sinus function (blue) can interpolate a too abrupt step reference tracking (orange). Figure 14: Difference between a step and a sinusoidal reference. For example in the plot the reference function has been defined as follows: x ( t ) = ∆ x 2 (1 + sin ( π ( t ∆ t − 1 2 )) (2) Where ∆ x = 1 m is the spatial displacement and ∆ t is the available time to perform the dis- placement. An advantage of using such a function is that the derivative at the beginning and at the end is zero, so it is well suited also to generate also a reference in velocity by simple derivation. On the other hand the velocity profile won’t be differentiable, hence the acceleration would be discon- tinuous, so it’s still a non completely physical reference to track. An option to solve this problem would be to compute the reference in position as the step response of a 3rd order filter: this would allow to obtain a continuous reference also for acceleration. The actual trajectory has then been tuned by modifying the time required to travel one side of the square and the amount of velocity reference to feed into the control law. In fact it was observed that, even if a velocity reference improves tracking and speed of response, it also introduces overshoots that can be critical in such an application, as exiting the tolerance region invalidates the score. Hence only a percentage of the computed target velocity will be used for the control. 4.3 Simulated trajectories The simulated trajectories look pretty similar for all the experimental results shown further on. An example is shown in Fig. 15, which is associated to a T side of 1.8 seconds and it includes velocity tracking. Figs. 16 show a comparison between simulated trajectories with and without (a percentage, 20%, of) velocity tracking. As previously stated, it can be observed that a velocity tracking improves the tracking and the speed of response. In this case no appreciable overshoot can be identified, yet in a real flight they become evident when the the velocity tracking is added. It has to be noted that also a control without velocity tracking can lead to an overshoot if the imposed time to travel a side is too short. 8 Figure 15: Example of simulated trajectory. (a) Simulated trajectory without velocity tracking (b) Simulated trajectory with velocity tracking Figure 16: Comparison of the x for simulated trajectories with and without velocity tracking. 4.4 Real trajectories 4.4.1 T side = 2 s, no velocity tracking Figure 17: T side = 2 s, no velocity tracking. In this case the initial overshoot was due to an error in the time imposed to perform the first horizontal displacement to get to the start point, which was solved in the following experimental analyses. For the rest the trajectory is pretty good except for an overshoot at the third corner. 9 4.4.2 T side = 2 s, with velocity tracking Figure 18: T side = 2 s, with velocity tracking. Here the problem that caused the first overshoot was solved, yet adding the velocity tracking generated large overshoots on 3 corners. 4.4.3 T side = 2.2 s, with velocity tracking Figure 19: T side = 2.2 s, with velocity tracking. In this final case increasing the time to travel a side of the square and tuning the amount of velocity tracking it was possible to find the best compromise between velocity on the trajectory and overshoot reduction. This trajectory was traveled in 11.0 seconds , as it can be observed in Fig. 20b. Figs. 20 show the time inside ( running ) and outside the trajectory tolerance region for an unsuccessful trajectory and for the final one. It is observed in Fig. 20a that the total time outside increases at each overshoot, as expected. Also, setting T side = 1.8 seconds allowed to obtain a lower time to travel the trajectory (9.5 seconds), but due to the overshoots the score is invalidated. Finally, a comparison can be done on the simulated and real final trajectory. Figs. 21 show such a comparison with respect to the ˆ x position of the drone. In this case the real trajectory shows a very good similarity with the simulated one, with the exception of some little initial and final differences. 10 (a) T side = 1.8 s, velocity tracking. (b) T side = 2.2 s, velocity tracking. Figure 20: Comparison of the time inside ( running ) and outside the trajectory tolerance region. (a) Simulated final trajectory. (b) Real final trajectory. Figure 21: Comparison of the simulated and real final trajectories. 5 Conclusions and perspectives The final trajectory provides satisfactory results, proving the effectiveness of the developed control law and of the choice of the control gains. Moreover, the trajectory tracking was designed in an effective way. It is possible to make the following remarks: • The trajectory control along z shows no problem in all of the real trajectories. This is clearly due to the fact the the z reference remains always fixed at -1 m, hence there’s no risk of overshoot. • Sometimes a trajectory can appear to be very performing when simulated, while the real one may be much worse if the expected performances cannot be met by the physical hardware (e.g. in case of saturation of the actuators; in those cases an anti-windup solution may be put in place). • A compromise has to be done between tracking velocity and position overshoot by tuning the amount of velocity tracking to be fed to the control law. • The first order modelization of the drone dynamics, as well as the small angles approximation, proved sufficiently precise. This work could be carried on with some further studies, for example: • The dynamics on which the identification was performed could be modelled more precisely (at least with a 2nd order dynamics). • The reference trajectory could be obtained as the step response of a 3rd order filter, as previ- ously suggested, in order to provide a continuous acceleration reference. 11