Attitude angle derivation device and attitude angle sensor
According to one embodiment, an attitude angle derivation device includes an acquisition part, a storage, and a processor. The acquisition part is configured to acquire a rotation angle related to a first coordinate system. The rotation angle is obtained from an angle sensor located in an object. The storage is configured to store the rotation angle and an attitude angle. The attitude angle is related to a second coordinate system of the object. The processor is configured to acquire the rotation angle and the attitude angle, update the attitude angle based on a temporal change of the rotation angle derived from the rotation angle, and output the updated attitude angle.
1. An attitude angle derivation device, comprising:
an electronic interface configured to acquire a rotation angle related to a first coordinate system, the rotation angle being obtained from an angle sensor located in an object;
a memory configured to store the rotation angle and an attitude angle, the attitude angle being related to a second coordinate system of the object; and
a processor configured to acquire the rotation angle and the attitude angle by sampling including an irregular sampling, update the attitude angle based on a temporal change of the rotation angle derived from the rotation angle, and output the updated attitude angle via the electronic interface.
2. The device according to claim 1 , wherein
the angle sensor includes an integrating gyroscope.
3. The device according to claim 1 , wherein
the angle sensor includes a movable member,
the movable member can vibrate,
the vibration of the movable member includes:
a first component along a first direction; and
a second component along a second direction crossing the first direction, and
the angle sensor is configured to output, as the rotation angle, a rotation angle of the movable member obtained based on the first and second components.
4. The device according to claim 1 , wherein,
the processor includes a first calculation part, and
the first calculation part is configured to derive the temporal change of the rotation angle.
5. The device according to claim 4 , wherein
the processor further includes a second calculation part, and
the second calculation part is configured to derive a change of the attitude angle based on the temporal change derived by the first calculation part and based on the attitude angle acquired from the storage.
6. The device according to claim 5 , wherein
the processor further includes a third calculation part, and
the third calculation part is configured to derive the updated attitude angle by adding:
the change of the attitude angle derived by the second calculation part; and
the attitude angle acquired from the storage.
7. The device according to claim 1 , wherein
the processor supplies the updated attitude angle to the storage, and
the storage is configured to store the updated attitude angle.
8. The device according to claim 1 , wherein
the rotation angle includes a first rotation angle related to a first axis, a second rotation angle related to a second axis, and a third rotation angle related to a third axis,
the second axis crosses the first axis, and
the third axis crosses a plane including the first and second axes.
9. The device according to claim 8 , wherein
the second axis is orthogonal to the first axis, and
the third axis is orthogonal to the first and second axes.
10. The device according to claim 9 , wherein
the rotation angle includes the first rotation angle A x(t) at a first time t, the second rotation angle A y(t) at the first time t, and the third rotation angle A z(t) at the first time t,
the processor derives a first temporal change dA x(t) , a second temporal change dA y(t) , and a third temporal change dA z(t) as the temporal change of the rotation angle, and
the first temporal change dA x(t) , the second temporal change dA y(t) , the third temporal change dA z(t) , the first rotation angle A x(t) , the second rotation angle A y(t) , the third rotation angle A z(t) , the first rotation angle A x(t-1) at a second time (t−1) before the first time t, the second rotation angle A y(t-1) at the second time (t−1), and the third rotation angle A z(t-1) at the second time (t−1) satisfy
dA x(t) =A x(t) −A x(t-1)
dA y(t) =A y(t) −A y(t-1)
dA z(t) =A z(t) −A z(t-1) . (1)
11. The device according to claim 10 , wherein
the attitude angle includes:
a first attitude angle related to a first attitude axis;
a second attitude angle related to a second attitude axis; and
a third attitude angle related to a third attitude axis,
the second attitude axis crosses the first attitude axis, and
the third attitude axis crosses a plane including the first and second attitude axes.
12. The device according to claim 11 , wherein
the second attitude axis is orthogonal to the first attitude axis, and
the third attitude axis is orthogonal to the first and second attitude axes.
13. The device according to claim 12 , wherein
the attitude angle includes the first attitude angle ϕ (t-1) at the second time (t−1), the second attitude angle θ (t-1) at the second time (t−1), and the third attitude angle ψ (t-1) at the second time (t−1),
the processor derives a first attitude angle change dϕ (t) , a second attitude angle change dθ (t) , and a third attitude angle change dψ (t) as a change of the attitude angle, and
the first attitude angle change dϕ (t) , the second attitude angle change dθ (t) , and the third attitude angle change dψ (t) satisfy
dϕ (t) =( dA y(t) sin ϕ (t-1) +dA x(t) cos ϕ (t-1) ) tan θ (t-1) +dA x(t)
dθ (t) =dA y(t) cos ϕ (t-1) −dA x(t) sin ϕ (t-1)
dψ (t) =( dA y(t) sin ϕ+ dA x(t) cos ϕ)/cos θ (t-1) . (2)
14. The device according to claim 13 , wherein
the processor derives the first attitude angle ϕ (t) at the first time t, the second attitude angle θ (t) at the first time t, and the third attitude angle ψ (t) at the first time t as the updated attitude angle, and
the first attitude angle ϕ (t) at the first time t, the second attitude angle θ (t) at the first time t, and the third attitude angle ψ (t) at the first time t satisfy
ϕ (t) =ϕ (t-1) +dϕ (t)
θ (t) =θ (t-1) +dθ (t)
ψ (t) =ψ (t-1) +dψ (t) . (3)
15. The device according to claim 10 , wherein
the attitude angle includes a first quaternion value, a second quaternion value, a third quaternion value, and a fourth quaternion value.
16. The device according to claim 15 , wherein
the attitude angle includes the first quaternion value α (t-1) at the second time (t−1), the second quaternion value β (t-1) at the second time (t−1), the third quaternion value γ (t-1) at the second time (t−1), and the fourth quaternion value δ (t-1) at the second time (t−1),
the processor derives a first quaternion value change dα (t) , a second quaternion value change dβ (t) , a third quaternion value change dγ (t) , and a fourth quaternion value change dδ (t) as a change of the attitude angle, and
the first quaternion value change dα (t) , the second quaternion value change dβ (t) , the third quaternion value change dγ (t) , and the fourth quaternion value change dδ (t) satisfy
dα (t) =−0.5(β (t-1) dA x(t) +γ (t-1) dA y(t) +δ (t-1) dA z(t) )
dβ (t) =0.5(α (t-1) dA x(t) −δ (t-1) dA y(t) +γ (t-1) dA z(t) )
dγ (t) =0.5(δ d (t-1) dA x(t) +α (t-1) dA y(t) −β (t-1) dA z(t) )
dδ (t) =−0.5(γ (t-1) dA x(t) −β (t-1) dA y(t) −α (t-1) dA z(t) ). (4)
17. The device according to claim 16 , wherein
the processor derives the first quaternion value α (t) at the first time t, the second quaternion value β (t) at the first time t, the third quaternion value γ (t) at the first time t, and the fourth quaternion value δ (t) at the first time t as the updated attitude angle, and
the first quaternion value α (t) at the first time t, the second quaternion value β (t) at the first time t, the third quaternion value γ (t) at the first time t, and the fourth quaternion value δ (t) at the first time t satisfy
α (t) =α (t-1) +dα (t)
β (t) =β (t-1) +dβ (t)
γ (t) =γ (t-1) +dγ (t)
δ (t) =δ (t-1) +dδ (t) (5)
18. The device according to claim 17 , wherein
the processor derives, as the updated attitude angle, the first attitude angle ϕ (t) at the first time t, the second attitude angle θ (t) at the first time t, and the third attitude angle ψ (t) at the first time t based on the first quaternion value α (t) at the first time t, the second quaternion value β (t) at the first time t, the third quaternion value γ (t) at the first time t, and the fourth quaternion value δ (t) at the first time t, and
the first attitude angle ϕ (t) at the first time t, the second attitude angle δ (t) at the first time t, and the third attitude angle ψ (t) at the first time t satisfy
ϕ
(
t
)
=
arctan
(
2
(
γ
(
t
)
δ
(
t
)
+
α
(
t
)
β
(
t
)
)
α
(
t
)
2
-
β
(
t
)
2
-
γ
(
t
)
2
+
δ
(
t
)
2
)
θ
(
t
)
=
arcsin
(
-
2
(
β
(
t
)
δ
(
t
)
-
α
(
t
)
γ
(
t
)
)
)
ψ
(
t
)
=
arctan
(
2
(
β
(
t
)
γ
(
t
)
+
α
(
t
)
δ
(
t
)
)
α
(
t
)
2
+
β
(
t
)
2
-
γ
(
t
)
2
-
δ
(
t
)
2
)
.
(
6
)
19. An attitude angle sensor, comprising:
the attitude angle derivation device according to claim 1 ; and
the angle sensor.
20. An attitude angle derivation device, comprising:
an electronic interface configured to acquire a rotation angle related to a first coordinate system, the rotation angle being obtained from an angle sensor located in an object by sampling including an irregular sampling;
a memory configured to store the rotation angle and an attitude angle, the attitude angle being related to a second coordinate system of the object; and
a processor configured to acquire the rotation angle and the attitude angle, update the attitude angle based on a temporal change of the rotation angle derived from the rotation angle, and output the updated attitude angle via the electronic interface.