Proceedings of the International Conference on Electrical Engineering and Informatics Institut Teknologi Bandung, Indonesia June 17-19, 2007 D-03 Development of AHRS (Attitude and Heading Reference System) for Autonomous UAV (Unmanned Aerial Vehicle) Widyawardana Adiprawita1*, Adang Suwandi Ahmad1, Jaka Sembiring1 1 School of Electric Engineering and Informatics, Bandung Institute of Technology, Jalan Ganesha 10, Bandung 40132, Indonesia This paper describes an attitude and heading system development based on inertial and magnetic sensors. This system will use triad of orthogonal accelerometers, gyroscopes and magnetometers to measure gravity vector, angular rate vector and earth magnetic field vector. The mechanization involved integration of angular rate measurement from gyroscope that enables high update rate but have unbounded error due to integration error and drift. To bound the error, this integrative measurement will be combined with absolute measurement from magnetometer and accelerometer using Wahba Problem formulation and TRIAD algorithm. The fusion algorithm used is Kalman Filter. 1. Background Previous work had been completed on implementing an autopilot system for autonomous aerial vehicle. One of the drawbacks of this autopilot system is the attitude and heading reference. The attitude reference system of the existing system is based on thermal horizon (temperature difference between ground and sky) sensing. This attitude measurement based on thermal horizon sensing relies on clear visibility to horizon, so when the airframe enters a homogeneous environment (such as high dense fog or cloud) this attitude reference will not work. Beside that, the thermopile sensors used for thermal horizon sensing have limited field of view (typically 100 degree) so the maximum roll and pitch angle that can be measured is limited to plus-minus of 50 degree. A new attitude and heading reference system (AHRS) needs to be developed to solve this problem. 2. Attitude Representations In order to control an aerial platform correctly, one of the input needed by the autopilot control system is attitude. Attitude is usually represented by 3 rotations of the aerial platform. These rotations are roll, pitch and yaw. There are several different rotations representations used. Among them are the euler angle, Cbn matrix and the quaternion angle representation. The euler angle (roll φ, pitch θ and yaw ψ) is very intuitive and widely used in aerospace field. But this representation suffers singularity near 90 degree of pitch angle. Cbn matrix or direction cosine matrix is a 3x3 matrix that represent sequential rotation of roll, pitch and yaw. This representation doesn't suffer from singularity, but it's not intuitive and uses 9 values to represent attitude. Here is the Cbn representation ⎡cosθ cosψ Cbn (φ ,θ ,ψ ) = ⎢⎢ cosθ sinψ ⎢⎣ − sin θ − cos φ sinψ + sin φ sin θ cosψ cos φ cosψ + cos φ sin θ sinψ sin φ cosθ sin φ sinψ + cos φ sin θ cosψ ⎤ − sin φ cosψ + cos φ sin θ sinψ ⎥⎥ ⎥⎦ cos φ cos θ (1) The quaternion representation uses four variables for rotation instead of three. Here is the quaternion representation Fig. 1. roll, pitch and yaw E = [e0 e1 e 2 e3] T (2) e0 = cos(f/2) e1 = Ax sin(f/2) e2 = Ay sin(f/2) e3 = Az sin(f/2) where A = unit vector along axis of rotation f = total rotation angle To measure the attitude there 2 approach that can be used. The first is inertial mechanization which uses discrete time integration over rotation rate measurement. In this approach the attitude determination depends not only to the most current measurement but also depends on the previous attitude value. The second is absolute attitude measurement which can compute the attitude based only on the most current measurement. 2.1 Body Rotation Rate / Inertial Mechanization In this approach the attitude is updated using body rotation rate. The body rotation rate is usually represented using a [p q r]T vector. Each of the value in the body rotation rate vector represents rotation rate in the x, y and z axis in the local coordinate frame of the aerial platform (body frame). The [p q r]T vector is measured using 3 gyroscopes on each of the x, y and z axis in the body frame. Each Attitude representation has its own formulation for update. Euler angle change rate formulation : * Responsible author. E-mail: [email protected] ISBN 978-979-16338-0-2 714 Proceedings of the International Conference on Electrical Engineering and Informatics Institut Teknologi Bandung, Indonesia June 17-19, 2007 ⎡•⎤ ⎢ φ• ⎥ ⎡1 sin φ tan θ ⎢θ ⎥ = ⎢0 cos φ ⎢•⎥ ⎢ ⎢ψ ⎥ ⎢⎣0 sin φ secθ ⎣⎢ ⎦⎥ D-03 cos φ tan θ ⎤ ⎡ p ⎤ − sin φ ⎥⎥ ⎢⎢ q ⎥⎥ cos φ secθ ⎥⎦ ⎢⎣ r ⎥⎦ (3) C bn = C bn −r 0 ⎡ 0 ⎢ r ⎢ ⎣⎢ − q p q ⎤ − p ⎥⎥ 0 ⎦⎥ (4) ⎡•⎤ ⎡− e1 − e2 − e3⎤ ⎢e•0⎥ ⎢ ⎥ ⎡ p⎤ ⎢ e1 ⎥ 1 ⎢ e0 − e3 − e2⎥ ⎢ ⎥ q = • ⎢ ⎥ 2 ⎢ e3 e0 − e1⎥ ⎢ ⎥ ⎢e•2⎥ ⎢ ⎢ ⎥ r⎥ e1 e0 ⎦ ⎣ ⎦ ⎢ e3⎥ ⎣ e2 ⎣ ⎦ (5) After obtaining the attitude change rate, the result can be integrated in discrete time to obtain the final attitude value. 2.2 Absolute Attitude Determination Integration of the Body Rotation Rate (Inertial Rate Mechanization) suffers from the risk of continuous discrete time integration error. So beside the attitude determination that relies on integration and angular rate [p q r]T measurement, it will be very advantageous if we have attitude determination mechanism that only relies on single measurement in time. Several approaches will be presented, and the sensors involved here are triad of accelerometers (to measure gravity vector) and magnetometers (to measures earth magnetic vector). 2.2.1 Roll and Pitch Absolute Determination Roll and pitch measurement from gravity vector measured by accelerometer: φ = arcsin( gx ) g gy g cosθ (6) ) And here is the formulation to get the gravity vector : ameasured = adynamic + ω × v − g body Yaw or heading can be calculated from 3 axis earth magnetic field [Mx My Mz]T and roll - pitch angle using the following equation Xh = Mx*cos(θ) + My*sin(φ)*sin(θ) - Mz*cos(φ)*sin(θ) Yh = Mx*cos(φ) + My*sin(φ) ψ = arctan(Yh/Xh) (8) Quaternion change rate formulation : θ = − arcsin( such as GPS receiver and 3 axis gyroscopes. 2.2.2 Yaw Measurement from Earth Magnetic Vector Cbn change rate formulation : • adynamic and ω × v can be obtained from other sensors (7) a measured : 3 axis accelerations measured by accelerometers a dynamic : 3 axis dynamic accelerations that cause the airframe to move ω × v : centrifugal acceleration, it's the cross product of 3 axis angular rate [p q r]T and the 3 axis velocity g body : the gravity vector in body frame It should be noted that this measurement depends on accurate tilt (roll and pitch) measurement, so yaw determination also depends on external reference to get the accurate roll and pitch measurement that free from centrifugal and dynamic acceleration effect. 2.2.3 TRIAD algorithm The previous two algorithms treat gravity and magnetic vector field as separate quantities. In TRIAD algorithm we can use both gravity and magnetic vector simultaneously to find the attitude represented by rotation matrix. This problem of determining a rotation matrix needed to rotate one reference vector (a vector reading in inertial / reference frame) to another vector (a vector reading in body frame) is first formulated by Wahba. Several solution has been introduced to solve the Wahba Problem, most of them recursive (i.e. QUEST algorithm). This recursive algorithm to solve Wahba Problem offers the most accurate solution, but from implementation consideration this recursive algorithm will not guarantee the real time aspect needed for the UAV autopilot control system. Fortunately there is one non recursive solution for the Wahba Problem known as TRIAD algorithm. Here is the formulation of the TRIAD algorithm The gravity vector and magnetic field vector can define a Cartesian coordinate system, with unit vectors along three axes i, j, and k. Two Cartesian systems can be determined by two pairs of (ab, mb ) and (aR, mR) a iB = B aB iR = aR aR jB = iB × mB iB × mB jR = iR × mR iR × mR (9) k B = iB × j B k R = iR × j R Cbn = i B iR + j B j R + k B k R T T T 3 Implementation ISBN 978-979-16338-0-2 715 Proceedings of the International Conference on Electrical Engineering and Informatics Institut Teknologi Bandung, Indonesia June 17-19, 2007 3.1 Hardware Implementation The Attitude and Heading Reference System will be implemented as an embedded system with the following main components • Microchip's dsPIC30F4013 as the main processor, • 3 Analog Device's Gyroscope ADXRS150 arranged orthogonal each other to form a gyro triad, • 1 Freescale's 3 Axis Accelerometer MMA7260Q, • 1 PNI's Micromag 3 axis magnetometer, • 1 uBlox TIM LA GPS receiver. D-03 using gyro reading [p q r]T. This quaternion angle representation is chosen because it doesn't suffer from singularity problem in 90 degrees pitch. This algorithm is executed in 0.02 second or 50Hz, this frequency is chosen because it's considerably matched to actuator servo frequency. The absolute attitude determination is implemented using TRIAD algorithm, because this algorithm is considered computationally efficient (no recursive computation) and combines the gravity acceleration and magnetic filled elegantly. To be able to correctly get the gravity acceleration external information of speed and dynamic acceleration from GPS receiver is needed, so this algorithm is executed every 0.25 second or 4 Hz. So in overall, we have attitude update every 0.02 second (50Hz) and the attitude integration error is zeroed every 0.25 second (4Hz). By combining those 2 algorithms we can have a stable AHRS for considerably long term operation time, including in high G turn (centrifugal or coordinated turn). The fusion algorithm is Kalman Filter. 3.3 Kalman Filter Formulation Fig. 2. Diagram block of the AHRS Hardware Time varying Kalman filter is used to combine the high update rate rotation rate integration (state update) and low update rate absolute attitude determination (measurement update). Here is the AHRS state space formulation xk+1 = Akxk + Bkuk zk = Hxk (10) x = [e0 e1 e2 e3 bp bq br]T u = [p q r]T z = [e0 e1 e2 e3] T from TRIAD algorithm solution, with this conversion Fig. 3. finished AHRS Hardware 3.2 Algorithm and Firmware Implementation The firmware is implemented in C language and compiled with Microchip's C30 compiler. The general algorithm is the combination of two algorithm • rotation rate integration to get high update rate, and • absolute attitude determination to bound the discrete integration error in rotation rate integration. The analog sensors (accelerometers and gyros) are sampled in 250Hz and filtered using IIR butterworth 2 order low pass filter. The cut of frequency chosen is 5Hz. This low pass filtering is needed to avoid the vibration caused by UAV's engine. The 5Hz cut of frequency is chosen with the assumption that the UAV's manoeuvre frequency is below 5Hz. The magnetometer is read through Synchronous Serial Interface / Serial Peripheral Interface (SPI) in 50 Hz update rate. The GPS receiver can give velocity and position updates every 0.25 second or 4Hz. The Rotation Rate Integration is implemented using quaternion angle representation. The quaternion is updated ISBN 978-979-16338-0-2 ⎡ c11 c12 c13 ⎤ Cbn = ⎢⎢c 21 c 22 c 23⎥⎥ ⎢⎣c31 c32 c33⎥⎦ 1 1 + c11 + c 22 + c33 2 1 e1 = (c32 − c 23) 4e 0 1 e2 = (c13 − c31) 4e0 1 e3 = (c 21 − c12) 4 e0 e0 = (10b) where [e0 e1 e2 e3]T : quaternion attitude representation [bp bq br]T : bias of rotation rate measurements [p q r]T : rotation rate measurements 716 Proceedings of the International Conference on Electrical Engineering and Informatics Institut Teknologi Bandung, Indonesia June 17-19, 2007 ⎡ ⎢1 ⎢ ⎢0 ⎢ ⎢0 Ak = ⎢ ⎢ ⎢0 ⎢ ⎢0 ⎢0 ⎢ ⎣⎢0 0 0 0 dt.e1k 2 dt.e0 k − 2 dt.e3 k − 2 dt.e2 k 2 1 dt.e2 k 2 dt.e1k 2 dt.e0 k − 2 dt.e1k − 2 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1 ⎡ dt .e1k ⎢− 2 ⎢ dt.e0 k ⎢ ⎢ 2 ⎢ − dt.e3k 2 Bk = ⎢ ⎢ dt.e2 k ⎢− 2 ⎢ ⎢ 0 ⎢ 0 ⎢ ⎣⎢ 0 ⎡1 ⎢0 H =⎢ ⎢0 ⎢ ⎣0 0 1 0 0 0 0 1 0 − 0 0 0 0 0 0 0 0 0 dt.e3 k ⎤ 2 ⎥ dt.e2 k ⎥ ⎥ − 2 ⎥ dt.e1k ⎥ 2 ⎥ dt.e0 k ⎥ − ⎥ 2 ⎥ 0 ⎥ 0 ⎥ ⎥ 1 ⎦⎥ dt.e3k ⎤ 2 ⎥ dt.e2 k ⎥ ⎥ 2 ⎥ dt.e1k ⎥ − 2 ⎥ dt.e0 k ⎥ ⎥ 2 ⎥ 0 ⎥ 0 ⎥ ⎥ 0 ⎦⎥ dt.e2 k 2 dt.e3k − 2 dt.e0 k 2 dt.e1k 2 0 0 − 0 0 0 1 D-03 Fig. 4. AHRS Monitoring Software 4 Testing Several testing has been done. These includes • Static roll and pitch test • Vibration test on ground • Vibration test using UAV test bed • Ground based magnetic heading test Qualitatively the AHRS performs very well on those tests. Quantitative data still needs to be analyzed. The final test is still needed to test the performance of the AHRS in centrifugal turn. 0⎤ 0⎥⎥ 0⎥ ⎥ 0⎦ Here is the Kalman filter formulation Time Update (high update rate, 50Hz) : Project state ahead : xˆk− = Axˆk −1 + Buk −1 (11) Project error covariance ahead : Pk− = APk −1 AT + Q (12) Measurement Update (low update rate, 4Hz) : Compute Kalman Gain : K k = Pk− H T ( HPk− H T + R) −1 (13) Fig. 5. Radio controlled airplane for AHRS aerial testing Update estimate with measurement zk − k − k xˆk = xˆ + K k ( zk − Hxˆ ) (14) Update the error covariance Pk = ( I − K k H ) Pk− References 1. (15) 2. 3.4 AHRS Monitoring Software AHRS monitoring software is developed to asses the performance of the developed AHRS. The software is developed with Borland C++ Builder. The visualization for attitude is using 3D graphics. This 3D graphics is developed with OpenGL library. 3. 4. 5. ISBN 978-979-16338-0-2 Sven Ronnback, Development of a INS/GPS navigation loop for an UAV, Lulea Tekniska Universiteit, 2000 Yong Li, Andrew Dempster, Binghao Li, Jinling Wang, Chris Rizos, A low-cost attitude heading reference system by combination of GPS and magnetometers and MEMS inertial sensors for mobile applications, University of New South Wales, Sydney, NSW 2052, Australia Stelian Persa, Pieter Jonker, Multi-sensor Robot Navigation System, Pattern Recognition Group, Technical University Delft, Lorentzweg 1, Delft, 2628 CJ,The Netherlands J.F. Vasconcelos, J. Calvario, P. Oliveira, C. Silvestre, GPS AIDED IMU FOR UNMANNED AIR VEHICLES, Instituto Superior Tecnico, Institute for Systems and Robotics, Lisboa, Portugal, 2004 Michael J. Caruso, Applications of Magnetic Sensors for Low Cost Compass Systems, Honeywell, SSEC 717

* Your assessment is very important for improving the work of artificial intelligence, which forms the content of this project

Download PDF

advertisement