Time Optimal Flight of a Hexacopter

Transcription

1 Time Optimal Flight of a Hexacopter Johan Vertens an Fabian Fischer Abstract This project presents a way to calculate a time optimal control path of a hexacopter using irect multiple shooting in Matlab an the coe generate integrators from the ACADO toolkit The path is along some waypoints an within constraints A physical moel of the hexacopter an some example trajectories are also presente The trajectory was simulate in Matlab an visualize with RViz from ROS T T T T6 T T I INTRODUCTION In this project we calculate an optimal control of a hexacopter using irect Multiple Shooting The goal was to let a multi-rotor system with 6 rotors fly along some waypoints an within some given constraints, eg on angle an spee We use a physical moel of the hexacopter an Matlab s fmincon in combination with a runge kutta integrator of the orer four for the control an trajectory calculation The trajectory was then visualize using a Matlab-ROS brige an the ROS tool RViz The hexacopter moel is presente in Section II, while the control calculation is presente in Section III In the final Section IV we present two example trajectories II H EXACOPTER M ODE For simulating the hexacopter within Matlab we implemente a moel of the system an p-regulators consisting of orinary ifferential equations (ODE) Movement of the hexacopter can be generate by changing the thrusts of the ifferent motors (T T6, Fig ) which leas to a change of the torques an the velocities In orer to compensate the rotational moment of the motors there are three motors spinning right an three motors spinning left The torques can be escribe with the following equation The motor thrusts are negative here because the z axis is pointing ownwars T Mx M = y Mz T T T () T T T6 T : total thrust T T6 : thrust of each motor Mx, My, Mz : torques of the x, y an z axis : istance between the motor axis an the center of the hexacopter Optimal Control an Estimation Course, Final Project Albert-uwigs University of Freiburg, Faculty of Engineering mg Fig Thrusts of the motors [] With the calculate torques it is easily possible to compute also the velocities an the accelerations of the system Here the translation-velocities are in the coorinate system of the worl, while the angular velocities an angular accelerations are in the coorinate system of the hexacopter We summarize all quantities of interest: v x, v y, v z : Accelerations in x-, y- an z ωx, ωy, ωz : Angular velocities of x-,y- an z x, y, z : Velocities of x-, y- un z Θ : Pitch angle Φ : Roll angle Ψ : Yaw angle Ky : Propeller-yaw ratio : Yaw moment of the motors Jmp : Inertia of a propeller Jx, Jy, Jz : Inertia of the hexacopter For the moel equation part we use the moel from Baranek et al presente in [6] The accelerations in the local coorinate system of the hexacopter can be moele in the following way: v x = vz ωy + vy ωz g sin(θ) v y = vx ωz + vz ωx + g cos(θ) sin(φ) T v z = vy ωx + vx ωy + g cos(θ) cos(φ) m ()

