System and Method for Determining the Orientation of an Inertial Measurement Unit (IMU)

• US 20160327396A1
• Filed: 10/02/2015
• Published: 11/10/2016
• Est. Priority Date: 05/08/2015
• Status: Active Grant

• Current Assignee: Sharp Kabushiki Kaisha (Hon Hai Precision Industry Co., Ltd.)
• Original Assignee: Sharp Laboratories of America Incorporated (Hon Hai Precision Industry Co., Ltd.)
• Inventors: Hallberg, Bryan
• Granted Patent: US 9,846,040 B2
• Time in Patent Office: Days
• US Class Current: 1/1
• CPC Class Codes: G01C 21/188 for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems

Abstract

A system and method are provided for determining the orientation of an inertial measurement unit (IMU). The method calculates a gyroscopic quaternion, and when the IMU accelerometer reading is about equal to gravity (1 G), a field quaternion is calculated using IMU accelerometer readings. Estimates are made of angular orientation errors due to IMU angular velocity and linear acceleration, and these angular orientation errors are used to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion. Alternatively, if the accelerometer reading is not about equal to 1 G, the gyroscopic quaternion is used as the current sample quaternion. In one aspect, IMU gyroscope readings and IMU accelerometer readings are calibrated in response to determining a lack of IMU movement. Near-zero gyroscope reading jitter is removed by setting the IMU gyroscopic reading to zero, when the gyroscopic reading is near zero.

Claims

1. A method for determining the orientation of an inertial measurement unit (IMU), the method comprising:

calculating a gyroscopic quaternion;

when an IMU accelerometer reading is about equal to gravity (1 G), calculating a field quaternion using IMU accelerometer readings;

estimating angular orientation errors due to IMU angular velocity and linear acceleration; and

,

using the angular orientation errors to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion.

2. The method of claim 1 further comprising:

estimating IMU magnetometer jitter errors;

wherein calculating the field quaternion includes calculating the field quaternion using the IMU accelerometer readings and IMU magnetometer readings; and

,

wherein selectively mixing the gyroscopic quaternion and field quaternion includes selectively mixing in response to the angular orientation errors and the estimated IMU magnetometer jitter errors.

3. The method of claim 2 wherein estimating the IMU magnetometer jitter errors includes estimating the IMU magnetometer jitters errors in response to determining a lack of IMU movement.

4. The method of claim 1 further comprising:

calibrating IMU gyroscope readings and IMU accelerometer readings in response to determining a lack of IMU movement.

5. The method of claim 1 further comprising:

removing near-zero gyroscope reading jitter by setting the IMU gyroscopic reading to zero, when the gyroscopic reading is near zero.

6. The method of claim 1 further comprising:

when the accelerometer reading is not about equal to 1 G, using the gyroscopic quaternion as the current sample quaternion.

7. The method of claim 1 further comprising:

prior to calculating the field quaternion, calculating a total angular orientation error by combining the following individually calculated angular errors;

an orientation angular error due to centripetal acceleration; and

,

an orientation angular error due to linear acceleration.

8. The method of claim 7 wherein calculating the total angular orientation error includes additionally combining an orientation angular error due to estimated IMU magnetometer jitter.

9. The method of claim 7 wherein selectively mixing the gyroscopic quaternion and field quaternion to supply the current sample quaternion includes:

calculating an angular difference between the gyroscopic quaternion and field quaternion;

comparing the angular difference to the total angular orientation error; and

adjusting the mixture of the gyroscopic quaternion and field quaternion in response to the comparison.

10. The method of claim 9 wherein calculating the total angular orientation error includes additionally combining an orientation angular error due to estimated IMU magnetometer jitter.

11. The method of claim 1 wherein calculating the field quaternion includes calculating a gravitational quaternion able to rotate an accelerometer reading so that a normalized Z-axis component of the accelerometer reading is equal to 1.

12. The method of claim 11 wherein calculating the field quaternion additionally includes:

using the gravitational quaternion to rotate a magnetometer reading;

setting the rotated magnetometer reading'"'"'s Z-axis component to zero, creating an adjusted magnetometer reading;

calculating a magnetometer quaternion able to further rotate the adjusted magnetometer reading so that a normalized X-axis component is equal to −

1; and

,

combining the gravitational quaternion and the magnetometer quaternion.

13. The method of claim 11 wherein calculating the field quaternion further includes:

combining a plurality of accelerometer readings to create a combined accelerometer reading; and

,

using the combined accelerometer reading to calculate an accelerometer quaternion.

14. The method of claim 13 wherein combining the plurality of accelerometer reading creates an average accelerometer reading.

15. The method of claim 12 wherein calculating the field quaternion includes combining a plurality of magnetometer readings to create a combined magnetometer reading;

and,

using the combined magnetometer reading to calculate a magnetometer quaternion.

