- Email: [email protected]
A Constrained Thrust Allocation Algorithm for Remotely Operated Vehicles
11th IFAC Conference on Control Applications in 11th IFAC Conference on Control Applications in 11th IFAC Conference on Control Applications in Marine Systems, Robotics, and Vehicles Marine Systems, Robotics, and Vehicles 11th IFAC Conference on Control Applications Availableinonline at www.sciencedirect.com Marine Systems, Robotics, and Vehicles Opatija, Croatia, September 10-12, 2018 Opatija, Croatia, September 10-12, 2018 11th IFAC Conference on Control Applications in Marine Systems, Robotics, and Vehicles Opatija, Croatia, September 10-12, 2018 Marine Systems, Robotics, and Vehicles Opatija, Croatia, September 10-12, 2018 Opatija, Croatia, September 10-12, 2018
ScienceDirect
IFAC PapersOnLine 51-29 (2018) 250–255
A Constrained Thrust Allocation A A Constrained Constrained Thrust Thrust Allocation Allocation A Constrained Thrust Allocation Algorithm for Remotely Operated Vehicles Algorithm for Remotely Operated Vehicles A Constrained Thrust Allocation Algorithm for Remotely Operated Vehicles Algorithm for Remotely Operated ∗ ∗∗ ∗ Algorithm for Remotely Operated Vehicles Vehicles
Alessandro ,, Riccardo ∗, Alessandro Baldini Baldini ∗∗ ,,∗Antonio Antonio Fasano Fasano∗∗∗ Riccardo Felicetti Felicetti ∗ ∗ ,, Alessandro Baldini ,∗Antonio Fasano∗∗∗ , Riccardo Felicetti Alessandro Freddi , Sauro Longhi , Andrea Monteri` u ∗ ∗ ∗ ∗∗ Alessandro Freddi,∗Antonio , Sauro Longhi , Andrea Monteri` u∗ , Alessandro Baldini Fasano∗∗∗ , Riccardo Felicetti Alessandro Freddi Monteri` u ∗ ∗, ∗ , Sauro Longhi Alessandro Baldini ,∗Antonio Fasano∗ ,, Andrea , Riccardo Felicetti Alessandro Freddi , Sauro Longhi Andrea Monteri` u∗ ∗ ∗ ∗ Department of Information Engineering, ∗ Freddi , Sauro Longhi , Andrea Monteri` Alessandro u of Information Engineering, ∗ Department Department of Information Engineering, Universit` a delle Marche, Via Brecce Bianche, 60131 Universit` a ∗Politecnica Politecnica delle Marche, Via Brecce Bianche, 60131 Department of Information Universit` a ∗Politecnica delle Marche,r.felicetti}@pm.univpm.it, ViaEngineering, Brecce Bianche, 60131 Ancona, Italy, email: {a.baldini, Department of Information Ancona, Italy, email:delle {a.baldini, r.felicetti}@pm.univpm.it, Universit` a {a.freddi, Politecnica Marche, ViaEngineering, Brecce Bianche, 60131 Ancona, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, s.longhi, a.monteriu}@univpm.it Universit` a Politecnica delle Marche, Via Brecce Bianche, 60131 s.longhi, a.monteriu}@univpm.it Ancona,{a.freddi, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, ∗∗ s.longhi, a.monteriu}@univpm.it a Ca’ Foscari, Venezia, Italy ∗∗ Universit` Ancona,{a.freddi, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, Universit` a Ca’ Foscari, Venezia, Italy ∗∗ {a.freddi, s.longhi, a.monteriu}@univpm.it Universit` a Ca’ Foscari, Venezia, Italy email: [email protected] {a.freddi, s.longhi, a.monteriu}@univpm.it ∗∗ email: [email protected] a Ca’ Foscari, Venezia, Italy email: [email protected] ∗∗ Universit` Universit` a Ca’ Foscari, Venezia, email: [email protected] Italy email: [email protected] Abstract: Abstract: In In this this paper, paper, the the constrained constrained thrust thrust allocation allocation problem problem is is studied studied and and solved solved for for Abstract: In this paper, the constrained thrust allocation problem isnd studied and solved for the class of over-actuated Remotely Operated Vehicles. In detail, we the solution of the class of In over-actuated Remotely Operated Vehicles. In detail, weisnd the solution of the the Abstract: this paper, the constrained thrust allocation problem studied and solved for the class of In over-actuated Remotely Operated Vehicles. In detail, weisnd the solution of the thrust allocation when the thrusters are affected by possible faults simultaneously, taking Abstract: this paper, constrained thrust allocation problem studied and solved for thrust allocation when the the thrusters are affected by possible faults and, and, simultaneously, taking the class of over-actuated Remotely Operated Vehicles. Inthrust detail, we nd the solution of the thrust allocation when the thrusters are affected by possible faults and, simultaneously, taking into account the thrusters’ saturation limits. The proposed allocation algorithm is tested the class of over-actuated Remotely Operated Vehicles. In detail, we nd the solution of the into account the thrusters’ saturation limits. The proposed thrust allocation algorithm is tested thrust allocation when the thrusters are affected by possible faults and, simultaneously, the thrusters’ saturation limits. The proposed thrust allocation into account algorithm is taking tested both numerically, with randomly generated input samples, and applied in simulation the thrust allocation when thrusters are affected possible faults and, simultaneously, taking bothaccount numerically, with the randomly generated inputby samples, and applied in algorithm simulationisfor for the into the thrusters’ saturation limits. The proposed thrust allocation tested both numerically, with randomly generated input samples, and applied in simulation for the trajectory tracking of a Remotely Operated Vehicle, with real parameters and in combination into account the thrusters’ saturation limits. The proposed thrust allocation algorithm is tested trajectory trackingwith of a randomly Remotely generated Operated Vehicle, with real parameters and in combination both numerically, input samples, and applied in simulation for the trajectory tracking of laws. a randomly Remotely OperatedisVehicle, with real parameters in combination with different control The algorithm compared with two commonly used approaches, both numerically, with generated input samples, and applied inand simulation for the with different control laws. The algorithm isVehicle, compared with two commonly used approaches, trajectory tracking of laws. apseudo-inverse Remotely Operated with real parameters and in combination with control algorithm is compared with two commonly used approaches, different The that are the saturated method and the Matlab Quadratic Programming solver; trajectory tracking of a Remotely Operated Vehicle, with real parameters and in combination that are the saturated pseudo-inverse method and the Matlab Quadratic Programming solver; with different control laws. The algorithm is compared withanalysis two commonly used approaches, that are the saturated pseudo-inverse method and theinMatlab Quadratic Programming solver; both accuracy and computation time are considered the of the performances. The with different control laws. The algorithm is compared with two commonly used approaches, both are accuracy and computation time method are considered inMatlab the analysis of the performances. The that the saturated pseudo-inverse and the Quadratic Programming solver; both accuracy andthat computation timealgorithm are considered inMatlab the analysis of the performances. The simulations show the proposed performs better, in terms of computation time, that are the saturated pseudo-inverse method and the Quadratic Programming solver; simulations show that the proposed algorithm performs better, in terms of performances. computation time, both accuracy and computation time are considered in the analysis of the The simulations show the proposed performs better, in terms of performances. computation time, w.r.t. the Quadratic Programming solver, while still retaining the optimality the both andthat computation timealgorithm are considered in the analysis of the The w.r.t. accuracy the Matlab Matlab Quadratic Programming solver, while still retaining the optimality of oftime, the simulations show that the proposed algorithm performs better, inalso terms ofimprovement computation w.r.t. Matlab Quadratic Programming the solver, while still retaining the optimality of solution despite the saturation constraints; moreover, there is an in the simulations show that the proposed algorithm performs better, in terms of computation time, solution despite the saturation constraints; moreover, there is also an improvement in the w.r.t. theerror Matlab Quadratic Programming solver, whilethere still is retaining optimality in of the solution despite the saturation constraints; moreover, also anthe improvement tracking regardless of adopted control law. w.r.t. theerror Matlab Quadratic solver, still is retaining optimality in of the the tracking regardless of the theProgramming adopted control law.whilethere solution despite the saturation constraints; moreover, also anthe improvement control law. tracking error regardless of the adopted solution despite the saturation constraints; moreover, there is also an improvement in the tracking error(International regardless ofFederation the adopted control Control) law. © 2018, IFAC of Automatic Hosting by Elsevier Ltd. All rights reserved. tracking error regardless of theFault-tolerant adopted control law. Fault Keywords: Thrust Allocation, systems, Keywords: Thrust Allocation, Fault-tolerant systems, Fault tolerance, tolerance, Fault Fault diagnosis, diagnosis, Fault Fault Keywords: Unmanned Thrust Allocation, Fault-tolerant systems, Fault tolerance, Fault diagnosis, Fault detection, Underwater Vehicles. detection, Unmanned Underwater Vehicles. systems, Fault tolerance, Fault diagnosis, Fault Keywords: Thrust Allocation, Fault-tolerant detection, Unmanned Underwater Vehicles. systems, Fault tolerance, Fault diagnosis, Fault Keywords: Thrust Allocation, Fault-tolerant detection, Unmanned Underwater Vehicles. detection, Unmanned Underwater Vehicles. 1. However, 1. INTRODUCTION INTRODUCTION However, over-actuation over-actuation introduces introduces additional additional complexity complexity 1. INTRODUCTION However, over-actuation introduces additional complexity as well, which must be taken into account when the as well, which must be taken into account when complexity the control control 1. INTRODUCTION However, over-actuation introduces additional as well, is which must In befact, taken intoofaccount when the control system designed. most the linear and nonlinear 1. INTRODUCTION However, over-actuation introduces additional complexity system is designed. In fact, most ofaccount the linear andthe nonlinear as well, which must be taken into when control system designed. fact, most theexploit linear and nonlinear control techniques do not directly input redunas well, is which must In be taken intoof when the control control techniques dofact, notmost directly exploit input redunsystem is designed. In ofaccount the linear and nonlinear control techniques do not directly exploit input redunOver-actuation represents an interesting feature in medancy; on the contrary, they are typically conceived to is designed. In fact, most of the linear and nonlinear Over-actuation represents an interesting feature in me- system dancy; on the contrary, they are typically conceived to control aon techniques do not directly exploit input redunOver-actuation represents an interesting feature in me- dancy; the contrary, they areis typically conceived to chanical systems: thanks to the availability of more actuacontrol number of outputs that equal to the number of control techniques do not directly exploit input redunchanical systems:represents thanks to an theinteresting availability feature of more in actuacontrol a number of outputs that is equal to the number of Over-actuation medancy; on the contrary, they are typically conceived to chanical systems: thanks to an thefault availability of control more in actuacontrol aon number of outputs that is typically equal to the number of tors than degrees of freedom, tolerant with variables. The most straightforward way to tackle Over-actuation represents interesting feature medancy; the contrary, they are conceived to tors than degrees of freedom, fault tolerant control with variables. The most straightforward way to tackle chanical systems: thanks to the availability of more actuacontrol a number of outputs that is equal to the number of tors thanto degrees of failures freedom, fault tolerantof control with control The most straightforward waynumber to inputs tackle respect actuator can be by the is arbitrarily dene some chanical systems: thanks to the availability more actuacontrol avariables. number outputs that is equal to virtual the of respect to actuator failures can be obtained obtained by means means the problem problem is to toof arbitrarily dene some virtual inputs tors thanto degrees of failures freedom, fault control with control variables. The most straightforward way to tackle respect actuator can betolerant obtained by means the problem is to arbitrarily dene some virtual inputs of control effort redistribution. Hence, over-actuation is a to simplify the control problem, and then to add a thrust tors than degrees of freedom, fault tolerant control with control variables. The most straightforward way to tackle of control effort redistribution. Hence, over-actuation is a the to simplify the control problem, and then to add a thrust respect to actuator failures can be obtained by means problem is to arbitrarily dene some virtual inputs of control effort redistribution. Hence, over-actuation isrea to simplify theis control problem, and then add a inputs thrust desirable in every requires allocation layer with the aim distributing the effort from respect toproperty actuator can bethat obtained byhigh means the problem to arbitrarily someto virtual desirable property in failures every scenario scenario that requires high reallocation layer with theproblem, aim of of dene distributing the effort from of control effort redistribution. Hence, over-actuation isrea allocation to simplify the control and then tothe add a way, thrust desirable property in every scenario that requires high layer with the aim of distributing effort froma liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In this of control effort redistribution. Hence, over-actuation is a to simplify the control problem, and then to add a thrust liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In this way, a desirable property in every scenario that requires high reallocation layer with the aim of distributing the effort from liability and safety (Chen and Wang that (2012); Oppenheimer the virtual inputs tothe theaim actual thrusters. In this way, a et al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with desirable property in every scenario requires high reallocation layer with of distributing the effort from et al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with liability and safety (Chen Wang (2012); Oppenheimer virtual inputs to the actual thrusters. In thisactuator way, et al. (2006); Khelassi et and al. (2010)). Thisofis the case of high level nonlinear control law can be synthesized witha vehicles employed for the transportation people, such athe number of virtual inputs, neglecting liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In thisactuator way, a vehicles employed for the transportation ofis people, such a suitable suitable number ofcontrol virtual inputs, neglecting et al. (2006); Khelassi et al. (2010)). This the case of high level nonlinear law can be synthesized with vehicles employed for the transportation of people, such a suitable number of virtual inputs, neglecting actuator as aircrafts or ships, where high costs are involved and, saturations and actuator faults. The thrust allocation alet al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with as aircrafts or ships, where high costs areofinvolved and, asaturations and actuator faults. The thrust allocation alvehicles employed for where the transportation people, such suitableis number of to virtual inputs, neglecting actuator as aircrafts or ships, highin costs areofBut involved and, and actuator faults. The thrust allocation alpossibly, users’ lives could be danger. this also gorithm employed map the virtual inputs in terms vehicles employed for where the transportation people, asaturations suitableis number of to virtual inputs, neglecting actuator possibly, users’ lives could be in costs danger. But this such also saturations gorithm employed map the virtual inputs in terms as aircrafts or ships, high are involved and, and actuator faults. The thrust allocation alpossibly, users’ lives where could be danger. But thiswhich also gorithm is employed to decoupling map theThe virtual inputs incontrol terms applies to Vehicles (UVs) in of efforts, the nonlinear as aircrafts or ships, highin costs aregeneral, involved and, saturations and actuator faults. thrust allocation alapplies to Unmanned Unmanned Vehicles (UVs) in general, which of real real actuator actuator efforts, decoupling the nonlinear control possibly, lives could be in danger. But thiswhich also of gorithm is employed tocontroller map theofvirtual inputs incontrol terms applies tousers’ Unmanned Vehicles (UVs) in in general, real actuator efforts, decoupling the nonlinear require high levels of autonomy, especially the maritime law and the low level each actuator: in this possibly, users’ lives could be in danger. But this also gorithm is employed to map the virtual inputs in terms require high levels of autonomy, especially in the maritime law and the low level controller of each actuator: in this applies to Unmanned Vehicles (UVs) in general, which of real actuator efforts, decoupling the nonlinear control require high levels of the autonomy, especially in the maritime and thefunction low efforts, level controller of each actuator: in this environment, where duration of is typiway, aa cost can be minimized in order to applies to Unmanned Vehicles (UVs) general, of real actuator decoupling the control environment, where the duration of the theinmissions missions iswhich typi- law way, cost function cancontroller be minimized in nonlinear order to optimize optimize require high levels of the autonomy, especially in the maritime law and thefunction low level of each actuator: in this environment, where duration of the missions is typiway, a cost can be minimized in order to optimize cally longer. the energy consumption, and also saturation limits can require high levels of the autonomy, especially in the maritime law and thefunction low level controller of each actuator: in this cally longer. the energy consumption, and also saturation limits can environment, where duration of the missions is typiway, a cost can be minimized in order to optimize cally longer. can the energy consumption, and also saturation limits be taken into account. For this reason, thrust allocation environment, where Vehicles the duration of the missions is typiway, a cost function can be minimized in order to optimize be taken into account. For this reason, thrust allocation Remotely Operated (ROVs) represent a class of cally longer. the taken energyinto consumption, saturation limits can account. Forand thisalso reason, thrust allocation Remotely Operated Vehicles (ROVs) represent a class of be one of exploited elds of (Jocally longer. the energyinto consumption, and also saturation limits can Remotely Operated Vehicles (ROVs) represent a class of represents represents one of the the most most exploited elds of control control (JoUnmanned Underwater Vehicles (UUVs), equipped with be taken account. For this reason, thrust allocation Unmanned Underwater Vehicles (UUVs), equipped with represents one of the most exploited elds of control (Johansen and Fossen (2013)): since the ’80s, the allocation Remotely Operated Vehicles (ROVs) represent a class of be taken into account. For this reason, thrust allocation Unmanned Underwater Vehicles (UUVs), equipped with hansen andone Fossen (2013)): since theelds ’80s, of the allocation remote and/or manipulation devices, and for represents of the most with exploited (JoRemotely Operated Vehicles (ROVs) represent class of problem remote sensing sensing and/or manipulation devices, anda used used for and Fossen (2013)): since theelds ’80s, thecontrol allocation has been studied, different approaches, such Underwater Vehicles (UUVs), equipped with represents one of the most with exploited of control (Joremote sensing and/or manipulation devices, and used for hansen problemand has been studied, different approaches, such aUnmanned wide class of applications, ranging from simple explohansen Fossen (2013)): since the ’80s, the allocation Unmanned Underwater Vehicles (UUVs), equipped with a wide sensing class of and/or applications, rangingdevices, from simple explohas Fossen beennumerical studied, with different approaches, such as analytical and minimization of aathe cost function remote manipulation and used for problem hansen and (2013)): since the ’80s, allocation aration wide class of applications, ranging from simple exploas analytical and numerical minimization of cost function to disaster recovery. Many approaches have been problem has and beennumerical studied, with different approaches, such remote sensing manipulation devices, and used for as analytical minimization of a cost Durham function to disaster recovery. Many approaches have been under constraints (Enns Bodson (2002); aration wide to class of and/or applications, ranging from simple exploproblem has and beennumerical studied, with different such ration disaster recovery. Many approaches have been under constraints (Enns (1998); (1998); Bodson approaches, (2002); Durham studied to increase autonomy and reliability as the misas analytical minimization of a cost function a wide class of applications, ranging from simple explostudied to increase autonomy and reliability as the misunder constraints (Enns (1998); Bodson (2002); Durham (1993)), real-time iterative optimization (Bodson (2002); ration to disaster recovery. Many approaches have been as analytical and numerical minimization of a cost function studied increaserecovery. autonomy and approaches reliability as(Antonelli the been mis- under (1993)), real-time (Enns iterative optimization (Bodson (2002); sions longer and more challenging constraints (1998); Bodson (2002); Durham ration totobecoming disaster sions are are becoming longer andMany more challengingashave (Antonelli real-time iterative optimization (Bodson (2002); Lindfors (1993); and Sousa (1999); Harkegard studied increase autonomy and reliability the mis- (1993)), under constraints (Enns (1998); Bodson (2002); Durham sions areto becoming longer andtolerance more challenging (Antonelli Lindfors (1993); Webster Webster and Sousa (1999); Harkegard (2003)), and actuator fault via over-actuation (1993)), real-time iterative optimization (Bodson (2002); studied to increase autonomy and reliability as the mis(2003)), and actuator fault tolerance via over-actuation (1993); Webster and Sousa (1999); Harkegard (2002); et al. explicit solutions for sions are and becoming longer andtolerance more challenging (Antonelli Lindfors (1993)), real-time iterative optimization (Bodson (2002); (2003)), actuator fault via over-actuation (2002); Johansen Johansen et al. (2004)), (2004)), explicit solutions for the the represents one of Lindfors (1993); Webster and Sousa (1999); Harkegard sions are and becoming longer andtolerance more challenging (Antonelli (2002); Johansen et al. (2004)), explicit solutions for the represents oneactuator of them. them. (2003)), fault via over-actuation Lindfors (1993); Webster and Sousa (1999); Harkegard represents one of them. (2002); Johansen et al. (2004)), explicit solutions for the (2003)), representsand oneactuator of them.fault tolerance via over-actuation (2002); Johansen et al. (2004)), explicit solutions for the represents them. Copyright 2018 IFAC 250 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © one 2018,of IFAC (International Federation of Automatic Control) Copyright © 2018 IFAC 250 Copyright 2018 IFAC 250 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 250 10.1016/j.ifacol.2018.09.511 Copyright © 2018 IFAC 250
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
unconstrained problem (Sørdalen (1997); Berge and Fossen (1997)) and so on. In our previous works, the approach consisted in a weighted version of the Moore-Penrose (MP) pseudoinverse method (Fasano et al. (2015); Ciabattoni et al. (2016)). This paper proposes an improved version of the aforementioned method, that takes into account the saturation limits and hence provides a better redistribution of the control efforts. In detail, we propose an optimal algorithm that solves the control allocation problem and outperforms the standard interior point method for the specic case of the considered ROV, taking into account the thrusters’ saturation limits and the possible thrusters’ faults. The paper is structured as follows. In Section 2 the model of the SNAMprogetti ROV is proposed, together with a suggested fault tolerant scheme to exploit the benets of the allocation algorithm. Section 3 is dedicated to the proposed thrust allocation technique: the rationale is provided, and the problem is formulated in terms of quadratic constrained optimization problems. In Section 4 some simulation results are proposed in order to compare the algorithm with known state of the art alternatives. Finally, conclusions and future works end the paper. 2. ROV MODEL AND CONTROL LAW In this section, the ROV mathematical model is rst presented, which is then required for the design of the trajectory tracking control laws. 2.1 Mathematical model The ROV mathematical model is here briey described. Due to the low speeds of the ROV and underwater currents, the ROV system can be considered as a single rigid body with six degrees of freedom, which are the three spatial positions and three orientation angles (Longhi and Rossolini (1989); Corradini et al. (2011)). Hence, as shown in Fig. 1, we consider an earth xed frame R−{O, X, Y, Z}, which describes the ROV centre of mass with the vector [x, y, z], and a body-xed frame Ra − {Oa , xa , ya , za }, which is attached to the system’s centre of mass. The translational dynamics can be expressed in terms of the centre of mass position [x, y, z]T and velocity [x, y, z] T. Assuming the roll, pitch, and yaw common notation, the ROV orientation dynamics can be expressed in function of the roll angle ϕ, pitch angle θ, yaw angle ψ, and the and the ψ. respective euler rates ϕ, θ, Generally, one can consider a time varying underwater current V c = [Vcx , Vcy , Vcz ]T , which is constant in space. The forces and moments, produced by the thrusters acting on the rigid body, are denoted with T = [Tx , Ty , Tz ]T and M = [Mx , My , Mz ]T respectively. We are interested on the tracking problem in a horizontal plane, hence we assume that the altitude, roll, and pitch dynamics are externally regulated. Without loss of generality, we can set z = z = ϕ = ϕ = θ = θ = 0. The ROV model collapses to a three degrees of freedom model, namely the translations along X, Y and the yaw rotation ψ around Z. The underwater current can be considered only in the plane, namely Vcx and Vcy , as well as the external forces, 251
251
in which we consider only Tx and Ty . The only moment component to consider is Mz , which is directly tied to the yaw angle. The resulting model can be formulated as in (1), where ψc represents the four quadrant inverse tangent of Vcy and Vcx (Corradini et al. (2011)). The parameters p1 , . . . , p9 represent constant model components, as described in (Baldini et al. (2018)). The variables Tx , Ty , and Mz are determined according to the actuator geometrical conguration. As shown in Fig. 2, we consider four thrusters with the xed angle α = π/4, while the forces are denoted with ui (i = 1, . . . , 4). Denoting τ = [Tx , Ty , Mz ]T and u = [u1 , u2 , u3 , u4 ]T , the allocation can be expressed as ⎤ ⎡ c(ψ−α) c(ψ−α) c(ψ+α) c(ψ+α) τ = Bu = ⎣s(ψ−α) s(ψ−α) s(ψ+α) s(ψ+α) ⎦ u (2) −dα dα −dα dα
where c(·) = cos(·), s(·) = sin(·), and dα = dx sα + dy cα , while dx and dy denote the ROV physical size, as in Fig. 2. Note that, for sake of simplicity, the dependence of B on ψ is omitted. 2.2 Control Law and stability
The general approach is to design the control law directly on the control model (1), generating a desired virtual input τ . Once τ is obtained, the simplest way to get the real input signal is to compute u = B −1 τ , where B −1 denotes a right inverse matrix for B. Generally, there can be multiple ways to choose B −1 . Moreover, due to constraints, the mapping between τ and u has to be solved in each time step by an on-line algorithm instead of a closed form formula. Hence, the allocation problem is generally considered as external with respect the control law design, since they are designed independently, as seen in Fig. 3. This holds true whenever the thrust allocation algorithm does not allocate the desired thrust by violating the saturation constraints. On the contrary, when saturation constraints are not satised, the control effort required by the control law is different from the actual input provided to the dynamic system, and this may lead to closed-loop system instability. Our approach thus aims at providing a thrust allocation which satises the saturation constraints, as long as it is possible (i.e., the optimization problem admits feasible solutions); at the same time, it is computationally more efficient than other standard constrained allocation techniques. In Section 4, we will consider different control laws for evaluating the proposed allocation algorithm performances in closed-loop. The control laws, in detail, are backstepping (BS), sliding mode (SMC), dynamic surface (DSC), and PID; the mathematical representations of these laws are described in our previous work (Baldini et al. (2018)). 3. OPTIMAL THRUST ALLOCATION The forces Tx , Ty , and moment Mz required by the control law must be mapped into the thrusts generated by the thrusters according to the model τ = BW u (3)
IFAC CAMS 2018 252 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
�
� 2 +V2 =T (Vcx − x) 2 + (Vcy − y) 2 + p4 x − p5 Vcx Vcx x cy � � 2 +V2 =T p1 y¨ − (p2 |sin (ψ)| + p3 |cos (ψ)|) (Vcy − y) (Vcx − x) 2 + (Vcy − y) 2 + p4 y − p5 Vcy Vcx y cy � � � � ψ − ψ � � c 2 2 + p9 = Mz p6 ψ¨ + p7 ψ �ψ � + p8 (Vcx + Vcy ) sin 2
p1 x ¨ − (p2 |cos (ψ)| + p3 |sin (ψ)|) (Vcx − x)
]
When a fault occurs in the i-th thruster, the corresponding weight satises 0 < wi < 1, thus modeling the reduced efficiency in supplying power. We do not consider here the case where thrusters possibly break down, namely the thruster failure, i.e., wi = 0 for some i ∈ {1, . . . , 4}, due to space limitation. However, it is possible to extend our result including this case, and this is part of our future work.
XPELOLFDO \DZDQJOH ᷤ ]D
LQHUWLDO IUDPH
\ 2
Let mi > 0, for i = 1, . . . , 4, be the maximal thrust allowed for the i-th thruster and let m = [m1 , . . . , m4 ]T . In order to model the physical constraints of the system, the components of u are constrained so that |ui | ≤ mi , for i = 1, . . . , 4, or more compactly −m ≤ u ≤ m (6) where inequalities are intended componentwise.
ERG\IUDPH
2D
[
[D
\D
UROODQJOH ij
(1)
SLWFKDQJOH ׇ
Now, using (3) together with (6), it is necessary to calculate the thrusts u corresponding to τ and nd the “optimal” distribution of control energy. In particular, u is the solution, provided it exists, of the following problem ⎧ ⎨ BW u = τ . (7) subject to ⎩ −m ≤ u ≤ m
Fig. 1. ROV inertial and body frames.
Note that (7) could be unfeasible, even in the case system (3) is compatible, due to the constraints (6) on u. Moreover, even when (7) is feasible, the solution is not unique, in general. This makes necessary to consider different cases and choose appropriate criteria for determining the optimal allocation for thrusts.
To this end, some preliminary considerations regarding matrix B in (3) are in order. It is possible to verify that, since α = π/4 and dα > 0, rank(B) = 3, regardless of the value of ψ ∈ R. Moreover, since W is invertible the coefficient matrix BW in (7) satises rank(BW ) = 3. (8) This makes the linear system (3) compatible for any τ ∈ R3 . However, since the solutions of (3) must satisfy the constraints (6), feasibility of (7) is affected.
Fig. 2. ROV thruster conguration.
Ŝ
Fig. 3. Overview of the control scheme.
In order to determine the optimal allocation of thrusts u, different cases must be considered, depending on the feasibility of (7). In particular, it is necessary to distinguish between two cases, namely whether (7) admits or not solutions.
where we have introduced the weighting matrix
3.1 System (7) admits solutions
W = diag{w1 , w2 , w3 , w4 }
(4)
with wi ∈]0, 1], for i = 1, ..., 4, which accounts for possible faults of thrusters, so that � 0 < wi < 1 if the i-th thruster is faulty . (5) wi = 1 if the i-th thruster is faultless 252
If system (7) admits solutions, BW u = τ is compatible and at least a solution satisfying −m ≤ u ≤ m exists. In general the solution is not unique and a we choose the one with the minimum energy, i.e., we solve the following convex optimization problem
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩
2
3.2 System (7) does not admit solution
min �u� u
subject to BW u = τ −m ≤ u ≤ m
(9)
where � · � is the 2-norm of (·). The problem in (9) admits a unique solution since the objective function is strictly convex and the feasible set 1 is not empty by hypothesis. To solve (9), let us start by considering the feasible set. The solutions of BW u = τ are ∞1 , since rank(BW ) = 3, and are all given by the following formula (James (1978)), where w ∈ R4 is arbitrary � � † † u = (BW ) τ + I − (BW ) (BW ) w = t0 + k
†
†
where t0 = (BW ) τ and k ∈ ker(BW ), with (·) being the right Moore-Penrose pseudo-inverse. T
Dening q = [1, −1,� −1, 1]� , it is possible to verify 2 that ker (BW ) = span W −1 q , where span (·) is the linear space generated by the vectors in the argument. Thus, any vector k ∈ ker (BW ) has the form k = γW −1 q, with γ ∈ R. Taking into account this result, all the solutions of BW u = τ can be rewritten as λ ∈ R. (10) u = t0 + γW −1 q The feasible set of (9) is given by the subset of vectors (10) that satisfy the constraints −m ≤ u ≤ m. These last entail corresponding constraints on the parameter γ, so that it is possible to prove that (11) −m ≤ t0 + γW −1 q ≤ m hold true whenever γ satises (12) γ ∈ [γmin , γmax ] with � γmin = max {W (−q ◦ t0 − m)} (13) γmax = min {W (−q ◦ t0 + m)} where ◦ is the Hadamard product (Davis (1962)), and the maximum (minimum) of a vector is intended as the maximum (minimum) of its entries. Finally, the feasible set of (9) is given by � � F = u ∈ R4 � � u = t0 + γW −1 q, with γ ∈ [γmin , γmax ] (14) where all the terms are dened above. Moreover, since � �2 2 2 (15) �u� = �t0 � + γ 2 �W −1 q � , as t0 is orthogonal to ker (BW ), we have that solving (9) is equivalent to determine γ ∈ [γmin , γmax ] such that (15) is minimum. It is possible to verify that the optimal value of γ, namely γ ∗ , is given by ⎧ ⎨γmin , if 0 < γmin γ∗ = 0 (16) , if γmin ≤ 0 ≤ γmax ⎩ γmax , if 0 > γmax and thus the optimal solution of (9) is given by †
u = (BW ) τ + γ ∗ W −1 q ∗ where γ is dened in (16). 1 2
253
(17)
If system (7) does not admit solutions, none of the ∞1 solutions 3 of BW u = τ , as given by (10), satises the constraints −m ≤ u ≤ m. A viable alternative in this case is to nd u by solving the following optimization problem ⎧ 2 ⎪ �BW u − τ � ⎨min u . (18) subject to ⎪ ⎩ −m ≤ u ≤ m Note that (18) admits a unique solution, since the feasible set is not empty and the objective function is strictly convex on the feasible set. Moreover, the solution u is such that for at least one of its components the constraint is active, i.e., there exists k ∈ {1, . . . , 4} such that equality in the constraint is achieved uk = −mk or uk = mk , where uk is the k-th component of u. Indeed, if this were not the case u would also be solution of the unconstrained 2 problem min �BW u − τ � and thus solution of BW u = u τ , but this is not possible since, by hypothesis none of the solutions of BW u = τ satises the constraints −m ≤ u ≤ m. So, let us denote by u the vector collecting (all) the components of u for which the constraint is active, i.e., uk = −mk or uk = mk . Moreover, denote by S the � the selection 4 matrix that guarantees u = Su and by S complementary selection matrix obtained from the 4 × 4 identity matrix I 4 by removing the rows of S. It is easy � satisfy S �T S � + ST S = I 4. to verify that that S and S With the above positions, it is possible to prove that the (unique) solution of (18) is given by the following expression � �† � � T � � T BW (19) τ − BW u + S u u=S � = WS � T , W = W S T , and where W � � −S 1 u = Sm = m S2
(20)
where S 1 , S 2 are selection matrices dened such that S 1 singles out the entries of u that achieve the lower bound, and S 2 singles out the entries achieving the upper bound. Any of S 1 and S 2 can be possibly void, in the case only the upper, or lower, bound is achieved, but not both. Note that (19) gives the structure of the solution and, provided that u is determined in some way, gives the optimal thrust allocation. So, in order to calculate the solution it is sufficient to cycle among all the admissible congurations where some components of u saturate, thus achieving the bounds, and then choosing the conguration 2 that minimizes �BW u − τ � . The admissible congurations are nite and it is possible to prove that they are 34 − 9, since some congurations cannot occur due to the nature of the problem. In this way, the solution is easily determined by means of (19). 3
Note that the system BW u = τ is always compatible. We dene selection matrix a submatrix of the identity matrix obtained by removing some rows.
4
This is the set of vectors satisfying the constraints. Indeed dim (ker (BW )) = 1 and Bq = 0.
253
IFAC CAMS 2018 254 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
4. NUMERICAL SIMULATION
Table 1. Testing results for the proposed allocation algorithm.
In this section, a comparison between the classical MP pseudo-inverse and the proposed approach is presented. As rst, we provide an empirical evidence of the optimality of the proposed solution, comparing the results of MP, the proposed one (OPT) and the Matlab QP solver (QP). Then the proposed algorithm has been deployed within a ROV simulator for closed-loop trajectory tracking, in order to evaluate the impact on realistic applications, in case of different dynamics controllers. 4.1 Randomly generated input samples Every algorithm has been executed in Matlab 100.000 times, with realistic random τ input samples, namely whose values can be feasibly allocated to the thrusters in the fault free case, and considering up to three faults thereafter. The timing on a personal computer (i7-6700K, 4.2 GHz) has been recorded as well. In Table 1, the worst case and average computation time is reported for every considered allocation algorithm. In particular, the physical relation between the power consumption in Table 1 and the instantaneous thrust is expressed by (Johansen et al. (2004)) 3/2
3/2
3/2
3/2
(21) P = k1 |u1 | + k2 |u2 | + k3 |u3 | + k4 |u4 | where k1 , k2 , k3 , k4 are constants that bind the power to the thrust for each propeller. Supposing that the propellers have the same characteristics, it is possible to assume ¯ for an arbitrary positive that k1 = k2 = k3 = k4 = k, ¯ Hence, the consumption can be estimated by constant k. T the dimensionless quantity 0 P (t)/ k¯ dt. The MP algorithm, which gives a non-optimal solution, is the fastest one; the proposed algorithm is slower than the MP algorithm, but is one order of magnitude faster than the Matlab QP solver, which is based on interior point method. This holds for both average and worst case running time.
In the feasible case (i.e., at least one algorithm has found a solution such that τ = BW u and u is feasible with respect to the saturation limits), the MP solution has the lowest average consumption, but this is obtained renouncing to optimality. In fact, the average error norm is not null, because some of the solutions are beyond the saturation limits. The proposed approach and the QP solver both return feasible solutions, and their accuracy and average consumption are comparable. In particular, the accuracy of the QP solver can be imposed by modifying the optimality tolerance, whereas the accuracy of the MP and OPT methods are determined by the oating point accuracy of the data. In the unfeasible case, the MP solution involves both higher consumption and average error norm. In fact, it initially ignores the saturation limits, and then truncates the result to satisfy them, introducing an error. The proposed solution and the QP solver returns almost the same results. 4.2 Reference trajectory tracking Twenty complete simulation scenarios for the SNAMprogetti ROV have been considered, with different reference 254
Running time Feasible case Unfeasible case
Worst case [ms] Avg. case [ms] Avg. consumption Avg. error norm Avg. consumption Avg. error norm
MP
OPT
QP
4.888 0.0376 4055.6 14.4273 3810.0 2618.5
9.116 0.240 4072.0 3 · 10−12 3715.0 2362.2
100.409 2.711 4073.7 5 · 10−11 3715.0 2362.2
Table 2. IAE and consumption with different allocation and control laws. Avg. IAE (MP) Avg. IAE (OPT) IAE difference Avg. cons. (MP) Avg. cons. (OPT) Cons. difference
BS
SMC
PID
DSC
22.945 14.179 −38.20% 2.929 3.032 +3.52%
42.566 32.262 −24.21% 2.994 3.092 +3.28%
46.052 38.630 −16.12% 3.047 3.158 +3.65%
43.330 35.969 −16.99% 3.027 3.138 +3.67%
trajectories, initial conditions and fault occurrences. The control scheme is reported in Fig. 3. Fault detection and diagnosis are supposed to be ideal, i.e., perfect accuracy and negligible detection time; for the purpose of this paper, this assumption simplies the analysis of the results. In order to quantify the tracking performance, we dene the Integral of Absolute Error (IAE) index as follows T 1 |x − xd | + |y − yd | + |ψ − ψd | dt (22) IAE = 10 0 where the coefficient 1/10 is introduced to equally weigh an error of 10◦ of the yaw angle and an error of 1 m along the X or Y axis. As it can be seen in Table 2, every control law takes advantage from the introduction of the proposed method in simulation scenarios. The IAE index is drastically reduced with the use of the proposed technique, in a range between −16.12% for the PID controller and −38.20% for the backstepping controller. The energy consumption is marginally higher, as expected: the use of an improved control allocation allows to exploit more efficiently the actuators in the neighbourhood of the saturation limits, leading to a larger control effort. In fact, when the classical MP returns an unattainable control effort, an elementwise saturation occurs, essentially trading the correctness of the solution with a faster computation time. The small increase in the consumption is acceptable, indeed, if the increase in the tracking performances is taken into account. 5. CONCLUSIONS The thrust allocation offers a series of advantages for the class of Unmanned Underwater Vehicles. In fact, the thrust allocation allows to design the motion control law without the detailed knowledge about the actuators, neglecting the actuator saturation limits, and despite of possible actuator faults. The objective of the present paper is to study and solve the constrained thrust allocation problem for the class of over-actuated ROVs. Exploiting and improving the results of our previous works (Fasano et al. (2015); Ciabattoni et al. (2016)), here we consider the thrust allocation problem taking into account the thruster saturation limits and the possible actuator faults. In detail,
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
we solve the optimization problem both in case the main problem admits solution, and also in case it does not admit solutions, using a viable alternative. The proposed thrust allocation algorithm is tested on a ROV with real parameters, in combination with different control laws, to evaluate the proposed allocation algorithm performances in closed-loop. The algorithm has been compared with both the saturated pseudo-inverse method and the solution of the QP solver: the algorithm achieves the same accuracy of the QP solver, with a running time that is one magnitude lower in both the average case and the worst case, while the saturated MP pseudo-inverse is marginally faster, but leads to poorer performances. Finally, the algorithm is validated in simulation, comparing the tracking performances of the proposed algorithm with the saturated MP pseudo-inverse method. The differences in the IAE with every control law justify the use of the proposed algorithm in the applications, in order to achieve a better redistribution of the control effort among the thrusters. We are still working on this problem and we are developing the solution of the thrust allocation problem taking into account the saturation limits, the thrusters’ faults, and also the thrusters’ failures. REFERENCES Antonelli, G. (2003). A survey of fault detection/tolerance strategies for AUVs and ROVs. Fault Diagnosis and Fault Tolerance for Mechatronic Systems:Recent Advances, 109–127. Baldini, A., Ciabattoni, L., Felicetti, R., Ferracuti, F., Freddi, A., and Monteri , A. (2018). Dynamic surface fault tolerant control for underwater remotely operated vehicles. ISA Transactions, 78, 10–20. Berge, S.P. and Fossen, T.I. (1997). Robust control allocation of overactuated ships; experiments with a model ship. In Proc. 4th IFAC Conf. Manoeuvring and Control of Marine Craft, 166–171. Citeseer. Bodson, M. (2002). Evaluation of optimization methods for control allocation. Journal of Guidance, Control, and Dynamics, 25(4), 703–711. Chen, Y. and Wang, J. (2012). Fast and global optimal energy-efficient control allocation with applications to over-actuated electric ground vehicles. IEEE Transactions on Control Systems Technology, 20(5), 1202–1211. Ciabattoni, L., Fasano, A., Ferracuti, F., Freddi, A., Longhi, S., and Monteri` u, A. (2016). A thruster failure tolerant control scheme for underwater vehicles. In 12th IEEE/ASME International Conference on Mechatronic and Embedded Systems and Applications, 1–6. IEEE.
255
255
Corradini, M.L., Monteri` u, A., and Orlando, G. (2011). An actuator failure tolerant control scheme for an underwater remotely operated vehicle. IEEE Transactions on Control System Technology, 19(5), 1036–1046. Davis, C. (1962). The norm of the Schur product operation. Numerische Mathematik, 4(1), 343–344. Durham, W.C. (1993). Constrained control allocation. Journal of Guidance, Control, and Dynamics, 16(4), 717–725. Enns, D. (1998). Control allocation approaches. In Guidance, Navigation, and Control Conference and Exhibit. Boston, Massachusetts, USA. Fasano, A., Ferracuti, F., Freddi, A., Longhi, S., and Monteri` u, A. (2015). A virtual thruster-based failure tolerant control scheme for underwater vehicles. IFACPapersOnLine, 48(16), 146–151. Harkegard, O. (2002). Efficient active set algorithms for solving constrained least squares problems in aircraft control allocation. In Proceedings of the 41st IEEE Conference on Decision and Control, volume 2, 1295– 1300. IEEE. James, M. (1978). The generalised inverse. The Mathematical Gazette, 62(420), 109–114. Johansen, T.A. and Fossen, T.I. (2013). Control allocation - a survey. Automatica, 49(5), 1087–1103. Johansen, T.A., Fossen, T.I., and Berge, S.P. (2004). Constrained nonlinear control allocation with singularity avoidance using sequential quadratic programming. IEEE Transactions on Control Systems Technology, 12(1), 211–216. Khelassi, A., Weber, P., and Theilliol, D. (2010). Recongurable control design for over-actuated systems based on reliability indicators. In IEEE Conference on Control and Fault-Tolerant Systems (SysTol), 365–370. Lindfors, I. (1993). Thrust allocation method for the dynamic positioning system. In 10th International Ship Control Systems Symposium (SCSS’93), 3–93. Longhi, S. and Rossolini, A. (1989). Adaptive control for an underwater vehicle: Simulation studies and implementation details. IFAC Workshop on Expert Systems and Signal Processing in Marine Automation, 271–280. Oppenheimer, M.W., Doman, D.B., and Bolender, M.A. (2006). Control allocation for over-actuated systems. In 14th Mediterranean Conference on Control and Automation, 1–6. IEEE. Sørdalen, O. (1997). Optimal thrust allocation for marine vessels. Control Engineering Practice, 5(9), 1223–1231. Webster, W. and Sousa, J. (1999). Optimum allocation for multiple thrusters.
ScienceDirect
IFAC PapersOnLine 51-29 (2018) 250–255
A Constrained Thrust Allocation A A Constrained Constrained Thrust Thrust Allocation Allocation A Constrained Thrust Allocation Algorithm for Remotely Operated Vehicles Algorithm for Remotely Operated Vehicles A Constrained Thrust Allocation Algorithm for Remotely Operated Vehicles Algorithm for Remotely Operated ∗ ∗∗ ∗ Algorithm for Remotely Operated Vehicles Vehicles
Alessandro ,, Riccardo ∗, Alessandro Baldini Baldini ∗∗ ,,∗Antonio Antonio Fasano Fasano∗∗∗ Riccardo Felicetti Felicetti ∗ ∗ ,, Alessandro Baldini ,∗Antonio Fasano∗∗∗ , Riccardo Felicetti Alessandro Freddi , Sauro Longhi , Andrea Monteri` u ∗ ∗ ∗ ∗∗ Alessandro Freddi,∗Antonio , Sauro Longhi , Andrea Monteri` u∗ , Alessandro Baldini Fasano∗∗∗ , Riccardo Felicetti Alessandro Freddi Monteri` u ∗ ∗, ∗ , Sauro Longhi Alessandro Baldini ,∗Antonio Fasano∗ ,, Andrea , Riccardo Felicetti Alessandro Freddi , Sauro Longhi Andrea Monteri` u∗ ∗ ∗ ∗ Department of Information Engineering, ∗ Freddi , Sauro Longhi , Andrea Monteri` Alessandro u of Information Engineering, ∗ Department Department of Information Engineering, Universit` a delle Marche, Via Brecce Bianche, 60131 Universit` a ∗Politecnica Politecnica delle Marche, Via Brecce Bianche, 60131 Department of Information Universit` a ∗Politecnica delle Marche,r.felicetti}@pm.univpm.it, ViaEngineering, Brecce Bianche, 60131 Ancona, Italy, email: {a.baldini, Department of Information Ancona, Italy, email:delle {a.baldini, r.felicetti}@pm.univpm.it, Universit` a {a.freddi, Politecnica Marche, ViaEngineering, Brecce Bianche, 60131 Ancona, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, s.longhi, a.monteriu}@univpm.it Universit` a Politecnica delle Marche, Via Brecce Bianche, 60131 s.longhi, a.monteriu}@univpm.it Ancona,{a.freddi, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, ∗∗ s.longhi, a.monteriu}@univpm.it a Ca’ Foscari, Venezia, Italy ∗∗ Universit` Ancona,{a.freddi, Italy, email: {a.baldini, r.felicetti}@pm.univpm.it, Universit` a Ca’ Foscari, Venezia, Italy ∗∗ {a.freddi, s.longhi, a.monteriu}@univpm.it Universit` a Ca’ Foscari, Venezia, Italy email: [email protected] {a.freddi, s.longhi, a.monteriu}@univpm.it ∗∗ email: [email protected] a Ca’ Foscari, Venezia, Italy email: [email protected] ∗∗ Universit` Universit` a Ca’ Foscari, Venezia, email: [email protected] Italy email: [email protected] Abstract: Abstract: In In this this paper, paper, the the constrained constrained thrust thrust allocation allocation problem problem is is studied studied and and solved solved for for Abstract: In this paper, the constrained thrust allocation problem isnd studied and solved for the class of over-actuated Remotely Operated Vehicles. In detail, we the solution of the class of In over-actuated Remotely Operated Vehicles. In detail, weisnd the solution of the the Abstract: this paper, the constrained thrust allocation problem studied and solved for the class of In over-actuated Remotely Operated Vehicles. In detail, weisnd the solution of the thrust allocation when the thrusters are affected by possible faults simultaneously, taking Abstract: this paper, constrained thrust allocation problem studied and solved for thrust allocation when the the thrusters are affected by possible faults and, and, simultaneously, taking the class of over-actuated Remotely Operated Vehicles. Inthrust detail, we nd the solution of the thrust allocation when the thrusters are affected by possible faults and, simultaneously, taking into account the thrusters’ saturation limits. The proposed allocation algorithm is tested the class of over-actuated Remotely Operated Vehicles. In detail, we nd the solution of the into account the thrusters’ saturation limits. The proposed thrust allocation algorithm is tested thrust allocation when the thrusters are affected by possible faults and, simultaneously, the thrusters’ saturation limits. The proposed thrust allocation into account algorithm is taking tested both numerically, with randomly generated input samples, and applied in simulation the thrust allocation when thrusters are affected possible faults and, simultaneously, taking bothaccount numerically, with the randomly generated inputby samples, and applied in algorithm simulationisfor for the into the thrusters’ saturation limits. The proposed thrust allocation tested both numerically, with randomly generated input samples, and applied in simulation for the trajectory tracking of a Remotely Operated Vehicle, with real parameters and in combination into account the thrusters’ saturation limits. The proposed thrust allocation algorithm is tested trajectory trackingwith of a randomly Remotely generated Operated Vehicle, with real parameters and in combination both numerically, input samples, and applied in simulation for the trajectory tracking of laws. a randomly Remotely OperatedisVehicle, with real parameters in combination with different control The algorithm compared with two commonly used approaches, both numerically, with generated input samples, and applied inand simulation for the with different control laws. The algorithm isVehicle, compared with two commonly used approaches, trajectory tracking of laws. apseudo-inverse Remotely Operated with real parameters and in combination with control algorithm is compared with two commonly used approaches, different The that are the saturated method and the Matlab Quadratic Programming solver; trajectory tracking of a Remotely Operated Vehicle, with real parameters and in combination that are the saturated pseudo-inverse method and the Matlab Quadratic Programming solver; with different control laws. The algorithm is compared withanalysis two commonly used approaches, that are the saturated pseudo-inverse method and theinMatlab Quadratic Programming solver; both accuracy and computation time are considered the of the performances. The with different control laws. The algorithm is compared with two commonly used approaches, both are accuracy and computation time method are considered inMatlab the analysis of the performances. The that the saturated pseudo-inverse and the Quadratic Programming solver; both accuracy andthat computation timealgorithm are considered inMatlab the analysis of the performances. The simulations show the proposed performs better, in terms of computation time, that are the saturated pseudo-inverse method and the Quadratic Programming solver; simulations show that the proposed algorithm performs better, in terms of performances. computation time, both accuracy and computation time are considered in the analysis of the The simulations show the proposed performs better, in terms of performances. computation time, w.r.t. the Quadratic Programming solver, while still retaining the optimality the both andthat computation timealgorithm are considered in the analysis of the The w.r.t. accuracy the Matlab Matlab Quadratic Programming solver, while still retaining the optimality of oftime, the simulations show that the proposed algorithm performs better, inalso terms ofimprovement computation w.r.t. Matlab Quadratic Programming the solver, while still retaining the optimality of solution despite the saturation constraints; moreover, there is an in the simulations show that the proposed algorithm performs better, in terms of computation time, solution despite the saturation constraints; moreover, there is also an improvement in the w.r.t. theerror Matlab Quadratic Programming solver, whilethere still is retaining optimality in of the solution despite the saturation constraints; moreover, also anthe improvement tracking regardless of adopted control law. w.r.t. theerror Matlab Quadratic solver, still is retaining optimality in of the the tracking regardless of the theProgramming adopted control law.whilethere solution despite the saturation constraints; moreover, also anthe improvement control law. tracking error regardless of the adopted solution despite the saturation constraints; moreover, there is also an improvement in the tracking error(International regardless ofFederation the adopted control Control) law. © 2018, IFAC of Automatic Hosting by Elsevier Ltd. All rights reserved. tracking error regardless of theFault-tolerant adopted control law. Fault Keywords: Thrust Allocation, systems, Keywords: Thrust Allocation, Fault-tolerant systems, Fault tolerance, tolerance, Fault Fault diagnosis, diagnosis, Fault Fault Keywords: Unmanned Thrust Allocation, Fault-tolerant systems, Fault tolerance, Fault diagnosis, Fault detection, Underwater Vehicles. detection, Unmanned Underwater Vehicles. systems, Fault tolerance, Fault diagnosis, Fault Keywords: Thrust Allocation, Fault-tolerant detection, Unmanned Underwater Vehicles. systems, Fault tolerance, Fault diagnosis, Fault Keywords: Thrust Allocation, Fault-tolerant detection, Unmanned Underwater Vehicles. detection, Unmanned Underwater Vehicles. 1. However, 1. INTRODUCTION INTRODUCTION However, over-actuation over-actuation introduces introduces additional additional complexity complexity 1. INTRODUCTION However, over-actuation introduces additional complexity as well, which must be taken into account when the as well, which must be taken into account when complexity the control control 1. INTRODUCTION However, over-actuation introduces additional as well, is which must In befact, taken intoofaccount when the control system designed. most the linear and nonlinear 1. INTRODUCTION However, over-actuation introduces additional complexity system is designed. In fact, most ofaccount the linear andthe nonlinear as well, which must be taken into when control system designed. fact, most theexploit linear and nonlinear control techniques do not directly input redunas well, is which must In be taken intoof when the control control techniques dofact, notmost directly exploit input redunsystem is designed. In ofaccount the linear and nonlinear control techniques do not directly exploit input redunOver-actuation represents an interesting feature in medancy; on the contrary, they are typically conceived to is designed. In fact, most of the linear and nonlinear Over-actuation represents an interesting feature in me- system dancy; on the contrary, they are typically conceived to control aon techniques do not directly exploit input redunOver-actuation represents an interesting feature in me- dancy; the contrary, they areis typically conceived to chanical systems: thanks to the availability of more actuacontrol number of outputs that equal to the number of control techniques do not directly exploit input redunchanical systems:represents thanks to an theinteresting availability feature of more in actuacontrol a number of outputs that is equal to the number of Over-actuation medancy; on the contrary, they are typically conceived to chanical systems: thanks to an thefault availability of control more in actuacontrol aon number of outputs that is typically equal to the number of tors than degrees of freedom, tolerant with variables. The most straightforward way to tackle Over-actuation represents interesting feature medancy; the contrary, they are conceived to tors than degrees of freedom, fault tolerant control with variables. The most straightforward way to tackle chanical systems: thanks to the availability of more actuacontrol a number of outputs that is equal to the number of tors thanto degrees of failures freedom, fault tolerantof control with control The most straightforward waynumber to inputs tackle respect actuator can be by the is arbitrarily dene some chanical systems: thanks to the availability more actuacontrol avariables. number outputs that is equal to virtual the of respect to actuator failures can be obtained obtained by means means the problem problem is to toof arbitrarily dene some virtual inputs tors thanto degrees of failures freedom, fault control with control variables. The most straightforward way to tackle respect actuator can betolerant obtained by means the problem is to arbitrarily dene some virtual inputs of control effort redistribution. Hence, over-actuation is a to simplify the control problem, and then to add a thrust tors than degrees of freedom, fault tolerant control with control variables. The most straightforward way to tackle of control effort redistribution. Hence, over-actuation is a the to simplify the control problem, and then to add a thrust respect to actuator failures can be obtained by means problem is to arbitrarily dene some virtual inputs of control effort redistribution. Hence, over-actuation isrea to simplify theis control problem, and then add a inputs thrust desirable in every requires allocation layer with the aim distributing the effort from respect toproperty actuator can bethat obtained byhigh means the problem to arbitrarily someto virtual desirable property in failures every scenario scenario that requires high reallocation layer with theproblem, aim of of dene distributing the effort from of control effort redistribution. Hence, over-actuation isrea allocation to simplify the control and then tothe add a way, thrust desirable property in every scenario that requires high layer with the aim of distributing effort froma liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In this of control effort redistribution. Hence, over-actuation is a to simplify the control problem, and then to add a thrust liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In this way, a desirable property in every scenario that requires high reallocation layer with the aim of distributing the effort from liability and safety (Chen and Wang that (2012); Oppenheimer the virtual inputs tothe theaim actual thrusters. In this way, a et al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with desirable property in every scenario requires high reallocation layer with of distributing the effort from et al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with liability and safety (Chen Wang (2012); Oppenheimer virtual inputs to the actual thrusters. In thisactuator way, et al. (2006); Khelassi et and al. (2010)). Thisofis the case of high level nonlinear control law can be synthesized witha vehicles employed for the transportation people, such athe number of virtual inputs, neglecting liability and safety (Chen and Wang (2012); Oppenheimer the virtual inputs to the actual thrusters. In thisactuator way, a vehicles employed for the transportation ofis people, such a suitable suitable number ofcontrol virtual inputs, neglecting et al. (2006); Khelassi et al. (2010)). This the case of high level nonlinear law can be synthesized with vehicles employed for the transportation of people, such a suitable number of virtual inputs, neglecting actuator as aircrafts or ships, where high costs are involved and, saturations and actuator faults. The thrust allocation alet al. (2006); Khelassi et al. (2010)). This is the case of high level nonlinear control law can be synthesized with as aircrafts or ships, where high costs areofinvolved and, asaturations and actuator faults. The thrust allocation alvehicles employed for where the transportation people, such suitableis number of to virtual inputs, neglecting actuator as aircrafts or ships, highin costs areofBut involved and, and actuator faults. The thrust allocation alpossibly, users’ lives could be danger. this also gorithm employed map the virtual inputs in terms vehicles employed for where the transportation people, asaturations suitableis number of to virtual inputs, neglecting actuator possibly, users’ lives could be in costs danger. But this such also saturations gorithm employed map the virtual inputs in terms as aircrafts or ships, high are involved and, and actuator faults. The thrust allocation alpossibly, users’ lives where could be danger. But thiswhich also gorithm is employed to decoupling map theThe virtual inputs incontrol terms applies to Vehicles (UVs) in of efforts, the nonlinear as aircrafts or ships, highin costs aregeneral, involved and, saturations and actuator faults. thrust allocation alapplies to Unmanned Unmanned Vehicles (UVs) in general, which of real real actuator actuator efforts, decoupling the nonlinear control possibly, lives could be in danger. But thiswhich also of gorithm is employed tocontroller map theofvirtual inputs incontrol terms applies tousers’ Unmanned Vehicles (UVs) in in general, real actuator efforts, decoupling the nonlinear require high levels of autonomy, especially the maritime law and the low level each actuator: in this possibly, users’ lives could be in danger. But this also gorithm is employed to map the virtual inputs in terms require high levels of autonomy, especially in the maritime law and the low level controller of each actuator: in this applies to Unmanned Vehicles (UVs) in general, which of real actuator efforts, decoupling the nonlinear control require high levels of the autonomy, especially in the maritime and thefunction low efforts, level controller of each actuator: in this environment, where duration of is typiway, aa cost can be minimized in order to applies to Unmanned Vehicles (UVs) general, of real actuator decoupling the control environment, where the duration of the theinmissions missions iswhich typi- law way, cost function cancontroller be minimized in nonlinear order to optimize optimize require high levels of the autonomy, especially in the maritime law and thefunction low level of each actuator: in this environment, where duration of the missions is typiway, a cost can be minimized in order to optimize cally longer. the energy consumption, and also saturation limits can require high levels of the autonomy, especially in the maritime law and thefunction low level controller of each actuator: in this cally longer. the energy consumption, and also saturation limits can environment, where duration of the missions is typiway, a cost can be minimized in order to optimize cally longer. can the energy consumption, and also saturation limits be taken into account. For this reason, thrust allocation environment, where Vehicles the duration of the missions is typiway, a cost function can be minimized in order to optimize be taken into account. For this reason, thrust allocation Remotely Operated (ROVs) represent a class of cally longer. the taken energyinto consumption, saturation limits can account. Forand thisalso reason, thrust allocation Remotely Operated Vehicles (ROVs) represent a class of be one of exploited elds of (Jocally longer. the energyinto consumption, and also saturation limits can Remotely Operated Vehicles (ROVs) represent a class of represents represents one of the the most most exploited elds of control control (JoUnmanned Underwater Vehicles (UUVs), equipped with be taken account. For this reason, thrust allocation Unmanned Underwater Vehicles (UUVs), equipped with represents one of the most exploited elds of control (Johansen and Fossen (2013)): since the ’80s, the allocation Remotely Operated Vehicles (ROVs) represent a class of be taken into account. For this reason, thrust allocation Unmanned Underwater Vehicles (UUVs), equipped with hansen andone Fossen (2013)): since theelds ’80s, of the allocation remote and/or manipulation devices, and for represents of the most with exploited (JoRemotely Operated Vehicles (ROVs) represent class of problem remote sensing sensing and/or manipulation devices, anda used used for and Fossen (2013)): since theelds ’80s, thecontrol allocation has been studied, different approaches, such Underwater Vehicles (UUVs), equipped with represents one of the most with exploited of control (Joremote sensing and/or manipulation devices, and used for hansen problemand has been studied, different approaches, such aUnmanned wide class of applications, ranging from simple explohansen Fossen (2013)): since the ’80s, the allocation Unmanned Underwater Vehicles (UUVs), equipped with a wide sensing class of and/or applications, rangingdevices, from simple explohas Fossen beennumerical studied, with different approaches, such as analytical and minimization of aathe cost function remote manipulation and used for problem hansen and (2013)): since the ’80s, allocation aration wide class of applications, ranging from simple exploas analytical and numerical minimization of cost function to disaster recovery. Many approaches have been problem has and beennumerical studied, with different approaches, such remote sensing manipulation devices, and used for as analytical minimization of a cost Durham function to disaster recovery. Many approaches have been under constraints (Enns Bodson (2002); aration wide to class of and/or applications, ranging from simple exploproblem has and beennumerical studied, with different such ration disaster recovery. Many approaches have been under constraints (Enns (1998); (1998); Bodson approaches, (2002); Durham studied to increase autonomy and reliability as the misas analytical minimization of a cost function a wide class of applications, ranging from simple explostudied to increase autonomy and reliability as the misunder constraints (Enns (1998); Bodson (2002); Durham (1993)), real-time iterative optimization (Bodson (2002); ration to disaster recovery. Many approaches have been as analytical and numerical minimization of a cost function studied increaserecovery. autonomy and approaches reliability as(Antonelli the been mis- under (1993)), real-time (Enns iterative optimization (Bodson (2002); sions longer and more challenging constraints (1998); Bodson (2002); Durham ration totobecoming disaster sions are are becoming longer andMany more challengingashave (Antonelli real-time iterative optimization (Bodson (2002); Lindfors (1993); and Sousa (1999); Harkegard studied increase autonomy and reliability the mis- (1993)), under constraints (Enns (1998); Bodson (2002); Durham sions areto becoming longer andtolerance more challenging (Antonelli Lindfors (1993); Webster Webster and Sousa (1999); Harkegard (2003)), and actuator fault via over-actuation (1993)), real-time iterative optimization (Bodson (2002); studied to increase autonomy and reliability as the mis(2003)), and actuator fault tolerance via over-actuation (1993); Webster and Sousa (1999); Harkegard (2002); et al. explicit solutions for sions are and becoming longer andtolerance more challenging (Antonelli Lindfors (1993)), real-time iterative optimization (Bodson (2002); (2003)), actuator fault via over-actuation (2002); Johansen Johansen et al. (2004)), (2004)), explicit solutions for the the represents one of Lindfors (1993); Webster and Sousa (1999); Harkegard sions are and becoming longer andtolerance more challenging (Antonelli (2002); Johansen et al. (2004)), explicit solutions for the represents oneactuator of them. them. (2003)), fault via over-actuation Lindfors (1993); Webster and Sousa (1999); Harkegard represents one of them. (2002); Johansen et al. (2004)), explicit solutions for the (2003)), representsand oneactuator of them.fault tolerance via over-actuation (2002); Johansen et al. (2004)), explicit solutions for the represents them. Copyright 2018 IFAC 250 Hosting by Elsevier Ltd. All rights reserved. 2405-8963 © one 2018,of IFAC (International Federation of Automatic Control) Copyright © 2018 IFAC 250 Copyright 2018 IFAC 250 Control. Peer review© under responsibility of International Federation of Automatic Copyright © 2018 IFAC 250 10.1016/j.ifacol.2018.09.511 Copyright © 2018 IFAC 250
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
unconstrained problem (Sørdalen (1997); Berge and Fossen (1997)) and so on. In our previous works, the approach consisted in a weighted version of the Moore-Penrose (MP) pseudoinverse method (Fasano et al. (2015); Ciabattoni et al. (2016)). This paper proposes an improved version of the aforementioned method, that takes into account the saturation limits and hence provides a better redistribution of the control efforts. In detail, we propose an optimal algorithm that solves the control allocation problem and outperforms the standard interior point method for the specic case of the considered ROV, taking into account the thrusters’ saturation limits and the possible thrusters’ faults. The paper is structured as follows. In Section 2 the model of the SNAMprogetti ROV is proposed, together with a suggested fault tolerant scheme to exploit the benets of the allocation algorithm. Section 3 is dedicated to the proposed thrust allocation technique: the rationale is provided, and the problem is formulated in terms of quadratic constrained optimization problems. In Section 4 some simulation results are proposed in order to compare the algorithm with known state of the art alternatives. Finally, conclusions and future works end the paper. 2. ROV MODEL AND CONTROL LAW In this section, the ROV mathematical model is rst presented, which is then required for the design of the trajectory tracking control laws. 2.1 Mathematical model The ROV mathematical model is here briey described. Due to the low speeds of the ROV and underwater currents, the ROV system can be considered as a single rigid body with six degrees of freedom, which are the three spatial positions and three orientation angles (Longhi and Rossolini (1989); Corradini et al. (2011)). Hence, as shown in Fig. 1, we consider an earth xed frame R−{O, X, Y, Z}, which describes the ROV centre of mass with the vector [x, y, z], and a body-xed frame Ra − {Oa , xa , ya , za }, which is attached to the system’s centre of mass. The translational dynamics can be expressed in terms of the centre of mass position [x, y, z]T and velocity [x, y, z] T. Assuming the roll, pitch, and yaw common notation, the ROV orientation dynamics can be expressed in function of the roll angle ϕ, pitch angle θ, yaw angle ψ, and the and the ψ. respective euler rates ϕ, θ, Generally, one can consider a time varying underwater current V c = [Vcx , Vcy , Vcz ]T , which is constant in space. The forces and moments, produced by the thrusters acting on the rigid body, are denoted with T = [Tx , Ty , Tz ]T and M = [Mx , My , Mz ]T respectively. We are interested on the tracking problem in a horizontal plane, hence we assume that the altitude, roll, and pitch dynamics are externally regulated. Without loss of generality, we can set z = z = ϕ = ϕ = θ = θ = 0. The ROV model collapses to a three degrees of freedom model, namely the translations along X, Y and the yaw rotation ψ around Z. The underwater current can be considered only in the plane, namely Vcx and Vcy , as well as the external forces, 251
251
in which we consider only Tx and Ty . The only moment component to consider is Mz , which is directly tied to the yaw angle. The resulting model can be formulated as in (1), where ψc represents the four quadrant inverse tangent of Vcy and Vcx (Corradini et al. (2011)). The parameters p1 , . . . , p9 represent constant model components, as described in (Baldini et al. (2018)). The variables Tx , Ty , and Mz are determined according to the actuator geometrical conguration. As shown in Fig. 2, we consider four thrusters with the xed angle α = π/4, while the forces are denoted with ui (i = 1, . . . , 4). Denoting τ = [Tx , Ty , Mz ]T and u = [u1 , u2 , u3 , u4 ]T , the allocation can be expressed as ⎤ ⎡ c(ψ−α) c(ψ−α) c(ψ+α) c(ψ+α) τ = Bu = ⎣s(ψ−α) s(ψ−α) s(ψ+α) s(ψ+α) ⎦ u (2) −dα dα −dα dα
where c(·) = cos(·), s(·) = sin(·), and dα = dx sα + dy cα , while dx and dy denote the ROV physical size, as in Fig. 2. Note that, for sake of simplicity, the dependence of B on ψ is omitted. 2.2 Control Law and stability
The general approach is to design the control law directly on the control model (1), generating a desired virtual input τ . Once τ is obtained, the simplest way to get the real input signal is to compute u = B −1 τ , where B −1 denotes a right inverse matrix for B. Generally, there can be multiple ways to choose B −1 . Moreover, due to constraints, the mapping between τ and u has to be solved in each time step by an on-line algorithm instead of a closed form formula. Hence, the allocation problem is generally considered as external with respect the control law design, since they are designed independently, as seen in Fig. 3. This holds true whenever the thrust allocation algorithm does not allocate the desired thrust by violating the saturation constraints. On the contrary, when saturation constraints are not satised, the control effort required by the control law is different from the actual input provided to the dynamic system, and this may lead to closed-loop system instability. Our approach thus aims at providing a thrust allocation which satises the saturation constraints, as long as it is possible (i.e., the optimization problem admits feasible solutions); at the same time, it is computationally more efficient than other standard constrained allocation techniques. In Section 4, we will consider different control laws for evaluating the proposed allocation algorithm performances in closed-loop. The control laws, in detail, are backstepping (BS), sliding mode (SMC), dynamic surface (DSC), and PID; the mathematical representations of these laws are described in our previous work (Baldini et al. (2018)). 3. OPTIMAL THRUST ALLOCATION The forces Tx , Ty , and moment Mz required by the control law must be mapped into the thrusts generated by the thrusters according to the model τ = BW u (3)
IFAC CAMS 2018 252 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
�
� 2 +V2 =T (Vcx − x) 2 + (Vcy − y) 2 + p4 x − p5 Vcx Vcx x cy � � 2 +V2 =T p1 y¨ − (p2 |sin (ψ)| + p3 |cos (ψ)|) (Vcy − y) (Vcx − x) 2 + (Vcy − y) 2 + p4 y − p5 Vcy Vcx y cy � � � � ψ − ψ � � c 2 2 + p9 = Mz p6 ψ¨ + p7 ψ �ψ � + p8 (Vcx + Vcy ) sin 2
p1 x ¨ − (p2 |cos (ψ)| + p3 |sin (ψ)|) (Vcx − x)
]
When a fault occurs in the i-th thruster, the corresponding weight satises 0 < wi < 1, thus modeling the reduced efficiency in supplying power. We do not consider here the case where thrusters possibly break down, namely the thruster failure, i.e., wi = 0 for some i ∈ {1, . . . , 4}, due to space limitation. However, it is possible to extend our result including this case, and this is part of our future work.
XPELOLFDO \DZDQJOH ᷤ ]D
LQHUWLDO IUDPH
\ 2
Let mi > 0, for i = 1, . . . , 4, be the maximal thrust allowed for the i-th thruster and let m = [m1 , . . . , m4 ]T . In order to model the physical constraints of the system, the components of u are constrained so that |ui | ≤ mi , for i = 1, . . . , 4, or more compactly −m ≤ u ≤ m (6) where inequalities are intended componentwise.
ERG\IUDPH
2D
[
[D
\D
UROODQJOH ij
(1)
SLWFKDQJOH ׇ
Now, using (3) together with (6), it is necessary to calculate the thrusts u corresponding to τ and nd the “optimal” distribution of control energy. In particular, u is the solution, provided it exists, of the following problem ⎧ ⎨ BW u = τ . (7) subject to ⎩ −m ≤ u ≤ m
Fig. 1. ROV inertial and body frames.
Note that (7) could be unfeasible, even in the case system (3) is compatible, due to the constraints (6) on u. Moreover, even when (7) is feasible, the solution is not unique, in general. This makes necessary to consider different cases and choose appropriate criteria for determining the optimal allocation for thrusts.
To this end, some preliminary considerations regarding matrix B in (3) are in order. It is possible to verify that, since α = π/4 and dα > 0, rank(B) = 3, regardless of the value of ψ ∈ R. Moreover, since W is invertible the coefficient matrix BW in (7) satises rank(BW ) = 3. (8) This makes the linear system (3) compatible for any τ ∈ R3 . However, since the solutions of (3) must satisfy the constraints (6), feasibility of (7) is affected.
Fig. 2. ROV thruster conguration.
Ŝ
Fig. 3. Overview of the control scheme.
In order to determine the optimal allocation of thrusts u, different cases must be considered, depending on the feasibility of (7). In particular, it is necessary to distinguish between two cases, namely whether (7) admits or not solutions.
where we have introduced the weighting matrix
3.1 System (7) admits solutions
W = diag{w1 , w2 , w3 , w4 }
(4)
with wi ∈]0, 1], for i = 1, ..., 4, which accounts for possible faults of thrusters, so that � 0 < wi < 1 if the i-th thruster is faulty . (5) wi = 1 if the i-th thruster is faultless 252
If system (7) admits solutions, BW u = τ is compatible and at least a solution satisfying −m ≤ u ≤ m exists. In general the solution is not unique and a we choose the one with the minimum energy, i.e., we solve the following convex optimization problem
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
⎧ ⎪ ⎪ ⎪ ⎨ ⎪ ⎪ ⎪ ⎩
2
3.2 System (7) does not admit solution
min �u� u
subject to BW u = τ −m ≤ u ≤ m
(9)
where � · � is the 2-norm of (·). The problem in (9) admits a unique solution since the objective function is strictly convex and the feasible set 1 is not empty by hypothesis. To solve (9), let us start by considering the feasible set. The solutions of BW u = τ are ∞1 , since rank(BW ) = 3, and are all given by the following formula (James (1978)), where w ∈ R4 is arbitrary � � † † u = (BW ) τ + I − (BW ) (BW ) w = t0 + k
†
†
where t0 = (BW ) τ and k ∈ ker(BW ), with (·) being the right Moore-Penrose pseudo-inverse. T
Dening q = [1, −1,� −1, 1]� , it is possible to verify 2 that ker (BW ) = span W −1 q , where span (·) is the linear space generated by the vectors in the argument. Thus, any vector k ∈ ker (BW ) has the form k = γW −1 q, with γ ∈ R. Taking into account this result, all the solutions of BW u = τ can be rewritten as λ ∈ R. (10) u = t0 + γW −1 q The feasible set of (9) is given by the subset of vectors (10) that satisfy the constraints −m ≤ u ≤ m. These last entail corresponding constraints on the parameter γ, so that it is possible to prove that (11) −m ≤ t0 + γW −1 q ≤ m hold true whenever γ satises (12) γ ∈ [γmin , γmax ] with � γmin = max {W (−q ◦ t0 − m)} (13) γmax = min {W (−q ◦ t0 + m)} where ◦ is the Hadamard product (Davis (1962)), and the maximum (minimum) of a vector is intended as the maximum (minimum) of its entries. Finally, the feasible set of (9) is given by � � F = u ∈ R4 � � u = t0 + γW −1 q, with γ ∈ [γmin , γmax ] (14) where all the terms are dened above. Moreover, since � �2 2 2 (15) �u� = �t0 � + γ 2 �W −1 q � , as t0 is orthogonal to ker (BW ), we have that solving (9) is equivalent to determine γ ∈ [γmin , γmax ] such that (15) is minimum. It is possible to verify that the optimal value of γ, namely γ ∗ , is given by ⎧ ⎨γmin , if 0 < γmin γ∗ = 0 (16) , if γmin ≤ 0 ≤ γmax ⎩ γmax , if 0 > γmax and thus the optimal solution of (9) is given by †
u = (BW ) τ + γ ∗ W −1 q ∗ where γ is dened in (16). 1 2
253
(17)
If system (7) does not admit solutions, none of the ∞1 solutions 3 of BW u = τ , as given by (10), satises the constraints −m ≤ u ≤ m. A viable alternative in this case is to nd u by solving the following optimization problem ⎧ 2 ⎪ �BW u − τ � ⎨min u . (18) subject to ⎪ ⎩ −m ≤ u ≤ m Note that (18) admits a unique solution, since the feasible set is not empty and the objective function is strictly convex on the feasible set. Moreover, the solution u is such that for at least one of its components the constraint is active, i.e., there exists k ∈ {1, . . . , 4} such that equality in the constraint is achieved uk = −mk or uk = mk , where uk is the k-th component of u. Indeed, if this were not the case u would also be solution of the unconstrained 2 problem min �BW u − τ � and thus solution of BW u = u τ , but this is not possible since, by hypothesis none of the solutions of BW u = τ satises the constraints −m ≤ u ≤ m. So, let us denote by u the vector collecting (all) the components of u for which the constraint is active, i.e., uk = −mk or uk = mk . Moreover, denote by S the � the selection 4 matrix that guarantees u = Su and by S complementary selection matrix obtained from the 4 × 4 identity matrix I 4 by removing the rows of S. It is easy � satisfy S �T S � + ST S = I 4. to verify that that S and S With the above positions, it is possible to prove that the (unique) solution of (18) is given by the following expression � �† � � T � � T BW (19) τ − BW u + S u u=S � = WS � T , W = W S T , and where W � � −S 1 u = Sm = m S2
(20)
where S 1 , S 2 are selection matrices dened such that S 1 singles out the entries of u that achieve the lower bound, and S 2 singles out the entries achieving the upper bound. Any of S 1 and S 2 can be possibly void, in the case only the upper, or lower, bound is achieved, but not both. Note that (19) gives the structure of the solution and, provided that u is determined in some way, gives the optimal thrust allocation. So, in order to calculate the solution it is sufficient to cycle among all the admissible congurations where some components of u saturate, thus achieving the bounds, and then choosing the conguration 2 that minimizes �BW u − τ � . The admissible congurations are nite and it is possible to prove that they are 34 − 9, since some congurations cannot occur due to the nature of the problem. In this way, the solution is easily determined by means of (19). 3
Note that the system BW u = τ is always compatible. We dene selection matrix a submatrix of the identity matrix obtained by removing some rows.
4
This is the set of vectors satisfying the constraints. Indeed dim (ker (BW )) = 1 and Bq = 0.
253
IFAC CAMS 2018 254 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
4. NUMERICAL SIMULATION
Table 1. Testing results for the proposed allocation algorithm.
In this section, a comparison between the classical MP pseudo-inverse and the proposed approach is presented. As rst, we provide an empirical evidence of the optimality of the proposed solution, comparing the results of MP, the proposed one (OPT) and the Matlab QP solver (QP). Then the proposed algorithm has been deployed within a ROV simulator for closed-loop trajectory tracking, in order to evaluate the impact on realistic applications, in case of different dynamics controllers. 4.1 Randomly generated input samples Every algorithm has been executed in Matlab 100.000 times, with realistic random τ input samples, namely whose values can be feasibly allocated to the thrusters in the fault free case, and considering up to three faults thereafter. The timing on a personal computer (i7-6700K, 4.2 GHz) has been recorded as well. In Table 1, the worst case and average computation time is reported for every considered allocation algorithm. In particular, the physical relation between the power consumption in Table 1 and the instantaneous thrust is expressed by (Johansen et al. (2004)) 3/2
3/2
3/2
3/2
(21) P = k1 |u1 | + k2 |u2 | + k3 |u3 | + k4 |u4 | where k1 , k2 , k3 , k4 are constants that bind the power to the thrust for each propeller. Supposing that the propellers have the same characteristics, it is possible to assume ¯ for an arbitrary positive that k1 = k2 = k3 = k4 = k, ¯ Hence, the consumption can be estimated by constant k. T the dimensionless quantity 0 P (t)/ k¯ dt. The MP algorithm, which gives a non-optimal solution, is the fastest one; the proposed algorithm is slower than the MP algorithm, but is one order of magnitude faster than the Matlab QP solver, which is based on interior point method. This holds for both average and worst case running time.
In the feasible case (i.e., at least one algorithm has found a solution such that τ = BW u and u is feasible with respect to the saturation limits), the MP solution has the lowest average consumption, but this is obtained renouncing to optimality. In fact, the average error norm is not null, because some of the solutions are beyond the saturation limits. The proposed approach and the QP solver both return feasible solutions, and their accuracy and average consumption are comparable. In particular, the accuracy of the QP solver can be imposed by modifying the optimality tolerance, whereas the accuracy of the MP and OPT methods are determined by the oating point accuracy of the data. In the unfeasible case, the MP solution involves both higher consumption and average error norm. In fact, it initially ignores the saturation limits, and then truncates the result to satisfy them, introducing an error. The proposed solution and the QP solver returns almost the same results. 4.2 Reference trajectory tracking Twenty complete simulation scenarios for the SNAMprogetti ROV have been considered, with different reference 254
Running time Feasible case Unfeasible case
Worst case [ms] Avg. case [ms] Avg. consumption Avg. error norm Avg. consumption Avg. error norm
MP
OPT
QP
4.888 0.0376 4055.6 14.4273 3810.0 2618.5
9.116 0.240 4072.0 3 · 10−12 3715.0 2362.2
100.409 2.711 4073.7 5 · 10−11 3715.0 2362.2
Table 2. IAE and consumption with different allocation and control laws. Avg. IAE (MP) Avg. IAE (OPT) IAE difference Avg. cons. (MP) Avg. cons. (OPT) Cons. difference
BS
SMC
PID
DSC
22.945 14.179 −38.20% 2.929 3.032 +3.52%
42.566 32.262 −24.21% 2.994 3.092 +3.28%
46.052 38.630 −16.12% 3.047 3.158 +3.65%
43.330 35.969 −16.99% 3.027 3.138 +3.67%
trajectories, initial conditions and fault occurrences. The control scheme is reported in Fig. 3. Fault detection and diagnosis are supposed to be ideal, i.e., perfect accuracy and negligible detection time; for the purpose of this paper, this assumption simplies the analysis of the results. In order to quantify the tracking performance, we dene the Integral of Absolute Error (IAE) index as follows T 1 |x − xd | + |y − yd | + |ψ − ψd | dt (22) IAE = 10 0 where the coefficient 1/10 is introduced to equally weigh an error of 10◦ of the yaw angle and an error of 1 m along the X or Y axis. As it can be seen in Table 2, every control law takes advantage from the introduction of the proposed method in simulation scenarios. The IAE index is drastically reduced with the use of the proposed technique, in a range between −16.12% for the PID controller and −38.20% for the backstepping controller. The energy consumption is marginally higher, as expected: the use of an improved control allocation allows to exploit more efficiently the actuators in the neighbourhood of the saturation limits, leading to a larger control effort. In fact, when the classical MP returns an unattainable control effort, an elementwise saturation occurs, essentially trading the correctness of the solution with a faster computation time. The small increase in the consumption is acceptable, indeed, if the increase in the tracking performances is taken into account. 5. CONCLUSIONS The thrust allocation offers a series of advantages for the class of Unmanned Underwater Vehicles. In fact, the thrust allocation allows to design the motion control law without the detailed knowledge about the actuators, neglecting the actuator saturation limits, and despite of possible actuator faults. The objective of the present paper is to study and solve the constrained thrust allocation problem for the class of over-actuated ROVs. Exploiting and improving the results of our previous works (Fasano et al. (2015); Ciabattoni et al. (2016)), here we consider the thrust allocation problem taking into account the thruster saturation limits and the possible actuator faults. In detail,
IFAC CAMS 2018 Opatija, Croatia, September 10-12, 2018 Alessandro Baldini et al. / IFAC PapersOnLine 51-29 (2018) 250–255
we solve the optimization problem both in case the main problem admits solution, and also in case it does not admit solutions, using a viable alternative. The proposed thrust allocation algorithm is tested on a ROV with real parameters, in combination with different control laws, to evaluate the proposed allocation algorithm performances in closed-loop. The algorithm has been compared with both the saturated pseudo-inverse method and the solution of the QP solver: the algorithm achieves the same accuracy of the QP solver, with a running time that is one magnitude lower in both the average case and the worst case, while the saturated MP pseudo-inverse is marginally faster, but leads to poorer performances. Finally, the algorithm is validated in simulation, comparing the tracking performances of the proposed algorithm with the saturated MP pseudo-inverse method. The differences in the IAE with every control law justify the use of the proposed algorithm in the applications, in order to achieve a better redistribution of the control effort among the thrusters. We are still working on this problem and we are developing the solution of the thrust allocation problem taking into account the saturation limits, the thrusters’ faults, and also the thrusters’ failures. REFERENCES Antonelli, G. (2003). A survey of fault detection/tolerance strategies for AUVs and ROVs. Fault Diagnosis and Fault Tolerance for Mechatronic Systems:Recent Advances, 109–127. Baldini, A., Ciabattoni, L., Felicetti, R., Ferracuti, F., Freddi, A., and Monteri , A. (2018). Dynamic surface fault tolerant control for underwater remotely operated vehicles. ISA Transactions, 78, 10–20. Berge, S.P. and Fossen, T.I. (1997). Robust control allocation of overactuated ships; experiments with a model ship. In Proc. 4th IFAC Conf. Manoeuvring and Control of Marine Craft, 166–171. Citeseer. Bodson, M. (2002). Evaluation of optimization methods for control allocation. Journal of Guidance, Control, and Dynamics, 25(4), 703–711. Chen, Y. and Wang, J. (2012). Fast and global optimal energy-efficient control allocation with applications to over-actuated electric ground vehicles. IEEE Transactions on Control Systems Technology, 20(5), 1202–1211. Ciabattoni, L., Fasano, A., Ferracuti, F., Freddi, A., Longhi, S., and Monteri` u, A. (2016). A thruster failure tolerant control scheme for underwater vehicles. In 12th IEEE/ASME International Conference on Mechatronic and Embedded Systems and Applications, 1–6. IEEE.
255
255
Corradini, M.L., Monteri` u, A., and Orlando, G. (2011). An actuator failure tolerant control scheme for an underwater remotely operated vehicle. IEEE Transactions on Control System Technology, 19(5), 1036–1046. Davis, C. (1962). The norm of the Schur product operation. Numerische Mathematik, 4(1), 343–344. Durham, W.C. (1993). Constrained control allocation. Journal of Guidance, Control, and Dynamics, 16(4), 717–725. Enns, D. (1998). Control allocation approaches. In Guidance, Navigation, and Control Conference and Exhibit. Boston, Massachusetts, USA. Fasano, A., Ferracuti, F., Freddi, A., Longhi, S., and Monteri` u, A. (2015). A virtual thruster-based failure tolerant control scheme for underwater vehicles. IFACPapersOnLine, 48(16), 146–151. Harkegard, O. (2002). Efficient active set algorithms for solving constrained least squares problems in aircraft control allocation. In Proceedings of the 41st IEEE Conference on Decision and Control, volume 2, 1295– 1300. IEEE. James, M. (1978). The generalised inverse. The Mathematical Gazette, 62(420), 109–114. Johansen, T.A. and Fossen, T.I. (2013). Control allocation - a survey. Automatica, 49(5), 1087–1103. Johansen, T.A., Fossen, T.I., and Berge, S.P. (2004). Constrained nonlinear control allocation with singularity avoidance using sequential quadratic programming. IEEE Transactions on Control Systems Technology, 12(1), 211–216. Khelassi, A., Weber, P., and Theilliol, D. (2010). Recongurable control design for over-actuated systems based on reliability indicators. In IEEE Conference on Control and Fault-Tolerant Systems (SysTol), 365–370. Lindfors, I. (1993). Thrust allocation method for the dynamic positioning system. In 10th International Ship Control Systems Symposium (SCSS’93), 3–93. Longhi, S. and Rossolini, A. (1989). Adaptive control for an underwater vehicle: Simulation studies and implementation details. IFAC Workshop on Expert Systems and Signal Processing in Marine Automation, 271–280. Oppenheimer, M.W., Doman, D.B., and Bolender, M.A. (2006). Control allocation for over-actuated systems. In 14th Mediterranean Conference on Control and Automation, 1–6. IEEE. Sørdalen, O. (1997). Optimal thrust allocation for marine vessels. Control Engineering Practice, 5(9), 1223–1231. Webster, W. and Sousa, J. (1999). Optimum allocation for multiple thrusters.