2 x The angular accelerations are moele with: ( ωy ωz (Jz Jy ) + Mx + Ky Jmp Mz ωy ) Jx ω y = ( ωx ωz (Jx Jz ) + My + Ky Jmp Mz ωx ) Jy Mz ω z = Jz z Pitch Θ ω x = () y Roll Φ Yaw Ψ The angular velocities are moele with: Θ = ωy cos(φ) ωz sin(φ) Φ = ωx + ωy sin(φ) tan(θ) + ωz cos(φ) tan(θ) cos(φ) sin(φ) + ωz Ψ = ωy cos(θ) cos(θ) () Fig ocal coorinate system of the hexacopter [] TABE I PARAMETERS The velocities of the hexacopter in the worl coorinate system are moele with: x = cos(φ) cos(θ)vx + ( sin(ψ) cos(φ) + cos(ψ) sin(θ) sin(φ))vy + (sin(ψ) sin(φ) + cos(ψ) sin(θ) cos(φ))vz Parameter Ky Jmp Jx Jy Jz Value 68 kgm s 6 kgm s 6 kgm s 98 kgm s Nm Parameter P P P D D D Value y = sin(φ) cos(θ)vx + ( cos(ψ) cos(φ) + sin(ψ) sin(θ) sin(φ))vy () + ( cos(ψ) sin(φ) + sin(ψ) sin(θ) cos(φ))vz z = sin(θ)vx + cos(θ) sin(φ)vy + cos(θ) cos(φ)vz A PD Regulators The PD-regulators are controlling the motor thrusts with respect to angle inputs in orer to be able to control roll, pitch an yaw (Fig ), while the total thrust is controlle irectly without any regulator The resulting controls from the PD s (Rpitch, Rroll, Ryaw ) are combine in a motor mixer function Eq9 to calculate the esire thrusts of each motor This function is part of the microcopter river [] In conclusion the control inputs of the moel are then upitch, uroll, uyaw an uthrust The parameters given in Tab I were obtaine from Soliworks simulations an experiments A overview of the inputs is represente in Fig epitch (t) Rroll = P eroll (t) + D eroll (t) Ryaw = P eyaw (t) + D eyaw (t) with the equations in (7,8): Rpitch = P epitch (t) + D eyaw (t) = Ψ(t) uyaw (t) T = uthrust + + δyaw T = uthrust + δroll + δyaw T = uthrust + δroll + δyaw T = uthrust δyaw T = uthrust δroll + δyaw T6 = uthrust δroll + δyaw with Eq : (8) (9) (6) ppitch proll δroll = 7 δyaw = pyaw = epitch (t) = Θ(t) upitch (t) eroll (t) = Φ(t) uroll (t) epitch (t) ωx eroll (t) ωy eyaw (t) ωz (7) () ()

3 Controls Desire angles PD Pitch PD Roll PD Yaw subject to: x x = f (s i,q i ) s i+ =,i {,,N } \ {i(),,i(m )} P f (s i,q i ) x i+ =,i {i(),i(),,i(m )} x min x i x max,i {,,N} u min u i u max,i {,,N } T min T i T max,i {,,N} Total thrust Fig Motor mixer ODE Moel structure III OPTIMIZATION MODE The problem is to fin a time optimal trajectory from a starting point x to a terminal point x m via some k = m waypoints x k A Problem escription Because this is a time optimal problem we use the metho in [] by aing a free variable T which is constant between two waypoints to the state x of the system The scaling factor T can be change, but only at a waypoint state s i(k) This way it is possible to fin a time optimal flight path between two waypoints, which then also leas to an overall time optimal flight path The augmente state s an system function f (s,q) can be seen in () The set I(k) := {i(),,i(m )} enotes the set of all inices where a new waypoint starts, with i() = an i(m) = N s i = f (s i,q i ) = [ xi T i ] [ ] Ti f moel (x i,q i ) The continous time optimal problem can be escribe as: min s,q m T i () m T i(k) () k= P = () where P is a projection matrix from s to x In the objective function we sum up all the ifferent T i(k) in the states s i(k) at the waypoints an a the T i() in the initial State s i() This sum ivie by m is the overall flight time of the hexacopter B Solving with fmincon For solving the above problem we use Direct Multiple Shooting [] in combination with the Matlab fmincon solver By choosing a fixe number of controls N we mae a esign variables vector D which contains all the states an controls D = s q s q s N s N s N () As an inital guess x an x N were set to x an x m an all T state variables to some initial flight time T In fmincon we use an upper- an lowerboun vector to constrain D in the following way: control constraints: By constraining the controls like in Table II we avoie negative or excessive thrust an oversteering with too high pitch an roll angle controls state constraints: By constraining all states to the minimal an maximal values in Table III we avoie behaviour which is not supporte by the hexacopter moel Also we exclue flying through the groun plane an a negative time-scaling factor T initial state, terminal state an waypoint constraints: By constraining the minimal an maximal values of the initial state s an terminal state s N to the initial an terminal points x an x N respectivly, we etermine the start an en point The k waypoints were etermine j N k+ in the same manner at every state for j = k This way every path between two points ha the same number of controls For the waypoints an the en point

