V P Kharchenko, S I Ilnytska - Analysis of unmanned aerial vehicle kinematic equations integration algorithms - страница 1
ISSN 1813-1166. Proceedings of the NAU. 2010. №3
UDC 629.735.051'844'847:629.735.33-519 (045)
1Volodymyr P. Kharchenko, D. E., Prof. Svitlana I. Ilnytska, Post-graduate student
ANALYSIS OF UNMANNED AERIAL VEHICLE KINEMATIC EQUATIONS INTEGRATION ALGORITHMS
National Aviation University 1E-mail: firstname.lastname@example.org 2E-mail: email@example.com
The integration algorithms of UAVs' rotational and translational motions using quaternions have been analyzed and the correspondent software in MATLAB has been developed. The proposed algorithms verification has been performed with using reference data at different sampling frequencies and under different conditions of UAV dynamics.
Досліджено алгоритми інтеграції обертальних та поступальних рухів безпілотного літального апарата з використанням кватерніонів. Створено відповідне програмне забезпечення в MATLAB. Проведено верифікацію запропонованих алгоритмів на еталонних значеннях із різними частотами дискретизації та за різних умов динаміки безпілотного літального апарата.
Исследованы алгоритмы интеграции вращательных и поступательных движений беспилотного летательного аппарата с использованием кватернионов. Создано соответствующее программное обеспечение в MATLAB. Проведена верификация предложенных алгоритмов на эталонных значениях с разными частотами дискретизации и при разных условиях динамики беспилотного летательного аппарата.
Statement of purpose
Satellite-based positioning and navigation systems are playing an ever-increasing role in today's society. There are a lot of applications for such systems, including personal navigation, car-navigation, unmanned aerial vehicles (UAVs) and general aviation [1; 2].
Unfortunately the only satellite-based systems are often insufficient and must be augmented with other sensors. For example for UAV flight control data rate should be at least 50 Hz, while Global Positioning System (GPS) typically updates position and velocity at 1 to 20 Hz and Inertial Navigation Systems (INSs) are capable of making measurements at several hundred Hz.
Therefore the integration of GPS and INSs is often used to provide accurate positioning and navigation information.
Algorithms of UAVs attitude and position determination play an important role in INS software.
© Volodymyr P. Kharchenko, Svitlana I. Ilnytska, 2010
Review of last publications
Nowadays in lot of countries the investigations devoted to creation of integrated INS/GPS systems are performed. The advantages of GPS/INS integrated systems, relative to GPS or INS only, are reported to be a full position, velocity and attitude solution, improved accuracy and availability, smoother trajectories, greater integrity and reduced susceptibility to jamming and interference are discussed in [2-6]. Inertial Navigation System algorithms of a body rotational and translational motions integration using quaternions are discusses in these researches [2-5; 7].
The task of this work is to develop software in MATLAB by applying UAV translational and rotational motions integration algorithms. The algorithms use quaternion technology, and angular velocities 2nd order spline-approximation, elementary quaternion approximation in the way proposed in other researches .
Then it's necessary to explore algorithms at different simulated conditions. In order to find the final INS navigation solution it would be necessary further to join the functionality of both rotational and translational motions integration algorithms within a single software.
Here the general relations that are used for the given task solution is proposed to be given.
There are several methods to represent UAV orientation in the inertial frame including Euler angle, Directional Cosine Matrix (DCM), and unit quaternion etc. The most commonly used one is Euler angle representation (roll ф, pitch6, and yawy). The UAV trajectory control can be converted to cascaded control of roll, pitch, and yaw. However, Euler angle representation has some singularity points. Unit quaternion provides another representation without introducing a gimbal lock problem. Unit quaternion could be defined as
q = [q0 q1 q2 q3]T,
where q0 + q12 + q2 + q32 =
Calculation of initial quaternion through Euler angles can be given as [4; 8]:
qo = c qi =s
q2 = c\ q3 = c\
ґа^Л ґа\ ґу\
ґа,\ ґа\ ґу\
V 2 J
V 2 J
ф '61 гул
v 2 j
v 2 j
• c "
v 2 j
v 2 j
Ф '61 (y^
v 2 j
v 2 J (ф 1
v 2 j
V2J v 2 j
v2 j v2 j (Ф1 (61
V 2 j
v2j (y 1
12 J 12 J
where с = cos, s = sin .
The Direction Cosine Matrix representing rotation from body-fixed to Earth-fixed frame is defined through unit quaternion as follows [3; 4; 8]:
'2(q2 + q32),2(q1q2
"q3q0),2(q3q1 + q2 q0)
R = | 2(q3q0 + - 2(q32 + q12),2(q2q3 - q0q1) I (2)
2(q3q1 - q2q0),2(q2q3 + q0ql),1 - 2(q2 + q22)
Calculation of Euler angles through unit quaternion and DCM RUtf^ (2) can be performed as follows [4; 5]:
ф = arctan
2(q2q3 + q°q1) 1 = arctan (^
і 2 2 2,2
6 = arcsin(-2(q1q3 - q0q2)) = - arctan
y = arctan
2,2 2 2 I
Equation describing vehicle coordinates change in North East Down frame that reflects Coriolis theorem about accelerations addition at
complex body motion is given as follows :
--am + g -2WxV;
am = a - g;
g = g-WxWx r,
where a, V is relative object acceleration and
am is accelerometer measurements (in literature is often seen as specific force f );
g is a result of gravitational and centripetal acceleration addition; W is Earth rotation rate; g is gravitational acceleration; r is radius-vector in geocentric coordinate system.
Algorithms exploration and results presentation
Having the ability to measure the acceleration by accelerometers, it's possible to calculate the change in velocity and position by performing successive mathematical integrations with respect to time. Meanwhile, in order to navigate with respect to the inertial reference frame, it is necessary to keep track of the direction in which the accelerometers are pointing. Rotational motion of the body with respect to the inertial reference frame is sensed using gyroscopes which determine the orientation of the accelerometers at all times.
Hence, by combining these two sets of measurements, it is possible to define the vehicle's translational and rotational motions within an inertial reference frame [2; 3].
It's known [2-9] that nowadays the most appropriate way to update the rotation motion parameters is one with using quaternions.
This method is used almost in all strapdown INS, since there is a possibility to find solutions in quadratures.
At this work it is explored the approach proposed in works of other authors  based on measured angular rates and accelerations approximation by 2nd order splines and elementary quaternion approximation.
At fig. 1 the block-scheme of UAV rotational motions integration algorithm is represented.
Realization of algorithms begins from reading the file length and quaternion initialization q0 = [1 0 0 0]T or, if the parameters of initial alignment are known, using equation (1).
Reading the sensors measurements is performed with some time intervals At, i.e. with sampling frequency
Consequently DCM R (q), velocity
v and coordinates r are calculated with time interval At . Therefore at the beginning of algorithm the sampling frequency f is settled.
Then the counter is settled to be equal to 4 (it's necessary for previous measurements accumulation).
Reading the measured data length m=length(data)
Reading the elementary quaternion value q(t_i-l)
Elementary quaternions multiplication
Reading the current values of angular rates and time
Fig. 1. The block-scheme of algorithm of UAV rotational motions integration
ISSN 1813-1166. Proceedings of the NAU. 2010. №>3
Then we check if current iteration doesn't exceed the file length. In case it doesn't current values of angular rates and time are read, in other case the work of algorithm is finished. Then the value of elementary quaternion q(ti-1)
is read. After this the quazicoordinates
(components of vector
J W dt)
calculation is performed.
For calculation of V6i the angular rate vector
0)(t )2nd order spline approximation is used .
Having values w(ti-2), w(ti- 1), co( ti)
quazicoordinate is calculated as follows:
And using orthogonality property of DCM it's possible to receive rotational matrix from Earth-fixed to body-fixed frame by transposing received from (2) matrix. Then current Euler angles are calculated either though quaternion or DCM using equation (3).
Therefore, it's possible to define the right part ~ of equation (4) using accelerometers measurements and DCM received from UAV rotational motions integration algorithm.
Then it's possible to write relations likewise to that described in equation (5) for v(ti), r(ti) definition, having a~ values at moments of time
ti-2 , ti-1 , ti :
(5w(ti) + 8w(ti-i) - w(ti-2)). (5) v(ti) = v(ti-i) + J adt
Similarly V6i-1 is calculated. Then the quaternion corresponding to rigid body small turn for a time At (dq(ti)) is approximated using quazicoordinate vector V6 : = v(tl4) + (5a (ti) + 8a(ti-i) - aa(ti-2) )•
1 2' (8)
r ) = r (ti- i) + v(ti-i)At + J v(ti )dt
Sqft) = 1
- V6,. 2 i
(V6i xV6,-1 )
■r (ti-) + |
(3aa(ti)+10a (ti-1) -
V -a(ti-2 )
where ЦІ is a vector norm.
Then body orientation is defined as sequential multiplication of "elementary" quaternions. In matrix representation it has following form:
<%1 ^0 ^3
q1 (ti-1 )
q 2 (ti-1 ) q3 (ti-1 )
where % 3 = %..3 (ti ) .
Equation (7) corresponds to the procedure of UAV rotational motions parameters update. Having the updated quaternion value it's possible to find DCM or rotational matrix from body-fixed to Earth-fixed frame, using equation (2).
Summarizing it's possible to say that relations (1)-(9) compose the integration algorithms of UAV kinematic equations.
The software based on these algorithms has been developed in MATLAB.
For algorithms verification and computational errors determination the following steps were performed.
In MATLAB Simulink the scheme (fig. 2)
that correlates Euler angles with angular rates has been realized according the following equations :
wx = p = Ф-y sin 6
wy = q = y cos 6 sin f + 6 cos ф,
wz = r =y cos 6 cos f-6 sin ф.
ISSN 1813-1166. Proceedings of the NAU. 2010. №№3
ISSN 1813-1166. Proceedings of the NAU. 2010. №>3
There are Euler angles at the input of this scheme and angular rates at the output. Therefore we have both sets of data: reference angular rates and Euler angles. It's necessary to note that this scheme is useful for us only for the algorithm testing, since in real conditions there might not be the possibility to receive reference values of angular rates and Euler angles.
If we set, for example, simulation time of scheme (fig. 2) 30 s, and sampling frequency 100 Hz, then in result we'll receive 3000 of reference angular rates and Euler angles. Then with the help of equations (1)-(7) according the algorithm represented at fig.1 it's possible to receive Euler angles.
It's clear that angular rates and elementary quaternions' approximation and rounding at calculations will cause the computational errors in angles determination.
To estimate the value of these errors the reference values (from the output of scheme at fig. 2) were compared to the ones received through the algorithm described at block-scheme at fig. 1.