Posture information calculation device, posture information calculation system, posture information calculation method, and information storage medium

• US 9,043,177 B2
• Filed: 02/04/2011
• Issued: 05/26/2015
• Est. Priority Date: 03/05/2010
• Status: Active Grant

• Current Assignee: Seiko Epson Corporation (Seiko Group)
• Original Assignee: Seiko Epson Corporation (Seiko Group)
• Inventors: Udagawa, Hirofumi, Kobayashi, Yoshihiro, Nakaoka, Yasushi
• Primary Examiner(s): Nghiem, Michael
• Assistant Examiner(s): RABOVIANSKI, IVAN I
• Application Number:
  US13/020,876
• Publication Number:
  US 20110218753A1
• Time in Patent Office: 1,572 Days
• Field of Search: 702/94, 702/141, 702/142, 702/150
• US Class Current: 702/94
• CPC Class Codes: G06F 15/00 Digital computers in general details G06F1/00 – G06F13/00; Data processing equipment in general
• CPC Class Codes: G16Z 99/00 Subject matter not provided for in other main groups of this subclass

Abstract

A sensor information acquisition section acquires angular velocity information (GX, GY, GZ) around three axes acquired by three angular velocity sensors, and acceleration information (AX, AY, AZ) in three axial directions acquired by three acceleration sensors. A posture information calculation section calculates a posture angle and position coordinates in a virtual three-dimensional space based on the angular velocity information (GX, GY, GZ) and the acceleration information (AX, AY, AZ). The posture information calculation section calculates a fixed coordinate system velocity vector based on an inertial coordinate system acceleration vector (A) obtained from the acceleration information (AX, AY, AZ), and calculates position coordinates in a virtual three-dimensional space corresponding to the fixed coordinate system velocity vector.

Claims

1. A posture information calculation device that calculates a posture angle and position coordinates in a virtual three-dimensional space based on three-axis angular velocity information detected by three angular velocity sensors, and three-axis acceleration information detected by three acceleration sensors, the three-axis angular velocity information being information about angular velocities around three axes that are not parallel to one another, and the three-axis acceleration information being information about accelerations in three axial directions that are not parallel to one another, the posture information calculation device comprising:

a sensor information acquisition section that acquires the three-axis angular velocity information, and the three-axis acceleration information; and

a posture information calculation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information acquired by the sensor information acquisition section,

the posture information calculation section calculating a fixed coordinate system velocity vector based on an inertial coordinate system acceleration vector obtained from the three-axis acceleration information, and calculating the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector,

the posture information calculation section including;

a posture information update section that updates a parameter necessary for specifying the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information;

a posture information correction section that corrects the parameter updated by the posture information update section under a constraint condition; and

a posture information generation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the parameter corrected by the posture information correction section,

the posture information update section including a velocity vector update section that updates the fixed coordinate system velocity vector at a time k based on the inertial coordinate system acceleration vector and the fixed coordinate system velocity vector at a time k−

1;

the posture information correction section including a velocity vector correction section that corrects the fixed coordinate system velocity vector at the time k updated by the velocity vector update section under a constraint condition that a norm of the fixed coordinate system velocity vector is zero;

the posture information generation section including a position coordinate generation section that generates the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction section; and

the sensor information acquisition section and the posture information calculation section being implemented by a processor, the three angular velocity sensors and the three acceleration sensors.

2. The posture information calculation device as defined in claim 1,

the velocity vector update section updating the fixed coordinate system velocity vector at the time k by:

converting the inertial coordinate system acceleration vector into a fixed coordinate system acceleration vector by rotating the inertial coordinate system acceleration vector based on a fixed coordinate system posture angle vector at the time k−

1;

subtracting a fixed coordinate system gravitational acceleration vector from the fixed coordinate system acceleration vector;

performing a first-order integration process on the subtraction result within a predetermined unit time to calculate a unit time fixed coordinate system velocity vector; and

adding the unit time fixed coordinate system velocity vector to the fixed coordinate system velocity vector at the time k−

1 corrected by the velocity vector correction section.

3. The posture information calculation device as defined in claim 1,

the position coordinate generation section setting the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction section to be the position coordinates at the time k.

4. The posture information calculation device as defined in claim 2,

the position coordinate generation section setting the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction section to be the position coordinates at the time k.

5. The posture information calculation device as defined in claim 1,

the posture information calculation section calculating a fixed coordinate system posture angle vector based on an inertial coordinate system angular velocity vector obtained from the three-axis angular velocity information, and calculating the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector;