16. The method of claim 15 wherein combining the plurality of magnetometer readings includes calculating an average magnetometer reading.

17. The method of claim 15 wherein calculating the field quaternion includes selecting a number of readings in the plurality of magnetometers readings in response to a distance between a current reading and past readings, and the estimated IMU magnetometer jitter.

18. The method of claim 1 further comprising:

low-pass filtering four vector elements of the current sample quaternion to provide an output quaternion.

19. The method of claim 1 further comprising:

comparing a sum of angular commonalties between each current sample quaternion in a group of current sample quaternions, with other current sample quaternions from the group; and

,

selecting the current sample quaternion associated with the largest sum as an output quaternion.

20. A method for an inertial measurement unit (IMU) to calculate an orientation-related quaternion, the method comprising:

the IMU measuring a first orientation-related vector and a second orientation-related vector;

when the angle between the first orientation-related vector and the second orientation-related vectors is obtuse, inverting the first orientation-related vector; and

,

calculating an orientation-related quaternion using the inverted first orientation-related vector and the second orientation-related vector.

21. The method of claim 20 further comprising:

when the angle between the first orientation-related vector and the second orientation vector is 90 degrees or acute, calculating the orientation-related quaternion using the first orientation-related vector and second orientation-related vector; and

,

when the angle between the first orientation-related vector and the second orientation vector is 180 degrees, setting the orientation-related quaternion to a normalized value of a previous orientation-related quaternion with the previous orientation-related quaternion r component set to zero.

22. An inertial measurement unit (IMU) comprising:

a gyroscope having an output to supply gyroscopic readings;

an accelerometer having an output to supply accelerometer readings;

a processor,

a non-transitory memory;

an IMU application residing in the non-transitory memory, comprising a sequence of processor executable instructions for calculating a gyroscopic quaternion in response to gyroscopic readings, calculating a field quaternion using accelerometer readings when an accelerometer reading is about equal to gravity (1 G), estimating angular orientation errors due to IMU angular velocity and linear acceleration, and using the angular orientation errors to selectively mix the gyroscopic quaternion and field quaternion to supply a current sample quaternion.

23. The IMU of claim 22 further comprising:

a magnetometer having an output to supply magnetometer readings; and

,

wherein the IMU application estimates magnetometer jitter errors in response to magnetometer readings, calculates the field quaternion using the accelerometer readings and magnetometer readings, and selectively mixes the gyroscopic quaternion and field quaternion includes in response to the angular orientation errors and the estimated magnetometer jitter errors.

24. The IMU of claim 23 wherein the IMU application estimates the IMU magnetometer jitters errors, and calibrates the gyroscope readings and accelerometer readings in response to determining a lack of IMU movement.

25. The IMU of claim 22 wherein the IMU application uses the gyroscopic quaternion as the current sample quaternion when the accelerometer reading is not about equal to 1 G.

26. The IMU of claim 22 wherein the IMU application, prior to calculating the field quaternion, calculates a total angular orientation error by combining an orientation angular error due to centripetal acceleration, an orientation angular error due to linear acceleration, and an orientation angular error due to estimated magnetometer jitter.

27. The IMU of claim 26 wherein the IMU application calculates an angular difference between the gyroscopic quaternion and field quaternion, compares the angular difference to the total angular orientation error, and selectively mixes the gyroscopic quaternion and field quaternion in response to the comparison.

28. The IMU of claim 22 wherein the IMU application calculates the field quaternion by:

calculating a gravitational quaternion able to rotate an accelerometer reading so that a normalized Z-axis component of the accelerometer reading is equal to 1;

using the gravitational quaternion to rotate a magnetometer reading;

setting the rotated magnetometer reading'"'"'s Z-axis component to zero, creating an adjusted magnetometer reading;

calculating a magnetometer quaternion able to further rotate the adjusted magnetometer reading so that a normalized X-axis component is equal to −

1; and

,

combining the gravitational quaternion and the magnetometer quaternion.

29. The IMU of claim 28 wherein the IMU application calculates the field quaternion by combining a plurality of accelerometer readings to create an average accelerometer reading, and using the average accelerometer reading to calculate an accelerometer quaternion.

30. The IMU of claim 28 wherein the IMU application calculates the field quaternion by combining a plurality of magnetometer readings to create an average magnetometer reading, and using the average magnetometer reading to calculate a magnetometer quaternion.

31. The IMU of claim 30 wherein the IMU application selects a number of readings in the plurality of magnetometers readings in response to a distance between a current reading and past readings, and the estimated magnetometer jitter.

32. The IMU of claim 22 wherein the IMU application low-pass filters four vector elements of the current sample quaternion to provide an output quaternion.

33. The IMU of claim 22 wherein the IMU application compares a sum of angular commonalties between each current sample quaternion in a group of current sample quaternions, with other current sample quaternions from the group, and selects the current sample quaternion associated with the largest sum as an output quaternion.