Attitude determination system with low cost magnetic and MEMS inertial sensors
YANG Ke-hu1, SHI Ying-gui2, JI Jing1, GUO Jian-jun3, YU Wen-sheng1
(1. Institute of Automation, Chinese Academy of Sciences, Beijing 100190, China; 2. Tianjin Navigation Instrument Research
Institute, Tianjin 300131, China; 3. HPI Innovation(Beijing) Co., Ltd, Beijing 100085, China)
Abstract: A low cost real-time attitude determination system which consists of a sensor module and an embed DSP microprocessor is described in this paper. The sensor module consists of a 3-axis magnetic compass and a MEMS Inertial Measurement Unit (IMU). An attitude estimate algorithm has been developed on the embed platform, and it uses a time-varying Additive Extended Kalman Filter based on the quaternion of rotation. Final experiment result demonstrates that this low cost system has a rather high performance, and the variances of attitude angles are below 0.02°. Key words: attitude determination; quaternion; Kalman Filter; MEMS inertial sensor
基于低成本磁场和MEMS惯性传感器的姿态测定系统
杨克虎1，史英桂2，姬 靖1，郭建军3，郁文生1
（1. 中国科学院 自动化研究所，北京 100190；2. 天津航海仪器研究所，天津 300131；
3. 宏易未来（北京）科技有限公司，北京 100085）
摘要：在机器人、小型无人自主飞行器等严格限制成本、体积、功耗的应用场合，一套合适的姿态测定系统显的尤为重要。为此，提出了一种低成本的实时姿态决定系统，该系统由传感器模块和数字信号处理器（DSP）组成。传感器模块由一个三轴的磁罗盘和一个基于MEMS技术的惯性测量单元构成，文中对系统的软硬件设计进行了详细的论述。在此嵌入式平台上面，开发了一套基于四元数和扩展卡尔曼滤波的姿态估计算法，对算法的原理进行了详细介绍。最后给出的实验结果显示此系统具有相当高的性能，姿态角误差的方差在0.02°以下。 关 键 词：姿态估计；四元数；卡尔曼滤波；MEMS惯性传感器 中图分类号：U666.1 文献标志码：A
Attitude determination system plays a very important role in the applications of robotics, aerospace, aircraft, underwater vehicles, virtual reality, automotive industries and so on. Traditionally, the inertial navigation system uses gyroscopes to determine the carrier’s attitude relative to a fixed frame. In such a system, the gyroscopes have exceptional long-term bias stability, therefore they are very expensive. In cost-sensitive applications like robotics, it is necessary to develop a low cost attitude determination system with inexpensive sensors. All the attitude determination systems are mainly made up of two components: sensors and algorithm. There are many types of sensors exploited in different attitude determination systems. For instances, R. P. Kornfeld[1] proposed an inexpensive attitude determination system for aviation application based on GPS; R. Hayward, D. Gebre-Egziabher [2,
3]
used a different approach where a triad of inexpensive automotive-grade rate gyroscopes are fused with an ultra-short baseline
differential GPS; Foxlin[4] presented an inertial system for head tracking applications, and this system utilized angular rate sensors, inclinometer and fluxgate compass; F. Raab[5] constructed an active magnetic tracking systems to determine both position and 收稿日期：2008-08-31；修回日期：2008-10-16
作者简介：杨克虎（1982—），男，博士生，研究方向，控制理论与控制工程。E-mail：kehu.yang@ia.ac.cn
第5期 杨克虎等：基于低成本磁场和MEMS惯性传感器的姿态测定系统 557 attitude by using small sensors mounted on a rigid body to sense a set of generated magnetic fields. In the satellite application [6, 7], the sun sensors, star tracker, magnetic sensors and inertial sensors are used to determine the attitude by matching some observed vectors in two frames. In the computer vision field, the camera is used to capture images which contain some feature points, lines or other geometric elements; then, the features will be extracted and matched; finally, the attitude will be derived by using some computer vision algorithms. In addition to these mentioned sensors, ultrasonic sensors, mechanical trackers, acoustic tracking systems and laser sensors are also used for attitude determination systems.
How to carry out the carrier’s attitude relative to a fixed frame using the information provided by the sensors is a main task of the software design. Optimal algorithms have been developed over the last four decades following two main approaches, namely, the classical least-squares approach and the Kalman filtering approach. The former is the so called Wahba’s problem[8] by matching a set of vector pairs in different frames. Later, in 1971, Davenport formulated and solved Wahba’s problem in terms of the attitude quaternion[9], and the algorithm developed is known in the literature as the q-method. The above algorithms are all lease-squares approach. Because the low cost sensors have serious bias drift, filtering techniques are needed to bound the attitude error growth, it is the Kalman filtering approach that allows the estimation of parameters other than attitude in a straightforward manner. Recent years, some attitude determination algorithms are proposed based on the Extended Kalman Filter (EKF), two famous approaches among which are the Additive EKF[10] and the Multiplicative EKF[11, 12].
In Section 2 of this paper, we give a detailed description of our low cost attitude determination system, including the hardware and the software construction. The quaternion based Kalman filter and the implementation on our platform will be introduced in Section 3. In the final section, the experiment result and the performance analysis will be presented.
1 System Description
1.1 Hardware construction
Our low cost attitude determination system combines the magnetic and inertial sensors. We designed a three-axes magnetic compass and a full 6DOF(Degree Of Free) IMU. The compass consists of two HMC1022 from Honeywell which is a two-axes anisotropic magneto-resistive sensor chip suitable for low cost compass application. We used two chips and let them vertical to each other to construct a fully three-axes compass. The IMU consists of a single chip three-axes accelerometer LIS3L02AS4 from ST and three pieces of single axis angular rate gyroscope ADIS16250 from Analog Device. The LIS3L02AS4 is a low-power linear accelerometer manufactured by MEMS technology with a selectable full scale measure range of ±2g/6g and 0.5 mg resolution over 100 Hz bandwidth. The ADIS16250 is a SPI interfaced digital output gyroscope manufactured by the Analog Device’s iSensor technology which is the enhanced version of Analog Device’s iMEMS sensor technology with an embedded signal processing circuit. The SPI interface provides access to measurements for the gyroscope, temperature, power supply, and all the operations of this gyroscope.
Because the HMC1022 and LIS3L02AS4 are both analog output, we must use an analog-to-digital converter to digitalize the analog output voltage which is proportional to its sensing physical quantities. We designed several small sensor PCBs (Print Circuit Board), each of which has a sensor chip and an ADS8343 which is a 4 channels 16-bit SPI interfaced A/D converter chip from Texas Instrument on it. Now, we have built a unified SPI interfaced sensor module which is convenient to communicate with MCU, DSP or other type microprocessors. For this system, we need six sensor boards in all: three ADIS16250 PCBs, two HMC1022 PCBs and one LIS3L02AS4 PCB. All the sensor boards are mounted on an aluminous cubic frame in order to keep the orthogonality of the sensitive axis. As one cubic has six sides, one side of which must be fixed on the carrier, each ADIS16250 is given one side, one HMC1022 PCB and one LIS3L02AS4 PCB shared another side, and the last side is left for the other HMC1022 PCB. The entire sensor module is showed in Fig. 1.
Due to the unified SPI interface of these sensor boards, the acquisition of the sensor information is easy. There are six SPI sensor boards, each one of which is a SPI slave device. A communication board is used to convert the six SPIs to a standard SPI bus
Fig.1 Sensor module
558 中国惯性技术学报 第16卷 with six slave select lines. The main board that we chose to gather the sensor information and do the attitude determination algorithm is ADDS-BF537-EZLITE from Analog Device. The main board is showed in Fig.2.
1.2 Software Description
Since the hardware platform itself cannot compute out the attitude angles, we must develop software which runs on the platform to get this done. The main tasks the software should be accomplished are: to manage all the communications with the sensor module and the computer, to convert the original digital voltage vector of the sensor module to the sensor output vectors we wanted and to carry out the carrier’s attitude by using the most important core algorithm in the software. Because of the outstanding control and signal process ability of the Blackfin processor, the switch between the tasks and the management of the communications is easy. We use a timer to control the whole software loop which begins at reading the voltage vectors, then does some signal process algorithm with the raw data to remove the noise caused by the circuit, converts the processed data to sensor output vectors, executes the attitude determination algorithm, finally, sends the attitude angles to the computer.
The conversion of the voltage vector to the sensor output vector is according to the sensor model[13]:
sv=KiTisr+b+n (1)
Fig.3System schematic diagram 3-axes Magnetic sensorsSPI Fig.2 Main board 3-axes Accelerometer A/D Converter SPI Main board RS232To PC3-axes Gyros sris the voltage vector, svis the sensor output vector. K is the scale factor matrix, T transforms the nonorthogonal gyro
sensitivity axes caused by assemblage into a set of orthogonal coordinate axes, b is the zero-bias vector, n is the noise vector. This sensor model is a universal model and suitable for various sensors. The parametersK,T,b,n are unknown and need to be determined in advance. Although there are some reference values in the sensor chips’ datasheet, they are so coarse to adopt in our system directly. So we need to calibrate the sensor module. For accelerometers cluster, if there is no other acceleration except gravity applied on the ternary accelerometers, the norm of the sensor output vector should be equal to the magnitude of gravity in spite of the sensor’s attitude. Since we can capture the sensor voltage vectors at a mass of different attitudes, with the property mentioned above and a cost function constructed by the sensor model, we can use Newton’s method to carry out the optimal resolution which can minimize the cost function with respect to the unknown parameters. The calibration of the magnetic sensor cluster is similar to that of the accelerometer cluster but the gravity is substituted by the earth’s magnetic field intensity. If the gyroscope were analog output, we would also need to calibrate the gyroscope cluster with a rotation table which can apply certain angular rates to the gyroscope cluster. But in our system, because the gyroscopes we used are factor-calibrated and the outputs are angular rates already, this procedure is no longer necessary
2 Attitude Determination
2.1 Quaternion
Quaternion was first invented by William Rowan Hamilton in 1843. But for a long time, quaternion did not draw enough attention due to the human’s impercipient of its advantages. Along with the development of SINS (Strapdown Inertial Navigation System) and some other technologies, quaternion shows much more efficiency in the description of rotation, and it has been widely used to resolve some problems such as the attitude determination. As we all known, a rotation has several mathematical expressions (Euler angles, rotation matrix and so on). Since a rotation only has 3DOF and the Euler angles is a 3-parameters expression, Euler
第5期 杨克虎等：基于低成本磁场和MEMS惯性传感器的姿态测定系统 559 angles is a minimal parameters expression which inevitably encounters the problem of singularity and the computation is huge (nonlinear operations of trigonometric functions); rotation matrix is a 9-parameters expression and it has no singularity problem, but there are so many redundant parameters and the six nonlinear constraints are difficult to deal with; the 4-parameters quaternion has the lowest dimensionality which is possible for a globally non-singular representation of a rotation, and the computation burden is moderate, the computation accuracy is much better than that of Euler angle and rotation matrix. So, as a majority selection, we prefer quaternion to represent rotation in our algorithm.
A quaternion is a four-component vector, defined as:
q=⎜
With:
q=(q1,q2,q3)=ρ⋅sin(θ2) (3) q0=cos(θ2) (4) Where ρis the unit Euler axis and θ is the rotation angle. Because quaternion is not the minimal parameters representation, the four components are not independent of each other. They satisfy a unit norm constraint given byq=1. The Rotation Matrix
T
⎛q0⎞T
⎟=(q0,q1,q2,q3) (2) ⎝q⎠
R is related to the quaternion by:
2
−qTqI+2qqT−2q0[q×] (5) R(q)=q0
()Where I is a 3×3 identity matrix, [q×] is the cross-product matrix defined byq:
⎡0
[q×]=⎢q3
⎢⎢⎣−q2
The quaternion kinematics equation is given by:
q=
i
−q3
0q1q2⎤
(6) −q1⎥⎥0⎥⎦
1
Ω(ω)q (7) 2
Where ω is the three-component angular rate vector measured in the body frame and Ω(ω) defined as:
Ω(ω)=⎢
⎡−[ω×]ω⎤
⎥ (8) T
0⎦⎣−ω
A major advantage of using the quaternion is that the kinematics equation is linear and also free of singularities. Another one is that successive rotations can be accomplished by using quaternion multiplications and the multiply order is the same as Rotation to a new vectorq1aq1, continue rotated by q2to another vectorq2q1aq1q2, Matrix. For example, a vector arotated by q1
whereais a quaternion that consists of vector aplus a zero scalar component, q is the conjugate quaternion of qand it is defined as q=[q0
∗
***
∗
-q].
T
2.2 Additive Extended Kalman Filter (AEKF)
The attitude determination method by matching a set of vector pairs measured in different frames through lease-squares approach is known as Wahba’s problem which only considers the current measurements, not the passed measurements and the carrier’s dynamic information. Although the AEKF doesn’t match the vectors directly, it also takes the similar consideration. It uses
ˆ and the system state vectorδq (the the angular rate vector measured in the body frame to evolve the estimate attitude quaternionq
ˆand the actual attitude quaternionq), then the measured vectorsvi in the body frame is rotated to the difference between q
ˆ, the errors between the rotated vectors and its reference vectorsui can be related with the system stateδq by reference frame byq
a measurement equation. The whole discrete state-space model of AEKF is:
δqk+1=φkδqk+Bkδωk, ek+1=Hk+1/kδqk+1+nk+1/k (9)
560 中国惯性技术学报 第16卷
φk, Bk, ek+1, Hk+1/k, nk+1/k are defined as:
φk=I4×4+Ω(ωk) (10)
⎡q1⎢1⎢−q0
Bk=
2⎢−q3⎢⎣q2q2q3−q0−q1
q3⎤−q2⎥⎥ (11) q1⎥⎥−q0⎦k
ˆ ek+1=vk+1−Dk+1/kuk+1 (12) ˆ nk+1/k=nv,k+1−Dk+1/knu,k+1 (13)
Hk+1/k=[h1
Where:
h2h3
h4] (14)
ˆˆ Dk+1/k=R(qk+1/k)
(15)
ˆk+1/k)uk+1 (16) hi=Ai(q
ˆk+1/k)= Ai(q
ˆ∂Dk+1/k
(17) ∂qi
δωk is a column vector whose elements are the three gyro error components, although the typical gyro errors model is more
complicated. We assumed that the errors are zero mean and white with a covariance matrix of Qk, and nv,k+1 and nu,k+1 are measure noise of the vectors in the body frame and reference frame which are assumed zero mean and white, and the covariance matrix of them are denoted by Rv,k+1 and Ru,k+1, so the covariance matrix of nk+1/k is given by:
ˆˆT Rk+1/k=Rv,k+1+Dk+1/kRu,k+1Dk+1/k (18)
The estimate attitude quaternion updated forward by:
ˆk+1/k=φkqˆk (19) q
ˆkis transferred by φkto generate a prior At the beginning of AEKF algorithm, the latest estimate attitude quaternionq
ˆk+1/k, then, based on the system model (9) and qˆk+1/k, the EKF-like algorithm will be executed to obtain a new estimation q
ˆk+1. As the four components of quaternion treated by AEKF are independent of each other, δqˆk+1 is added to estimation of δq
ˆk+1/k and form the post estimation of the attitude quaternion q′+1. Because this operation doesn’t consider the unit constraint, ˆkq
ˆk′+1 is probably not a unit quaternion anymore, a normalized procedure is performed outside the Kalman Filter to ensure the unit q
property of the estimate quaternion. It has been shown[14] that an optimal normalization in the Euclidean sense of the quaternion is to be simply divided by its norm. So, the final quaternion is obtained by:
ˆk+1=qˆk′+1/qˆk+1 q
(20)
Compared with the traditional EKF algorithm, the AEKF has an additional normalized operation, therefore, the effect of this normalized operation needs to be examined. Since this examination procedure and the derivation of all the above equations can be found in [10], we will not repeat it here.
3 Experiment
3.1 Algorithm Implementation
The AEKF that we described last section is an algorithm frame for attitude determination, and for different platform and
第5期 杨克虎等：基于低成本磁场和MEMS惯性传感器的姿态测定系统 561 system, the implementation is different more or less. In our system, we use the angular rates output by gyros to form the state transition matrix which evolves the system state and the estimate quaternion. For the measured vectors, we choose the gravity vector measured by the accelerometers and the earth’s magnetic field vector measured by the magnetic sensors. As within a small area the gravity can be considered constant, and if there is no magnetic disturbance, the earth’s magnetic field also can be considered constant, so when the carrier moves in an ideal environment, we can use the measurement vectors output by the accelerometers and the magnetic sensors to match the gravity and earth’s magnetic field resolved in a reference frame. In other applications like satellite, the AEKF may measure more than two vectors for one time, and this can lead to a better performance. But how many vectors do we need at least? Two would be the answer, if we have only one vector, the direction which perpendicular to the vector can not be determined, in which case we need at least another vector that is not parallel to the first one to determine all the directions. In our implementation, for each update period, even though we can use two vectors together, we use only one of them, and switch to the other vector for the next update. By this strategy, we can reduce much computation burden, and the loss of performance is not apparent. 3.2 Experimental Result
All the data used in our experiment is captured directly from the sensor module with a sampling rate of 256SPS. The Blackfin processor captures the data from the sensor module and then sends the data to the PC through RS232 interface. We prefer our attitude algorithm running on the MATLAB rather than on the Blackfin platform, although we have already developed the embed algorithm. That we are doing this is because it is convenient to evaluate the algorithm’s performance by the MATLAB’s powerful GUI function.
At the beginning of the experiment, we fix the sensor module at a certain attitude which will be chosen as the reference frame, the gravity and the earth’s magnetic field we captured at this attitude are regarded as the reference vectors. Then, the sensor module will be rotated to another attitude; after that, the attitude algorithm will be executed to determine the carrier’s attitude relative to the reference frame. The initial values used in the algorithm are:
Degree 173172.5172AEKFExpectx-axis Attitude Angle q0=(1,1,2,2), δq0=(1,1,1,1)×10−2
The algorithm result is shown in Fig.4. The response of the AEKF is rather fast, and the attitude angles are convergent only after 10 updates (about 40 ms). We will obtain even faster response if the higher sampling rate is used. The variances of the steady attitude angles are: 0.0156°for x-axis, 0.0187°for y-axis and 0.0199°for z-axis. 3.3 Analysis
TT171.5171170.5170169.5169 050010001500200025003000Update Times3500400045005000Fig.4 X-axis attitude angles. The algorithm evaluation result in the last section shows a rather high precision of our attitude determination system, and the performance is obtained under some specific conditions: First, the environment magnetic field is constant, if there are some magnetic disturbances (ferromagnetic materials or external magnetic field) nearby, the magnetic field measured by the sensor module is most likely to be different from the earth’s magnetic field; Second, the motion of the carrier must be moderate, if the carrier undergoes high acceleration or vibration, the output of accelerometers are not the resolution of the gravity along the sensitive axes any more, indeed, the accelerometers sense the gravity plus the accelerations caused by the dynamics. If we encounter the situation described above, the vectors measured in the body frame are not the vectors in the reference frame, so, it is impossible to match the two vectors set, and this will lead to performance decrease of the attitude algorithm. Suppose that the magnetic disturbance or the acceleration is intermittent, during the disturbance, we tend to trust the gyroscopes because they are highly reliable in a short term. When the disturbance disappears, the vectors measurement of gravity and earth’s magnetic field provide a precise reference to compensate the drift of the gyroscopes. The system can work well by this compensatory manner even there are intermittent disturbance exist, but if the disturbances are continuous, the system will fail to work because the accelerometers and magnetic sensors are unreliable all the time and the long-term drift of the low grade gyroscopes will cause large error to the estimate
562 中国惯性技术学报 第16卷 attitude quaternion. In order to settle the disturbance problem, we must model the magnetic disturbance and the carrier’s acceleration properly [15, 16], and then integrate these models to the state-space model, the magnetic disturbance and carrier’s acceleration will be estimated along with the system state. These ideas will not be discussed in this paper and will be done in the near future.
4 Conclusion
A low cost attitude determination system is described in this paper, it uses low grade MEMS inertial sensors and cheap magnetic sensors whose whole budget is below ＄200 (including the sensors and the Blackfin processor), hence, it is very suitable for the cost limited applications. The attitude determination algorithm is based on the Additive Extended Kalman Filter and the attitude quaternion. Experimental result shows that the performance of this system is rather satisfying under certain conditions. We analyzed the problems that might be encountered in the atrocious situations if there are magnetic disturbance nearby or the carrier undergo accelerations and proposed some improving ideas. Our future research will be concentrated on the implementation these ideas in our system.
References:
[1] R P Kornfeld, R J Hansman, J J Dyest. Single antenna GPS based attitude determination[C]//Institute of Navigation National
Technical Meeting. Long Beach, A, 998.
[2] R Hayward, D Gebre-Egziabher, M Schwall, et ac. Inertially aided GPS based attitude heading reference system (AHRS) for
general aviation aircraft[C]// Institute of Navigation ION-GPS Conference. Kansas City, Missouri, USA, 1997: 1415–1424. [3] D Gebre-Egziabher, R C Hayward, J D Powell. A low cost GPS/Inertial attitude heading reference system (AHRS) for general
aviation applications[C]// IEEE Position Location and Navigation Symposium, PLANS. Rancho Mirage, CA, 1998: 518–525. [4] E Foxlin. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman Filter[C]// IEEE VRAIS. Washington
DC, USA, 1996: 185–194.
[5] F Raab, E Blood, O Steiner, H Jones. Magnetic position and orientation tracking system[J]. IEEE Trans. on Aerospace and
Electronics Systems, 1979, AES-15: 709–717.
[6] B O Sunde. Attitude determination for the student satellite nCubeII: Kalman filter[D]. Trondheim: Department of Engineering
Cybernetics, Norwegian University of Science and Technology, 2003.
[7] K Noguchi, K Sato, R Kasikawa, et ac. CCD star tracker for attitude determination and control of a satellite for a space VLBI
mission[C]// SPIE Space Science craft Control and Tracking in the New Millennium, 1996: 190–200. [8] G Wahba. A least squares estimate of spacecraft attitude[J]. SIAM Review, 1965, 7: 409.
[9] J Keat. Analysis of least squares attitude determination routine DOAOP[R]. Sciences Corp., CSC/TM–77/6034, Silver Spring,
MD, 1977.
[10] I Y Bar-Itzhack, Y Oshman,. Attitude determination from vector observations: Quaternion estimation[J]. IEEE Trans. on
Aerospace and Electronics Systems, 1985, AES–21: 128–136.
[11] F L Markley. Attitude error representations for Kalman Filtering[J]. Journal of Guidance, Control, and Dynamics, 2003, 26:
311–317.
[12] E J Lefferts, F L Markley, M D Shuster. Kalman Filtering for spacecraft attitude estimation[J]. Journal of Guidance, Control,
and Dynamics, 1982, 5: 417–429.
[13] I Skog, P Handel. Calibration of a MEMS inertial measurement unit[C]// XVII IMEKO World Congress Metrology for a
Sustainable Development, Rio de Janeiro, Brazil, Sept. 2006.
[14] I Y Bar-Itzhack. Optimum normalization of a computed quaternion of rotation[J]. IEEE Trans. on Aerospace and Electronics
Systems, 1971, AES–7: 401–402.
[15] H J Luinge. Inertial sensing of human movement[D]. Ph.D. Enschede: University of Twente, 2002. [16] R Daniel. Inertial and magnetic sensing of human motion[D]. Enschede: University of Twente, 2006.
因篇幅问题不能全部显示，请点此查看更多更全内容