- Email: [email protected]
UAV control with switched GNSS-Estimator navigation system∗
Proceedings of the 2015 on Advanced and Navigation Aerospace Proceedings of for theAutonomous 2015 IFAC IFAC Workshop Workshop on Vehicles Advanced Control Control and Autonomous Aerospace Proceedings of for theSeville, 2015 IFAC Workshop on Vehicles Advanced Control June 10-12, 2015. Spain and Navigation Navigation for Autonomous Aerospace Vehicles Available online at www.sciencedirect.com JuneNavigation 10-12, 2015. 2015. Seville, Spain Aerospace Vehicles and forSeville, Autonomous June 10-12, Spain June 10-12, 2015. Seville, Spain
ScienceDirect
IFAC-PapersOnLine 48-9 (2015) 126–131
UAV control with switched UAV control with switched UAV control with switched GNSS-Estimator navigation system UAV control with switched GNSS-Estimator navigation system GNSS-Estimator navigation system GNSS-Estimator navigation system ∗,∗∗ ∗∗,∗∗∗
Boris Andrievsky ∗,∗∗ Nikolay Kuznetsov ∗∗,∗∗∗ ∗,∗∗∗∗Nikolay Kuznetsov ∗∗ ∗∗,∗∗∗ Boris Andrievsky Boris Andrievsky Kuznetsov Gennady Leonov Svetlana Seledzhi ∗∗ ∗,∗∗∗∗Nikolay ∗∗,∗∗∗ Boris Andrievsky Nikolay Kuznetsov ∗∗ Gennady Leonov Svetlana Seledzhi Gennady Leonov ∗∗ Svetlana Seledzhi ∗∗ ∗∗ Gennady Leonov Svetlana Seledzhi ∗ ∗ Institute for Problems of Mechanical Engineering of RAS, ∗ Institute for Problems of Mechanical Engineering of RAS, of Mechanical Engineering of RAS, Saint Petersburg, Russia, ∗ Institute for Problems Institute for Problems of Mechanical Engineering of RAS, Saint Petersburg, Russia, Saint Petersburg, Russia, e-mail: [email protected] Saint Petersburg, Russia, e-mail: [email protected] ∗∗ e-mail: [email protected] Saint Petersburg State University, ∗∗ e-mail: [email protected] ∗∗ Saint Petersburg State University, Petersburg StateSaint University, 28 Universitetsky prospekt, 198504, Petersburg, Russia, ∗∗ Saint Saint Petersburg State University, 28 Universitetsky prospekt, 198504, Saint Petersburg, 28e-mail: Universitetsky prospekt, 198504, Saint Petersburg, Russia, Russia, [email protected], [email protected] 28 Universitetsky prospekt, 198504, Saint Petersburg,Finland Russia, e-mail: [email protected], [email protected] ∗∗∗ e-mail: [email protected], [email protected] University of Jyv¨ a skyl¨ a , PO Box 35, FI-40014, ∗∗∗ e-mail: [email protected], [email protected] ∗∗∗ University of Jyv¨ a skyl¨ a ,, PO Box 35, FI-40014, Finland University of Jyv¨ a skyl¨ a PO Box 35, FI-40014, Finland ∗∗∗ University of Jyv¨ askyl¨ a, PO Box 35, FI-40014, Finland Abstract: In the paper the switched GNSS-Estimator navigation system, recently proposed by Abstract: paper GNSS-Estimator system, recently by Abstract: Inisthe the paper the the switched GNSS-Estimator navigation system, recentlyofproposed proposed by the authors,In described andswitched numerically studied in thenavigation framework of evaluation the overall Abstract: In the paper the switched GNSS-Estimator navigation system, recently proposed by the authors, is described and numerically studied in the framework of evaluation of the overall the described and numerically studied in the framework of evaluation of the overall UAVauthors, control issystem accuracy. the authors, is described and numerically studied in the framework of evaluation of the overall UAV control system accuracy. UAV control system accuracy. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. UAV control system accuracy. Keywords: UAV, onboard algorithms, guidance, navigation, flight control Keywords: Keywords: UAV, UAV, onboard onboard algorithms, algorithms, guidance, guidance, navigation, navigation, flight flight control control Keywords: UAV, onboard algorithms, guidance, navigation, flight control 1. INTRODUCTION more detailed and accurate UAV dynamics model than 1. more accurate dynamics model than 1. INTRODUCTION INTRODUCTION more detailed detailed and accurate UAV UAV dynamics model than commonly usedand for GNSS/INS Kalman filtering is needed. 1. INTRODUCTION more detailed and accurate UAV dynamics model than commonly used for GNSS/INS Kalman filtering is needed. commonly used et foral., GNSS/INS Kalman filtering is for needed. (Andrievsky 2013), a switching algorithm data At present, the Global Navigation Satellite System (GNSS) In commonly used for GNSS/INS Kalman filtering is needed. In (Andrievsky et al., 2013), a switching algorithm for At present, the Global Navigation Satellite System (GNSS) In (Andrievsky et al.,navigation 2013), a switching for data data of a satellite system algorithm and standard onAt present, the Global Satellite System (GNSS) fusion is very widely used inNavigation a lot of navigation applications, In (Andrievsky et low-cost al.,navigation 2013), a switching algorithm for data fusion of system and standard onAt present, the Global Satellite System (GNSS) is very widely used in aacontrol lot navigation applications, of aa satellite satellite navigation system andvehicle standard onboard sensors for unmanned aerial (UAV) is very widely used and inNavigation lot of of of navigation applications, including navigation unmanned aerial vehi- fusion fusion of a satellite navigation system and standard onboard sensors for low-cost unmanned aerial vehicle (UAV) is very widely used in a lot of navigation applications, including navigation and control of unmanned aerial vehiboard sensors for low-cost unmanned aerial vehicle (UAV) is proposed and numerically studied by the example of including navigation of unmanned aerial vehicles (UAV). However,and thecontrol GNSS alone cannot provide the board sensors for low-cost unmanned aerial vehicle (UAV) is proposed and numerically studied by the example of including navigation and control of unmanned aerial vehicles (UAV). However, the GNSS alone cannot provide the is proposed and small numerically studiedUAV. by the example of hypothetical jet-propelled In the present cles However, alonedata cannot provide the the UAV(UAV). autopilot with allthe theGNSS necessary of fast variables is proposed and numerically studied by the example of the hypothetical small jet-propelled UAV. In the present cles (UAV). However, the GNSS alone cannot provide the UAV autopilot with all the necessary data of fast variables the hypothetical small jet-propelled In the present the data fusion algorithm of UAV. (Andrievsky et al., UAV with alllow thedata necessary dataa of fast variables due toautopilot the relatively rate and possible loss of paper the hypothetical small jet-propelled UAV. In the present paper the data fusion algorithm of (Andrievsky et al., UAV autopilot with all the necessary data of fast variables due to the relatively low data rate and a possible loss of paper the data fusion algorithm of (Andrievsky et al., 2013) is considered in the framework of the closed-loop due to the low data rate and1997; a possible lossand of signals fromrelatively the satellites (Strachan, Mutuel theconsidered data algorithm of errors, (Andrievsky et the al., 2013) is the framework of closed-loop due to the relatively low data rate and1997; aThe possible lossand of paper signals from the satellites (Strachan, 1997; Mutuel and 2013)control is considered in the framework of the the closed-loop UAV and fusion the in UAV trajectory caused by signals from the satellites (Strachan, Mutuel Speyer, 2000; Blomenhofer, 1996–1997). typical 2013) is considered in the framework of the closed-loop UAV control and the UAV trajectory errors, caused by the signals from the satellites (Strachan, 1997; Mutuel Speyer, 2000; Blomenhofer, 1996–1997). The typical and UAV control the UAVdata, trajectory errors, caused by the of the and navigation are numerically evaluated Speyer, 2000; Blomenhofer, 1996–1997). Theoftypical and errors most indisputable solution lies in the fusion data from control thetheir UAVdata, trajectory errors, caused by the errors of navigation are numerically evaluated Speyer, 2000; Blomenhofer, 1996–1997). The and UAV most solution lies in the of data from errors of the the and navigation data, are on numerically evaluated for examination of influence the overall system most indisputable solution lies in the fusion fusion oftypical data(INS). from GNSSindisputable and onboard integrated navigation system errors of the navigation data, are on numerically evaluated for examination of their influence the overall system most indisputable solution lies in the fusion of data from GNSS and onboard integrated navigation system (INS). for examination of their influence on the overall system performance. GNSS integrated system For thisand aim,onboard the Kalman filteringnavigation is commonly used. (INS). In the for examination of their influence on the overall system performance. GNSS and system (INS). For this aim, the is used. In For this(Andrievsky aim,onboard the Kalman Kalman filtering is commonly used. In the the performance. paper etintegrated al.,filtering 2013)navigation ancommonly alternative approach The rest of the paper is organized as follows. The navperformance. For this aim, the Kalman filtering is commonly used. In the paper (Andrievsky et al., 2013) an alternative approach paper (Andrievsky et al., 2013) an alternative approach for low-cost UAVs was proposed, assuming absence of the The of is follows. navThe rest restalgorithm of the the paper paper is organized organizedetas asal., follows. The navigation of (Andrievsky 2013) The is briefly paper (Andrievsky et al., 2013)for an alternative for UAVs proposed, assuming absence of for low-cost UAVs was proposed, assuming absence of the the The INSlow-cost onboard. The was requirement continuity is approach provided rest of the paper is organized as follows. The navigation algorithm of (Andrievsky et al., 2013) is briefly igation algorithm of (Andrievsky et al., 2013) is briefly described in Sec. 2. Accuracy evaluation of UAV control for low-cost UAVs was proposed, assuming ofThe the igation INS onboard. The requirement for continuity is provided provided INS onboard. The requirement for continuity is by the Kalman filter and customary onboardabsence sensors. algorithm of (Andrievsky et al., 2013) is briefly described in Sec. 2. Accuracy evaluation of UAV control described in Sec. 2. Accuracy evaluation of UAV with switched navigation system is presented in control Sec. 3. INS onboard. The continuity is provided by the Kalman filter and onboard sensors. The by the Kalman filterrequirement and customary customary onboard sensors. The described Kalman estimator receives datafor from the sensors (gyroin Sec. 2. Accuracy evaluation of UAV control with navigation system is Sec. 3. with switched switched navigation system is presented presented ingiven Sec. in 3. Suplementary information on the UAV model isin by the Kalman filter and customary onboard sensors. The Kalman estimator receives data from the sensors (gyroKalman estimator receives datasensors, from the (gyro- with scopes, accelerometers, altitude etc.)sensors and provides switched navigation system is presented ingiven Sec. in 3. Suplementary information on the UAV model is Suplementary information on the UAV model is given in Appendix A. Kalman estimator receives data from the sensors (gyroscopes, accelerometers, altitude sensors, etc.) and provides scopes, accelerometers, altitude sensors, etc.) andestimates provides Suplementary the guidance system with high-speed UAV state information on the UAV model is given in Appendix A. Appendix A. scopes, accelerometers, altitude sensors, etc.) and provides the guidance system high-speed UAV state the guidance system with high-speed UAV state estimates at the rate that is with necessary for guidance andestimates control Appendix A. the guidance system with high-speed UAV state estimates at the rate that for 2. UAV NAVIGATION METHOD USING GNSS AND at the the ratetrajectory. that is is necessary necessary for guidance guidance and control along GNSS signals are usedand to control update 2. METHOD USING at ratetrajectory. that for guidance and along the GNSS signals are to update 2. UAV UAV NAVIGATION NAVIGATION METHOD USING GNSS GNSS AND AND KALMAN ESTIMATOR along the trajectory. GNSS signals are used used to control update the the estimates at is thenecessary instants (possibly random) when 2. UAV NAVIGATION METHOD USING GNSS AND KALMAN ESTIMATOR along the trajectory. GNSS signals are used to update the estimates at the instants (possibly random) when KALMAN ESTIMATOR the estimates at the instants (possibly GNSS is accessible. Time sampling time of random) renovationwhen can KALMAN ESTIMATOR the estimates at the (possibly when GNSS is Time sampling time of renovation can GNSS is accessible. accessible. Time sampling time used of random) renovation can Let us consider a small-size UAV, equipped with the significantly exceed theinstants sampling time for Kalman Let us consider small-size UAV, with GNSS is accessible. Time sampling time of renovation can significantly exceed the sampling time used for Kalman Let us consider small-size UAV, equipped equipped with the the sensors:aa the GNSS receiver; the gyro-sensors significantly exceed the sampling time used for Kalman following filtering. Let us consider a small-size UAV, equipped withangle the following sensors: the GNSS receiver; the gyro-sensors significantly exceed the sampling time used for Kalman filtering. following sensors:(pitch the GNSS receiver; the ψ, gyro-sensors for Euler angles angle ϑ, yaw angle roll filtering. sensors:(pitch receiver; the gyro-sensors for Euler angles angle ϑ, yaw angle ψ, roll angle 1 the GNSS It is worth mentioning that in the series of papers, de- following filtering. for Euler angles (pitch angle ϑ, yaw angle ψ, roll angle γ) measurement ; the rate gyros for ω , ω , ω ; x y z the 1 It is mentioning in of deEuler angles (pitch ϑ, yaw angle roll 1 ; theangle γ) measurement rate gyros for ω ,,jψ, ω ,, ω ; the It is worth worth mentioning that in the the series series of papers, papers, de- for voted to navigation of that autonomous aerial, underwater x y zangle γ) measurement ; the rate gyros for ω ω ω barometric altimeter; g-meters for j , j , ; the rudder x y z ; the x y z 1 It worth mentioning in the series of de- γ) voted to of autonomous aerial, underwater measurement ; the rate gyros for ω , ω , ω ; the barometric altimeter; g-meters for j , j , j ; the rudder voted to navigation navigation of that autonomous aerial, underwater andis ground vehicles, the vehicle dynamics arepapers, taken into x y z x y z barometric g-meters for jxcontrolling , jy , jz ; the inputs: rudder sensors for altimeter; δe , δr , δa .the UAV has four voted to navigation of autonomous and ground vehicles, the vehicle are taken into barometric altimeter; the g-meters for j , j , j ; the rudder sensors for δ , δ , δ . UAV has four controlling inputs: and ground vehicles, the vehicle ofdynamics dynamics areunderwater taken into the consideration for enhancement the aerial, existing INS perx y z e r a sensors for δe , δr ,and δa . differential UAV has four controlling inputs: symmetrical taileron deflection (δa , and groundcfvehicles, theetvehicle are taken into the consideration for of the existing INS persensors δe , δrdeflection ,and δa . differential UAV(δhas four controlling inputs: taileron deflection (δ consideration for enhancement enhancement ofdynamics theKoifman existing INS Barperformance, (Koifman al., 1995; and the symmetrical and differential taileron deflection (δaa ,, δe ), symmetrical the for rudder the engine throttle r ), and consideration for enhancement of the existing INS performance, cf (Koifman et al., 1995; Koifman and Barsymmetrical and differential taileron deflection δδ(thrust ), the rudder deflection (δ ), and the engine throttle formance, cf (Koifman et al., 1995;1994; Koifman andetBarItzhack, 1999; Algrain and Saniie, Perera al., the a, e ), the position rudder δdeflection (δrr ), and theprocessing engine throttle data of (δ the e th ). Preliminary formance, cf (Koifman et al., 1995;2011; Koifman andet Itzhack, 1999; Algrain and Saniie, 1994; Perera al., ), the rudder deflection (δ ), and the engine throttle position δ ). Preliminary data processing of the Itzhack, 1999; Algrain and Saniie, 1994; Perera etBaral., δ(thrust 2010; Hegrenaes and Hallingstad, Sazdovski and e r th position δth ). Preliminary processing of the GNSS signals is realized by means ofdata a separate algorithm, Itzhack, 1999; Algrain and Saniie, 1994; Morgado Perera et al., (thrust 2010; and Sazdovski (thrust position δth ). Preliminary data processing of the GNSS signals is realized by means of aa separate algorithm, 2010; Hegrenaes and Hallingstad, Hallingstad, 2011; Sazdovski and Silson,Hegrenaes 2011; Vasconcelos et al., 2011,2011; 2010; etand al., GNSS signals is realized by means of separate algorithm, converting the pseudorange measurements from the GNSS 2010; Hallingstad, Sazdovski Silson, 2011; et al., 2011, 2010; et al., signals ispseudorange realized by means of aposition separate algorithm, converting the measurements from the GNSS Silson, 2011; Vasconcelos etof al.,(Andrievsky 2011,2011; 2010; Morgado Morgado etand al.,a GNSS 2013). Hegrenaes To useVasconcelos the and method et al., 2013), converting the pseudorange measurements from the GNSS receiver into the estimates of UAV in a certain Silson, 2011; al.,(Andrievsky 2011, 2010; Morgado et al.,aa converting 2013). use the et thethe pseudorange from GNSS receiver into estimates of in a 2013). To To useVasconcelos the method methodetof of (Andrievsky et al., al., 2013), 2013), into the estimates measurements of UAV UAV position position in the a certain certain 2013). To use the method of (Andrievsky et al., 2013), a 1receiver receiver into the estimates of UAV in a certain The Russian notations are used in this position paper. They differ from This work was supported by Russian Scientific Foundation (project 1
notations are this They differ This work work was was by Scientific Foundation This 1 The those of Russian the ISO 1151-5 standard, seein Appendix A for the definitions. 14-21-00041) andsupported Saint-Petersburg State University. The Russian notations are used used in this paper. paper. They differ from from supported by Russian Russian Scientific Foundation (project (project 1 The those of the standard, see Appendix A the 14-21-00041) and Saint-Petersburg State University. notations are used this paper. They differ from This work was by Russian Scientific Foundation (project those of Russian the ISO ISO 1151-5 1151-5 standard, seein Appendix A for for the definitions. definitions. 14-21-00041) andsupported Saint-Petersburg State University. those of the ISO 1151-5 standard, see Appendix A for the definitions. 14-21-00041) and Saint-Petersburg State University. Copyright © 2015 IFAC 126 2405-8963 © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2015 IFAC 126 Copyright 2015 responsibility IFAC 126Control. Peer review© of International Federation of Automatic Copyright ©under 2015 IFAC 126 10.1016/j.ifacol.2015.08.071
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
Earth-fixed frame (x, h, z) and UAV velocity with respect to the Earth (Vk ). These data are used for renovation of the Kalman estimates at time instants {tK }. It is assummed below that tK = K · TS (K = 0, 1, 2, . . ., where TS is a sampling time, TS Ts . The value TS is not used for the estimator algorithm design, therefore the case when the instants {tK } are not equally spread is also possible: tK = tK−1 + TS (K), say, with the random increments TS (K). The process of the estimator design has several phases. Firstly, the linearized UAV model is obtained. Secondly, the extended model, including the model of disturbances is obtained. Thirdly by means of the discretization on time with given sample period Ts the discrete-time extended model is found. This leads to the switched navigation algorithm (Tanwani et al., 2013; Balluchi et al., 2012). The Kalman-estimator design technique is applied to this model and the discrete-time estimator equations are finally obtained. Extended UAV model involves the UAV dynamics equations and the internal model of disturbances (where measuring errors: position, velocity, attitude errors, gyro drift, accelerometer bias, etc. can be added). In the present study only the wind disturbances are included into the extended model, more specifically, the systematic components of the wind velocity vector W = col{Wx , Wy , Wz } are taken into account. Assuming they are constants, one ˙ x = 0, gets the following internal model of disturbances: W ˙ ˙ Wy = 0, Wz = 0. Two extended UAV models are used. The first one (“Model-I”) is aimed to be a constituent of the real-time estimation algorithm, and the next one (“Model-II”) is used for the estimator design. Model-I is used for the UAV state and disturbances estimation inside the intervals between the corrections based on GNSS signals. In compliance with the given above list of sensors, output vector yI of this model can be taken as yI = col{ωx , ωy , ωz , h, ϑ, γ, ψ, jx , jy , jz }. Measurable input vector is taken as uI = col{δe , δr , δa , Vˆk }, where Vˆk stands for the velosity estimate, obtained by means of the extrinsic algorithm. Thus, the state-space equations of the Model-I x˙ I (t) = AI xI (t)+BI uI (t), yI (t) = CI xI (t)+DI uI (t) have matrices AI , BI , CI , DI in the following block form: A2,...,12 | B(2,...,12),(4,5,6) ∈ R14×14 , AI = 03×14 B(2,...,12),(1,2,3) | A(2,...,12),1 ∈ R14×3 , BI = 03×4 CI = C(4,5,6,8,10,11,12,15,16,17),(2,...,12) | . D(4,5,6,8,10,11,12,15,16,17),(4,5,6) ∈ R10×14 , DI = D(4,5,6,8,10,11,12,15,16,17),(1,2,3) | (1) C(4,5,6,8,10,11,12,15,16,17),1 ∈ R10×3 ,
where indices show the numbers of the rows and columns of the elements of corresponding matrices in (A.5) that are 127
127
used in Model-I. The “shift” ϕI for this model is found as ∗ ∗ x 2,...,12 u 1,2,3 f (x∗ , u∗ ) 2,...,12 − AI − BI . ϕI = ∗ 03×1 03×1 x1
Model-II is used for computation of the Kalman estimator feedback gain matrix. It has to satisfy the observability condition and only measurable outputs can be used in this model and only observable states can be estimated. It should be mentioned that Model-II, as itself, is not destined for getting the state estimates. It is used only in order to obtain the estimator feedback gain matrix. Therefore, this model can be obtained by decomposition into the observable/unobservable subspaces (or, in the other words, by transformation to the Observability Staircase Form). For the considered problem the matrices of ModelII state-space equations can be written as follows:
AII =
A2,4,5,6,8,10,11,12 | B(2,4,5,6,8,10,11,12),(4,5,6)
BII =
03×11 B(2,4,5,6,8,10,11,12),(1,2,3) 03×3
,
∈ R11×3 ,
CII = C(4,5,6,8,10,11,12,15,16,17),(2,4,5,6,8,10,11,12) | D(4,5,6,8,10,11,12,15,16,17),(4,5,6) ∈ R10×11 , DII = D(4,5,6,8,10,11,12,15,16,17),(1,2,3) .
(2)
Data processing in the considered systems has a discrete mode, so the discrete-time models have to be found. Assuming the input signal u(t) to be constant between the sampling instants tk = kTs (k = 0, 1, 2, . . .), one gets the following discrete-time form of (A.5): x[k + 1] = P x[k]+Qu[k]+Ts ϕ, y[k] = Cx[k]+Du[k], (3) and from u[k] ≡ u(tk ) and x[0] = x(t0 ) follows that x[k] ≡ x(tk ) and y[k] ≡ y(tk ), where P = exp(ATs ), Ts Q= exp(Aτ )dτ B. 0
Applying this procedure to Models-I,II, one gets them in the form of difference equations, respectively: Model-I/D: xI [k + 1] = PI xI [k] + QI u[k] + Ts ϕ, (4) yI [k] = CI xI [k] + DI u[k], Model-II/D: xII [k + 1] = PII xI [k] + QII u[k], (5) yII [k] = CII xII [k] + DII u[k], Ts where Pi = exp(Ai Ts ), Qi = exp(Ai τ )dτ Bi , i=I,II. 0
The state, output and input vectors of these models are as follows: xI = col{θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ, Wx , Wy , Wz }∈ R14 , yI = yII = col{ωx , ωy , ωz , h, ϑ, γ, ψ, jx , jy , jz } ∈ R10 , xII = col{θ, ωx , ωy , ωz , h, ϑ, γ, ψ, Wx , Wy , Wz } ∈ R11 , uI = col{δe , δr , δa , Vˆk } ∈ R4 , uII = col{δe , δr , δa } ∈ R3 .
IFAC ACNAAV 2015 128 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
Discrete full-order state estimator (Kalman Estimator, State Observer) for the plant model x[k + 1] = P x[k] + Qu[k], y[k] = Cx[k] + Du[k] is as follows: x ˆ[k + 1] = P x ˆ[k] + Qu[k] + L[k](y[k] − yˆ[k]), yˆ[k] = C x ˆ[k] + Du[k],
(6)
where x ˆ[k] denotes the estimate of the plant state vector x(tk ) at the kth instant (for real-time systems, tk = kTs , Ts is a sample time). The problem of the estimator design lies in finding the appropriate gain matrix function L[k] ∈ Rn×m . 3. ACCURACY OF UAV CONTROL WITH SWITCHED NAVIGATION SYSTEM The UAV flight accuracy depends on the navigation errors and on the dynamics of the closed-loop system “UAV/controller” as well. In this study the most attention is paid to the autonomous flight pass control, since in this case the navigation errors have a high impact on the trajectory control. 3.1 Control law Let the control aim consist in ensuring of a level flight with a given (constant) altitude h∗ , velocity V ∗ and zero azimuth deviation z. The problem of the control law synthesis can be divided into several sub-problems: speed and altitude holding; heading holding; attitude holding. Different motions of the UAV have different rates, so the principle of the subordination control can be used for the sake of simplification of the autopilot synthesis. This principle is also useful because of the possibility to put the different specifications to the different kinds of motion. Velocity/altitude controller. One of the methods for the center of gravity longitudinal control consists of the combined control on UAV velocity Vk and altitude h by means of deviations of the propellant flow rate ms (or the thrust position δth ) and the pitching angle ϑ. Let us consider these variables as a controlling input vector, uv = col{ms , ϑ} and the variables {Vk , θ, h, Vk dt, hdt} as the state vector xv . Let us build the matrices B(1,2,8),4 A(1,2,8),10 A1,2,8 03×2 0 0 Av = 1 0 0 0 0 , Bv = , 0 0 1 0 0 0 0 (7) where A, B are the matrices of the linearized UAV model (A.5). Let us apply the standard LQ-synthesis technique for the plant model x˙ v = Av xv + Bv uv with the matrices (7). Introduce the cost function Jv = ∞ T (xv Qv xx + uTv Rv uv ) dt with the chosen weight matrices 0
Qv ∈ R5×5 , Rv ∈ R2×2 . The LQ-optimal control law has a form uv = −Kv xv , where the gain matrix Kv is taken via the steady-state solution of the matrix Riccati equation. It can be numerically found by means of the standard Matlab routine lqr for Linear-Quadratic Regulator design. In the considered case the following matrices are taken: Rv = diag{106 , 2500}, Qv = diag{0.02, 2.25 · 104 , 0.01, 2 · 10−4 , 10−4 }. This approach gives LQ-optimal PI-controller
128
for flight velocity and altitude. The control law has the following form: ev (t) = Vk (t) − Vk∗ (t), eh (t) = h(t) − h∗ (t), e˙ v (t) = ev (t), e˙ h (t) = eh (t), T (8) uv (t) = −Kv ev (t), θ(t), eh (t), e v (t), e h (t) ,
where Vk∗ (t), h∗ (t) are the reference signals, ev (t), eh (t) denote the tracking errors, uv (t) is the controlling signal, uv (t) = col {σms (t), ϑ∗ (t)} The eigenvalues si of the closed-loop system (of the matrix Av − Bv Kv ) are found as: s1 = −6.12, s2,3 = −0.12 ± 0.04, s4,5 = −0.10 ± 0.04. The discrete control law can be get from (8) as follows: ev [k] = Vk (tk ) − Vk∗ (tk ), eh [k] = h(tk ) − h∗ (tk ), e v [k] = e v [k − 1] + Ts ev [k], (9) e h [k] = e h [k − 1] + Ts eh (t), T uv [k] = −Kv ev [k], θ(tk ), eh [k], e v [k], e h [k] ,
where Ts is the sample time, k = E(t/Ts ), tk = kTs . If the zero-order hold is used, the analog control action is found as uv (t) = uv [k] if tk−1 ≤ t < tk .
Pitch control. The pitch attitude control law can be found in the form of PD-controller, ensuring the necessary tracking rate and the longitudinal damping. The pitch control law is taken as σe (t) = kϑ (ϑ − ϑ∗ ) + kωz ωz (t),
(10)
where kϑ , kωz are the gain coefficients; the tracking signal ϑ∗ emanates from the algorithm (8). Note that the law (10) can be directly applied at the discrete-time controller. Lateral/bank motion control. The heading control law can be found in the form of the PI-controller by analogy with the altitude control law (8). In detail, let us define the vector xz (t) = col{Ψ(t), ωy (t), z(t), ψ(t), zdt} and apply the LQ- optimization technique to the system x˙ z (t) = Az xz (t) + Bz uz (t). At the considered case the following values for Az and Bz are taken: B(3,5,9,12),2 A3,5,9,12 04×1 Az = . , Bz = 0 0 1 0 0 0
(11)
The controlling action for the lateral motion is the rudder control signal σr (t). Let us use the weight coefficient Rz = 3300 and the weight matrix Qz = diag 3300, 13·103 , 0.5, 3300, 5 · 10−3 . The lateral control law is written as ez (t) = z(t) − z ∗ (t), e˙ z (t) = ez (t), T (12) σr (t) = −Kz Ψ(t), ωy (t), ez (t), ψ(t), e z (t) ,
where z ∗ (t) is required (reference) value of z(t). Gain vector Kz is found via the lqr routine. The discrete-time form of the law (12) can be written similarly with the (9). For bank stabilization the following PD-control law is used: σa (t) = kγ γ(t) + kωx ωx (t).
(13)
Gains kγ , kωz are picked up on the base of the linearized model (A.5) parameters.
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
129
Dynamical properties of the closed-loop system are illustrated by the time histories, shown in Figs. 1, 2. Tracking the Vk (t), h(t), z(t) in the case of the ideal UAV position measurements and constant wind disturbances is demonstrated.
Fig. 1. Vk (t), h(t) and z(t) time histories. Fig. 2. Angular motions time histories (a – nonlinear model, b – linear model).
3.2 UAV control with GNSS/Estimator algorithm Now let us take into account that some flight data are not explicitly measurable, but are estimated by the navigation algorithm. Therefore, the control laws (8), (12) have to be changed and take the following form: ˆ − h∗ (t), ev (t) = Vˆk (t) − Vk∗ (t), eh (t) = h(t) e˙ v (t) = ev (t), e˙ h (t) = eh (t), (14) T ˆ eh (t), e (t), e (t) , uv (t) = −Kv ev (t), θ(t), v h ez (t) = zˆ(t) − z ∗ (t), e˙ z (t) = ez (t), T ˆ σr (t) = −Kz Ψ(t), ωy (t), ez (t), ψ(t), e z (t) ,
(15)
3.3 Numerical analysis of the UAV straight line flight control with GNSS/Estimator In the present Section the results of the closed-loop system with GNSS/Kalman estimator simulation are presented in Figs. 3 – 6. The disturbances are taken coincided with those ones at the previous part of the study; additionally, the 6% error in ms measuring is considered. It is seen that the maximum positioning errors has the lateral coordinate z(t). The fore-and-aft level straight-line flight at a height of 6000 m, speed 250 m/s and zero azimuth deviation is considered as a required one.
ˆ zˆ, θ, ˆ Vˆk , Ψ ˆ are the estimates of the corresponding where h, variables. The other signals in the considered scheme are taken from the onboard sensors. Some modifications were made in the structure of the Kalman estimator with respect to the previous work (Andrievsky et al., 2013), where propellant flow rate ms was not considered as the component of the controlling signal u(t). At the present study the control vector u is taken as u = col{δe , δr , δa , ms }, which leads to changes of Kalman estimator matrices. 129
Fig. 3. Speed Vk and estimate Vˆk .
IFAC ACNAAV 2015 130 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
ˆ Fig. 4. Track angle Ψ and estimate Ψ.
Fig. 7. Path angle θ(t).
ˆ Fig. 5. Altitude h(t) and estimate h(t).
Fig. 8. Speed holding.
Fig. 6. Lateral coordinate z and estimate zˆ. Statistical properties of the estimation errors are shown in the Tables 1 2. 2 Table 1. Estimation errors ∆Vk , M σ
∆Wz , ∆Ψ, ∆Wx ,∆Wy , ∆x deg m/s m/s m/s m
∆z,
m/s
∆θ, deg
1.3
0.025 0.03 3.0
0.18 0.24 4.5
3.6
6.0
8.7
1.1
0.66 0.21 1.2
3.2
3.0
3.5
3.1
1.5
2.2
∆h, m
m
L, m
4. CONCLUSIONS
Table 2. Holding errors
M σ
Vk , m/s, 1.37 1.34
θ, deg. 2·10−3 0.42
Ψ, deg. 10−3 0.51
h, m -3.4 3.6
begin at the point with coordinates x = 5 km, h = 6 km, z = 0 and the prescribed landing point has the coorinates x∗ = 25 km, h∗ = 0.03 km, z ∗ = 0. The UAV starting point coordinates are assumed to be zero. Control law (14), (15) with GNSS/Kalman estimator is used, where the reference altitude is taken as a function of the current estimated coordinate x ˆ(t). The UAV speed should be be held constant, Vk∗ = 250 m/s. The simulation results are shown in Figs. 7 – 8. The terminal error ∆LT comes to ≈ 13 m (∆xT = −1 m, ∆hT = 5 m, ∆zT = 11.2 m). At the same time, from the altitude of about 2.5 km the phugoid oscillations of the flight-path azumuth increased (from 1.5 deg up to 5 deg in the amplitude with period about 20 s). This effect can be eliminated by turning the control law parameters dependent on the current altitude h.
z, m 0.24 5.8
3.4 UAV landing control Consider now the problem of the UAV landing autonomous control. The reference trajectory is assumed to be given in advance as the program trajectory and no information on the actual position of the desired touch-down point is used during the flight. Let the landing trajectory be rectilineal, 2
These statistical data are rough because they are obtained along a single trajectory; this method is correct for the stationary ergodic processes only. The considered process is not stationary because presence of the transients.
130
The algorithm for UAV control based on position estimation with the onboard sensors and GNSS corrections is described and impact of the navigation errors on the closed-loop control system of the hypothetical UAV is numerically evaluated. More tests involving realistic sensor errors are required to quantify if this solution is suitable for UAV. The future work intentions are: usage of more realistic model of sensor errors, GNSS primary data processing, and wind disturbances; examination of the accuracy, achievable by means of the optimal Kalman filtering; investigation of the suitability of the on-line identification methods for tuning the estimator during the flight. It is also planned to implement and experimentally study the algorithm on the real-world UAV. Appendix A. UAV DYNAMICS MODEL Translational dynamics
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
V˙ k 0 l ˙ m = mDtr −g Vk θ ˙ cos θ 0 −Vk Ψ P −cx bf l 0 + qS cy +Dtr Dl , (A.1) cz 0 where Vk is the vehicle ground speed; θ, Ψ are the path and track angles; P is the thrust; m is the mass of the vehicle; g is the gravity acceleration; q is the dynamic pressure; S is the characteristic cross-section area of the vehicle; ci (i = x, y, z) are the aerodynamic coefficients. Rotational dynamics (Jz − Jy )ωy ωz mx Jx ω˙ x Jy ω˙ y + (Jx − Jz )ωx ωz = qSB my , (A.2) Jz ω˙ z (Jy − Jx )ωx ωy mz where Ji (i = x, y, z) are the principal moments of inertia; ωi are the angular rates in the body axes frame; B is the diagonal matrix of the characteristic vehicle dimensions; mi are the aerodynamic torques derivatives. Translational kinematics are described as T T x˙ h˙ z˙ = Dltr [Vk 0 0] , where x, h, z (h ≡ y) are the vehicle coordinates in the Earth reference frame. Rotational kinematics ϑ˙ = ωy sin γ + ωz cos γ, γ˙ = ωx + tan ϑ(ωz sin γ − ωy cos γ), (A.3) ˙ ψ = cos ϑ−1 (ωy cos γ − ωz sin γ) , where ϑ, ψ, γ are the Euler angles (pitch, yaw, roll).
Supplementary relations Vx Vk Wx l tr l 0 − Dbf Wy , Vy = Dbf Dl 0 Wz Vz (A.4) 2 2 2 V = Vx + Vy + Vz , α = −atan(Vy /Vx ), β = asin(Vz /V ), = f (h), a = fa (h), M = V /a, q = V 2 /2, where α, β are the angle-of-attack and the sideslip angle; Wi (i = x, y, z) are the components of the wind velocity vector in the Earth reference frame; (h) is the atmosphere density altitude; q is the dynamic pressure. Transformation matrices Dlbf , Dltr depend on the angles of axes rotation. Linearized model. Let u∗ (t) ≡ u∗ , x∗ (t) ≡ x∗ be a certain “reference point”. This leads to the following linear model with a “bias” ϕ: x(t) ˙ = Ax(t)+Bu(t)+ϕ, y(t) = Cx(t)+Du(t), (A.5) where ϕ = f (x∗ , u∗ ) − Ax∗ − Bu∗ , matrices A, B, C, D are found by means of Taylor approximation in the vicinty of u∗ , x∗ , x(t), u(t); y(t) stand for deviations from x∗ , u∗ , y ∗ = h(x∗ , u∗ ). We use the following state x ∈ R12 , input u ∈ R7 and output y ∈ R17 vectors: x = col{Vk , θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ}, u = col{δe , δr , δa , Wx , Wy , Wz , ∆P }, (A.6) y = col{Vk , θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ, α, β, jx , jy , jz }. 131
131
REFERENCES Algrain, M. and Saniie, J. (1994). Interface Kalman filtering of 3D-angular motion based on Euler’s nonlinear equations. IEEE Trans. Aerosp. Electron. Syst., 30(1), 175–185. Andrievsky, B., Kuznetsov, N., Kuznetsova, O., and Leonov, G. (2013). Switching algorithm for data fusion of small low-cost UAV navigation system. In Proc. 2nd IFAC Workshop on Research, Education and Development of Unmanned Aerial Systems, volume 2, 206–211. IFAC, Compiegne, France. Balluchi, A., Benvenuti, L., Di Benedetto, M.D., and Sangiovanni-Vincentelli, A.L. (2012). The design of dynamical observers for hybrid systems: Theory and application to an automotive control problem. Technical Report, No 1, Sapienza University of Rome, Dep. Computer and System Sci. Blomenhofer, H. (1996–1997). Accuracy, integrity, and availability of GLS-based autopilot-coupled aircraft landings. Navigation: J. of the ION, 43(4), 419–435. Hegrenaes, O. and Hallingstad, O. (2011). Model-aided INS with sea current estimation for robust underwater navigation. IEEE J. Oceanic Eng., 36(2), 316–337. Koifman, M. and Bar-Itzhack, I. (1999). Inertial navigation system aided by aircraft dynamics. IEEE Trans. Contr. Syst. Technol., 7(4), 487–493. Koifman, M., Bar-Itzhack, I., and Mernav, S. (1995). Dynamics aided Inertial Navigation System. In Proc. AIAA-95-3195-CP. Guidance. – Navigation and Control Conference, 204–213. Morgado, M., Oliveira, P., and Silvestre, C. (2013). Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles: An experimental validation. Journal of Field Robotics, 30(1), 142–170. Mutuel, L. and Speyer, J. (2000). Fault–tolerant INS/GPS navigation system with application to unmanned aerial vehicle. In Proc. ION GPS 2000 Convention. Salt Lake City, Utah, USA. Perera, L., Wijesoma, W., and Adams, M. (2010). SLAM with joint sensor bias estimation: Closed form solutions on observability, error bounds and convergence rates. IEEE Trans. Contr. Syst. Technol., 18(3), 732 – 740. Sazdovski, V. and Silson, P. (2011). Inertial navigation aided by vision-based simultaneous localization and mapping. IEEE Sensors J., 11(8), 1646–1656. Strachan, V. (1997). The global positioning system as a sole means of navigation. In Proc. Civil Avionics ’97 Conference. Tanwani, A., Shim, H., and Liberzon, D. (2013). Observability for switched linear systems: Characterization and observer design. IEEE Trans. Automat. Contr., 58(14), 891–904. Vasconcelos, J., Silvestre, C., and Oliveira, P. (2011). INS/GPS aided by frequency contents of vector observations with application to autonomous surface crafts. IEEE J. Oceanic Eng., 36(2), 347 – 363. Vasconcelos, J., Silvestre, C., Oliveira, P., and Guerreiro, B. (2010). Embedded UAV model and LASER aiding techniques for inertial navigation systems. Control Engineering Practice, 18(3), 262 – 278.
ScienceDirect
IFAC-PapersOnLine 48-9 (2015) 126–131
UAV control with switched UAV control with switched UAV control with switched GNSS-Estimator navigation system UAV control with switched GNSS-Estimator navigation system GNSS-Estimator navigation system GNSS-Estimator navigation system ∗,∗∗ ∗∗,∗∗∗
Boris Andrievsky ∗,∗∗ Nikolay Kuznetsov ∗∗,∗∗∗ ∗,∗∗∗∗Nikolay Kuznetsov ∗∗ ∗∗,∗∗∗ Boris Andrievsky Boris Andrievsky Kuznetsov Gennady Leonov Svetlana Seledzhi ∗∗ ∗,∗∗∗∗Nikolay ∗∗,∗∗∗ Boris Andrievsky Nikolay Kuznetsov ∗∗ Gennady Leonov Svetlana Seledzhi Gennady Leonov ∗∗ Svetlana Seledzhi ∗∗ ∗∗ Gennady Leonov Svetlana Seledzhi ∗ ∗ Institute for Problems of Mechanical Engineering of RAS, ∗ Institute for Problems of Mechanical Engineering of RAS, of Mechanical Engineering of RAS, Saint Petersburg, Russia, ∗ Institute for Problems Institute for Problems of Mechanical Engineering of RAS, Saint Petersburg, Russia, Saint Petersburg, Russia, e-mail: [email protected] Saint Petersburg, Russia, e-mail: [email protected] ∗∗ e-mail: [email protected] Saint Petersburg State University, ∗∗ e-mail: [email protected] ∗∗ Saint Petersburg State University, Petersburg StateSaint University, 28 Universitetsky prospekt, 198504, Petersburg, Russia, ∗∗ Saint Saint Petersburg State University, 28 Universitetsky prospekt, 198504, Saint Petersburg, 28e-mail: Universitetsky prospekt, 198504, Saint Petersburg, Russia, Russia, [email protected], [email protected] 28 Universitetsky prospekt, 198504, Saint Petersburg,Finland Russia, e-mail: [email protected], [email protected] ∗∗∗ e-mail: [email protected], [email protected] University of Jyv¨ a skyl¨ a , PO Box 35, FI-40014, ∗∗∗ e-mail: [email protected], [email protected] ∗∗∗ University of Jyv¨ a skyl¨ a ,, PO Box 35, FI-40014, Finland University of Jyv¨ a skyl¨ a PO Box 35, FI-40014, Finland ∗∗∗ University of Jyv¨ askyl¨ a, PO Box 35, FI-40014, Finland Abstract: In the paper the switched GNSS-Estimator navigation system, recently proposed by Abstract: paper GNSS-Estimator system, recently by Abstract: Inisthe the paper the the switched GNSS-Estimator navigation system, recentlyofproposed proposed by the authors,In described andswitched numerically studied in thenavigation framework of evaluation the overall Abstract: In the paper the switched GNSS-Estimator navigation system, recently proposed by the authors, is described and numerically studied in the framework of evaluation of the overall the described and numerically studied in the framework of evaluation of the overall UAVauthors, control issystem accuracy. the authors, is described and numerically studied in the framework of evaluation of the overall UAV control system accuracy. UAV control system accuracy. © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. UAV control system accuracy. Keywords: UAV, onboard algorithms, guidance, navigation, flight control Keywords: Keywords: UAV, UAV, onboard onboard algorithms, algorithms, guidance, guidance, navigation, navigation, flight flight control control Keywords: UAV, onboard algorithms, guidance, navigation, flight control 1. INTRODUCTION more detailed and accurate UAV dynamics model than 1. more accurate dynamics model than 1. INTRODUCTION INTRODUCTION more detailed detailed and accurate UAV UAV dynamics model than commonly usedand for GNSS/INS Kalman filtering is needed. 1. INTRODUCTION more detailed and accurate UAV dynamics model than commonly used for GNSS/INS Kalman filtering is needed. commonly used et foral., GNSS/INS Kalman filtering is for needed. (Andrievsky 2013), a switching algorithm data At present, the Global Navigation Satellite System (GNSS) In commonly used for GNSS/INS Kalman filtering is needed. In (Andrievsky et al., 2013), a switching algorithm for At present, the Global Navigation Satellite System (GNSS) In (Andrievsky et al.,navigation 2013), a switching for data data of a satellite system algorithm and standard onAt present, the Global Satellite System (GNSS) fusion is very widely used inNavigation a lot of navigation applications, In (Andrievsky et low-cost al.,navigation 2013), a switching algorithm for data fusion of system and standard onAt present, the Global Satellite System (GNSS) is very widely used in aacontrol lot navigation applications, of aa satellite satellite navigation system andvehicle standard onboard sensors for unmanned aerial (UAV) is very widely used and inNavigation lot of of of navigation applications, including navigation unmanned aerial vehi- fusion fusion of a satellite navigation system and standard onboard sensors for low-cost unmanned aerial vehicle (UAV) is very widely used in a lot of navigation applications, including navigation and control of unmanned aerial vehiboard sensors for low-cost unmanned aerial vehicle (UAV) is proposed and numerically studied by the example of including navigation of unmanned aerial vehicles (UAV). However,and thecontrol GNSS alone cannot provide the board sensors for low-cost unmanned aerial vehicle (UAV) is proposed and numerically studied by the example of including navigation and control of unmanned aerial vehicles (UAV). However, the GNSS alone cannot provide the is proposed and small numerically studiedUAV. by the example of hypothetical jet-propelled In the present cles However, alonedata cannot provide the the UAV(UAV). autopilot with allthe theGNSS necessary of fast variables is proposed and numerically studied by the example of the hypothetical small jet-propelled UAV. In the present cles (UAV). However, the GNSS alone cannot provide the UAV autopilot with all the necessary data of fast variables the hypothetical small jet-propelled In the present the data fusion algorithm of UAV. (Andrievsky et al., UAV with alllow thedata necessary dataa of fast variables due toautopilot the relatively rate and possible loss of paper the hypothetical small jet-propelled UAV. In the present paper the data fusion algorithm of (Andrievsky et al., UAV autopilot with all the necessary data of fast variables due to the relatively low data rate and a possible loss of paper the data fusion algorithm of (Andrievsky et al., 2013) is considered in the framework of the closed-loop due to the low data rate and1997; a possible lossand of signals fromrelatively the satellites (Strachan, Mutuel theconsidered data algorithm of errors, (Andrievsky et the al., 2013) is the framework of closed-loop due to the relatively low data rate and1997; aThe possible lossand of paper signals from the satellites (Strachan, 1997; Mutuel and 2013)control is considered in the framework of the the closed-loop UAV and fusion the in UAV trajectory caused by signals from the satellites (Strachan, Mutuel Speyer, 2000; Blomenhofer, 1996–1997). typical 2013) is considered in the framework of the closed-loop UAV control and the UAV trajectory errors, caused by the signals from the satellites (Strachan, 1997; Mutuel Speyer, 2000; Blomenhofer, 1996–1997). The typical and UAV control the UAVdata, trajectory errors, caused by the of the and navigation are numerically evaluated Speyer, 2000; Blomenhofer, 1996–1997). Theoftypical and errors most indisputable solution lies in the fusion data from control thetheir UAVdata, trajectory errors, caused by the errors of navigation are numerically evaluated Speyer, 2000; Blomenhofer, 1996–1997). The and UAV most solution lies in the of data from errors of the the and navigation data, are on numerically evaluated for examination of influence the overall system most indisputable solution lies in the fusion fusion oftypical data(INS). from GNSSindisputable and onboard integrated navigation system errors of the navigation data, are on numerically evaluated for examination of their influence the overall system most indisputable solution lies in the fusion of data from GNSS and onboard integrated navigation system (INS). for examination of their influence on the overall system performance. GNSS integrated system For thisand aim,onboard the Kalman filteringnavigation is commonly used. (INS). In the for examination of their influence on the overall system performance. GNSS and system (INS). For this aim, the is used. In For this(Andrievsky aim,onboard the Kalman Kalman filtering is commonly used. In the the performance. paper etintegrated al.,filtering 2013)navigation ancommonly alternative approach The rest of the paper is organized as follows. The navperformance. For this aim, the Kalman filtering is commonly used. In the paper (Andrievsky et al., 2013) an alternative approach paper (Andrievsky et al., 2013) an alternative approach for low-cost UAVs was proposed, assuming absence of the The of is follows. navThe rest restalgorithm of the the paper paper is organized organizedetas asal., follows. The navigation of (Andrievsky 2013) The is briefly paper (Andrievsky et al., 2013)for an alternative for UAVs proposed, assuming absence of for low-cost UAVs was proposed, assuming absence of the the The INSlow-cost onboard. The was requirement continuity is approach provided rest of the paper is organized as follows. The navigation algorithm of (Andrievsky et al., 2013) is briefly igation algorithm of (Andrievsky et al., 2013) is briefly described in Sec. 2. Accuracy evaluation of UAV control for low-cost UAVs was proposed, assuming ofThe the igation INS onboard. The requirement for continuity is provided provided INS onboard. The requirement for continuity is by the Kalman filter and customary onboardabsence sensors. algorithm of (Andrievsky et al., 2013) is briefly described in Sec. 2. Accuracy evaluation of UAV control described in Sec. 2. Accuracy evaluation of UAV with switched navigation system is presented in control Sec. 3. INS onboard. The continuity is provided by the Kalman filter and onboard sensors. The by the Kalman filterrequirement and customary customary onboard sensors. The described Kalman estimator receives datafor from the sensors (gyroin Sec. 2. Accuracy evaluation of UAV control with navigation system is Sec. 3. with switched switched navigation system is presented presented ingiven Sec. in 3. Suplementary information on the UAV model isin by the Kalman filter and customary onboard sensors. The Kalman estimator receives data from the sensors (gyroKalman estimator receives datasensors, from the (gyro- with scopes, accelerometers, altitude etc.)sensors and provides switched navigation system is presented ingiven Sec. in 3. Suplementary information on the UAV model is Suplementary information on the UAV model is given in Appendix A. Kalman estimator receives data from the sensors (gyroscopes, accelerometers, altitude sensors, etc.) and provides scopes, accelerometers, altitude sensors, etc.) andestimates provides Suplementary the guidance system with high-speed UAV state information on the UAV model is given in Appendix A. Appendix A. scopes, accelerometers, altitude sensors, etc.) and provides the guidance system high-speed UAV state the guidance system with high-speed UAV state estimates at the rate that is with necessary for guidance andestimates control Appendix A. the guidance system with high-speed UAV state estimates at the rate that for 2. UAV NAVIGATION METHOD USING GNSS AND at the the ratetrajectory. that is is necessary necessary for guidance guidance and control along GNSS signals are usedand to control update 2. METHOD USING at ratetrajectory. that for guidance and along the GNSS signals are to update 2. UAV UAV NAVIGATION NAVIGATION METHOD USING GNSS GNSS AND AND KALMAN ESTIMATOR along the trajectory. GNSS signals are used used to control update the the estimates at is thenecessary instants (possibly random) when 2. UAV NAVIGATION METHOD USING GNSS AND KALMAN ESTIMATOR along the trajectory. GNSS signals are used to update the estimates at the instants (possibly random) when KALMAN ESTIMATOR the estimates at the instants (possibly GNSS is accessible. Time sampling time of random) renovationwhen can KALMAN ESTIMATOR the estimates at the (possibly when GNSS is Time sampling time of renovation can GNSS is accessible. accessible. Time sampling time used of random) renovation can Let us consider a small-size UAV, equipped with the significantly exceed theinstants sampling time for Kalman Let us consider small-size UAV, with GNSS is accessible. Time sampling time of renovation can significantly exceed the sampling time used for Kalman Let us consider small-size UAV, equipped equipped with the the sensors:aa the GNSS receiver; the gyro-sensors significantly exceed the sampling time used for Kalman following filtering. Let us consider a small-size UAV, equipped withangle the following sensors: the GNSS receiver; the gyro-sensors significantly exceed the sampling time used for Kalman filtering. following sensors:(pitch the GNSS receiver; the ψ, gyro-sensors for Euler angles angle ϑ, yaw angle roll filtering. sensors:(pitch receiver; the gyro-sensors for Euler angles angle ϑ, yaw angle ψ, roll angle 1 the GNSS It is worth mentioning that in the series of papers, de- following filtering. for Euler angles (pitch angle ϑ, yaw angle ψ, roll angle γ) measurement ; the rate gyros for ω , ω , ω ; x y z the 1 It is mentioning in of deEuler angles (pitch ϑ, yaw angle roll 1 ; theangle γ) measurement rate gyros for ω ,,jψ, ω ,, ω ; the It is worth worth mentioning that in the the series series of papers, papers, de- for voted to navigation of that autonomous aerial, underwater x y zangle γ) measurement ; the rate gyros for ω ω ω barometric altimeter; g-meters for j , j , ; the rudder x y z ; the x y z 1 It worth mentioning in the series of de- γ) voted to of autonomous aerial, underwater measurement ; the rate gyros for ω , ω , ω ; the barometric altimeter; g-meters for j , j , j ; the rudder voted to navigation navigation of that autonomous aerial, underwater andis ground vehicles, the vehicle dynamics arepapers, taken into x y z x y z barometric g-meters for jxcontrolling , jy , jz ; the inputs: rudder sensors for altimeter; δe , δr , δa .the UAV has four voted to navigation of autonomous and ground vehicles, the vehicle are taken into barometric altimeter; the g-meters for j , j , j ; the rudder sensors for δ , δ , δ . UAV has four controlling inputs: and ground vehicles, the vehicle ofdynamics dynamics areunderwater taken into the consideration for enhancement the aerial, existing INS perx y z e r a sensors for δe , δr ,and δa . differential UAV has four controlling inputs: symmetrical taileron deflection (δa , and groundcfvehicles, theetvehicle are taken into the consideration for of the existing INS persensors δe , δrdeflection ,and δa . differential UAV(δhas four controlling inputs: taileron deflection (δ consideration for enhancement enhancement ofdynamics theKoifman existing INS Barperformance, (Koifman al., 1995; and the symmetrical and differential taileron deflection (δaa ,, δe ), symmetrical the for rudder the engine throttle r ), and consideration for enhancement of the existing INS performance, cf (Koifman et al., 1995; Koifman and Barsymmetrical and differential taileron deflection δδ(thrust ), the rudder deflection (δ ), and the engine throttle formance, cf (Koifman et al., 1995;1994; Koifman andetBarItzhack, 1999; Algrain and Saniie, Perera al., the a, e ), the position rudder δdeflection (δrr ), and theprocessing engine throttle data of (δ the e th ). Preliminary formance, cf (Koifman et al., 1995;2011; Koifman andet Itzhack, 1999; Algrain and Saniie, 1994; Perera al., ), the rudder deflection (δ ), and the engine throttle position δ ). Preliminary data processing of the Itzhack, 1999; Algrain and Saniie, 1994; Perera etBaral., δ(thrust 2010; Hegrenaes and Hallingstad, Sazdovski and e r th position δth ). Preliminary processing of the GNSS signals is realized by means ofdata a separate algorithm, Itzhack, 1999; Algrain and Saniie, 1994; Morgado Perera et al., (thrust 2010; and Sazdovski (thrust position δth ). Preliminary data processing of the GNSS signals is realized by means of aa separate algorithm, 2010; Hegrenaes and Hallingstad, Hallingstad, 2011; Sazdovski and Silson,Hegrenaes 2011; Vasconcelos et al., 2011,2011; 2010; etand al., GNSS signals is realized by means of separate algorithm, converting the pseudorange measurements from the GNSS 2010; Hallingstad, Sazdovski Silson, 2011; et al., 2011, 2010; et al., signals ispseudorange realized by means of aposition separate algorithm, converting the measurements from the GNSS Silson, 2011; Vasconcelos etof al.,(Andrievsky 2011,2011; 2010; Morgado Morgado etand al.,a GNSS 2013). Hegrenaes To useVasconcelos the and method et al., 2013), converting the pseudorange measurements from the GNSS receiver into the estimates of UAV in a certain Silson, 2011; al.,(Andrievsky 2011, 2010; Morgado et al.,aa converting 2013). use the et thethe pseudorange from GNSS receiver into estimates of in a 2013). To To useVasconcelos the method methodetof of (Andrievsky et al., al., 2013), 2013), into the estimates measurements of UAV UAV position position in the a certain certain 2013). To use the method of (Andrievsky et al., 2013), a 1receiver receiver into the estimates of UAV in a certain The Russian notations are used in this position paper. They differ from This work was supported by Russian Scientific Foundation (project 1
notations are this They differ This work work was was by Scientific Foundation This 1 The those of Russian the ISO 1151-5 standard, seein Appendix A for the definitions. 14-21-00041) andsupported Saint-Petersburg State University. The Russian notations are used used in this paper. paper. They differ from from supported by Russian Russian Scientific Foundation (project (project 1 The those of the standard, see Appendix A the 14-21-00041) and Saint-Petersburg State University. notations are used this paper. They differ from This work was by Russian Scientific Foundation (project those of Russian the ISO ISO 1151-5 1151-5 standard, seein Appendix A for for the definitions. definitions. 14-21-00041) andsupported Saint-Petersburg State University. those of the ISO 1151-5 standard, see Appendix A for the definitions. 14-21-00041) and Saint-Petersburg State University. Copyright © 2015 IFAC 126 2405-8963 © 2015, IFAC (International Federation of Automatic Control) Hosting by Elsevier Ltd. All rights reserved. Copyright © 2015 IFAC 126 Copyright 2015 responsibility IFAC 126Control. Peer review© of International Federation of Automatic Copyright ©under 2015 IFAC 126 10.1016/j.ifacol.2015.08.071
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
Earth-fixed frame (x, h, z) and UAV velocity with respect to the Earth (Vk ). These data are used for renovation of the Kalman estimates at time instants {tK }. It is assummed below that tK = K · TS (K = 0, 1, 2, . . ., where TS is a sampling time, TS Ts . The value TS is not used for the estimator algorithm design, therefore the case when the instants {tK } are not equally spread is also possible: tK = tK−1 + TS (K), say, with the random increments TS (K). The process of the estimator design has several phases. Firstly, the linearized UAV model is obtained. Secondly, the extended model, including the model of disturbances is obtained. Thirdly by means of the discretization on time with given sample period Ts the discrete-time extended model is found. This leads to the switched navigation algorithm (Tanwani et al., 2013; Balluchi et al., 2012). The Kalman-estimator design technique is applied to this model and the discrete-time estimator equations are finally obtained. Extended UAV model involves the UAV dynamics equations and the internal model of disturbances (where measuring errors: position, velocity, attitude errors, gyro drift, accelerometer bias, etc. can be added). In the present study only the wind disturbances are included into the extended model, more specifically, the systematic components of the wind velocity vector W = col{Wx , Wy , Wz } are taken into account. Assuming they are constants, one ˙ x = 0, gets the following internal model of disturbances: W ˙ ˙ Wy = 0, Wz = 0. Two extended UAV models are used. The first one (“Model-I”) is aimed to be a constituent of the real-time estimation algorithm, and the next one (“Model-II”) is used for the estimator design. Model-I is used for the UAV state and disturbances estimation inside the intervals between the corrections based on GNSS signals. In compliance with the given above list of sensors, output vector yI of this model can be taken as yI = col{ωx , ωy , ωz , h, ϑ, γ, ψ, jx , jy , jz }. Measurable input vector is taken as uI = col{δe , δr , δa , Vˆk }, where Vˆk stands for the velosity estimate, obtained by means of the extrinsic algorithm. Thus, the state-space equations of the Model-I x˙ I (t) = AI xI (t)+BI uI (t), yI (t) = CI xI (t)+DI uI (t) have matrices AI , BI , CI , DI in the following block form: A2,...,12 | B(2,...,12),(4,5,6) ∈ R14×14 , AI = 03×14 B(2,...,12),(1,2,3) | A(2,...,12),1 ∈ R14×3 , BI = 03×4 CI = C(4,5,6,8,10,11,12,15,16,17),(2,...,12) | . D(4,5,6,8,10,11,12,15,16,17),(4,5,6) ∈ R10×14 , DI = D(4,5,6,8,10,11,12,15,16,17),(1,2,3) | (1) C(4,5,6,8,10,11,12,15,16,17),1 ∈ R10×3 ,
where indices show the numbers of the rows and columns of the elements of corresponding matrices in (A.5) that are 127
127
used in Model-I. The “shift” ϕI for this model is found as ∗ ∗ x 2,...,12 u 1,2,3 f (x∗ , u∗ ) 2,...,12 − AI − BI . ϕI = ∗ 03×1 03×1 x1
Model-II is used for computation of the Kalman estimator feedback gain matrix. It has to satisfy the observability condition and only measurable outputs can be used in this model and only observable states can be estimated. It should be mentioned that Model-II, as itself, is not destined for getting the state estimates. It is used only in order to obtain the estimator feedback gain matrix. Therefore, this model can be obtained by decomposition into the observable/unobservable subspaces (or, in the other words, by transformation to the Observability Staircase Form). For the considered problem the matrices of ModelII state-space equations can be written as follows:
AII =
A2,4,5,6,8,10,11,12 | B(2,4,5,6,8,10,11,12),(4,5,6)
BII =
03×11 B(2,4,5,6,8,10,11,12),(1,2,3) 03×3
,
∈ R11×3 ,
CII = C(4,5,6,8,10,11,12,15,16,17),(2,4,5,6,8,10,11,12) | D(4,5,6,8,10,11,12,15,16,17),(4,5,6) ∈ R10×11 , DII = D(4,5,6,8,10,11,12,15,16,17),(1,2,3) .
(2)
Data processing in the considered systems has a discrete mode, so the discrete-time models have to be found. Assuming the input signal u(t) to be constant between the sampling instants tk = kTs (k = 0, 1, 2, . . .), one gets the following discrete-time form of (A.5): x[k + 1] = P x[k]+Qu[k]+Ts ϕ, y[k] = Cx[k]+Du[k], (3) and from u[k] ≡ u(tk ) and x[0] = x(t0 ) follows that x[k] ≡ x(tk ) and y[k] ≡ y(tk ), where P = exp(ATs ), Ts Q= exp(Aτ )dτ B. 0
Applying this procedure to Models-I,II, one gets them in the form of difference equations, respectively: Model-I/D: xI [k + 1] = PI xI [k] + QI u[k] + Ts ϕ, (4) yI [k] = CI xI [k] + DI u[k], Model-II/D: xII [k + 1] = PII xI [k] + QII u[k], (5) yII [k] = CII xII [k] + DII u[k], Ts where Pi = exp(Ai Ts ), Qi = exp(Ai τ )dτ Bi , i=I,II. 0
The state, output and input vectors of these models are as follows: xI = col{θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ, Wx , Wy , Wz }∈ R14 , yI = yII = col{ωx , ωy , ωz , h, ϑ, γ, ψ, jx , jy , jz } ∈ R10 , xII = col{θ, ωx , ωy , ωz , h, ϑ, γ, ψ, Wx , Wy , Wz } ∈ R11 , uI = col{δe , δr , δa , Vˆk } ∈ R4 , uII = col{δe , δr , δa } ∈ R3 .
IFAC ACNAAV 2015 128 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
Discrete full-order state estimator (Kalman Estimator, State Observer) for the plant model x[k + 1] = P x[k] + Qu[k], y[k] = Cx[k] + Du[k] is as follows: x ˆ[k + 1] = P x ˆ[k] + Qu[k] + L[k](y[k] − yˆ[k]), yˆ[k] = C x ˆ[k] + Du[k],
(6)
where x ˆ[k] denotes the estimate of the plant state vector x(tk ) at the kth instant (for real-time systems, tk = kTs , Ts is a sample time). The problem of the estimator design lies in finding the appropriate gain matrix function L[k] ∈ Rn×m . 3. ACCURACY OF UAV CONTROL WITH SWITCHED NAVIGATION SYSTEM The UAV flight accuracy depends on the navigation errors and on the dynamics of the closed-loop system “UAV/controller” as well. In this study the most attention is paid to the autonomous flight pass control, since in this case the navigation errors have a high impact on the trajectory control. 3.1 Control law Let the control aim consist in ensuring of a level flight with a given (constant) altitude h∗ , velocity V ∗ and zero azimuth deviation z. The problem of the control law synthesis can be divided into several sub-problems: speed and altitude holding; heading holding; attitude holding. Different motions of the UAV have different rates, so the principle of the subordination control can be used for the sake of simplification of the autopilot synthesis. This principle is also useful because of the possibility to put the different specifications to the different kinds of motion. Velocity/altitude controller. One of the methods for the center of gravity longitudinal control consists of the combined control on UAV velocity Vk and altitude h by means of deviations of the propellant flow rate ms (or the thrust position δth ) and the pitching angle ϑ. Let us consider these variables as a controlling input vector, uv = col{ms , ϑ} and the variables {Vk , θ, h, Vk dt, hdt} as the state vector xv . Let us build the matrices B(1,2,8),4 A(1,2,8),10 A1,2,8 03×2 0 0 Av = 1 0 0 0 0 , Bv = , 0 0 1 0 0 0 0 (7) where A, B are the matrices of the linearized UAV model (A.5). Let us apply the standard LQ-synthesis technique for the plant model x˙ v = Av xv + Bv uv with the matrices (7). Introduce the cost function Jv = ∞ T (xv Qv xx + uTv Rv uv ) dt with the chosen weight matrices 0
Qv ∈ R5×5 , Rv ∈ R2×2 . The LQ-optimal control law has a form uv = −Kv xv , where the gain matrix Kv is taken via the steady-state solution of the matrix Riccati equation. It can be numerically found by means of the standard Matlab routine lqr for Linear-Quadratic Regulator design. In the considered case the following matrices are taken: Rv = diag{106 , 2500}, Qv = diag{0.02, 2.25 · 104 , 0.01, 2 · 10−4 , 10−4 }. This approach gives LQ-optimal PI-controller
128
for flight velocity and altitude. The control law has the following form: ev (t) = Vk (t) − Vk∗ (t), eh (t) = h(t) − h∗ (t), e˙ v (t) = ev (t), e˙ h (t) = eh (t), T (8) uv (t) = −Kv ev (t), θ(t), eh (t), e v (t), e h (t) ,
where Vk∗ (t), h∗ (t) are the reference signals, ev (t), eh (t) denote the tracking errors, uv (t) is the controlling signal, uv (t) = col {σms (t), ϑ∗ (t)} The eigenvalues si of the closed-loop system (of the matrix Av − Bv Kv ) are found as: s1 = −6.12, s2,3 = −0.12 ± 0.04, s4,5 = −0.10 ± 0.04. The discrete control law can be get from (8) as follows: ev [k] = Vk (tk ) − Vk∗ (tk ), eh [k] = h(tk ) − h∗ (tk ), e v [k] = e v [k − 1] + Ts ev [k], (9) e h [k] = e h [k − 1] + Ts eh (t), T uv [k] = −Kv ev [k], θ(tk ), eh [k], e v [k], e h [k] ,
where Ts is the sample time, k = E(t/Ts ), tk = kTs . If the zero-order hold is used, the analog control action is found as uv (t) = uv [k] if tk−1 ≤ t < tk .
Pitch control. The pitch attitude control law can be found in the form of PD-controller, ensuring the necessary tracking rate and the longitudinal damping. The pitch control law is taken as σe (t) = kϑ (ϑ − ϑ∗ ) + kωz ωz (t),
(10)
where kϑ , kωz are the gain coefficients; the tracking signal ϑ∗ emanates from the algorithm (8). Note that the law (10) can be directly applied at the discrete-time controller. Lateral/bank motion control. The heading control law can be found in the form of the PI-controller by analogy with the altitude control law (8). In detail, let us define the vector xz (t) = col{Ψ(t), ωy (t), z(t), ψ(t), zdt} and apply the LQ- optimization technique to the system x˙ z (t) = Az xz (t) + Bz uz (t). At the considered case the following values for Az and Bz are taken: B(3,5,9,12),2 A3,5,9,12 04×1 Az = . , Bz = 0 0 1 0 0 0
(11)
The controlling action for the lateral motion is the rudder control signal σr (t). Let us use the weight coefficient Rz = 3300 and the weight matrix Qz = diag 3300, 13·103 , 0.5, 3300, 5 · 10−3 . The lateral control law is written as ez (t) = z(t) − z ∗ (t), e˙ z (t) = ez (t), T (12) σr (t) = −Kz Ψ(t), ωy (t), ez (t), ψ(t), e z (t) ,
where z ∗ (t) is required (reference) value of z(t). Gain vector Kz is found via the lqr routine. The discrete-time form of the law (12) can be written similarly with the (9). For bank stabilization the following PD-control law is used: σa (t) = kγ γ(t) + kωx ωx (t).
(13)
Gains kγ , kωz are picked up on the base of the linearized model (A.5) parameters.
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
129
Dynamical properties of the closed-loop system are illustrated by the time histories, shown in Figs. 1, 2. Tracking the Vk (t), h(t), z(t) in the case of the ideal UAV position measurements and constant wind disturbances is demonstrated.
Fig. 1. Vk (t), h(t) and z(t) time histories. Fig. 2. Angular motions time histories (a – nonlinear model, b – linear model).
3.2 UAV control with GNSS/Estimator algorithm Now let us take into account that some flight data are not explicitly measurable, but are estimated by the navigation algorithm. Therefore, the control laws (8), (12) have to be changed and take the following form: ˆ − h∗ (t), ev (t) = Vˆk (t) − Vk∗ (t), eh (t) = h(t) e˙ v (t) = ev (t), e˙ h (t) = eh (t), (14) T ˆ eh (t), e (t), e (t) , uv (t) = −Kv ev (t), θ(t), v h ez (t) = zˆ(t) − z ∗ (t), e˙ z (t) = ez (t), T ˆ σr (t) = −Kz Ψ(t), ωy (t), ez (t), ψ(t), e z (t) ,
(15)
3.3 Numerical analysis of the UAV straight line flight control with GNSS/Estimator In the present Section the results of the closed-loop system with GNSS/Kalman estimator simulation are presented in Figs. 3 – 6. The disturbances are taken coincided with those ones at the previous part of the study; additionally, the 6% error in ms measuring is considered. It is seen that the maximum positioning errors has the lateral coordinate z(t). The fore-and-aft level straight-line flight at a height of 6000 m, speed 250 m/s and zero azimuth deviation is considered as a required one.
ˆ zˆ, θ, ˆ Vˆk , Ψ ˆ are the estimates of the corresponding where h, variables. The other signals in the considered scheme are taken from the onboard sensors. Some modifications were made in the structure of the Kalman estimator with respect to the previous work (Andrievsky et al., 2013), where propellant flow rate ms was not considered as the component of the controlling signal u(t). At the present study the control vector u is taken as u = col{δe , δr , δa , ms }, which leads to changes of Kalman estimator matrices. 129
Fig. 3. Speed Vk and estimate Vˆk .
IFAC ACNAAV 2015 130 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
ˆ Fig. 4. Track angle Ψ and estimate Ψ.
Fig. 7. Path angle θ(t).
ˆ Fig. 5. Altitude h(t) and estimate h(t).
Fig. 8. Speed holding.
Fig. 6. Lateral coordinate z and estimate zˆ. Statistical properties of the estimation errors are shown in the Tables 1 2. 2 Table 1. Estimation errors ∆Vk , M σ
∆Wz , ∆Ψ, ∆Wx ,∆Wy , ∆x deg m/s m/s m/s m
∆z,
m/s
∆θ, deg
1.3
0.025 0.03 3.0
0.18 0.24 4.5
3.6
6.0
8.7
1.1
0.66 0.21 1.2
3.2
3.0
3.5
3.1
1.5
2.2
∆h, m
m
L, m
4. CONCLUSIONS
Table 2. Holding errors
M σ
Vk , m/s, 1.37 1.34
θ, deg. 2·10−3 0.42
Ψ, deg. 10−3 0.51
h, m -3.4 3.6
begin at the point with coordinates x = 5 km, h = 6 km, z = 0 and the prescribed landing point has the coorinates x∗ = 25 km, h∗ = 0.03 km, z ∗ = 0. The UAV starting point coordinates are assumed to be zero. Control law (14), (15) with GNSS/Kalman estimator is used, where the reference altitude is taken as a function of the current estimated coordinate x ˆ(t). The UAV speed should be be held constant, Vk∗ = 250 m/s. The simulation results are shown in Figs. 7 – 8. The terminal error ∆LT comes to ≈ 13 m (∆xT = −1 m, ∆hT = 5 m, ∆zT = 11.2 m). At the same time, from the altitude of about 2.5 km the phugoid oscillations of the flight-path azumuth increased (from 1.5 deg up to 5 deg in the amplitude with period about 20 s). This effect can be eliminated by turning the control law parameters dependent on the current altitude h.
z, m 0.24 5.8
3.4 UAV landing control Consider now the problem of the UAV landing autonomous control. The reference trajectory is assumed to be given in advance as the program trajectory and no information on the actual position of the desired touch-down point is used during the flight. Let the landing trajectory be rectilineal, 2
These statistical data are rough because they are obtained along a single trajectory; this method is correct for the stationary ergodic processes only. The considered process is not stationary because presence of the transients.
130
The algorithm for UAV control based on position estimation with the onboard sensors and GNSS corrections is described and impact of the navigation errors on the closed-loop control system of the hypothetical UAV is numerically evaluated. More tests involving realistic sensor errors are required to quantify if this solution is suitable for UAV. The future work intentions are: usage of more realistic model of sensor errors, GNSS primary data processing, and wind disturbances; examination of the accuracy, achievable by means of the optimal Kalman filtering; investigation of the suitability of the on-line identification methods for tuning the estimator during the flight. It is also planned to implement and experimentally study the algorithm on the real-world UAV. Appendix A. UAV DYNAMICS MODEL Translational dynamics
IFAC ACNAAV 2015 June 10-12, 2015. Seville, Spain
Boris Andrievsky et al. / IFAC-PapersOnLine 48-9 (2015) 126–131
V˙ k 0 l ˙ m = mDtr −g Vk θ ˙ cos θ 0 −Vk Ψ P −cx bf l 0 + qS cy +Dtr Dl , (A.1) cz 0 where Vk is the vehicle ground speed; θ, Ψ are the path and track angles; P is the thrust; m is the mass of the vehicle; g is the gravity acceleration; q is the dynamic pressure; S is the characteristic cross-section area of the vehicle; ci (i = x, y, z) are the aerodynamic coefficients. Rotational dynamics (Jz − Jy )ωy ωz mx Jx ω˙ x Jy ω˙ y + (Jx − Jz )ωx ωz = qSB my , (A.2) Jz ω˙ z (Jy − Jx )ωx ωy mz where Ji (i = x, y, z) are the principal moments of inertia; ωi are the angular rates in the body axes frame; B is the diagonal matrix of the characteristic vehicle dimensions; mi are the aerodynamic torques derivatives. Translational kinematics are described as T T x˙ h˙ z˙ = Dltr [Vk 0 0] , where x, h, z (h ≡ y) are the vehicle coordinates in the Earth reference frame. Rotational kinematics ϑ˙ = ωy sin γ + ωz cos γ, γ˙ = ωx + tan ϑ(ωz sin γ − ωy cos γ), (A.3) ˙ ψ = cos ϑ−1 (ωy cos γ − ωz sin γ) , where ϑ, ψ, γ are the Euler angles (pitch, yaw, roll).
Supplementary relations Vx Vk Wx l tr l 0 − Dbf Wy , Vy = Dbf Dl 0 Wz Vz (A.4) 2 2 2 V = Vx + Vy + Vz , α = −atan(Vy /Vx ), β = asin(Vz /V ), = f (h), a = fa (h), M = V /a, q = V 2 /2, where α, β are the angle-of-attack and the sideslip angle; Wi (i = x, y, z) are the components of the wind velocity vector in the Earth reference frame; (h) is the atmosphere density altitude; q is the dynamic pressure. Transformation matrices Dlbf , Dltr depend on the angles of axes rotation. Linearized model. Let u∗ (t) ≡ u∗ , x∗ (t) ≡ x∗ be a certain “reference point”. This leads to the following linear model with a “bias” ϕ: x(t) ˙ = Ax(t)+Bu(t)+ϕ, y(t) = Cx(t)+Du(t), (A.5) where ϕ = f (x∗ , u∗ ) − Ax∗ − Bu∗ , matrices A, B, C, D are found by means of Taylor approximation in the vicinty of u∗ , x∗ , x(t), u(t); y(t) stand for deviations from x∗ , u∗ , y ∗ = h(x∗ , u∗ ). We use the following state x ∈ R12 , input u ∈ R7 and output y ∈ R17 vectors: x = col{Vk , θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ}, u = col{δe , δr , δa , Wx , Wy , Wz , ∆P }, (A.6) y = col{Vk , θ, Ψ, ωx , ωy , ωz , x, h, z, ϑ, γ, ψ, α, β, jx , jy , jz }. 131
131
REFERENCES Algrain, M. and Saniie, J. (1994). Interface Kalman filtering of 3D-angular motion based on Euler’s nonlinear equations. IEEE Trans. Aerosp. Electron. Syst., 30(1), 175–185. Andrievsky, B., Kuznetsov, N., Kuznetsova, O., and Leonov, G. (2013). Switching algorithm for data fusion of small low-cost UAV navigation system. In Proc. 2nd IFAC Workshop on Research, Education and Development of Unmanned Aerial Systems, volume 2, 206–211. IFAC, Compiegne, France. Balluchi, A., Benvenuti, L., Di Benedetto, M.D., and Sangiovanni-Vincentelli, A.L. (2012). The design of dynamical observers for hybrid systems: Theory and application to an automotive control problem. Technical Report, No 1, Sapienza University of Rome, Dep. Computer and System Sci. Blomenhofer, H. (1996–1997). Accuracy, integrity, and availability of GLS-based autopilot-coupled aircraft landings. Navigation: J. of the ION, 43(4), 419–435. Hegrenaes, O. and Hallingstad, O. (2011). Model-aided INS with sea current estimation for robust underwater navigation. IEEE J. Oceanic Eng., 36(2), 316–337. Koifman, M. and Bar-Itzhack, I. (1999). Inertial navigation system aided by aircraft dynamics. IEEE Trans. Contr. Syst. Technol., 7(4), 487–493. Koifman, M., Bar-Itzhack, I., and Mernav, S. (1995). Dynamics aided Inertial Navigation System. In Proc. AIAA-95-3195-CP. Guidance. – Navigation and Control Conference, 204–213. Morgado, M., Oliveira, P., and Silvestre, C. (2013). Tightly coupled ultrashort baseline and inertial navigation system for underwater vehicles: An experimental validation. Journal of Field Robotics, 30(1), 142–170. Mutuel, L. and Speyer, J. (2000). Fault–tolerant INS/GPS navigation system with application to unmanned aerial vehicle. In Proc. ION GPS 2000 Convention. Salt Lake City, Utah, USA. Perera, L., Wijesoma, W., and Adams, M. (2010). SLAM with joint sensor bias estimation: Closed form solutions on observability, error bounds and convergence rates. IEEE Trans. Contr. Syst. Technol., 18(3), 732 – 740. Sazdovski, V. and Silson, P. (2011). Inertial navigation aided by vision-based simultaneous localization and mapping. IEEE Sensors J., 11(8), 1646–1656. Strachan, V. (1997). The global positioning system as a sole means of navigation. In Proc. Civil Avionics ’97 Conference. Tanwani, A., Shim, H., and Liberzon, D. (2013). Observability for switched linear systems: Characterization and observer design. IEEE Trans. Automat. Contr., 58(14), 891–904. Vasconcelos, J., Silvestre, C., and Oliveira, P. (2011). INS/GPS aided by frequency contents of vector observations with application to autonomous surface crafts. IEEE J. Oceanic Eng., 36(2), 347 – 363. Vasconcelos, J., Silvestre, C., Oliveira, P., and Guerreiro, B. (2010). Embedded UAV model and LASER aiding techniques for inertial navigation systems. Control Engineering Practice, 18(3), 262 – 278.