Graduate School of Electrical Engineering Southern Taiwan University Graduate School of Electrical Engineering Improving Accuracy of Orientation Determination In Auto Balance Systems Using Mems IMU By Kalman Student : Trinh Dinh Quan   　Advisor：Tsung-Fu Chien    December, 2014

Outline Introduction Optimal in orientation determination PID Control Algorithm The Balancing Camera System Experiment Conlusion

Outline Introduction Literature Review Proposal auto balancing platform Optimal in orientation determination PID Control Algorithm The Balancing Camera System Experiment Conlusion

Literature Review To keep titling the camera of an Unmanned Aviation Vehicle (UAV) mobile mapping system, an auto-balancing is proposed. A proposed auto-balancing photogrammetric platform includes an Inertial Measurement Unit (IMU) for measuring three angles of Roll, Pitch, and Yaw. With this proposed platform, we can easily take a photo from camera with small disturbance by auto adjusting three angles of Roll, Pitch, and Yaw to get desired direction

Proposal auto balancing platform A proposed platform includes a 4-link mechanism for motions around three directions corresponding to three angles of Roll, Pitch, and Roll which are measured by an Inertial Measurement Unit (IMU). Three servo motors are used as actuator for rotating around three directions. This proposed platform will be mounted at UAV. Direction of platform can be set and controlled by a microcontroller via PID algorithm. Schematic of a proposed platform was shown in Figure .

Outline Introduction Optimal in orientation determination Reference frame Inertial frame Local lever frame Body frame Optimal INS mechanization Attitude estimation with kalman filter Model design for kalman filter Model design for EKF

Inertial Frame The origin of the i-frame at the center of mass of the earth. z-axis apparel to the rotation axis (north polar axis) of the earth. x-axis in the equatorial plane pointing toward the vernal equinox (The direction of the intersection of the earth with the plan of the earth’s orbit around the sun). y-axis completes a right-hand system. Figure : Inertial frame

Local Level Frame Its origin coincides with that of the body frame. Its xn-axis points toward to the geodetic north. Its yn-axis points toward to the geodetic east, and its zn-axis is orthogonal to the reference ellipsoid pointing up (with NEU) or down (with NED) Figure 2.2: Earth-centered, Earth-fixed frame (blue), geodetic frame (yellow), and navigation frame (green).

Body frame The origin of the body frame is the center of the IMU For rotation representation, roll (), pitch (θ), and yaw () are rotation angles around the x-, y-, and z-axes, respectively Figure 2.3 : Describes the body frame

Optimal INS mechanization INS mechanization is a set of mathematical equations that calculate the navigation solutions, including position, velocity, and attitude, from the output of an IMU This study introduces INS mechanization to derive attitude (roll, pitch, heading) in the l-frame from gyroscope only. In principle, the attitude is update from the output of gyroscope by below equation: 𝐶 𝑏 𝑙 = 𝐶 𝑏 𝑙 𝛺 𝑖𝑏 𝑏 − 𝛺 𝑖𝑙 𝑏 𝐶 𝑏 𝑙 is the time derivative of the attitude. 𝐶 𝑏 𝑙 is the transformation matrix from the body frame to the local level frame. 𝛺 𝑖𝑏 𝑏 is the angular velocity of the body frame relative to the inertial frame parameterized in the body frame. 𝛺 𝑖𝑙 𝑏 is the rotation rate of the Inertial-frame with respect to the local-level frame.

Figure : INS mechanization for orientation determination Optimal INS mechanization To avoid singularity problem in INS mechanization , a quaternion based method is applied in this research as shown in the Figure Figure : INS mechanization for orientation determination

Figure 2.6 IMU/Magnetometer integration Integration architecture Attitude estimation with Kalman filter The purpose of the KF is to estimate the posterior probability distribution of a designed state vector based on principle of minimum corresponding covariance matrix [citation] Figure 2.6 IMU/Magnetometer integration Integration architecture

Attitude estimation with Kalman filter The system model is built based on INS error model Where 𝑥= 𝛿𝜓 𝑏 𝑔 𝑠 𝑔 9×1 𝑇 is state vector, its components include position, velocity, attitude errors, biases and scale factor of accelerometers and gyroscopes. 𝛷 𝑘−1;𝑘 is the state transition matrix from epoch k − 1 to k. wk is system noise. The measurement model is built based on GPS measurement: Where zk is measurement vector. Hk is mapping matrix. nk is measurement noises at time k, respectively.

Attitude estimation with Kalman filter Estimation with Extended Kalman filter

Outline Introduction Optimal in orientation determination PID Control Algorithm PID Control Algorithm in an Embedded System PID control for Auto-balancing System The Balancing Camera System Experiment Conlusion

PID Control Algorithm in an Embedded System The combination of the proportional, integral, and derivative actions can be done in different ways. In the so-called ideal or non-interacting form, the PID controller is described by the following transfer function : 𝐶 𝑠 = 𝐾 𝑃 1+ 1 𝐾 𝐼 𝑠 + 𝐾 𝐷 𝑠 𝐾 𝑃 is the proportional gain. 𝐾 𝐼 is the integral time constant. 𝐾 𝐷 is the derivative time constant The PID controller representations can be shown in Fig : where, e(t) is controller input (system error), 𝑢 𝑐 (𝑡) is PID control signal

