2014 IEEE 17th International Conference on Computational Science and Engineering
Development of attitude determination system for moving carrier based on MEMS sensors 1
Tianxiang Jia1 , Yanan Liu 2 National Space Science Center, Chinese Academy of Sciences, Beijing, China 2 Datang Mobile Communication Equipment Co, Beijing, China
[email protected],
[email protected] MEMS accelerometer and magnetometer detect the gravity vector and the magnetic vector and figure out the attitude angle of the carrier. MEMS gyros can detect the triaxial angular rate, and then calculate the attitude angle by the quaternion method [4]. Finally Kalman filtering technique is adopted for data fusion to guarantee the dynamic property and long-term accuracy of the system [5]. As shown in the figure 1, the design of electronic system introduces dual-core mode that based on FPGA and DSP. The attitude determination algorithm has many multiply and accumulates operation. DSP has a specialized multiplieraccumulator, which takes a great advantage in complex operations. Moreover, In order to meet real-time requirements, this system chooses to use the device which can achieve a high-speed computing and a large number of floating-point operations. The main control unit exchanges the acquisition data with peripheral devices, although the DSP chips have a variety of peripheral interfaces, when taking account of high update rate of MEMS sensors data, the system adopts FPGA to implement the integration of multiple interfaces. FPGA reads sensors measurement data to internal FIFO and then provides interrupt signal to DSP, DSP executes the multiple sensors data fusion to accomplish the attitude determination. The design scheme gives full use of the performance of DSP and FPGA, which satisfy the function requirements of attitude determination system [6]. y [ ]
Abstract—The moving carrier need accurate positioning and localization for their guidance, navigation and control. In order to get the space attitude of the carrier accurately and real-time, an attitude determination system of MEMS gyros, accelerometers and magnetometer is proposed in this paper. The design of electronic system adopts dual-core mode that based on FPGA and DSP, FPGA reads the measurement data of combined sensors to internal FIFO and then sends interrupt signal to DSP, Float-point DSP is used as the attitude determination processor to exert the multiple sensors data fusion to accomplish the attitude determination. The paper illuminates the hardware realization of the scheme, as well as the software which is composed of overall process and the implement of I2C protocol. Furthermore, the software of the multiple sensors data fusion is developed to calculate the accurate result of the carrier’s attitude. Finally, the experiment of prototype vehicle verification was made. The result indicates that the static accuracy and dynamic accuracy of the attitude angle are respectively better than 0.15º and 2º, which meets the moving carrier’s navigational requirements such as high reliability and high precision. Keywords—Attitude determination system; MEMS sensors; DSP; FPGA; I2C; Quaternion;
I.
INTRODUCTION
Attitude determination has been widely applied in the fields of aerospace, robot, navigation and human motion analysis. The attitude and heading measurement system which contains gyroscope, accelerometer and magnetometer can realize the attitude parameters measuring of moving carrier. Compared with the traditional measurement methods, the system has the advantage of small size, light weight, high reliability and high precision. . The motion attitude mainly refers to the carrier’s angular orientation that relative to a certain reference system, therefore, the attitude is always changing during the movement [1], [2]. An attitude determination system of MEMS gyros, accelerometers and magnetometer is proposed in this paper, through the measurement of gravity vector angle and the system’s rotation angular velocity, we have figured out the attitude angles real-time and accurately, thus provide the basis for the motion attitude control and the trajectory calculation [3]. II.
Fig. 1. Block diagram of embedded micro attitude measurement system
III.
A. The design of the sensor system The attitude determination system collects motion parameters through the micro MEMS sensors. The sensors system mainly include gyroscope(IMU3000), accelerometer(ADXL345) and Magnetometer(HMC5883), they separately measure the angular velocity, acceleration and Magnetic Field[7],[8],[9].
THE DESIGN SCHEME OF ATTITUDE DETERMINATION SYSTEM
The mainly mission of the attitude determination system is to measure the angular motion state along three orthogonal axes, and figure out the carrier’s three relative attitude angles, namely course angle φ , pitch angle θ and roll angle γ . 978-1-4799-7981-3/14 $31.00 © 2014 IEEE DOI 10.1109/CSE.2014.112
THE HARDWARE DESIGN OF THE SYSTEM
465
TABLE I. Sensor type Size/mm Working current/mA range interface Supply voltage/V
THE CHARACTERISTIC PARAMETER OF MEMS SENSORS gyroscope ITG3200 4×4×9 6.5
accelerometer ADXL345 3×5×1 0.023
Magnetometer HMC5883L 3×3×0.9 0.1
±2000(o)/s I2C 3.3
±16g I2C 3.3
±8 Gauss I2C 3.3
signal XZCS0AND1 , read and write control XWE and XRD of the DSP are all connected to IO ports of FPGA[12]. The interrupt signal of the DSP is used as judgment signal, readwrite selected signal are used as enable signal to complete communication to FPGA. The measured data is deposited in the internal FIFO memory within FPGA. DSP reads the data form FIFO and then perform the calculation of attitude determination algorithm.
All the selected MEMS sensors have I2C bus interface and can be mounted directly on the I2C bus as the slave device. I2C is a two-wire, bidirectional serial bus that provides a simple, efficient method of data exchange between devices. The typical applications structure is shown in figure 2. This is just two wires, called SCL and SDA. SCL is the clock line. It is used to synchronize all data transfers over the I2C bus. SDA is the data line. The SCL and SDA lines are connected to all devices on the I2C bus. FPAG develops relative state codes for all work status during translation, and performs pre-mange to the bus accordingg to these codes [10]. [ ]
Fig. 3. The connection of FPGA and DSP
IV.
A. Quaternion algorithm The task of the attitude determination is real-time measuring the three orthogonal axis angular motion state of the carrier, that is course angle φ, pitch angle θ，and roll angle γ . The attitude of the moving carrier can be represented by the angle between static reference coordinate system and motion reference coordinate system that fixed connection to the moving object. The design selects geographic coordinate system(e series)and carrier coordinate system(b series) to determine the moving carrier's attitude. Geographic coordinate system(e series) is located in the geographical horizontal plane, the x, y and z axises are respectively pointing in the direction of the east, north and sky. In relation to e series, b series can be obtained from e series rotating around its z, x, y-axis, the three rotate angles are successively defined as course angle φ , pitch angle θ ， and roll angle γ [13]. Attitude matrix T reflects the conversion relationship with e series and b series. The matrix T is given by:
Fig. 2. I2C bus structure
B. The dual-CPU architecture with DSP and FPGA According to the design requirements of attitude determination system, the digital signal processor TMS320F28335 produced by TI Company is adopted as main control chip. TMS320F28335 has 150MHz high processing speed, 32 bits floating point processing unit, 6 DMA channels which support ADC, McBSP and EMIF. With the Floating Point Unit, we can easily complete the attitude algorithm [11]. The external interface data bus of TMS320F28335 chip is connected with FPGA. As shown in the figure 3. The 16-bit data bus XD[0:15], 15-bit address bus XA[0:14], chip selected
cos cos sin sin sin
T sin sin cos cos sin sin cos
ESTIMATION OF ATTITUDES
cos sin cos cos sin
sin cos cos sin sin sin sin cos sin cos cos cos
(1)
Quaternion and Euler angle transformation relations is
Attitude matrix can also be represented by quaternion method. Attitude kinematics equation that expressed by quaternion method is the linear equation about quaternions. It possesses less calculation and has been widely used. The quaternions were first described by Irish mathematician Sir William Rowan Hamilton in 1843. The quaternions is given by
cos(2 ) cos(2 ) cos(2 ) sin(2 ) sin(2 ) sin(2 ) q0
q sin( ) cos( ) cos( ) cos( ) sin( ) sin( ) 2 2 2 2 2 1 2 q2
cos( ) sin( ) cos( ) sin( ) cos( ) sin( ) 2 2 2 2 2 2 q3
cos( ) cos( ) sin( ) sin( ) sin( ) cos( ) 2 2 2 2 2 2
Q q0 q1ib q2 jb q3 kb (2) where ib ， jb ， kb respectively represents the unit
vector that paralleling to the x, y and z axises of carrier coordinate system. Quaternions can represent the rotation message, the scalar part indicates the size of the rotation, and the vector part indicates the direction of the rotation.
466
(3)
calculate the pitch angle θ and roll angle γ.
The attitude matrix T that represented by quaternion is
q q q q 2(q1q2 q0q3) 2(q1q3 q0q2) 2 2 2 2 T 2(q1q2 q0q3) q0 q1 q2 q3 2(q2q3 q0q1) (4) 2(q1q3 q0q2) 2(q2q3 q0q1) q02 q12 q22 q32 2 0
2 1
2 2
a tan 2( f y , f z ) 2 2 a tan 2( f x , f y f z )
2 3
Secondly reads the three-axis magnetometer’s measurement value M b M xb M yb M zb , then using the
According to the corresponding position elements in two matrix (1) and (4), the carrier’s attitude angles that represented by quaternion is
arctan
arctan
Mxn Mxb cos Myb sin sin Mzb cos sin n b b My My cos Mz sin
(5)
a tan 2(
The relationship between quaternion and angular velocity is (6)
Where
bx
，
bx 0 by bz
by
，
by bz 0 bx
bz
bz q0 by q1 bx q2 0 q3
M yn M xn
)
(11)
B. The data fusion algorithm based on Kalman filter. The data fusion algorithm is shown as figure 4. Firstly use the measurement data of accelerometer and magnetometer to calculate the attitude quaternion, then read the angle velocity data of gyroscope to figure out the current quaternion by fourth order Runge-Kutta attitude algorithm. The data fusion is developed by Kalman filter according to these two groups of quaternion to get updating quaternion and finally figure out the attitude angle, the new quaternion is saved as the initial value for the following calculation [17].
The matrix format is
q0 0 q 1 1 bx q 2 2 by bz q3
(10)
The course angle φ will be figure out by the value of title compensation[16].
2(q0 q2 q1q3 ) q02 q12 q22 q32
1 q (t ) q (t ) b 2
value of pitch angle θ and roll angle γ to complete the title compensation .
2(q0 q3 q1q2 ) q02 q12 q22 q32
arcsin 2(q2 q3 q0 q1 )
(9)
(7)
separately represent the
attitude angle velocity of X,Y,Z. The data can be measured by MEMS gyroscope. q0 ， q1 ， q2 ， q3 should satisfy the constraints q0 q1 q2 q3 1 . Runge-Kutta method can be used to solve the attitude differential equations which are denoted by quaternion algorithm. Where q (t ) and b (t ) denotes the quaternion q and 2
2
2
2
the attitude angle velocity at time t, T is sampling period, then the quaternion q (t T ) can be figured out at time t T . 1 K 1 2 q (t ) b (t ) K 1 ( q (t ) K 1 T ) (t T ) b 2 2 2 2 (8) K T 1 2 T ) b (t ) K 3 ( q (t ) 2 2 2 1 K 4 2 ( q (t ) K 3T ) b (t T ) q (t T ) q (t ) T ( K 2 K 2 K K ) 1 2 3 4 6 According to the three-axis accelerometer and three-axis magnetometer measurements data, the initial quaternion is determined by the relationship between quaternions and Euler angles in the initial start-up speed. Firstly reads the of accelerometer to measurement value ( f x , f y , f z )
Fig. 4. Data processing algorithm of attitude determination
V.
THE SOFTWARE DESIGN OF THE MOTION ATTITUDE MEASUREMENT SYSTEM
The software design of the system mainly charges the software flow control between FPGA module and DSP module. FPGA module includes the construction of asynchronous FIFO and the realization of I2C protocol. DSP module is used to achieve the communication of FPGA and complete the algorithm processing of motion carrier attitude. A. The overall process of software When the system is powered on, the first step is initial configuration; DSP sets its register to make sure all functional modules run correctly. The initial configuration mainly includes configuring the system clock, SPI, SCI and other peripheral clock signal, initializing on-chip FLASH parameters, GPIO, interrupt vector table and other operations.
467
After the right configuration, FPGA starts receiving the sensors data and save them into the internal asynchronous FIFO. When the half-full flag of FIFO is set, then FPGA send an interrupt signal DSP. DSP module performs the data preprocessing after it receiving the interrupt signal from FPGA. The data pre-processing mainly refers to figure out the motion attitude of carrier through the data fusion theory. The software flowchart is shown as figure 5.
Fig. 6. Data frame translation of I2C bus
According to I2C bus transmission protocol, there are several special states in the I2C bus transfer process, so we have designed a synchronous state machine to develop I2C module.
Fig. 7. State transition diagram of I2C protocol
Start state: When Start=1, sends the START signal; Write state: Sends the write control words and address signal to slave device. If WR=1, FPGA writes out the data one by one and receive the ACK, if all data have been written to bus then set Stop=1, turn to stop state and enable the stop signal. If WR=0, turn to read state. Read state: Start I2C protocol again and send out the read control words. If WR=0, FPGA reads the data one by one and sends the NO ACK bit, if all data have been read then set Stop=1, turn to stop state. Stop state: Send stop signal.
Fig. 5. Software design flow chart of the execution
B. The I2C protocol implementation based on FPGA The MEMS devices in the system are all have I2C bus interface, so the process of data acquisition is the implementation of I2C protocol. I2C communication process is divided into initiation, response, terminated signals. When the master (FPGA) wishes to talk to a slave, it begins issuing a start sequence on the I2C bus. The start sequence and stop sequence are special in that these are the only places where the SDA (data line) is allowed to change while the SCL (clock line) is high. When data is being transferred, SDA must remain stable and not change while SCL is high. The start and stop sequences mark the beginning and end of a transaction with the slave device. FPGA give out the start signal, the following 7bits is the address signal and the R/W bit is representing the data transmission direction. Data is transferred in sequences of 8 bits. The bits are placed on the SDA line starting with the MSB. If the receiving device sends back a low ACK bit, then it has received the data and is ready to accept another byte. If it sends back a high then it is indicating it cannot accept any further data and the master should terminate the transfer by sending a stop sequence [18].
VI.
RESULT AND ANALYSIS OF THE EXPERIMENT
A. Static stability experiment Three-Axis turntable is used to calibrate the attitude determination system. The measurement system is placed on the stationary turntable. Firstly warm up the measurement equipment 30 minutes and then test its static stability. The experiment sampled 3000 groups of calculated data continuously, the test result is shown as figure 8. As we can see form the picture, the three attitude angles are all convergence, the error of the pitch angle and roll angle are both less than 0.1 ° , meanwhile the error of course angle is less than 0.15°.
468
it will have a broad application prospects. REFERENCES [1]
[2]
[3] Fig. 8. The test error of attitude angles in static stability experiment [4]
B. Dynamic Stability experiment The attitude determination device is installed on the high precision three-axis turntable, then control the turntable rotating along the direction of X,Y and Z, the angle of rotation is changed from 0 to 90° and then back to 0. The test result is shown as figure 9. As we can see form the picture, the error of pitch angle and roll angle are both less than 1.2 ° , meanwhile the error of course angle is less than 2 ° . The result indicates that the device has good dynamic stability.
[5]
[6]
[7] [8] [9] [10]
[11] [12] Fig. 9. The test error of attitude angles in dynamic stability experiment
[13]
VII. CONCLUSION According to the requirement of the attitude determination system, the paper detailedly introduces the attitude measurement scheme based on MEMS gyros, accelerometers and magnetometer. DSP and FPGA are adopted to control the data acquisition and processing, FPGA concurrency control the data acquisition of MEMS sensors by I2C protocol, which has strong anti-jamming capability. Floatpoint DSP implements the algorithm conveniently and has a high real time performance. The paper also proposes the quaternion algorithm to calculate the attitude angle and applies the Kalman filter to realize the multiple sensors data fusion method, and then the attitude message of moving carrier is achieved. The result of experiments demonstrates that the combined sensors technology is an effective method to improve the accuracy of attitude determination. The system has the features of integration, intelligence and platform, and
[14]
[15]
[16]
[17]
[18]
469
Tatsuya Harada, Hiroto Uchino, Taketoshi Mori and Tomomasa Sato. “Portable Orientation Estimation Device Based on Accelerometers, Magnetometers and Gyroscope Sensors for Sensor Network,” Proceeding of 2003 IEEE Conference on Multisensor Fusion and Integration for Intelligent Systems, 2003, pp.191-196. Zhang Ying, Qi Hongxiang, Li Deqiang,et,al. “Development of MEMS Inertial Navigation System in Aerospace Field,” Aerospace Standardization, no 3, pp 40-43,2010. Zhao Lin, Liu Fu-qiang, Wang Wen-jing, Xu Bo. “Design of Minisize Integrated Attitude Measurement System Based on MEMS Inertial Sensors” , Journal of system simulation, vol.21, no.14, pp. 4433-4436, 2009. P. Martin and E.Salaun, “Design and implementation of a low-cost observer-based attitude and heading reference system”, Control Engineering Practice, vol. 18, no. 7, 2010. X. Yun and E. Bachmann, “Design, implementation, and experimental results of quaternion- based Kalman filter for human body motion tracking ,” IEEE Trans. Robot., vol. 22, no. 6, pp 1216-1227, Dec. 2006. Li Yan, Liang Zheng-tao, Li Li-jing,et,al. “Design of optical-fiber micro-vibration sensor data acquisition system based on FPGA and DSP,” Modern electronic technique, vol.35, no.20, pp 19-24, Oct. 2012. InvenSense Inc. “IMU-3000 Motion Processing Unit Product Specification,” Invensense, 2010. Analog Devices Inc. “Three-axis, ± 2g/ ± 4g/ ± 8g/ ± 16g digital accelerometer ADXL345 data sheet,” Analog Devices, 2013. Honeywell Inc. “3-Axis Digital Compass IC HMC5883L data sheet,” Honeywell, 2013. Ke Gao, Bo Mo, Jin Lin, “Design and Implementation of Low Cost Aircraft Control Bus System upon I2C,” Practical Applications of Intelligent Systems, vol. 124, 2012, pp 513-521. Texas Instruments Inc. “TMS3320F2833X, TMS320F2823X digital signal controllers data manual,” TI, 2007. Cui Xutao, Yang Rijie, He You, “Development of signal processing system based on DSP and FPGA,” Chinese journal of scientific instrument, vol.28, no.5, May 2007. Yonghui Hu, Yong Yan, “A miniature, low-cost MEMS AHRS with application to posture control of robotic fish,” Proceeding of 2013 IEEE International Instrumentation and Measurement Technology Conference (I2MTC), 2013, pp 1392-1395. D. Jurman, M. Jankovec, R. Kamnik and M.Topic, “Calibration and Data Fusion Solution for the Miniature Attitude and Heading Reference System,” Sensors and Actuators, A-Physical, vol. 138, pp.411-420, 2007. Ye Zengfeng, Feng Enxin, “Attitude Stabilization Based on Quaternion and Kalman Filter for Two-Wheeled Vehicle,” Chinese journal of sensors and actuators, vol.25, no.4, Apr.2012. M. Tanenhaus, D. Carhoun, T. Geis, E. Wan, and A. Holland, “Miniature IMU/INS with optimally fused low drift MEMS gyro and accelerometers for application in GPS-denied environments,” in Proc. IEEE/ION PLANS, Apr. 2012. S. H. P. Won, W. W. Melek, and F.Golnaraghi, “A Kalman/particle filter-based position and orientation estimation method using a position sensor/inertial measurement unit hybrid system,” IEEE Trans.Ind.Electron., vol. 57, pp. 1787–1798, 2010. Pan Xiaodong, Chen Zexiang, Huang Zili, Gao Shengjiu. “Design of FSM simulation for I2C bus based on FPGA,” China Measurement Technology, vol.33, no.1, pp 105-107. Jan 2007.