the posture information update section including a posture angle vector update section that updates the fixed coordinate system posture angle vector at the time k based on the inertial coordinate system angular velocity vector and the fixed coordinate system posture angle vector at the time k−

1;

the posture information correction section including a posture angle vector correction section that corrects the fixed coordinate system posture angle vector at the time k updated by the posture angle vector update section under a constraint condition based on the inertial coordinate system acceleration vector; and

the posture information generation section including a posture angle generation section that generates the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector corrected by the posture angle vector correction section.

6. The posture information calculation device as defined in claim 2,

the posture information calculation section calculating a fixed coordinate system posture angle vector based on an inertial coordinate system angular velocity vector obtained from the three-axis angular velocity information, and calculating the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector;

the posture information update section including a posture angle vector update section that updates the fixed coordinate system posture angle vector at the time k based on the inertial coordinate system angular velocity vector and the fixed coordinate system posture angle vector at the time k−

1;

the posture information correction section including a posture angle vector correction section that corrects the fixed coordinate system posture angle vector at the time k updated by the posture angle vector update section under a constraint condition based on the inertial coordinate system acceleration vector; and

the posture information generation section including a posture angle generation section that generates the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector corrected by the posture angle vector correction section.

7. The posture information calculation device as defined in claim 3,

the posture information calculation section calculating a fixed coordinate system posture angle vector based on an inertial coordinate system angular velocity vector obtained from the three-axis angular velocity information, and calculating the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector;

the posture information update section including a posture angle vector update section that updates the fixed coordinate system posture angle vector at the time k based on the inertial coordinate system angular velocity vector and the fixed coordinate system posture angle vector at the time k−

1;

the posture information correction section including a posture angle vector correction section that corrects the fixed coordinate system posture angle vector at the time k updated by the posture angle vector update section under a constraint condition based on the inertial coordinate system acceleration vector; and

the posture information generation section including a posture angle generation section that generates the posture angle in the virtual three-dimensional space based on the fixed coordinate system posture angle vector corrected by the posture angle vector correction section.

8. The posture information calculation device as defined in claim 5,

the posture angle vector update section updating the fixed coordinate system posture angle vector at the time k by performing a first-order integration process on the inertial coordinate system angular velocity vector within a predetermined unit time to calculate a unit time rotational angle quaternion, and calculating the fixed coordinate system posture angle vector at the time k−

1 corrected by the posture angle vector correction section with the unit time rotational angle quaternion.

9. The posture information calculation device as defined in claim 5,

the posture angle generation section setting the fixed coordinate system posture angle vector at the time k corrected by the posture angle vector correction section to be the posture angle at the time k.

10. The posture information calculation device as defined in claim 8,

the posture angle generation section setting the fixed coordinate system posture angle vector at the time k corrected by the posture angle vector correction section to be the posture angle at the time k.

11. The posture information calculation device as defined in claim 1,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

12. The posture information calculation device as defined in claim 2,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

13. The posture information calculation device as defined in claim 3,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

14. The posture information calculation device as defined in claim 5,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

15. The posture information calculation device as defined in claim 8,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

16. The posture information calculation device as defined in claim 9,

the sensor information acquisition section receiving temperature information detected by a temperature sensor;

the posture information update section including;

an angular velocity offset update section that updates an offset of the three angular velocity sensors at the time k based on the temperature information and the offset of the three angular velocity sensors at the time k−

1; and

an acceleration offset update section that updates an offset of the three acceleration sensors at the time k based on the temperature information and the offset of the three acceleration sensors at the time k−

1; and

the posture information correction section including;

an angular velocity offset correction section that corrects the offset of the three angular velocity sensors at the time k updated by the angular velocity offset update section; and

an acceleration offset correction section that corrects the offset of the three acceleration sensors at the time k updated by the acceleration offset update section.

17. A posture information calculation system comprising:

three angular velocity sensors that respectively detect three-axis angular velocity information that is information about angular velocities around three axes that are not parallel to one another;

three acceleration sensors that respectively detect three-axis acceleration information that is information about accelerations in three axial directions that are not parallel to one another; and

a posture information calculation device that calculates a posture angle and position coordinates in a virtual three-dimensional space based on the three-axis angular velocity information detected by the three angular velocity sensors, and the three-axis acceleration information detected by the three acceleration sensors, the posture information calculation device comprising;

a sensor information acquisition section that acquires the three-axis angular velocity information, and the three-axis acceleration information; and