PID Control Algorithm in an Embedded System The typical industrial control system is represented in Fig : Figure 3.2: The typical industrial control system

PID Control Algorithm in an Embedded System A general methodology used in designing an embedded system is shown in Table. Design phase Design phase details Requirements Functional requirements and non-functional requirements (size, weight, power consumption and cost) User specifications User interface details along with operations needed to satisfy user request Architecture Hardware components (processor, peripherals, programmable logic and ASSPs), software components (major programs and their operations) Component design Pre-designed components, modified components and new components System integration (hardware and software) Verification scheme to uncover bugs quickly

PID Control Algorithm in an Embedded System Structure of a general-purpose embedded system is shown in following Fig

PID control for Auto-balancing System In this research, a PID controller was applied to control the angle rotation of each link of frame. A mathematical description of the PID controller in time-domain is as follows 𝑢 𝑡 = 𝐾 𝑝 𝑒 𝑡 + 𝐾 𝑖 𝑒 𝑡 𝑑𝑡 + 𝐾 𝑑 𝑑𝑒 𝑑𝑡 u(t) which was the PMW output of the microcontroller e(t) The variable represents the tracking error which was the difference between the set input value r(t) and the actual output y(t). e(t) The error signal was used to generate the proportional, integral, and derivative actions, with the resulting signals weighted and summed to form the control signal u(t) applied to the needle heating system K­p, Ki, and Kd are proportional gain, integral gain, and derivative gain, respectively

PID control for Auto-balancing System In this paper, three parameters of the PID controller were adjusted by the Ziegler-Nichols method Actual pitch, raw, roll Desired angles pitch, raw, roll PWM signal + e(t) PID controller Auto-balancing system _ r(t) as reference input signal was the set angles of links including pitch, raw, and roll angle. y(t)-output signal was the actual angle of the link

Outline Introduction Optimal in orientation determination PID Control Algorithm The Balancing Camera System Auto balance camera system Controller Experiment Conlusion

Auto balance camera system Axis Brushless Gimbal Camera

Auto balance camera system Model: BGM4108-130 Turns: 130 turns Cooper Wire(mm): 0.15 Camera Range: 1600g~2000g magnet: 18 Slots:24 Shaft : 4.0 Weight:110g Motor Size (mm):46*25  Ri: 17.0ohm Brushless Gimbal Motor BGM4108-130

Controller The Arduino Mega 2560 is a microcontroller board based on the ATmega2560 Summary Microcontroller ATmega2560 Operating Voltage 5V Input Voltage (recommended) 7-12V Input Voltage (limits) 6-20V Digital I/O Pins 54 (of which 14 provide PWM output) Analog Input Pins 16 DC Current per I/O Pin 40 mA DC Current for 3.3V Pin 50 mA Flash Memory 256 KB of which 8 KB used by bootloader SRAM 8 KB EEPROM 4 KB Clock Speed 16 MHz Microcontroller – Arduino Mega 2560

Controller The InvenSense MPU-6050 sensor contains a MEMS accelerometer and a MEMS gyro in a single chip. It is very accurate, as it contains 16-bits analog to digital conversion hardware for each channel MPU-6050 QFN Package & Axis Orientation Accelerometer Specifications Top side of the MPU-6050

Controller IMU MPU6050 specification

Controller Arduino Software

Physical characteristics Experiment To evaluate the performance of the proposed algorithms and system, two tests were implemented in this research In the first test a high-end grated IMU, SPAN-LCI with fiber-optical gyro (Novatel) was used as the reference system. The specification if the LCI-CPT is shown on Table Physical characteristics IMU performance Output rate (Hz) 200 In run Gyro bias (degrees/h) 0.1 Gyro scale factor (ppm) 150 Accelerometer bias (mg) 1.0 Accelerometer scale factor (ppm) 300 Physical characteristics IMU performance Output rate (Hz) 50 Gyro bias (degrees/h) Gyro scale factor (ppm) 5,0 00 Accelerometer bias (mg) 6.0 Accelerometer scale factor (ppm) 19, 700 The specifications of the SPAN-LCI (Novatel) MIDG-II specifications

Experiment IMU-gyroscopes output IMU-accelerometers output Calculated orientation

Experiment Calculated orientation

Experiment Attitude output

Experiment Attitude errors

Experiment accelerometer bias estimated by EKF

Experiment gyro bias estimated by EKF

Conclusion For the research, an auto balance system is design composed of a body, a low cost IMU, a microcontroller unit (MCU), and four motors. The attitude determination and control system with PID is fed to the MCU to control the system to the expected attitude. A quaternion-based method is applied to avoid singularity in calculation. To improve the heading accuracy, the integration of IMU and magnetometer is implemented using EKF for data fusion To evaluate the performance of the proposed data fusion technique in attitude determination, two tests was implemented. The test result indicated that with the proposed algorithm and the data fusion technique, the accuracy of the attitude determination improves significantly (about 30-50%) compared to the common algorithms

Thank You ! Dinh Quan , Trinh trinhdinhquan87@gmail.com