4 TABE II OWER AND UPPER BOUND OF THE CONTROS Control min max Thrust N N Pitch Roll Yaw 8 8 TABE III OWER AND UPPER BOUND OF THE STATES State min max Velocity in X-irection -6 m/s 6 m/s Velocity in Y-irection -6 m/s 6 m/s Velocity in Z-irection -6 m/s 6 m/s Pitch Roll Yaw 8 8 we also ae, respectivly subtracte, some small elta values to the states, which helps fining a solution The objective function use by fmincon returne the overall flight time calculate in Eq () We moele the system ynamics using the nonlnconstr function provie to fmincon In this function we calculate the vector c eq which can be seen in (6) The integrator function f (s i,q i ) is explaine in etail in Section III-C Since the integrator function provie the sensitivity with respect to s i an q i it was possible to buil the Jacobian G ceq of the nonlnconstr function, which can be seen in (7) By switching the GraConstr option on, fmincon uses the Jacobian for fining a solution faster f (s,q ) s f (s,q ) s c eq = (6) f (s N,q N ) s N G ceq = A B I A B I A N B N I A i = f (s,q) (7) s B i = f q (s,q) We then elete the continuity conition for the T state variable at every waypoint-state s i(k) with k = m in c eq, so the optimizer is allowe to change the time scaling factor T in this state We also elete the respective line in the Jacobian G ceq of the nonlinconstr function The solver fmincon then calculate an optimal solution using the interior-point algorithm Fig C Acao Integrator Flight visualization in RViz To spee up fmincon we use the ACADO Toolkit an its built-in Matlab interface [] ACADO converte the hexacopter moel from Section II to C coe, which was then compile with GCC to a binary MEX-file, callable from Matlab We then use an RK integrator with built-in steps provie by ACADO The integrator step uration was set to N, because our augmente problem formulation was scale from to This integrator function f (s i,q i ), which also provie the sensitivity in s an q irection, was use to compute the nonlinear equality constraints an their Jacobian in Section III-B A Visualization IV RESUTS For visualizing the whole trajectory we implemente a Matlab-ROS brige, which sens the real time motion of the hexacopter to a ROS noe The ROS noe computes the rotation an translation an transfers the ata irectly to RViz On the RViz sie we use a CAD Moel of the hexacopter for showing the trajectory in a D space (Fig ) 6 Thrust Ctrl Pitch Ctrl Roll Ctrl Yaw Ctrl Fig Simple flight controls

5 B Simple flight In Fig an Fig6 we present a simple hexacopter flight from position (,,) to (,-,) via (-,,) In the en point the hexacopter shoul stop with nearly zero velocity The uration of the flight was s 6 z axis in m Thrust Ctrl Pitch Ctrl Roll Ctrl Yaw Ctrl Fig 7 Angular-constraine controls y axis in m x axis in m Fig 6 C Angular-constraine flight Simple flight trajectory In Fig 7 an Fig 8 we present a hexacopter flight along the waypoints given in Tab IV On every waypoints the velocities shoul be nearly zero The uration of the flight was 99 s TABE IV ANGUAR-CONSTRAINED WAYPOINTS Waypoint X Y Z Pitch Roll Yaw S init S - S - S term z axis in m x axis in m Fig 8 y axis in m Angular-constraine trajectory REFERENCES [] Bachelor Thesis: D-Objektkartierung mit Multi-Rotor-Systemen, Johan Vertens, [] wwwmikrokoptere, July 7,, flight-ctrl [] M Diehl, ecture Notes on Optimal Control an Estimation, July,, p 6 [] M Diehl, ecture Notes on Optimal Control an Estimation, July,, pp 6-6 [] wwwacaotoolkitorg, July 7, [6] Baranek, R an Solc, F, Moelling an control of a hexa-copter, May,