a posture information calculation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information acquired by the sensor information acquisition section,

the posture information calculation section calculating a fixed coordinate system velocity vector based on an inertial coordinate system acceleration vector obtained from the three-axis acceleration information, and calculating the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector,

the posture information calculation section including;

a posture information update section that updates a parameter necessary for specifying the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information;

a posture information correction section that corrects the parameter updated by the posture information update section under a constraint condition; and

a posture information generation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the parameter corrected by the posture information correction section,

the posture information update section including a velocity vector update section that updates the fixed coordinate system velocity vector at a time k based on the inertial coordinate system acceleration vector and the fixed coordinate system velocity vector at a time k−

1;

the posture information correction section including a velocity vector correction section that corrects the fixed coordinate system velocity vector at the time k updated by the velocity vector update section under a constraint condition that a norm of the fixed coordinate system velocity vector is zero;

the posture information generation section including a position coordinate generation section that generates the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction section; and

the sensor information acquisition section and the posture information calculation section being implemented by a processor the three angular velocity sensors and the three acceleration sensors.

18. A posture information calculation method that is implemented by a posture information calculation device, and calculates a posture angle and position coordinates in a virtual three-dimensional space based on three-axis angular velocity information detected by three angular velocity sensors, and three-axis acceleration information detected by three acceleration sensors, the three-axis angular velocity information being information about angular velocities around three axes that are not parallel to one another, and the three-axis acceleration information being information about accelerations in three axial directions that are not parallel to one another, the posture information calculation method comprising:

a sensor information acquisition step that acquires the three-axis angular velocity information, and the three-axis acceleration information; and

a posture information calculation step that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information acquired by the sensor information acquisition step,

the posture information calculation step calculating a fixed coordinate system velocity vector based on an inertial coordinate system acceleration vector obtained from the three-axis acceleration information, and calculating the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector,

the posture information calculation step including;

a posture information update step that updates a parameter necessary for specifying the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information;

a posture information correction step that corrects the parameter updated by the posture information update step under a constraint condition; and

a posture information generation step that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the parameter corrected by the posture information correction step,

the posture information update step including a velocity vector update step that updates the fixed coordinate system velocity vector at a time k based on the inertial coordinate system acceleration vector and the fixed coordinate system velocity vector at a time k−

1;

the posture information correction step including a velocity vector correction step that corrects the fixed coordinate system velocity vector at the time k updated by the velocity vector update step under a constraint condition that a norm of the fixed coordinate system velocity vector is zero;

the posture information generation step including a position coordinate generation step that generates the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction step; and

the sensor information acquisition step and the posture information calculation step being performed by a processor the three angular velocity sensors and the three acceleration sensors.

19. A non-transitory computer-readable information storage medium storing a program that calculates a posture angle and position coordinates in a virtual three-dimensional space based on three-axis angular velocity information detected by three angular sensors, and three-axis acceleration information detected by three acceleration sensors, the three-axis angular velocity information being information about angular velocities around three axes that are not parallel to one another, and the three-axis acceleration information being information about accelerations in three axial directions that are not parallel to one another, the program causing a computer to function as:

a posture information calculation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information,

the posture information calculation section calculating a fixed coordinate system velocity vector based on an inertial coordinate system acceleration vector obtained from the three-axis acceleration information, and calculating the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector,

the posture information calculation section including;

a posture information update section that updates a parameter necessary for specifying the posture angle and the position coordinates in the virtual three-dimensional space based on the three-axis angular velocity information and the three-axis acceleration information;

a posture information correction section that corrects the parameter updated by the posture information update section under a constraint condition; and

a posture information generation section that calculates the posture angle and the position coordinates in the virtual three-dimensional space based on the parameter corrected by the posture information correction section,

the posture information update section including a velocity vector update section that updates the fixed coordinate system velocity vector at a time k based on the inertial coordinate system acceleration vector and the fixed coordinate system velocity vector at a time k−

1;

the posture information correction section including a velocity vector correction section that corrects the fixed coordinate system velocity vector at the time k updated by the velocity vector update section under a constraint condition that a norm of the fixed coordinate system velocity vector is zero;

the posture information generation section including a position coordinate generation section that generates the position coordinates in the virtual three-dimensional space corresponding to the fixed coordinate system velocity vector at the time k corrected by the velocity vector correction section; and

the posture information calculation section being implemented by a processor the three angular velocity sensors and the three acceleration sensors.