Sensor Fusion Method for Determining Orientation of an Object

• US 20170074689A1
• Filed: 09/09/2016
• Published: 03/16/2017
• Est. Priority Date: 09/11/2015
• Status: Active Application

• Current Assignee: Renesas Design Netherlands BV
• Original Assignee: Dialog Semiconductor B.V. (Renesas Electronics Corporation)
• Inventors: MacAulay, Robert, Lubberhuizen, Wessel Harm
• Application Number:
  US15/260,807
• Publication Number:
  US 20170074689A1
• Time in Patent Office: Days
• US Class Current: 1/1
• CPC Class Codes: G01B 21/04 by measuring coordinates of points
• CPC Class Codes: G01C 17/28 Electromagnetic compasses with north seeking magnetic elements and having electric pick-offs G01C17/26
• CPC Class Codes: G01C 19/00 Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
• CPC Class Codes: G01D 5/56 using electric or magnetic means
• CPC Class Codes: G01P 15/18 in two or more dimensions
• CPC Class Codes: G01R 33/02 Measuring direction or magnitude of magnetic fields or magnetic flux G01R33/20 takes precedence

Abstract

A sensor fusion method of calculating an orientation of an object by combining readings from different types of orientation sensors to estimate the orientation of an object. An analytical solution is provided which is computationally efficient and can be implemented in fixed or floating point architecture. The method comprises receiving an input orientation; receiving a reading from a first orientation sensor; receiving a reading from a second orientation sensor; where said first and second orientation sensors are of different types; and determining an updated orientation by calculating a rotation based on the orientation sensor readings and applying the calculated rotation to the input orientation.

Claims

1. A method of calculating an orientation of an object comprising:

receiving an input orientation;

receiving a reading from a first orientation sensor;

receiving a reading from a second orientation sensor;

where said first and second orientation sensors are of different types; and

determining an updated orientation by calculating a rotation based on the orientation sensor readings and applying the calculated rotation to the input orientation;

wherein calculating a rotation comprises;

calculating a first rotation which rotates the reading from one of the orientation sensors to be aligned with a first reference direction;

applying the first rotation to the reading from the other of the orientation sensors to obtain an intermediate orientation;

calculating a second rotation that rotates the intermediate orientation to be aligned with a reference plane which is spanned by axes including an axis aligned with the first reference direction; and

combining the first and second rotations.

2. The method of claim 1, wherein calculating a second rotation comprises calculating a second rotation that rotates the intermediate orientation to be aligned with a second reference direction which is orthogonal to the first reference direction.

3. The method of claim 1, wherein the first sensor comprises an accelerometer and the second sensor comprises a magnetometer;

and wherein

calculating a first rotation comprises rotating the reading from the accelerometer into an accelerometer reference axis and rotating the reading from the magnetometer into a magnetometer reference plane.

4. The method of claim 3, wherein the accelerometer reference axis comprises a gravitational axis and the magnetometer reference plane comprises a north-down plane.

5. The method of claim 1, further comprising receiving a reading from a third orientation sensor being of a different type from said first and second orientation sensors and wherein calculating a rotation comprises combining a third rotation derived from the third orientation sensor together with said first and second rotations.

6. The method of claim 5, wherein the third sensor comprises a gyroscope.

7. The method of claim 6, wherein calculating a rotation comprises applying a rotation to the input orientation based on the readings from the gyroscope to obtain a preliminary orientation;

and then applying said first and second rotations to the preliminary orientation estimate.

8. The method of claim 1, wherein the first and second orientation sensor readings are converted to quaternion form and the calculated rotations comprise unit quaternions.

9. The method of claim 5, wherein the third orientation sensor reading is converted to quaternion form and the calculated rotations comprise unit quaternions.

10. The method of claim 9, wherein the combination of successive rotations comprises moving along the surface of a unit quaternion hypersphere.

11. The method of claim 1, wherein the sensors have different sampling rates;

