- Email: [email protected]
Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
Contents lists available at ScienceDirect
Journal of Applied Mathematics and Mechanics journal homepage: www.elsevier.com/locate/jappmathmech
Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions夽 C.E. Perelyaev ∗ , Yu.N. Chelnokov LLC Aero Special Project, Zhukovskii, Institute of Problems in Fine Mechanics and Control of the Russian Academy of Sciences, Saratov, Russia
a r t i c l e
i n f o
Article history: Received 15 October 2016 Available online xxx
a b s t r a c t Equations and algorithms for determining the orientation of a moving object in inertial and normal geographic coordinate systems are considered with separation of the integration of the fast and slow motions into ultrafast, fast and slow cycles. Ultrafast cycle algorithms are constructed using a Riccati-type kinematic quaternion equation and the Picard method of successive approximations, and the increments in the integrals of the projections of the absolute angular velocity vector of the object onto the coordinate axes (quasicoordinates) associated with them are used as input information. The fast cycle algorithm realizes the calculation of the classical rotation quaternion of an object on a step of the fast cycle in an inertial system of coordinates. The slow cycle algorithm is used in calculating the orientation quaternion of an object in the normal geographic coordinate system and aircraft angles. Results of modelling different versions of the fast and ultrafast cycle algorithms for calculating the inertial orientation of an object are presented and discussed. The experience of the authors in developing algorithms for determining the orientation of moving objects using a strapdown inertial navigation system is described and results obtained by them earlier in this field are developed and extended. © 2017 Elsevier Ltd. All rights reserved.
The authors of this paper have been occupied with the development of the theory and algorithms of strapdown inertial navigational systems (SINS) from the middle of the 1970s.1–14 Their experience in developing the provision of algorithms for SINS in relation to the construction of on-board algorithms for determining the orientation of moving objects in inertial and normal geographic coordinate systems (NGCS) is extended in this paper. Ultrafast, fast and slow cycles of the orientation algorithms of SINS which, using the on-board computer, carry out the separate integration of the fast and slow angular motions of an object and the (NGCS) using initial integral information from the gyroscopes concerning the rotational motion of the object are also considered. The immediate use of this information makes it impossible to employ classical methods for integrating the differential equations of the inertial orientation of an object and requires the development of special algorithms for integrating these equations. In the ultrafast and fast cycles, the fast absolute motions of the object are integrated in the inertial system of coordinates and, in the slow cycle, the slow angular motions of the NGCS are integrated in the same system of coordinates. The fast cycle algorithms were constructed using a Riccati-type kinematic quaternion equation (KQE)7 (also, see Refs 9–14) and the Picard method of successive approximations. The rotation quaternion x with a zero scalar part appears as the variable and a correspondence is established with the classical Hamiltonian rotation quaternion 7,15,16 using a quaternion analogue of Cayley’s formula.7,17 The algorithms are of the second, third and fourth order of accuracy and, using the initial integral information from the gyroscopes regarding the rotational motion of the object, on one step of the ultrafast cycle they form the increment ␦x in the rotation quaternion x in the inertial system of coordinates. To sum the increments ␦x1 , ␦x2 ,..., ␦xn within the ultrafast cycle (to form the increment x of the rotation quaternion x on a step of the fast cycle), a new formula for the addition of the finite rotations described by the quaternions ␦xi or approximations of it of different orders of accuracy12,14 is used. The fast cycle algorithm realizes the change from the increment x in the rotation quaternion x to the increment of the classical rotation quaternion on a step of the fast cycle using a quaternion analogue of Cayley’s formula7,17 and also implements the quaternion
夽 Prikl. Mat. Mekh., Vol. 81, No. 1, pp. 18–32, 2017. ∗ Corresponding author. E-mail addresses: [email protected] (C.E. Perelyaev), [email protected] (Yu.N. Chelnokov). http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002 0021-8928/© 2017 Elsevier Ltd. All rights reserved.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 2
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
formula for the addition of finite rotations proposed by Branets and Shmyglevskii7,15,16 for calculating the orientation quaternion of an object in the inertial system of coordinates at the current instant. Results of the mathematical modelling of different versions of the fast and ultrafast cycle algorithms for calculating the inertial orientation parameters of the object are presented and analysed later. The slow cycle algorithm is used to calculate the classical orientation quaternion in the NGCS using increments (on a step of the slow cycle) of the classical orientation quaternions and the NGCS in the inertial system of coordinates. The algorithm either implements a quaternion computational scheme in increments or a matrix computational scheme using commutating quaternion matrices of two types.7,18 The effect of the commutativity of quaternion matrices enables the construction of simpler algorithms for the slow cycle. The quaternion of the inertial orientation of the NGCS is calculated using numerical integration of the KQE describing the rotation of the NGCS by the mean velocity method (with an algorithm of the second order accuracy) using information generated by the SINS. It has already been noted that the separation of the SINS orientation algorithms into ultrafast, fast and slow cycles makes it possible, using the on-board computer, to accomplish the separate integration of the fast and slow angular motions of an object and the NGCS which allows us to increase the accuracy of the numerical solution of the orientation problem and to reduce the load on the on-board computer as well as to develop compact highly accurate specialized algorithms for the integration of fast motions (that is, the absolute angular motions of an object) that make immediate use of the initial integral information from the gyroscopes concerning the angular motion of the object. Note that the introduction of ultrafast and fast cycles allows us to compensate for the error in the determination of the inertial orientation of manoeuvrable and highly manoeuvrable aircraft due to conical motions of the base of the SINS caused by high frequency vibrations, angular accelerations and other dynamic loadings and overloadings of the base of the SINS. Conical motions of the base of the inertial sensing elements are a considerable source of errors and noise in contemporary SINS. To compensate for them, it is therefore necessary to calculate the motions in the ultrafast SINS cycle with a frequency that exceeds the frequency of the motions of the base of the SINS by a minimum of 10 to 20 times. An increase in the accuracy of the determination of the orientation of objects is essential for all SINS. A reduction in the load on the onboard computer is especially urgent for the SINS used in systems for the orientation, navigation and control of the motion of small objects of civilian and military importance (miniature robots, drones, missiles, bombs (“smart” weapons) since the resources of the on-board computers (processors) of the navigation and motion control systems of such objects are quite limited. These and other reasons that give rise to the need to separate the SINS orientation algorithms into ultrafast, fast and slow cycles are considered in greater detail in Sections 2 and 3.
1. Equations of the problem of determining the orientation of a moving object using a SINS in Euler (Rodrigues–Hamilton) parameters The operational algorithms of a SINS, that serve to determine the orientation of a moving object in the inertial and normal geographic coordinate systems, enable the on-board calculation of some or other of its orientation parameters using signals from the sensors of the SINS (the projections of the absolute angular velocity vector of the object onto the axes fixed to the object (or the increments in the integrals of these projections)) and information on the geographic latitude and longitude of the location of the object or information on the projections of the absolute angular velocity of the NGCS on its coordinate axes and the altitude and latitude of the object. The construction of these algorithms is based on the equations for the ideal operation of a SINS, that is, on the basis of differential and functional relations connecting the orientation parameters of an object with the quantities measured by the SINS sensors (subject to the condition of their ideal operation) or ideally generated by the SINS. Different versions of the equations for the ideal operation of a SINS are possible3–7,16,19–24 that differ in the form of the kinematic orientation and navigation parameters used and in the choice of the coordinate systems in which the differential orientation and navigation equations are integrated. In solving problems of navigation and determining the object orientation using a SINS, it is necessary to integrate the kinematic orientation equations in some form or other and to transform the coordinates by means of some or other kinetic parameters. Euler–Krylov angles, direction cosines and Euler (Rodrigues–Hamilton (RH)) parameters are among the most widely used kinematic parameters in inertial navigation. Inertial orientation and navigation equations and relations in direction cosines and Euler parameters have well known analytical and computational advantages over the equations and relations in Euler–Krylov angles.1,2,7,15,16,25,26 Euler–Krylov angles are therefore not competitively capable in the majority of problems concerning the orientation and navigation of a moving object solved by means of a SINS when compared with direction cosines and Euler (RH) parameters. When direction cosines are used in the equations for the ideal operation of a SINS, differential equations for the object orientation and the NGCS in direction cosines are employed that have the form of kinematic Poisson equations. Analysis shows2,7 that, in the case when an algorithm of first order accuracy (that is, the simplest Euler method that is the first approximation to the mean velocity method2,7,15,23 ) is used to integrate the differential orientation equations, direction cosines have an advantage in the sense of the volume of calculations carried out in one step in the execution of the inertial orientation equations of the SINS. In cases when algorithms of the second or higher order of accuracy are used to integrate the orientation equations, Euler parameters have the advantage in this sense (in particular, this already holds when used in the algorithm for the numerical integration of the second approximation to the mean velocity method). When integrating the orientation equations in Euler parameters and the similar equations in direction cosines using an algorithm of first order accuracy, the step size in the integration must be small (as a rule, it is thousandths of a second). Numerical integration algorithms of the second and third orders of accuracy allow the step size to be considerably increased (by an order of magnitude or more) and the load on the on-board computer to be reduced. It should also be noted that the numerical integration algorithm in Euler parameters that executes one of the basic methods, that is, the mean velocity method widely used in SINS for cosmic assignments,23 gives a better accuracy in determining the orientation of an object than the similar algorithm in direction cosines.2,7 Moreover, in determining the object orientation with respect to the normal system of coordinates (geographic or orthodromic), the use of Euler parameters allows a rational computational scheme in increments that utilize the effect of the commutability of the two types of quaternion matrices to be obtained.7 According to the above discussion, the equations for the ideal operation of a SINS, written using Euler (RH) parameters, have, in the majority of cases, an advantage over equations written using Euler–Krylov angles or direction cosines. The equations and algorithms for determining the orientation of a moving object using a SINS are therefore written here in Euler parameters as the basic parameters. Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
3
The following systems of coordinates are used in the treatment of moving object orientation problems: O1 1 2 3 () is a geocentric inertial system of coordinates with its origin at the centre of mass of the Earth that is considered as an ellipsoid of revolution. The 1 axis is directed along the polar axis of the Earth (along the vector u of the angular velocity of the rotation of the Earth) and the 2 and 3 axes are located in the equatorial plane and do not participate in the daily rotation of the Earth. O1 1 2 3 () is a geocentric coordinate system rigidly associated with the Earth. The 1 axis is directed along the 1 axis, the 2 and 3 axes are located in the equatorial plane and the 2 axis coincides with the line of intersection of the equatorial plane and the Greenwich meridian. O2 Y1 Y2 Y3 (Y) is the NGCS, the origin O2 of which coincides with one of the points of the object (with the point of location of the Newton meter sensor masses). The Y2 axis is directed to the north along the tangent to the meridian h of the ellipsoid towards the north and the Y3 axis is directed to the east along the tangent to the parallel. O2 X1 X2 X3 (X) is the coordinate system associated with the object. The X1 axis is directed along the longitudinal axis of the object, the X2 axis is directed along the normal axis and the X3 axis is directed along the transverse axis. We will specify the mutual orientation of the coordinate trihedra by means of the natural rotation quaternions , v and , the components of which are the Euler (RH) parameters j , vj and j (j = 0, 1, 2, 3) respectively. The quaternions and v characterize the orientation of the object (of the trihedron X) in the inertial and geographic coordinate systems and Y respectively and the quaternion characterizes the orientation of geographic coordinate system Y in the inertial coordinate system . The classical KQE for the rotational motion of a rigid body7,15,16
(1.1) is often used to determine the inertial orientation of a moving object (its orientation in the inertial coordinate system) and the equation of the angular motion of the object. Here, the quaternion x is the mapping of the vector of the absolute angular velocity of the object onto the coordinate axes Xi associated with it (i is the projection of the vector onto the associated coordinate axis Xi ), i1 , i2 , i3 are the Hamiltonian imaginary unit vectors, the central small circle is the quaternion multiplication symbol and the superscript dot denotes a derivative with respect to the time t (when differentiating a quaternion, only its scalar components are differentiated). In the linear KQE (1.1), the classical rotation quaternion , that has a norm equal to unity, serves as the variable. A system of four linear differential equations in the Euler (RH) parameters j having the form
(1.2) corresponds to the quaternion equation in scalar notation. The orientation of a moving object in the NGCS Y can be defined by the quaternion v using the quaternion relation (1.3) From here on, an upper bar is the symbol of quaternion conjugation. The components j of the quaternion for the orientation of the NGCS in the inertial coordinate system appearing in equalities (1.3) can be found in terms of the geographic latitude and longitude of the location of the object which are produced by the SINS or obtained from external sources (for example, employing the satellite navigation system GLONASS) using the relations
(1.4) where a is the absolute longitude of the moving body, 0 is the value of the angle of rotation of the coordinate system with respect to at the initial instant t and u is the angular velocity of the daily rotation of the Earth. The components j of the quaternion , when the north N , vertical H and east E components of the object relative velocity (with respect to the Earth surface) are calculated in the SINS, can be found by integrating the KQE: (1.5)
(1.6) In Eq. (1.5), y is the mapping of the vector of the absolute angular velocity of the NGCS onto its coordinate axes (N , H and E are the north, vertical and east components of the angular velocity of the NGCS), In relations (1.6), H is the height of the object above sea level and the fundamental geodesic constants and parameters of the universal Earth ellipsoid, taken in the PZ-90.02 coordinate system have Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 4
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
the following values: the angular velocity of the Earth u = 7.292115 · 10−5 1/s, the square of the first eccentricity e2 = 0.006694368, and the semi–major axis of Earth’s ellipsoid of revolution A = 6378136 m. In scalar notation, the system of four differential equations in the Euler (RH) parameters j
(1.7) corresponds to Eq. (1.5). The orientation of a moving object in the NGCS Y can also be determined by the quaternion v by integrating the KQE for the relative motion of the object (1.8) A system of four differential equations in the Euler (RH) parameters vj of the form
(1.9) corresponds to this KQE in scalar notation. Differential equations (1.8) and (1.9) must be supplemented with algebraic relations (1.6) for the components N , H and E of the absolute angular velocity of the NGCS. From an analytical point of view, the convenience in using KQE (1.8) or scalar equations (1.9) lies in their compactness (instead of the two KQE (1.1) and (1.5) or the eight scalar differential equations (1.2), (1.7) and the quaternion algebraic relation (1.3), only one KQE (1.8) or the four scalar differential equations (1.9) is used) as well as in the fact, that the projections i of the vector of the absolute angular velocity of the object onto the axes associated with the object, measured on board the object with a spatial absolute angular velocity measuring device and the projections N , H and E of the absolute angular velocity vector of the NGCS on its coordinate axes generated by the inertial navigation system are immediately used in it. The relations
are used to calculate the aircraft angles (the geographical course , the pitching and the bank ␥). Here, cij (i, j = 1, 2, 3) are the direction cosines of the angles between the axes of the coordinate system associated with the object and the axes of the NGCS. Note that there are special algorithms that also use the values of other elements cij of the matrix of the direction cosines of the angles found in terms of the Euler parameters vj for the unique determination of the angles , and ␥ in terms of the Euler parameters vj . 2. Special features of the integration of the SINS quaternion orientation equations in the on-board computer The inertial orientation of a moving object can be determined in Euler (RH) parameters using real time numerical integration of linear KQE (1.1) in the on-board SINS computer using the instantaneous i (t) measured by the gyroscopes or integral initial information concerning the absolute angular motion of the object (the increments in the integrals ␥i of the projections i of the absolute angular velocity vector of the object) measured on board using the gyroscopes. The specific properties of the algorithms for the integration of KQE (1.11) are stipulated by the fact that, in many cases, no instantaneous information is available on board concerning the absolute angular motion of the object but integral initial information (that is, not the values of i but the increments in the integrals ␥i of these quantities). Direct use of the classical numerical methods for the integration of differential orientation equations such as, for example, the widely used Runge–Kutta method is found to be impossible in these cases. Moreover, it should be noted that algorithms for the numerical integration of KQE (1.1) that use integral initial information concerning the absolute angular motion of an object are preferable to the classical numerical integration algorithms that make direct use of the values of the projections j not only because, in the majority of cases, the devices measuring the absolute angular velocity of the object (the SINS sensors) produce integral initial information rather than instantaneous information, but also because of the fact that, as modelling has shown, a better accuracy in the determination of the orientation of the object is achieved using them when there are instrumental errors (noise in the readings) in the gyroscopes. Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
5
It has already been pointed out that an algorithm for determining the orientation of an object relative to the NGCS can be constructed that is either based on the KQE for the relative angular motion of the object (1.8) or based on KQE (1.1) and (1.5) for the absolute angular motions of the object and the NGCS using the algebraic quaternion relation (1.3). The coefficients i of KQE (1.8) characterize the instantaneous absolute angular motion of the object and are rapidly varying functions of time, and the coefficients N , H and E of this equation characterize the instantaneous absolute angular motion of the NGCS and are slowly changing functions of time. The quantities i are several orders of magnitude greater than the quantities N , H and E . Nevertheless, in spite of the fundamental difference in the instantaneous absolute angular motions of the object and the accompanying trihedron (NGCS), they are jointly integrated using KQE (1.8) by the same numerical method (since the instantaneous fast angular motion of the object and the instantaneous slow absolute angular motion of the accompanying trihedron are simultaneously encompassed by these equations). In this case, the algorithms of the second and higher order of accuracy for the numerical integration of KQE (1.8) that use integral initial information concerning the angular motion of the object turn out to be complex and bulky. There are additional difficulties associated with their execution in the on-board digital computer with the required degree of accuracy due to the limitedness of the word length of the on-board digital computer (on account of the different orders of magnitude of i and N , H and E ). When the one KQE (1.8) is replaced by the combination of KQE (1.1) and (1.5), there is a natural separation of the angular motions into the fast absolute angular motions of the object (described by KQE (1.1)) and the slow absolute angular motions of the NGCS (described by KQE (1.5)). Hence, when KQE (1.1) and (1.5) are used, the instantaneous fast and slow motions are integrated separately, and numerical methods of integration that differ in complexity can be used for this (for example, an algorithm that realizes the first or second approximation of the mean velocity method that is of second order accuracy can be used to integrate KQE (1.5) and an algorithm of third or fourth order accuracy for the integration of KQE (1.1)). Instrumental errors in the accelerometers, that appear as an error in the formation of the quantities N , H and E (as in the integration of KQE (1.8)) will have no effect on the accuracy of the integration of the instantaneous absolute motions of the object. Furthermore, when the orientation parameters vj characterizing the orientation of the object in the NGCS are calculated by integrating KQE (1.1) and (1.5) and using relation (1.3), an efficient computational scheme in increments can be used which, by utilizing the effect of the commutativity of the two types of quaternion matrices and the properties of Euler parameters, allows the construction of the most rational algorithms for calculating the parameters vj . The following conclusion, based on the above discussion, can be drawn: the use of KQE (1.1) and (1.5), that describe the absolute angular motions of an object and the NGCS, for constructing algorithms for the orientation of an object in the NGCS is preferable to the use of KQE (1.8) that describes the angular motion of an object relative to the NGCS. This serves as the justification for integrating the fast motions of the object in ultrafast and fast cycles and integrating the slow motions of the NGCS in a slow cycle. The advisability of separating the fast motions of an object into ultrafast and fast cycles is justified in Section 3.
3. The use of auxiliary variables in determining the inertial orientation of a moving object The approach to the determination of the inertial orientation of a moving object, that is described in Sections 1 and 2 and based on the direct integration of KQE (1.1) for calculating the inertial orientation quaternion of the object, is widely used in the creation of the mathematical support for modern SINS7,15,16,23 together with other approaches7–14,16,23,27–30 based on the use of auxiliary (intermediate) variables that also describe the inertial orientation of the object but satisfy other kinematic equations. Here, the quaternion is calculated using the corresponding formulae in terms of the auxiliary variables with an equal or greater discreteness than the increments in the auxiliary variables. The advisability of using auxiliary variables calculated in the ultrafast cycle is explained by the following reasons. ∗
∗
∗
1. The component 0 of the scalar part and the components i (i = 1, 2, 3) of the vector part of the increment quaternion * of the ∗ quaternion variable , calculated on an integration step differ by several orders of magnitude: the magnitude of 0 is close to unity ∗ −5 while the quantities i are of the order of 10 and less (depending on the form of the angular motion of the object and the step size ∗ in the integration). The closeness of the component 0 of the increment * in the quaternion variable , calculated on an integration step, to unity can lead to a loss of accuracy in determining the orientation in the case of the small limited word length of the on-board ∗ ∗ computer. Unlike the increments 0 and i in the components of the quaternion variable , all the increments in the vector or quaternion components, or matrix auxiliary variable on the integration step have the same order of smallness equal to 10−5 or less. Hence, the above mentioned difficulty that arises when the quaternion variable is used will not arise when auxiliary variables are used. 2. The vector or quaternion of the absolute angular velocity of the moving object or the angular velocity matrix corresponding to them enters into the inhomogeneous parts of the kinematic differential equations which the auxiliary variables satisfy in an additive way, unlike in the classical KQE (1.1) in which the quaternion x of the absolute angular velocity of the object only enters multiplicatively. The increment in the integral of the absolute angular velocity vector of the object on the integration step that is directly measured on board the moving object by the majority of modern angular velocity sensors will therefore be the first approximation of the solutions for the auxiliary variables on the integration step constructed by the Picard method of successive approximations. This makes the above equations more convenient in constructing the ultrafast cycles of algorithms for determining the orientation of a moving object12,23,30 that use integral initial information on the motion of the object compared to the classical KQE (1.1) (or scalar equations (1.2)). The orientation quaternion is calculated here in the fast cycles of the orientation determining algorithms using the results of the ultrafast cycle calculations as the input information for these algorithms.
4. Ultrafast, fast and slow cycles of the SINS orientation algorithms It is advisable (from the point of view of ensuring the required accuracy in determining the orientation and the volume of necessary calculations) to separate the SINS orientation algorithms, intended for determining the orientation of a moving object in the inertial and normal geographic coordinate systems, into ultrafast, fast and slow cycles. The ultrafast cycle algorithms are intended for the integration of fast absolute angular motions of an object using intermediate variables. The fast cycle algorithm calculates the classical rotation quaternion Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
6
of an object in a step of the fast cycle in the inertial coordinate system. The slow cycle algorithm is used to calculate the classical orientation quaternion of an object in the NGCS and the aircraft angles (yaw, pitch and roll). 4.1. Ultrafast cycle Ultrafast cycle algorithms can be constructed using the Riccati-type KQE7,11,13,14 (4.1) and the Picard method of successive approximations (from here on, × is the symbol for a vector product) In Eq. (4.1), the rotation quaternion x with a zero scalar part serves as the variable. This quaternion is matched with the classical Hamiltonian rotation quaternion (see the second formula of (1.1)) using a quaternion analogue of Cayley’s formula;7,11,13,14
The components of the quaternion x are defined by the formulae xi = tg(/4)ei (i = 1, 2, 3) in which is the Euler rotation angle of the moving object in the inertial coordinate system, and and ei are the projections of the unit vector e of the Euler axis of finite rotation of the moving object onto the axes of the inertial coordinate system and the coordinate system associated X (they are the same if the axes of the same kind of X and systems coincide in the initial position). As usual, the Euler (RH) parameters j (j = 0, 1, 2, 3), characterizing the object orientation in the inertial basis , are the components of the rotation quaternion . The quaternion x is also used in Eq, (4.1) (see the last formula of (1.1)). Note that the finite rotation vectors = tg(/4)e and = ctg(/4)e and the corresponding kinematic vector equations as well as the fourth order of accuracy two-step algorithm for the numerical integration of the differential equation for the vector = tg(/4)e using integral initial information concerning the rotational motion of an object have been considered by Panov.28,29 So, the kinematic vector equation for the vector = tg(/4)e has the form28,29 (4.2) where the central dot is the symbol for a scalar product of vectors, and i is the projection of the vector onto the associated coordinate axis Xi . The Bortz vector differential equation27 (4.3) is frequently used in constructing ultrafast cycle algorithms, where = e is the Euler finite rotation vector of a rigid body. Local derivatives with respect to time are used in Eqs (4.2) and (4.3). The quaternion x corresponds to a four-dimensional skew-symmetric matrix (SM4) and the vectors and to three-dimensional skewsymmetric matrices (SM3). It is known7,17 that SM3 (of odd order) and SM4 (of even order) have qualitatively different properties: SM3 are singular (their determinants are equal to zero) but SM4 are not (their determinants always differ from zero). Moreover, a polynomial of any degree of an SM3 reduces to a second degree polynomial, but a polynomial of any degree of an SM4 reduces to a first degree polynomial. This makes it more convenient and effective to use KQE (4.1) in constructing algorithms for determining the orientation of moving objects using a SINS compared to the kinematic vector equations (4.2) and (4.3). The ultrafast cycle algorithms proposed by us are of the second and third order of accuracy and form an increment ␦x in the rotation quaternion x on a step h of the ultrafast cycle using integral initial information on the rotational motion of the object. A new formula for the addition of the finite rotations and its approximations of different order of accuracy12,14 is used for summing the increments in the rotation quaternion x within the ultrafast cycle. The ultrafast cycle is carried out on the time interval [tm−1 , tm ], equal to the step size H of the fast cycle with a step size h = tn − tn−1 = H/l using the algorithms cited below (l is the number of steps in the ultrafast cycle). 4.1.1. Ultrafast cycle algorithms 1. Algorithms for calculating the increments ␦xn in the quaternion variable x occurring in Eq. (4.1) on a step h of the ultrafast cycle based on algorithms proposed by the authors:8–11 the simplified algorithm (4.4) and the unsimplified algorithm (4.5) Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
7
that are one-step algorithms of third order accuracy, where
and a, b and c are numerical coefficients. Other ultrafast cycle algorithms of third order accuracy (one-step algorithms) and fourth order accuracy (two-step algorithms that do not require a preliminary run of the algorithms), constructed using the Riccati-type kinematic quaternion equation (4.1), have also been proposed.8–11 2. Algorithms for calculating the increment x = xj in the quaternion x on a fast cycle step using exact or simplified formulae for adding the increments in the quaternion x within the ultrafast cycle obtained by the authors:12,14 algorithms of the second (4.6) and third (4.7) order of accuracy and the exact algorithm (4.8) where n = 1, 2,..., l. The quaternion
calculated using one of the ultrafast cycle algorithms (4.4), (4.6); (4.4), (4.7); (4.4), (4.8); (4.5), (4.6); (4.5), (4.7); (4.5), (4.8) is used to calculate the orientation quaternion (tm ) in the fast cycle. 3. Savidge’s23,30 ultrafast cycle algorithm that uses the Euler rotation vector :
(4.9) ∗0
0, ∗1
␥1 , ∗2
␥1 + ␥2 , ∗3
= = = = ␥1 + ␥2 + ␥3 , . . .when n = 1, 2, 3, 4 respectively. In this algorithm, Note that an algorithm similar to (4.9) was obtained by Branets.23 4.2. Fast cycle The proposed fast cycle algorithm realizes the change from the increment x = xj in the rotation quaternion x to the increment * in the classical rotation quaternion on the step of the fast cycle using a quaternion analogue of Cayley’s formula and also executes the quaternion formula for the addition of finite rotations proposed by Branets and Shmyglevskii15 for calculating the orientation quaternion of an object in the inertial coordinate system. To determine only the inertial orientation of the object, the fast cycle is carried out once with a step size H = tm − tm−1 on the time interval [tm−1 , tm ] using the general quaternion recurrence scheme
or, in vector notation, according to the scheme
∗
∗
∗
∗
where and are the vector parts of the quaternions and * or vectors with the components 1 , 2 , 3 and 1 , 2 , 3 respectively, (tm−1 ) and (tm ) are the inertial orientation quaternions at the instants tm - 1 and tm respectively, and * is the increment in the inertial orientation quaternion in the time interval H = tm − tm−1 . Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
8 ∗
∗
The scalar 0 and vector parts of the quaternion *, when the KQE in the skew-symmetric operators (4.1) is used, are calculated using the formulae
Here, x = x(tm ) is the increment in the quaternion variable x with a zero scalar part on the step H of the fast cycle calculated at the end of the ultrafast cycle using one of the algorithms presented in Subsections 1 and 2, and x is the norm of the quaternion x. When the Bortz kinematic equation (4.3) is used, the increment * in the inertial orientation quaternion is calculated using the known formula
where m = (tm ) is the increment in the vector variable on the step H of the fast cycle, calculated at the end of the ultrafast cycle using algorithm (4.9). When there is a slow cycle of calculating the orientation of the object in the NGCS, the fast cycle, as a rule, consists of p steps rather than of one step. In this case, the duration of the fast cycle pH = H*, where H* is the step in the slow cycle of the calculations. 4.3. Slow cycle The slow cycle algorithm is used to calculate the classical orientation quaternion of the object in the NGCS using the increments (on a step of the slow cycle) in the classical orientation quaternion of the object and the NGCS in the inertial coordinate system. The algorithm either executes a quaternion computational scheme in increments or a matrix computational Scheme7 that uses two types of commutating quaternary matrices. In the first case, the calculations are carried out using the quaternion formula
(4.10) ∗
where v is the orientation quaternion of the object in the NGCS, s and L∗s are the increments in the orientation quaternions and and the ∗ NGCS on the slow cycle step in the inertial coordinate system, and js and ∗js (j = 0, 1, 2, 3) are the increments in the Euler (RH) parameters ∗
∗
j and j (the increments in the quaternions and ) on the slow cycle step. In the case when the fast cycle consists of a one step, s = . The special feature of quaternion formula (4.10) lies in not using the complete orientation quaternions and when determining the relative orientation of the object but their increments on the step of the slow cycle and, here, the summation of the increments is but of a multiplicative character corresponding to the multiplicative quaternion formula for the addition of finite rotations, rather than of an additive character. In the second case, the calculations are carried out using the matrix formula
(4.11) ∗
∗
where n{s } and m{∗s } are quaternion rotation matrices of types n and m7 that are compared with the quaternions s and ∗s , the superscript T is the transposition symbol, vs and vs−1 are four-dimensional column vectors composed of the Euler parameters vsj and vs−1j (the components of the quaternions vs and vs−1 ). Use is made of the property of the commutativity of the two types of quaternion matrices ∗ (n and m) when obtaining matrix algorithm (4.11) which enabled the matrix product n{s }mT {∗s } to be separated out, the elements of ∗ which are the increments js and ∗js j = 0, in the Euler parameters. This matrix notation of the algorithm for finding the parameters for ∗
the relative orientation of the object allows the algorithm to be simplified using the smallness of the increments is and ∗is (i = 1, 2, 3). The increment ∗s in the inertial orientation quaternion of the NGCS is calculated using numerical integration of KQE (1.5), describing the rotation of the NGCS in the inertial coordinate system, by the mean velocity method (an algorithm of second order accuracy) which, in scalar notation, has the form
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
9
5. Results of the mathematical modelling of the ultrafast and fast orientation algorithms The accuracy of the determination of the inertial orientation of an object is mainly determined by the accuracy of the ultrafast and fast cycle algorithms. We therefore present the results of the modelling of these algorithms carried out by L. A. Chelnokova. The methodical errors
in the determination of the orientation angles , and ␥ (yaw, pitch and roll) were estimated using the ultrafast and fast cycle algorithms in the presence of harmonic oscillations of the object with respect to each of the angular variables for small (+ =1E, + = 2E, ␥+ = 3E) and large (+ =15E, + = 5E, ␥+ = 15E) amplitudes of the angular harmonic oscillations of the object. The angular frequencies of the oscillations were assumed to be equal in both cases: = ␥ = 2 rad/s, = rad/s (frequencies of the oscillations: f = f␥ = 1 Hz, f = 0.5 Hz). When the object orientation was determined using a one-step orientation algorithm of third order accuracy11,13 (without separation of the algorithm for determining the orientation into ultrafast and fast cycles) the integration step size was chosen to be equal to from 0.0005s to 0.1s. In determining the orientation of the object using the ultrafast and fast cycle algorithms, the step size h for the ultrafast cycle integration was chosen to be equal to 0.0005 s or 0.001 s. The time of the motion of the object (of the integration) was 600 s. Analysis of the results of the modelling allowed us to draw the following conclusions. 1. The use in ultrafast cycle orientation algorithms of the approximate formula (4.6) of second order accuracy for the addition of finite rotations described by the quaternions xi leads to a loss in accuracy in determining the object orientation by two to four orders while the use of the approximate formula for the addition of finite rotations of third order accuracy (4.7) gives practically the same accuracy as the exact (but more complex) formula (4.8) for the addition of finite rotations. 2. As would be expected, execution of the ultrafast cycle algorithm of third order accuracy (4.5), (4.8) or (4.5), (4.7) with the same integration step size as the traditional execution of the one-step orientation algorithm of third order accuracy (without separation into ultrafast and fast cycles) gives the same accuracy in determining the orientation as the traditional execution. 3. The simplified one-step ultrafast algorithm of third order accuracy (4.4), (4.8) or (4.4), (4.7) has an accuracy that is an order of magnitude less than the unsimplified one-step ultrafast cycle algorithm of third order of accuracy (4.5), (4.8) or (4.5), (4.7) (the accuracy with respect to the angles and ␥ is lower by one and a half orders of magnitude). 4. A reduction in the step size of the ultrafast cycle by a factor of two increases the accuracy of the determination of the orientation by a factor of five to eight. 5. The simplified one-step ultrafast cycle algorithm of third order accuracy (4.4), (4.7) has an accuracy in determining the inertial orientation of an object that exceeds the accuracy of Savidge’s algorithm (4.9) which integrates the kinematic Bortz equation by one and a half to two orders of magnitude and the unsimplified one-step ultrafast cycle algorithm of third order accuracy (4.5), (4.7) by more than two to four orders of magnitude (for large amplitudes of the angular oscillations of the object and a step size h = 0.001s). For large amplitudes of the angular oscillations and a step size h = 0.0005s, the accuracy of Savidge’s algorithm is comparable with the accuracy of the algorithm (4.4), (4.6). In this case, the methodical errors in Savidge’s algorithm with respect to the variables , and ␥ constitute the quantities (in degrees) 7.27 H 10−4 , 7.11 H 10−5 and 1.11 H 10−3 respectively and the algorithm (4.4), (4.6) gives the values 3.27 H 10−4 , 1.98 H 10−4 and 7.01 H 10−4 respectively. At the same time, the accuracy of the algorithm (4.5), (4.7) for this case of the object motion is much higher and constitutes the quantities (in degrees) 7.12 H 10−8 , 3.42 H 10−7 and 1.05 H 10−7 respectively. Acknowledgement We are grateful to L. A. Chelnokova for the large amount of mathematical modelling of the orientation algorithms carried out. References 1. Chelnokov YuN. On determining vehicle orientation in the Rodrigues–Hamilton parameters from its angular velocity. Mech Solids 1977;37(3):8–16. 2. Plotnikov PK, Chelnokov YuN. Comparative error analysis of algorithms for determining vehicle attitude in Rodrigues–Hamilton parameters and direction cosines. Cosmic Res 1979;17(3):308–13. 3. Chelnokov YuN, Petrov SV. On problems of the orientation and navigation of an object in geographical and orthodromic coordinate systems. Dep. in VIMI 27.05.88, No. D07701. 4. Chelnokov YuN, Chelnokova LA, Landenok IV. Algorithm for the ideal operation of an orientation system for a moving object. In: Problems of Aviation Science and Technology. Collection of Papers. 1988. p. 57–65. Issue10. 5. Chelnokova LA, Chelnokov YuN. Modelling of the operation of a strapdown inertial navigation system determining the object orientation in orthodromic and geographical coordinate systems on general-purpose digital computers. Sarat Politekh Inst Saratov 1988. Dep. in VINITI 11.05.88, No. 3763-B88. 6. Chelnokova LA, Chelnokov YuN. Modelling of the operation of a strapdown inertial navigation system on general-purpose digital computers. Sarat Politekh Inst Saratov 1989. Dep. in VINITI 13.06.89, No. 3909-B89. 7. Chelnokov YuN. Quaternion and Biquaternion Models and Methods of the Mechanics of a Rigid Body and their Applications. Geometry and Kinematics of Motion. Moscow: Fizmatlit; 2006. 8. Chelnokov YuN, Perelyaev SE, Chelnokova LA. New equations and algorithms for the orientation of a strapdown inertial navigation system in four-dimensional skewsymmetric operators. In: Collection of Synopses of Papers presented at the 14th Intern Scient Conf “System analysis, control and navigation”. Moscow: Izd MAI-PRINT; 2009. p. 35–6. 9. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Kinematic differential equations of the rotational motion of a rigid body with four-dimensional skew-symmetric operators and new orientation algorithms for a strapdown inertial navigation system. In: Materials of the All-Russian Scient Conf with Intern Particip “Problems of Critical Situations in Fine Mechanics and Control”. Saratov: OOO Izd Tsentr “Nauka”; 2013. p. 315–20. 10. Chelnokov YuN, Perelyaev SE. New equations and algorithms for orientation and navigation of SDINS with four-dimensional skew-symmetric operators. In: 21st St Petersburg Intern Conf on Integrated Navigation Systems ICINS 2014 Proc. St Petersburg: SRC RF CSRI “Elektropribor”;2014;365–9. 11. Perelyaev SE, Chelnokov YuN. New algorithms for determining the inertial orientation of an object. J Appl Math Mech 2014;78(6):560–7. 12. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Ultrafast, fast and slow loops in orientation algorithms for strapdown INS. In: 22nd St Petersburg Intern Conf on Integrated Navigation Systems ICINS 2015 Proceedings. St Petersburg: SRC RF CSRI “Elektropribor”; 2015. p. 240–4.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
13. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Investigation of algorithms for the determination of the inertial orientation of a moving object. Izv Sarat Univ Nov Ser Ser Mat Mekh Inform 2016;16(1):80–95. 14. Chelnokov YuN. Equations of the kinematics of a rigid body with four-dimensional skew-symmetric operators and their applications in inertial navigation. J Appl Math Mech 2016;80(6):525–40. 15. Branets VN, Shmyglevskii IP. The Application of Quaternions to the Rigid Body Orientation Problems. Moscow: Nauka; 1973. 16. Branets VN, Shmyglevskii IP. Introduction to the Theory of Strapdown Inertial Navigation Systems. Moscow: Nauka; 1992. 17. Gantmacher FR. The Theory of Matrices. New York: Chelsea; 1974. 18. Bellman R. Introduction to Matrix Analysis. N.Y.: McGraw-Hill; 1970. 19. Andreyev VD. The Theory of Inertial Navigation. Autonomous Systems. Moscow: Fizmatgiz; 1966. 20. Zakharin MI, Zakharin FM. The Kinematics of Inertial Navigation Systems. Moscow: Mashinostr; 1968. 21. Bromberg PV. The Theory of Inertial Navigation Systems. Moscow: Nauka; 1979. 22. Anuchin ON, Yemel’yantsev GI. Strapdown Inertial Systems of Navigation and Orientation (SDINS and SDIOS). Student Textbook. St Petersburg:ITMO;1995. 23. Branets VN. Lectures on the Theory of Strapdown Inertial Navigation Control Systems. Moscow: MFTI; 2009. 24. Matveyev VV, Paspopov VYa. Fundamentals of the Construction of Strapdown Inertial Navigation Systems. Student Textbook for the Speciality “Instrument Construction”. St Petersburg: GNTs RF TsNII “Elektropribor”; 2009. 25. Ishlinskii AYu. Orientation, Gyroscopes and Inertial Navigation. Moscow: Nauka; 1976. 26. Lurie AI. Analytical Mechanics. Berlin: Springer; 2001. 27. Bortz J.E. A new mathematical formulation for strapdown inertial navigation. IEEE Trans. Aero-space Electr. Syst. 1971. AES-7. No. 1. p. 61–66. 28. Panov AP. On the choice of kinematic parameters and equations for numerical integration using a digital computer. Kibern Vychisl Tekhn 1984;Issue 62:104–11. 29. Panov AP. Mathematical Foundations of the Theory of Orientation. Kiev: Nauk Dumka; 1995. 30. Savage PG. Strapdown inertial navigation integration algorithm design. Part 1: Attitude algorithms. J. Guidance Control Dynamics 1998;21(1):19–28.
Translated by E.L.S.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
Contents lists available at ScienceDirect
Journal of Applied Mathematics and Mechanics journal homepage: www.elsevier.com/locate/jappmathmech
Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions夽 C.E. Perelyaev ∗ , Yu.N. Chelnokov LLC Aero Special Project, Zhukovskii, Institute of Problems in Fine Mechanics and Control of the Russian Academy of Sciences, Saratov, Russia
a r t i c l e
i n f o
Article history: Received 15 October 2016 Available online xxx
a b s t r a c t Equations and algorithms for determining the orientation of a moving object in inertial and normal geographic coordinate systems are considered with separation of the integration of the fast and slow motions into ultrafast, fast and slow cycles. Ultrafast cycle algorithms are constructed using a Riccati-type kinematic quaternion equation and the Picard method of successive approximations, and the increments in the integrals of the projections of the absolute angular velocity vector of the object onto the coordinate axes (quasicoordinates) associated with them are used as input information. The fast cycle algorithm realizes the calculation of the classical rotation quaternion of an object on a step of the fast cycle in an inertial system of coordinates. The slow cycle algorithm is used in calculating the orientation quaternion of an object in the normal geographic coordinate system and aircraft angles. Results of modelling different versions of the fast and ultrafast cycle algorithms for calculating the inertial orientation of an object are presented and discussed. The experience of the authors in developing algorithms for determining the orientation of moving objects using a strapdown inertial navigation system is described and results obtained by them earlier in this field are developed and extended. © 2017 Elsevier Ltd. All rights reserved.
The authors of this paper have been occupied with the development of the theory and algorithms of strapdown inertial navigational systems (SINS) from the middle of the 1970s.1–14 Their experience in developing the provision of algorithms for SINS in relation to the construction of on-board algorithms for determining the orientation of moving objects in inertial and normal geographic coordinate systems (NGCS) is extended in this paper. Ultrafast, fast and slow cycles of the orientation algorithms of SINS which, using the on-board computer, carry out the separate integration of the fast and slow angular motions of an object and the (NGCS) using initial integral information from the gyroscopes concerning the rotational motion of the object are also considered. The immediate use of this information makes it impossible to employ classical methods for integrating the differential equations of the inertial orientation of an object and requires the development of special algorithms for integrating these equations. In the ultrafast and fast cycles, the fast absolute motions of the object are integrated in the inertial system of coordinates and, in the slow cycle, the slow angular motions of the NGCS are integrated in the same system of coordinates. The fast cycle algorithms were constructed using a Riccati-type kinematic quaternion equation (KQE)7 (also, see Refs 9–14) and the Picard method of successive approximations. The rotation quaternion x with a zero scalar part appears as the variable and a correspondence is established with the classical Hamiltonian rotation quaternion 7,15,16 using a quaternion analogue of Cayley’s formula.7,17 The algorithms are of the second, third and fourth order of accuracy and, using the initial integral information from the gyroscopes regarding the rotational motion of the object, on one step of the ultrafast cycle they form the increment ␦x in the rotation quaternion x in the inertial system of coordinates. To sum the increments ␦x1 , ␦x2 ,..., ␦xn within the ultrafast cycle (to form the increment x of the rotation quaternion x on a step of the fast cycle), a new formula for the addition of the finite rotations described by the quaternions ␦xi or approximations of it of different orders of accuracy12,14 is used. The fast cycle algorithm realizes the change from the increment x in the rotation quaternion x to the increment of the classical rotation quaternion on a step of the fast cycle using a quaternion analogue of Cayley’s formula7,17 and also implements the quaternion
夽 Prikl. Mat. Mekh., Vol. 81, No. 1, pp. 18–32, 2017. ∗ Corresponding author. E-mail addresses: [email protected] (C.E. Perelyaev), [email protected] (Yu.N. Chelnokov). http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002 0021-8928/© 2017 Elsevier Ltd. All rights reserved.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 2
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
formula for the addition of finite rotations proposed by Branets and Shmyglevskii7,15,16 for calculating the orientation quaternion of an object in the inertial system of coordinates at the current instant. Results of the mathematical modelling of different versions of the fast and ultrafast cycle algorithms for calculating the inertial orientation parameters of the object are presented and analysed later. The slow cycle algorithm is used to calculate the classical orientation quaternion in the NGCS using increments (on a step of the slow cycle) of the classical orientation quaternions and the NGCS in the inertial system of coordinates. The algorithm either implements a quaternion computational scheme in increments or a matrix computational scheme using commutating quaternion matrices of two types.7,18 The effect of the commutativity of quaternion matrices enables the construction of simpler algorithms for the slow cycle. The quaternion of the inertial orientation of the NGCS is calculated using numerical integration of the KQE describing the rotation of the NGCS by the mean velocity method (with an algorithm of the second order accuracy) using information generated by the SINS. It has already been noted that the separation of the SINS orientation algorithms into ultrafast, fast and slow cycles makes it possible, using the on-board computer, to accomplish the separate integration of the fast and slow angular motions of an object and the NGCS which allows us to increase the accuracy of the numerical solution of the orientation problem and to reduce the load on the on-board computer as well as to develop compact highly accurate specialized algorithms for the integration of fast motions (that is, the absolute angular motions of an object) that make immediate use of the initial integral information from the gyroscopes concerning the angular motion of the object. Note that the introduction of ultrafast and fast cycles allows us to compensate for the error in the determination of the inertial orientation of manoeuvrable and highly manoeuvrable aircraft due to conical motions of the base of the SINS caused by high frequency vibrations, angular accelerations and other dynamic loadings and overloadings of the base of the SINS. Conical motions of the base of the inertial sensing elements are a considerable source of errors and noise in contemporary SINS. To compensate for them, it is therefore necessary to calculate the motions in the ultrafast SINS cycle with a frequency that exceeds the frequency of the motions of the base of the SINS by a minimum of 10 to 20 times. An increase in the accuracy of the determination of the orientation of objects is essential for all SINS. A reduction in the load on the onboard computer is especially urgent for the SINS used in systems for the orientation, navigation and control of the motion of small objects of civilian and military importance (miniature robots, drones, missiles, bombs (“smart” weapons) since the resources of the on-board computers (processors) of the navigation and motion control systems of such objects are quite limited. These and other reasons that give rise to the need to separate the SINS orientation algorithms into ultrafast, fast and slow cycles are considered in greater detail in Sections 2 and 3.
1. Equations of the problem of determining the orientation of a moving object using a SINS in Euler (Rodrigues–Hamilton) parameters The operational algorithms of a SINS, that serve to determine the orientation of a moving object in the inertial and normal geographic coordinate systems, enable the on-board calculation of some or other of its orientation parameters using signals from the sensors of the SINS (the projections of the absolute angular velocity vector of the object onto the axes fixed to the object (or the increments in the integrals of these projections)) and information on the geographic latitude and longitude of the location of the object or information on the projections of the absolute angular velocity of the NGCS on its coordinate axes and the altitude and latitude of the object. The construction of these algorithms is based on the equations for the ideal operation of a SINS, that is, on the basis of differential and functional relations connecting the orientation parameters of an object with the quantities measured by the SINS sensors (subject to the condition of their ideal operation) or ideally generated by the SINS. Different versions of the equations for the ideal operation of a SINS are possible3–7,16,19–24 that differ in the form of the kinematic orientation and navigation parameters used and in the choice of the coordinate systems in which the differential orientation and navigation equations are integrated. In solving problems of navigation and determining the object orientation using a SINS, it is necessary to integrate the kinematic orientation equations in some form or other and to transform the coordinates by means of some or other kinetic parameters. Euler–Krylov angles, direction cosines and Euler (Rodrigues–Hamilton (RH)) parameters are among the most widely used kinematic parameters in inertial navigation. Inertial orientation and navigation equations and relations in direction cosines and Euler parameters have well known analytical and computational advantages over the equations and relations in Euler–Krylov angles.1,2,7,15,16,25,26 Euler–Krylov angles are therefore not competitively capable in the majority of problems concerning the orientation and navigation of a moving object solved by means of a SINS when compared with direction cosines and Euler (RH) parameters. When direction cosines are used in the equations for the ideal operation of a SINS, differential equations for the object orientation and the NGCS in direction cosines are employed that have the form of kinematic Poisson equations. Analysis shows2,7 that, in the case when an algorithm of first order accuracy (that is, the simplest Euler method that is the first approximation to the mean velocity method2,7,15,23 ) is used to integrate the differential orientation equations, direction cosines have an advantage in the sense of the volume of calculations carried out in one step in the execution of the inertial orientation equations of the SINS. In cases when algorithms of the second or higher order of accuracy are used to integrate the orientation equations, Euler parameters have the advantage in this sense (in particular, this already holds when used in the algorithm for the numerical integration of the second approximation to the mean velocity method). When integrating the orientation equations in Euler parameters and the similar equations in direction cosines using an algorithm of first order accuracy, the step size in the integration must be small (as a rule, it is thousandths of a second). Numerical integration algorithms of the second and third orders of accuracy allow the step size to be considerably increased (by an order of magnitude or more) and the load on the on-board computer to be reduced. It should also be noted that the numerical integration algorithm in Euler parameters that executes one of the basic methods, that is, the mean velocity method widely used in SINS for cosmic assignments,23 gives a better accuracy in determining the orientation of an object than the similar algorithm in direction cosines.2,7 Moreover, in determining the object orientation with respect to the normal system of coordinates (geographic or orthodromic), the use of Euler parameters allows a rational computational scheme in increments that utilize the effect of the commutability of the two types of quaternion matrices to be obtained.7 According to the above discussion, the equations for the ideal operation of a SINS, written using Euler (RH) parameters, have, in the majority of cases, an advantage over equations written using Euler–Krylov angles or direction cosines. The equations and algorithms for determining the orientation of a moving object using a SINS are therefore written here in Euler parameters as the basic parameters. Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
3
The following systems of coordinates are used in the treatment of moving object orientation problems: O1 1 2 3 () is a geocentric inertial system of coordinates with its origin at the centre of mass of the Earth that is considered as an ellipsoid of revolution. The 1 axis is directed along the polar axis of the Earth (along the vector u of the angular velocity of the rotation of the Earth) and the 2 and 3 axes are located in the equatorial plane and do not participate in the daily rotation of the Earth. O1 1 2 3 () is a geocentric coordinate system rigidly associated with the Earth. The 1 axis is directed along the 1 axis, the 2 and 3 axes are located in the equatorial plane and the 2 axis coincides with the line of intersection of the equatorial plane and the Greenwich meridian. O2 Y1 Y2 Y3 (Y) is the NGCS, the origin O2 of which coincides with one of the points of the object (with the point of location of the Newton meter sensor masses). The Y2 axis is directed to the north along the tangent to the meridian h of the ellipsoid towards the north and the Y3 axis is directed to the east along the tangent to the parallel. O2 X1 X2 X3 (X) is the coordinate system associated with the object. The X1 axis is directed along the longitudinal axis of the object, the X2 axis is directed along the normal axis and the X3 axis is directed along the transverse axis. We will specify the mutual orientation of the coordinate trihedra by means of the natural rotation quaternions , v and , the components of which are the Euler (RH) parameters j , vj and j (j = 0, 1, 2, 3) respectively. The quaternions and v characterize the orientation of the object (of the trihedron X) in the inertial and geographic coordinate systems and Y respectively and the quaternion characterizes the orientation of geographic coordinate system Y in the inertial coordinate system . The classical KQE for the rotational motion of a rigid body7,15,16
(1.1) is often used to determine the inertial orientation of a moving object (its orientation in the inertial coordinate system) and the equation of the angular motion of the object. Here, the quaternion x is the mapping of the vector of the absolute angular velocity of the object onto the coordinate axes Xi associated with it (i is the projection of the vector onto the associated coordinate axis Xi ), i1 , i2 , i3 are the Hamiltonian imaginary unit vectors, the central small circle is the quaternion multiplication symbol and the superscript dot denotes a derivative with respect to the time t (when differentiating a quaternion, only its scalar components are differentiated). In the linear KQE (1.1), the classical rotation quaternion , that has a norm equal to unity, serves as the variable. A system of four linear differential equations in the Euler (RH) parameters j having the form
(1.2) corresponds to the quaternion equation in scalar notation. The orientation of a moving object in the NGCS Y can be defined by the quaternion v using the quaternion relation (1.3) From here on, an upper bar is the symbol of quaternion conjugation. The components j of the quaternion for the orientation of the NGCS in the inertial coordinate system appearing in equalities (1.3) can be found in terms of the geographic latitude and longitude of the location of the object which are produced by the SINS or obtained from external sources (for example, employing the satellite navigation system GLONASS) using the relations
(1.4) where a is the absolute longitude of the moving body, 0 is the value of the angle of rotation of the coordinate system with respect to at the initial instant t and u is the angular velocity of the daily rotation of the Earth. The components j of the quaternion , when the north N , vertical H and east E components of the object relative velocity (with respect to the Earth surface) are calculated in the SINS, can be found by integrating the KQE: (1.5)
(1.6) In Eq. (1.5), y is the mapping of the vector of the absolute angular velocity of the NGCS onto its coordinate axes (N , H and E are the north, vertical and east components of the angular velocity of the NGCS), In relations (1.6), H is the height of the object above sea level and the fundamental geodesic constants and parameters of the universal Earth ellipsoid, taken in the PZ-90.02 coordinate system have Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 4
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
the following values: the angular velocity of the Earth u = 7.292115 · 10−5 1/s, the square of the first eccentricity e2 = 0.006694368, and the semi–major axis of Earth’s ellipsoid of revolution A = 6378136 m. In scalar notation, the system of four differential equations in the Euler (RH) parameters j
(1.7) corresponds to Eq. (1.5). The orientation of a moving object in the NGCS Y can also be determined by the quaternion v by integrating the KQE for the relative motion of the object (1.8) A system of four differential equations in the Euler (RH) parameters vj of the form
(1.9) corresponds to this KQE in scalar notation. Differential equations (1.8) and (1.9) must be supplemented with algebraic relations (1.6) for the components N , H and E of the absolute angular velocity of the NGCS. From an analytical point of view, the convenience in using KQE (1.8) or scalar equations (1.9) lies in their compactness (instead of the two KQE (1.1) and (1.5) or the eight scalar differential equations (1.2), (1.7) and the quaternion algebraic relation (1.3), only one KQE (1.8) or the four scalar differential equations (1.9) is used) as well as in the fact, that the projections i of the vector of the absolute angular velocity of the object onto the axes associated with the object, measured on board the object with a spatial absolute angular velocity measuring device and the projections N , H and E of the absolute angular velocity vector of the NGCS on its coordinate axes generated by the inertial navigation system are immediately used in it. The relations
are used to calculate the aircraft angles (the geographical course , the pitching and the bank ␥). Here, cij (i, j = 1, 2, 3) are the direction cosines of the angles between the axes of the coordinate system associated with the object and the axes of the NGCS. Note that there are special algorithms that also use the values of other elements cij of the matrix of the direction cosines of the angles found in terms of the Euler parameters vj for the unique determination of the angles , and ␥ in terms of the Euler parameters vj . 2. Special features of the integration of the SINS quaternion orientation equations in the on-board computer The inertial orientation of a moving object can be determined in Euler (RH) parameters using real time numerical integration of linear KQE (1.1) in the on-board SINS computer using the instantaneous i (t) measured by the gyroscopes or integral initial information concerning the absolute angular motion of the object (the increments in the integrals ␥i of the projections i of the absolute angular velocity vector of the object) measured on board using the gyroscopes. The specific properties of the algorithms for the integration of KQE (1.11) are stipulated by the fact that, in many cases, no instantaneous information is available on board concerning the absolute angular motion of the object but integral initial information (that is, not the values of i but the increments in the integrals ␥i of these quantities). Direct use of the classical numerical methods for the integration of differential orientation equations such as, for example, the widely used Runge–Kutta method is found to be impossible in these cases. Moreover, it should be noted that algorithms for the numerical integration of KQE (1.1) that use integral initial information concerning the absolute angular motion of an object are preferable to the classical numerical integration algorithms that make direct use of the values of the projections j not only because, in the majority of cases, the devices measuring the absolute angular velocity of the object (the SINS sensors) produce integral initial information rather than instantaneous information, but also because of the fact that, as modelling has shown, a better accuracy in the determination of the orientation of the object is achieved using them when there are instrumental errors (noise in the readings) in the gyroscopes. Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
5
It has already been pointed out that an algorithm for determining the orientation of an object relative to the NGCS can be constructed that is either based on the KQE for the relative angular motion of the object (1.8) or based on KQE (1.1) and (1.5) for the absolute angular motions of the object and the NGCS using the algebraic quaternion relation (1.3). The coefficients i of KQE (1.8) characterize the instantaneous absolute angular motion of the object and are rapidly varying functions of time, and the coefficients N , H and E of this equation characterize the instantaneous absolute angular motion of the NGCS and are slowly changing functions of time. The quantities i are several orders of magnitude greater than the quantities N , H and E . Nevertheless, in spite of the fundamental difference in the instantaneous absolute angular motions of the object and the accompanying trihedron (NGCS), they are jointly integrated using KQE (1.8) by the same numerical method (since the instantaneous fast angular motion of the object and the instantaneous slow absolute angular motion of the accompanying trihedron are simultaneously encompassed by these equations). In this case, the algorithms of the second and higher order of accuracy for the numerical integration of KQE (1.8) that use integral initial information concerning the angular motion of the object turn out to be complex and bulky. There are additional difficulties associated with their execution in the on-board digital computer with the required degree of accuracy due to the limitedness of the word length of the on-board digital computer (on account of the different orders of magnitude of i and N , H and E ). When the one KQE (1.8) is replaced by the combination of KQE (1.1) and (1.5), there is a natural separation of the angular motions into the fast absolute angular motions of the object (described by KQE (1.1)) and the slow absolute angular motions of the NGCS (described by KQE (1.5)). Hence, when KQE (1.1) and (1.5) are used, the instantaneous fast and slow motions are integrated separately, and numerical methods of integration that differ in complexity can be used for this (for example, an algorithm that realizes the first or second approximation of the mean velocity method that is of second order accuracy can be used to integrate KQE (1.5) and an algorithm of third or fourth order accuracy for the integration of KQE (1.1)). Instrumental errors in the accelerometers, that appear as an error in the formation of the quantities N , H and E (as in the integration of KQE (1.8)) will have no effect on the accuracy of the integration of the instantaneous absolute motions of the object. Furthermore, when the orientation parameters vj characterizing the orientation of the object in the NGCS are calculated by integrating KQE (1.1) and (1.5) and using relation (1.3), an efficient computational scheme in increments can be used which, by utilizing the effect of the commutativity of the two types of quaternion matrices and the properties of Euler parameters, allows the construction of the most rational algorithms for calculating the parameters vj . The following conclusion, based on the above discussion, can be drawn: the use of KQE (1.1) and (1.5), that describe the absolute angular motions of an object and the NGCS, for constructing algorithms for the orientation of an object in the NGCS is preferable to the use of KQE (1.8) that describes the angular motion of an object relative to the NGCS. This serves as the justification for integrating the fast motions of the object in ultrafast and fast cycles and integrating the slow motions of the NGCS in a slow cycle. The advisability of separating the fast motions of an object into ultrafast and fast cycles is justified in Section 3.
3. The use of auxiliary variables in determining the inertial orientation of a moving object The approach to the determination of the inertial orientation of a moving object, that is described in Sections 1 and 2 and based on the direct integration of KQE (1.1) for calculating the inertial orientation quaternion of the object, is widely used in the creation of the mathematical support for modern SINS7,15,16,23 together with other approaches7–14,16,23,27–30 based on the use of auxiliary (intermediate) variables that also describe the inertial orientation of the object but satisfy other kinematic equations. Here, the quaternion is calculated using the corresponding formulae in terms of the auxiliary variables with an equal or greater discreteness than the increments in the auxiliary variables. The advisability of using auxiliary variables calculated in the ultrafast cycle is explained by the following reasons. ∗
∗
∗
1. The component 0 of the scalar part and the components i (i = 1, 2, 3) of the vector part of the increment quaternion * of the ∗ quaternion variable , calculated on an integration step differ by several orders of magnitude: the magnitude of 0 is close to unity ∗ −5 while the quantities i are of the order of 10 and less (depending on the form of the angular motion of the object and the step size ∗ in the integration). The closeness of the component 0 of the increment * in the quaternion variable , calculated on an integration step, to unity can lead to a loss of accuracy in determining the orientation in the case of the small limited word length of the on-board ∗ ∗ computer. Unlike the increments 0 and i in the components of the quaternion variable , all the increments in the vector or quaternion components, or matrix auxiliary variable on the integration step have the same order of smallness equal to 10−5 or less. Hence, the above mentioned difficulty that arises when the quaternion variable is used will not arise when auxiliary variables are used. 2. The vector or quaternion of the absolute angular velocity of the moving object or the angular velocity matrix corresponding to them enters into the inhomogeneous parts of the kinematic differential equations which the auxiliary variables satisfy in an additive way, unlike in the classical KQE (1.1) in which the quaternion x of the absolute angular velocity of the object only enters multiplicatively. The increment in the integral of the absolute angular velocity vector of the object on the integration step that is directly measured on board the moving object by the majority of modern angular velocity sensors will therefore be the first approximation of the solutions for the auxiliary variables on the integration step constructed by the Picard method of successive approximations. This makes the above equations more convenient in constructing the ultrafast cycles of algorithms for determining the orientation of a moving object12,23,30 that use integral initial information on the motion of the object compared to the classical KQE (1.1) (or scalar equations (1.2)). The orientation quaternion is calculated here in the fast cycles of the orientation determining algorithms using the results of the ultrafast cycle calculations as the input information for these algorithms.
4. Ultrafast, fast and slow cycles of the SINS orientation algorithms It is advisable (from the point of view of ensuring the required accuracy in determining the orientation and the volume of necessary calculations) to separate the SINS orientation algorithms, intended for determining the orientation of a moving object in the inertial and normal geographic coordinate systems, into ultrafast, fast and slow cycles. The ultrafast cycle algorithms are intended for the integration of fast absolute angular motions of an object using intermediate variables. The fast cycle algorithm calculates the classical rotation quaternion Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
6
of an object in a step of the fast cycle in the inertial coordinate system. The slow cycle algorithm is used to calculate the classical orientation quaternion of an object in the NGCS and the aircraft angles (yaw, pitch and roll). 4.1. Ultrafast cycle Ultrafast cycle algorithms can be constructed using the Riccati-type KQE7,11,13,14 (4.1) and the Picard method of successive approximations (from here on, × is the symbol for a vector product) In Eq. (4.1), the rotation quaternion x with a zero scalar part serves as the variable. This quaternion is matched with the classical Hamiltonian rotation quaternion (see the second formula of (1.1)) using a quaternion analogue of Cayley’s formula;7,11,13,14
The components of the quaternion x are defined by the formulae xi = tg(/4)ei (i = 1, 2, 3) in which is the Euler rotation angle of the moving object in the inertial coordinate system, and and ei are the projections of the unit vector e of the Euler axis of finite rotation of the moving object onto the axes of the inertial coordinate system and the coordinate system associated X (they are the same if the axes of the same kind of X and systems coincide in the initial position). As usual, the Euler (RH) parameters j (j = 0, 1, 2, 3), characterizing the object orientation in the inertial basis , are the components of the rotation quaternion . The quaternion x is also used in Eq, (4.1) (see the last formula of (1.1)). Note that the finite rotation vectors = tg(/4)e and = ctg(/4)e and the corresponding kinematic vector equations as well as the fourth order of accuracy two-step algorithm for the numerical integration of the differential equation for the vector = tg(/4)e using integral initial information concerning the rotational motion of an object have been considered by Panov.28,29 So, the kinematic vector equation for the vector = tg(/4)e has the form28,29 (4.2) where the central dot is the symbol for a scalar product of vectors, and i is the projection of the vector onto the associated coordinate axis Xi . The Bortz vector differential equation27 (4.3) is frequently used in constructing ultrafast cycle algorithms, where = e is the Euler finite rotation vector of a rigid body. Local derivatives with respect to time are used in Eqs (4.2) and (4.3). The quaternion x corresponds to a four-dimensional skew-symmetric matrix (SM4) and the vectors and to three-dimensional skewsymmetric matrices (SM3). It is known7,17 that SM3 (of odd order) and SM4 (of even order) have qualitatively different properties: SM3 are singular (their determinants are equal to zero) but SM4 are not (their determinants always differ from zero). Moreover, a polynomial of any degree of an SM3 reduces to a second degree polynomial, but a polynomial of any degree of an SM4 reduces to a first degree polynomial. This makes it more convenient and effective to use KQE (4.1) in constructing algorithms for determining the orientation of moving objects using a SINS compared to the kinematic vector equations (4.2) and (4.3). The ultrafast cycle algorithms proposed by us are of the second and third order of accuracy and form an increment ␦x in the rotation quaternion x on a step h of the ultrafast cycle using integral initial information on the rotational motion of the object. A new formula for the addition of the finite rotations and its approximations of different order of accuracy12,14 is used for summing the increments in the rotation quaternion x within the ultrafast cycle. The ultrafast cycle is carried out on the time interval [tm−1 , tm ], equal to the step size H of the fast cycle with a step size h = tn − tn−1 = H/l using the algorithms cited below (l is the number of steps in the ultrafast cycle). 4.1.1. Ultrafast cycle algorithms 1. Algorithms for calculating the increments ␦xn in the quaternion variable x occurring in Eq. (4.1) on a step h of the ultrafast cycle based on algorithms proposed by the authors:8–11 the simplified algorithm (4.4) and the unsimplified algorithm (4.5) Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
7
that are one-step algorithms of third order accuracy, where
and a, b and c are numerical coefficients. Other ultrafast cycle algorithms of third order accuracy (one-step algorithms) and fourth order accuracy (two-step algorithms that do not require a preliminary run of the algorithms), constructed using the Riccati-type kinematic quaternion equation (4.1), have also been proposed.8–11 2. Algorithms for calculating the increment x = xj in the quaternion x on a fast cycle step using exact or simplified formulae for adding the increments in the quaternion x within the ultrafast cycle obtained by the authors:12,14 algorithms of the second (4.6) and third (4.7) order of accuracy and the exact algorithm (4.8) where n = 1, 2,..., l. The quaternion
calculated using one of the ultrafast cycle algorithms (4.4), (4.6); (4.4), (4.7); (4.4), (4.8); (4.5), (4.6); (4.5), (4.7); (4.5), (4.8) is used to calculate the orientation quaternion (tm ) in the fast cycle. 3. Savidge’s23,30 ultrafast cycle algorithm that uses the Euler rotation vector :
(4.9) ∗0
0, ∗1
␥1 , ∗2
␥1 + ␥2 , ∗3
= = = = ␥1 + ␥2 + ␥3 , . . .when n = 1, 2, 3, 4 respectively. In this algorithm, Note that an algorithm similar to (4.9) was obtained by Branets.23 4.2. Fast cycle The proposed fast cycle algorithm realizes the change from the increment x = xj in the rotation quaternion x to the increment * in the classical rotation quaternion on the step of the fast cycle using a quaternion analogue of Cayley’s formula and also executes the quaternion formula for the addition of finite rotations proposed by Branets and Shmyglevskii15 for calculating the orientation quaternion of an object in the inertial coordinate system. To determine only the inertial orientation of the object, the fast cycle is carried out once with a step size H = tm − tm−1 on the time interval [tm−1 , tm ] using the general quaternion recurrence scheme
or, in vector notation, according to the scheme
∗
∗
∗
∗
where and are the vector parts of the quaternions and * or vectors with the components 1 , 2 , 3 and 1 , 2 , 3 respectively, (tm−1 ) and (tm ) are the inertial orientation quaternions at the instants tm - 1 and tm respectively, and * is the increment in the inertial orientation quaternion in the time interval H = tm − tm−1 . Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model
ARTICLE IN PRESS
JAMM-2392; No. of Pages 10
C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
8 ∗
∗
The scalar 0 and vector parts of the quaternion *, when the KQE in the skew-symmetric operators (4.1) is used, are calculated using the formulae
Here, x = x(tm ) is the increment in the quaternion variable x with a zero scalar part on the step H of the fast cycle calculated at the end of the ultrafast cycle using one of the algorithms presented in Subsections 1 and 2, and x is the norm of the quaternion x. When the Bortz kinematic equation (4.3) is used, the increment * in the inertial orientation quaternion is calculated using the known formula
where m = (tm ) is the increment in the vector variable on the step H of the fast cycle, calculated at the end of the ultrafast cycle using algorithm (4.9). When there is a slow cycle of calculating the orientation of the object in the NGCS, the fast cycle, as a rule, consists of p steps rather than of one step. In this case, the duration of the fast cycle pH = H*, where H* is the step in the slow cycle of the calculations. 4.3. Slow cycle The slow cycle algorithm is used to calculate the classical orientation quaternion of the object in the NGCS using the increments (on a step of the slow cycle) in the classical orientation quaternion of the object and the NGCS in the inertial coordinate system. The algorithm either executes a quaternion computational scheme in increments or a matrix computational Scheme7 that uses two types of commutating quaternary matrices. In the first case, the calculations are carried out using the quaternion formula
(4.10) ∗
where v is the orientation quaternion of the object in the NGCS, s and L∗s are the increments in the orientation quaternions and and the ∗ NGCS on the slow cycle step in the inertial coordinate system, and js and ∗js (j = 0, 1, 2, 3) are the increments in the Euler (RH) parameters ∗
∗
j and j (the increments in the quaternions and ) on the slow cycle step. In the case when the fast cycle consists of a one step, s = . The special feature of quaternion formula (4.10) lies in not using the complete orientation quaternions and when determining the relative orientation of the object but their increments on the step of the slow cycle and, here, the summation of the increments is but of a multiplicative character corresponding to the multiplicative quaternion formula for the addition of finite rotations, rather than of an additive character. In the second case, the calculations are carried out using the matrix formula
(4.11) ∗
∗
where n{s } and m{∗s } are quaternion rotation matrices of types n and m7 that are compared with the quaternions s and ∗s , the superscript T is the transposition symbol, vs and vs−1 are four-dimensional column vectors composed of the Euler parameters vsj and vs−1j (the components of the quaternions vs and vs−1 ). Use is made of the property of the commutativity of the two types of quaternion matrices ∗ (n and m) when obtaining matrix algorithm (4.11) which enabled the matrix product n{s }mT {∗s } to be separated out, the elements of ∗ which are the increments js and ∗js j = 0, in the Euler parameters. This matrix notation of the algorithm for finding the parameters for ∗
the relative orientation of the object allows the algorithm to be simplified using the smallness of the increments is and ∗is (i = 1, 2, 3). The increment ∗s in the inertial orientation quaternion of the NGCS is calculated using numerical integration of KQE (1.5), describing the rotation of the NGCS in the inertial coordinate system, by the mean velocity method (an algorithm of second order accuracy) which, in scalar notation, has the form
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
9
5. Results of the mathematical modelling of the ultrafast and fast orientation algorithms The accuracy of the determination of the inertial orientation of an object is mainly determined by the accuracy of the ultrafast and fast cycle algorithms. We therefore present the results of the modelling of these algorithms carried out by L. A. Chelnokova. The methodical errors
in the determination of the orientation angles , and ␥ (yaw, pitch and roll) were estimated using the ultrafast and fast cycle algorithms in the presence of harmonic oscillations of the object with respect to each of the angular variables for small (+ =1E, + = 2E, ␥+ = 3E) and large (+ =15E, + = 5E, ␥+ = 15E) amplitudes of the angular harmonic oscillations of the object. The angular frequencies of the oscillations were assumed to be equal in both cases: = ␥ = 2 rad/s, = rad/s (frequencies of the oscillations: f = f␥ = 1 Hz, f = 0.5 Hz). When the object orientation was determined using a one-step orientation algorithm of third order accuracy11,13 (without separation of the algorithm for determining the orientation into ultrafast and fast cycles) the integration step size was chosen to be equal to from 0.0005s to 0.1s. In determining the orientation of the object using the ultrafast and fast cycle algorithms, the step size h for the ultrafast cycle integration was chosen to be equal to 0.0005 s or 0.001 s. The time of the motion of the object (of the integration) was 600 s. Analysis of the results of the modelling allowed us to draw the following conclusions. 1. The use in ultrafast cycle orientation algorithms of the approximate formula (4.6) of second order accuracy for the addition of finite rotations described by the quaternions xi leads to a loss in accuracy in determining the object orientation by two to four orders while the use of the approximate formula for the addition of finite rotations of third order accuracy (4.7) gives practically the same accuracy as the exact (but more complex) formula (4.8) for the addition of finite rotations. 2. As would be expected, execution of the ultrafast cycle algorithm of third order accuracy (4.5), (4.8) or (4.5), (4.7) with the same integration step size as the traditional execution of the one-step orientation algorithm of third order accuracy (without separation into ultrafast and fast cycles) gives the same accuracy in determining the orientation as the traditional execution. 3. The simplified one-step ultrafast algorithm of third order accuracy (4.4), (4.8) or (4.4), (4.7) has an accuracy that is an order of magnitude less than the unsimplified one-step ultrafast cycle algorithm of third order of accuracy (4.5), (4.8) or (4.5), (4.7) (the accuracy with respect to the angles and ␥ is lower by one and a half orders of magnitude). 4. A reduction in the step size of the ultrafast cycle by a factor of two increases the accuracy of the determination of the orientation by a factor of five to eight. 5. The simplified one-step ultrafast cycle algorithm of third order accuracy (4.4), (4.7) has an accuracy in determining the inertial orientation of an object that exceeds the accuracy of Savidge’s algorithm (4.9) which integrates the kinematic Bortz equation by one and a half to two orders of magnitude and the unsimplified one-step ultrafast cycle algorithm of third order accuracy (4.5), (4.7) by more than two to four orders of magnitude (for large amplitudes of the angular oscillations of the object and a step size h = 0.001s). For large amplitudes of the angular oscillations and a step size h = 0.0005s, the accuracy of Savidge’s algorithm is comparable with the accuracy of the algorithm (4.4), (4.6). In this case, the methodical errors in Savidge’s algorithm with respect to the variables , and ␥ constitute the quantities (in degrees) 7.27 H 10−4 , 7.11 H 10−5 and 1.11 H 10−3 respectively and the algorithm (4.4), (4.6) gives the values 3.27 H 10−4 , 1.98 H 10−4 and 7.01 H 10−4 respectively. At the same time, the accuracy of the algorithm (4.5), (4.7) for this case of the object motion is much higher and constitutes the quantities (in degrees) 7.12 H 10−8 , 3.42 H 10−7 and 1.05 H 10−7 respectively. Acknowledgement We are grateful to L. A. Chelnokova for the large amount of mathematical modelling of the orientation algorithms carried out. References 1. Chelnokov YuN. On determining vehicle orientation in the Rodrigues–Hamilton parameters from its angular velocity. Mech Solids 1977;37(3):8–16. 2. Plotnikov PK, Chelnokov YuN. Comparative error analysis of algorithms for determining vehicle attitude in Rodrigues–Hamilton parameters and direction cosines. Cosmic Res 1979;17(3):308–13. 3. Chelnokov YuN, Petrov SV. On problems of the orientation and navigation of an object in geographical and orthodromic coordinate systems. Dep. in VIMI 27.05.88, No. D07701. 4. Chelnokov YuN, Chelnokova LA, Landenok IV. Algorithm for the ideal operation of an orientation system for a moving object. In: Problems of Aviation Science and Technology. Collection of Papers. 1988. p. 57–65. Issue10. 5. Chelnokova LA, Chelnokov YuN. Modelling of the operation of a strapdown inertial navigation system determining the object orientation in orthodromic and geographical coordinate systems on general-purpose digital computers. Sarat Politekh Inst Saratov 1988. Dep. in VINITI 11.05.88, No. 3763-B88. 6. Chelnokova LA, Chelnokov YuN. Modelling of the operation of a strapdown inertial navigation system on general-purpose digital computers. Sarat Politekh Inst Saratov 1989. Dep. in VINITI 13.06.89, No. 3909-B89. 7. Chelnokov YuN. Quaternion and Biquaternion Models and Methods of the Mechanics of a Rigid Body and their Applications. Geometry and Kinematics of Motion. Moscow: Fizmatlit; 2006. 8. Chelnokov YuN, Perelyaev SE, Chelnokova LA. New equations and algorithms for the orientation of a strapdown inertial navigation system in four-dimensional skewsymmetric operators. In: Collection of Synopses of Papers presented at the 14th Intern Scient Conf “System analysis, control and navigation”. Moscow: Izd MAI-PRINT; 2009. p. 35–6. 9. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Kinematic differential equations of the rotational motion of a rigid body with four-dimensional skew-symmetric operators and new orientation algorithms for a strapdown inertial navigation system. In: Materials of the All-Russian Scient Conf with Intern Particip “Problems of Critical Situations in Fine Mechanics and Control”. Saratov: OOO Izd Tsentr “Nauka”; 2013. p. 315–20. 10. Chelnokov YuN, Perelyaev SE. New equations and algorithms for orientation and navigation of SDINS with four-dimensional skew-symmetric operators. In: 21st St Petersburg Intern Conf on Integrated Navigation Systems ICINS 2014 Proc. St Petersburg: SRC RF CSRI “Elektropribor”;2014;365–9. 11. Perelyaev SE, Chelnokov YuN. New algorithms for determining the inertial orientation of an object. J Appl Math Mech 2014;78(6):560–7. 12. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Ultrafast, fast and slow loops in orientation algorithms for strapdown INS. In: 22nd St Petersburg Intern Conf on Integrated Navigation Systems ICINS 2015 Proceedings. St Petersburg: SRC RF CSRI “Elektropribor”; 2015. p. 240–4.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002
G Model JAMM-2392; No. of Pages 10 10
ARTICLE IN PRESS C.E. Perelyaev, Yu.N. Chelnokov / Journal of Applied Mathematics and Mechanics xxx (2017) xxx–xxx
13. Chelnokov YuN, Perelyaev SE, Chelnokova LA. Investigation of algorithms for the determination of the inertial orientation of a moving object. Izv Sarat Univ Nov Ser Ser Mat Mekh Inform 2016;16(1):80–95. 14. Chelnokov YuN. Equations of the kinematics of a rigid body with four-dimensional skew-symmetric operators and their applications in inertial navigation. J Appl Math Mech 2016;80(6):525–40. 15. Branets VN, Shmyglevskii IP. The Application of Quaternions to the Rigid Body Orientation Problems. Moscow: Nauka; 1973. 16. Branets VN, Shmyglevskii IP. Introduction to the Theory of Strapdown Inertial Navigation Systems. Moscow: Nauka; 1992. 17. Gantmacher FR. The Theory of Matrices. New York: Chelsea; 1974. 18. Bellman R. Introduction to Matrix Analysis. N.Y.: McGraw-Hill; 1970. 19. Andreyev VD. The Theory of Inertial Navigation. Autonomous Systems. Moscow: Fizmatgiz; 1966. 20. Zakharin MI, Zakharin FM. The Kinematics of Inertial Navigation Systems. Moscow: Mashinostr; 1968. 21. Bromberg PV. The Theory of Inertial Navigation Systems. Moscow: Nauka; 1979. 22. Anuchin ON, Yemel’yantsev GI. Strapdown Inertial Systems of Navigation and Orientation (SDINS and SDIOS). Student Textbook. St Petersburg:ITMO;1995. 23. Branets VN. Lectures on the Theory of Strapdown Inertial Navigation Control Systems. Moscow: MFTI; 2009. 24. Matveyev VV, Paspopov VYa. Fundamentals of the Construction of Strapdown Inertial Navigation Systems. Student Textbook for the Speciality “Instrument Construction”. St Petersburg: GNTs RF TsNII “Elektropribor”; 2009. 25. Ishlinskii AYu. Orientation, Gyroscopes and Inertial Navigation. Moscow: Nauka; 1976. 26. Lurie AI. Analytical Mechanics. Berlin: Springer; 2001. 27. Bortz J.E. A new mathematical formulation for strapdown inertial navigation. IEEE Trans. Aero-space Electr. Syst. 1971. AES-7. No. 1. p. 61–66. 28. Panov AP. On the choice of kinematic parameters and equations for numerical integration using a digital computer. Kibern Vychisl Tekhn 1984;Issue 62:104–11. 29. Panov AP. Mathematical Foundations of the Theory of Orientation. Kiev: Nauk Dumka; 1995. 30. Savage PG. Strapdown inertial navigation integration algorithm design. Part 1: Attitude algorithms. J. Guidance Control Dynamics 1998;21(1):19–28.
Translated by E.L.S.
Please cite this article in press as: Perelyaev CE, Chelnokov YuN. Algorithms for the orientation of a moving object with separation of the integration of fast and slow motions. J Appl Math Mech (2017), http://dx.doi.org/10.1016/j.jappmathmech.2017.07.002