and wherein the method is repeated and makes use of any available readings that have been made at or between successive iterations of the method.

12. The method of claim 1, wherein the rotation applied for the readings of each sensor is modified according to a weight factor and the updated object orientation depends on the weighted contributions.

13. The method of claim 12, wherein the weight factors for each rotation depend on the relative noise levels associated with each sensor.

14. The method of claim 12 wherein the rotation is modified for each sensor before data from the next sensor is processed.

15. The method of claim 12 wherein the rotations for each sensor are modified after data from all the sensors have been processed.

16. The method of claim 1, wherein calculations that involve known zeros are omitted.

17. The method of claim 1, implemented in a floating point architecture.

18. The method of claim 1, implemented in a fixed point architecture.

19. An apparatus for determining the orientation of an object comprising one or more sensors associated with the object, and a processor arranged to receive an input orientation;

receive a reading from a first orientation sensor;

receive a reading from a second orientation sensor, where said first and second orientation sensors are of different types; and

to determine an updated orientation by calculating a rotation based on the orientation sensor readings and apply the calculated rotation to the input orientation;

wherein calculating a rotation comprises calculating a first rotation which rotates the reading from one of the orientation sensors to be aligned with a first reference direction;

applying the first rotation to the reading from the other of the orientation sensors to obtain an intermediate orientation;

calculating a second rotation that rotates the intermediate orientation to be aligned with a reference plane which is spanned by axes including an axis aligned with the first reference direction; and

combining the first and second rotations.

20. The apparatus of claim 19, wherein calculating a second rotation comprises calculating a second rotation that rotates the intermediate orientation to be aligned with a second reference direction which is orthogonal to the first reference direction.

21. The apparatus of claim 19, wherein the first sensor comprises an accelerometer and the second sensor comprises a magnetometer;

and wherein

calculating a first rotation comprises rotating the reading from the accelerometer into an accelerometer reference axis and rotating the reading from the magnetometer into a magnetometer reference plane.

22. The apparatus of claim 21, wherein the accelerometer reference axis comprises a gravitational axis and the magnetometer reference plane comprises a north-down plane.

23. The apparatus of claim 19, which receives a reading from a third orientation sensor being of a different type from said first and second orientation sensors and wherein calculating a rotation comprises combining a third rotation derived from the third orientation sensor together with said first and second rotations.

24. The apparatus of claim 23, wherein the third sensor comprises a gyroscope.

25. The apparatus of claim 24, wherein calculating a rotation comprises applying a rotation to the input orientation based on the readings from the gyroscope to obtain a preliminary orientation;

and then applying said first and second rotations to the preliminary orientation estimate.

26. The apparatus of claim 19, wherein the first and second orientation sensor readings are converted to quaternion form and the calculated rotations comprise unit quaternions.

27. The apparatus of claim 23, wherein the third orientation sensor reading is converted to quaternion form and the calculated rotations comprise unit quaternions.

28. The apparatus of claim 27, wherein the combination of successive rotations comprises moving along the surface of a unit quaternion hypersphere.

29. The apparatus of claim 19, wherein the sensors have different sampling rates;

and wherein the method is repeated and makes use of any available readings that have been made at or between successive iterations of the method.

30. The apparatus of claim 19, wherein the rotation applied for the readings of each sensor is modified according to a weight factor and the updated object orientation depends on the weighted contributions.

31. The apparatus of claim 30, wherein the weight factors for each rotation depend on the relative noise levels associated with each sensor.

32. The apparatus of claim 30, wherein the rotation is modified for each sensor before data from the next sensor is processed.

33. The apparatus of claim 30, wherein the rotations for each sensor are modified after data from all the sensors have been processed.

34. The apparatus of claim 19, wherein calculations that involve known zeros are omitted.

35. The apparatus of claim 19, implemented in a floating point architecture.

36. The apparatus of claim 19, implemented in a fixed point architecture.