Orientation angle detector
First Claim
Patent Images
1. An orientation angle detector comprising:
- a plurality of gyroscopes (301-303) disposed in parallel with a plurality of orthogonal axes (Xs-Zs) defining a detector co-ordinate on said detector, respectively, for measuring angular velocities (Jx, Jy, Jz) around respective axes (Xs-Zs), a motion angle calculator (310) coupled to said plurality of gyroscopes (301-303) for calculating a motion angle (Δ
X, Δ
Y, Δ
Z) from said angular velocities (Jx, Jy, Jz), at least one accelerometer (403, 404) disposed in parallel with at least one of said orthogonal axes (Xs-Zs) for measuring linear acceleration (Ax, Ay), a static angle calculator (405) coupled to said at least one accelerometer (403, 404) for calculating a static angle (R, P) from said acceleration (Ax, Ay), and an orientation angle calculator (60) coupled to said motion angle calculator (310) and said static angle calculator (405) for integrating said motion angle (Δ
X, Δ
Y, Δ
Z) to an integrated angle and calculating an orientation angle (α
, β
, γ
) from said integrated angle, said static angle (R, P) and an azimuthal deviation angle (Φ
), wherein said orientation angle (α
, β
, γ
) is an angular difference between said detector co-ordinate (Xs-Ys-Zs) and a reference co-ordinate (X-Y-Z) in a space including said detector, wherein two magnetometers (401, 402) are disposed in parallel with two of said plurality of orthogonal axes (Xs, Ys) to measure terrestrial magnetic components (Mx, My) in the two of said plurality of axes (Xs, Ys), and wherein said static angle calculator (405) is coupled to said two magnetometers (401, 402) for calculating an azimuth from said terrestrial magnetic components (Mx, My) to produce from said azimuth said azimuthal deviation angle (Φ
) from said reference co-ordinate (X-Y-Z) as an additional factor of said static angle (R, P).
2 Assignments
0 Petitions
Accused Products
Abstract
An orientation angle detector using gyroscopes (301-303) for detecting X-, Y- and Z-angular velocities which are time-integrated (60) to produce pitch, roll and yaw angles (γ, β, α) of the orientation. Two accelerometers (403, 404) are used to obtain tentative pitch and roll angles in order to correct the pitch and roll angles, and two terrestrial magnetometers (401, 402) are used to obtain a tentative yaw angle so as to correct the yaw angle. When the tentative pitch, roll and yaw angles are defined accurate (50), the integrated pitch, roll and yaw angles are corrected (60) by the tentative pitch, roll and yaw angles.
186 Citations
64 Claims
-
1. An orientation angle detector comprising:
-
a plurality of gyroscopes (301-303) disposed in parallel with a plurality of orthogonal axes (Xs-Zs) defining a detector co-ordinate on said detector, respectively, for measuring angular velocities (Jx, Jy, Jz) around respective axes (Xs-Zs), a motion angle calculator (310) coupled to said plurality of gyroscopes (301-303) for calculating a motion angle (Δ
X, Δ
Y, Δ
Z) from said angular velocities (Jx, Jy, Jz),at least one accelerometer (403, 404) disposed in parallel with at least one of said orthogonal axes (Xs-Zs) for measuring linear acceleration (Ax, Ay), a static angle calculator (405) coupled to said at least one accelerometer (403, 404) for calculating a static angle (R, P) from said acceleration (Ax, Ay), and an orientation angle calculator (60) coupled to said motion angle calculator (310) and said static angle calculator (405) for integrating said motion angle (Δ
X, Δ
Y, Δ
Z) to an integrated angle and calculating an orientation angle (α
, β
, γ
) from said integrated angle, said static angle (R, P) and an azimuthal deviation angle (Φ
),wherein said orientation angle (α
, β
, γ
) is an angular difference between said detector co-ordinate (Xs-Ys-Zs) and a reference co-ordinate (X-Y-Z) in a space including said detector,wherein two magnetometers (401, 402) are disposed in parallel with two of said plurality of orthogonal axes (Xs, Ys) to measure terrestrial magnetic components (Mx, My) in the two of said plurality of axes (Xs, Ys), and wherein said static angle calculator (405) is coupled to said two magnetometers (401, 402) for calculating an azimuth from said terrestrial magnetic components (Mx, My) to produce from said azimuth said azimuthal deviation angle (Φ
) from said reference co-ordinate (X-Y-Z) as an additional factor of said static angle (R, P).- View Dependent Claims (2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64)
where β
(n−
1) and γ
(n−
1) are the pitch angle β and
roll angle γ
at t=n−
1 which are read out from said orientation angle memory (70), Jx(n), Jy(n) and Jz(n) being first through third digital signals of said first through third incoming time-serial digital signals at t=n.
-
-
9. An orientation angle detector as claimed in claim 8, which further comprise three high-pass filter correction circuits (311) coupled between said first through third analogue-to-digital convertors (307-309) and said motion angle calculator, (310) for compensating errors accompanied at said first through third high-pass filters to produce first through third corrected signals, said first through third corrected signals being delivered to said motion angle calculator (310) as said first through incoming time-serial digital signals, each of said high-pass filter correction circuits (311) performs calculation given by the following equation:
-
10. An orientation angle detector as claimed in claim 9, wherein said high-pass filter correction circuit (311) comprises an integrator comprising a first adder (31101) and a registor (31102) for integrating said digital values of said corresponding time serial digital signal to hold an integrated value in said registor (31102), a constant value generator (31103) for generating said constant value, a multiplier (31104) coupled to said registor (31102) and said constant value generator (31103) for multiplying said integrated value by said constant value to produce a multiplied value, and a second adder (31105) for adding said multiplied value to said digital values time-serially incoming to produce said corrected signal.
-
11. An orientation angle detector as claimed in claim 10, wherein said high-pass filter correction circuit (311) further comprises an offset error compensating circuit (31106-3108) for compensating an offset error caused by calculation in said integrator (31101-31102) and said second. adder (31105), said offset error compensating circuit comprising an output definer (31106) for observing when said corrected signal is maintained constant to define the offset error level, an error corrector (31107) coupled to said output definer and responsive to said error level for producing an error correction signal, said error correction signal being supplied to said registor (31102) for correcting said integrated value, and a constant value correction circuit (31108) coupled to said error corrector (31107) and responsive to said error correction signal for correcting saidconstant value of said constant value generator (31103).
-
12. An orientation angle detector as claimed in claim 4, wherein said first through third high-pass filters are first through third digital high-pass filter devices (FIG. 8) to produce first through third time-serial digital filter outputs, respectivley, each of said first through third digital high-pass filter devices (FIG. 8) comprising an analogue-to-digital convertor (313) coupled to a corresponding one of said first through third gyroscopes (301-303) for sampling a corresponding one of said first through third gyro outputs at sampling intervals St to produce a time-serial digital signal, a digital filter (315) coupled to said analogue-to-digital convertor (313) for filtering said time-serial digital signal to produce a corresponding one of said first through third time-serial digital filter outputs, said motion angle calculator (310) receiving said first through third time-serial digital filter outputs as first through incoming third time-serial digital signals to produce three time-serial moving angle signals (Δ
- X, Δ
Y, Δ
Z) representing said motion angle, said orientation angle calculator (60) receiving said three time-serial moving angle signals (Δ
X, Δ
Y, Δ
Z) to time-serially produce said orientation angle, and said orientation angle memory (70) storing said orientation angle time-serially delivered from said orientation angle calculator.
- X, Δ
-
13. An orientation angle detector as claimed in claim 12, wherein said first through third time-serial moving angle signals represent the time-serial X-moving angle Δ
- X(t), t=n, (n−
1), (n−
2), . . . , 1, where St=n−
(n−
1), the time serial Y-moving angle Δ
Y(t), and the time-serial Δ
Z(t), said motion angle calculator (310) calculates the X-moving angle Δ
X(n), the Y-moving angle Δ
Y(n) and the Z-moving angle Δ
Z(n) at t=n according to the following equations;
where β
(n−
1) and γ
(n−
1) are the pitch angle β and
roll angle γ
at t=n−
1 which are read out from said orientation angle memory, Jx(n), Jy(n) and Jz(n) being first through third digital signals of said first through third incoming time-serial digital signals at t=n.
- X(t), t=n, (n−
-
14. An orientation angle detector as claimed in claim 13, wherein said digital filter is an infinite impulse response type (315) having delay buffer values (P1, P2), and wherein said digital high-pass filter device (FIG. 8) further comprises a positive/negative definer (316) coupled to an output side of said digital filter (315) for defining from said time-serial digital filter output when said time-serial digital signal changes between positive or negative to produce a change signal, and a delay buffer value reset circuit (317) coupled to said digital filter (315) and said positive/negative definer (316) and responsive to said change signal for resetting said delay buffer values (P1, P2) of said digital filter (315).
-
15. An orientation angle detector as claimed in claim 14, wherein said digital high-pass filter device (FIG. 8) further comprises an offset value generator (319) for generating an offset value, a multiplier (320) coupled to said offset value generator (319) and connecting said analogue-to-digital convertor (313) with said digital filter (315) for multiplying said time-serial digital signal by said offset value, and an offset value correcting circuit (318) coupled to said offset value generator (319) and said positive/negative definer (316) and responsive to said change signal for correcting said offset value.
-
16. An orientation angle detector as claimed in claim 3, which further comprises a printed circuit board device (201) having a first and a second side plate sections perpendicular to each other and a third side plate section to form a right-angled triangular cylinder, said first through third piezoelectric ceramic vibrators of said first through third gyroscopes (301, 302, 303) being fixedly mounted on said first, second and third side plate sections, respectively, so that said first through third gyroscopes are disposed in parallel with Xs-axis, Y-axis and Zs-axis of said detection co-ordinate.
-
17. An orientation angle detector as claimed in claim 16, wherein said printed circuit board device (201) has an end edge of said right-angled triangular cylinder which extends on a single plane, and terminals (204) extending from said end edge for electrical connection with external electric parts.
-
18. An orientation angle detector as claimed in claim 17, wherein said third side plate section extends in a direction opposite to said end edge beyond said first and second side plate sections.
-
19. An orientation angle detector as claimed in claim 18, wherein said printed circuit board device (201) comprises a single flexible printed circuit board bent to form into said right-angled triangular cylinder having said first through third side plate portions.
-
20. An orientation angle detector as claimed in claim 19, wherein said flexible printed circuit board (201) has two slits (202, 203) at bent portions at which said printed circuit board is bent.
-
21. An orientation angle detector as claimed in claim 16, wherein each of said first through third piezoelectric vibrators is made of any one of piezoelectric ceramics, piezoelectric single crystal material, and silicon.
-
22. An orientation angle detector as claimed in claim 2, wherein said static angle calculator (405) comprises first and second analogue-to-digital convertors (40501, 40502) coupled to said first and second accelerometers (403, 404) for sampling said first and second acceleration detection signals (Ax, Ay) every sampling intervals St to produce first and second time-serial digital acceleration signals, a tentative pitch angle calculator (40504) coupled to said first analogue convertor (40501) for calculating said tentative pitch angle P from said first time-serial digital acceleration signal, and a tentative roll angle calculator (40503) coupled to said second analogue-to-digital convertor (40502) and said tentative pitch calculator (40504) for calculating said tentative roll angle R from said second time-serial acceleration signal and said tentative pitch angle P, said tentative roll angle R and said tentative pitch angle P being factors of said static angle.
-
23. An orientation angle detector as claimed in claim 16, wherein said first and second time-serial digital acceleration signals are represented by Ax(t) and Ay(t), where t=n, n−
- 1, . . . , 1, St=n−
(n−
1), and said tentative pitch angle calculator (40504) and said tentative roll angle calculator (40503) calculate the tentative pitch angle P and the tentative roll angle R according to the following equations, respectively;
- 1, . . . , 1, St=n−
-
24. An orientation angle detector as claimed in claim 23, wherein said static angle calculator (405) comprises third and fourth analogue-to-digital convertors (40505, 40506) coupled to said first and second terrestrial magnetometers (401, 402) for sampling said first and second magnetic detection signals Mx and My every sampling intervals St to produce first and second time-serial digital magnetic detection signals, an co-ordinate transforming calculator (40507) coupled to said third and fourth analogue-to-digital convertors (40505, 40506), said tentative roll angle calculator (40503) and said tentative pitch calculator (40504) for transforming said first and second time-serial digital magnetic detection signals into first and second time-serial digital terrestrial magnetic component signals representing two corresponding terrestrial magnetic components Hx and Hy in the X-Y plane of said reference X-Y-Z co-ordinate, an azimuth calculator (40508) coupled to said co-ordinate transforming calculator (40507) for calculating an azimuth P from said terrestrial magnetic components Hx and Hy to produce a tentative yaw angle Φ
- as said azimuthal deviation angle, and an azimuth memory (40512) coupled to said azimuth calculator (40508) for storing said azimuth ψ
.
- as said azimuthal deviation angle, and an azimuth memory (40512) coupled to said azimuth calculator (40508) for storing said azimuth ψ
-
25. An orientation angle detector as claimed in claim 24, wherein said static angle calculator (405) further comprises a Z-direction magnetic component generator (40509-40515) coupled to said third and fourth analogue-to-digital convertors (40505, 40506) for producing a time-serial digital Z-direction magnetic component signal Mz from said first and second time-serial magnetic detection signals Mx and My, a terrestrial magnetism Ht previously measured, an initial slant angle θ
-
0 of a Xs-Ys plane defined by Xs and Ys axes of said detector co-ordinate from said X-Y plane, and a current slant angle θ
2 of said Xs-Ys plane from said X-Y plane, and said co-ordinate transforming calculator (40507) calculate said terrestrial magnetic components Hx and Hy from said first and second time-serial magnetic detection signals Mx and My, said time-serial digital Z-direction magnetic component signal Mz, and said tentative pitch and roll angles P and R, according to the following equations;
where Hx(n), Hy(n), Mx(n), My(n), Mz(n), P(n) and R(n) represent Hx, Hy, Mx, My, Mz, P and R at t=n, and said azimuth calculator (40508) calculates ψ
(n) of said azimuthψ
at t=n from Hx(n) and Hy(n) according to the following equation;
-
0 of a Xs-Ys plane defined by Xs and Ys axes of said detector co-ordinate from said X-Y plane, and a current slant angle θ
-
26. An orientation angle detector as claimed in claim 25, wherein said Z-direction magnetic component generator (40509-40515) comprises:
-
a terrestrial magnetism generator (40509, 40510) for producing said terrestrial magnetism Ht;
a current Z-direction magnetic component absolute value calculator (40511) coupled to said terrestrial magnetism generator (40509, 40510), said third and fourth analogue-to-digital convertors (40505, 40506) for calculating a Z-direction magnetic component absolute value |Mz(n)| from said terrestrial magnetism Ht, said current value Mx(n) and My(n) of said first and second time-serial magnetic detection signals Mx and My at t=n according to the following equation;
-
-
27. An orientation angle detector as claimed in claim 26, wherein said terrestrial magnetism generator (40509, 40510) comprises:
-
an initial terrestrial magnetism memory (40509) selectively coupled to said third and fourth analogue digital convertors (40505, 40506) through two switching elements (S1, S2), respectively, for memorizing initial X-direction, Y-direction and Z-direction terrestrial magnetic components Mx(0), My(0) and Mz(0) from said third and fourth analogue-to-digital convertors (40505, 40506) through said two switch elements (S1, S2) selectively turned on at initial time; and
a processor (40510) coupled to said initial terrestrial magnetism memory (40509) for processing said initial X-direction, Y-direction and Z-direction terrestrial magnetic components Mx(0), My(0) and Mz(0) into said terrestrial magnetism Ht according to the following equation;
-
-
28. An orientation angle detector as claimed in claim 27, wherein, at initial state:
-
said orientation angle detector is, on one hand, positioned so that a particular one of said first and second terrestrial magnetometers (401, 402) is in parallel with said Z axis of said reference co-ordinate, then a corresponding one of said two switch elements (S1, S2) being turned on to memorize the terrestrial magnetic component detected by said particular terrestrial magnetometer (401 or 402) in said initial terrestrial magnetism memory (40509) as said initial Z-direction terrestrial magnetic component Mz(0); and
said orientation angle detector is, on the other hand, positioned so that said first and second terrestrial magnetometers (401, 402) are in parallel with said X and Y axes of said reference co-ordinate, then said two switch elements (S1, S2) being turned on to memorize the terrestrial magnetic components detected by said first and second magnetometers (401, 402) in said initial terrestrial magnetism memory (40509) as said initial X-direction terrestrial magnetic component Mx(0) and said initial Y-direction terrestrial magnetic component My(0).
-
-
29. An orientation angle detector as claimed in claim 26, wherein said current slant angle calculator (40513) calculates said current slant angle θ
-
2 from an angular value ψ
(n−
1) of said azimuth ψ
at t=n−
1 read out from said azimuth memory (40512), and roll and pitch angle values β
(n−
1) and γ
(n−
1) of said roll and pitch angles β and
γ
red out from said orientation angle memory (70), according to the following equation;
-
2 from an angular value ψ
-
30. An orientation angle detector as claimed in claim 29, wherein said sign definer (40514) defines, on one hand, said sign as positive (+) when said current slant angle θ
-
2 is larger than said initial slant angle θ
0 but is smaller than 90 angular degree, and defines, on the other hand, said sign as negative (−
) when said current slant angle θ
2 is equal to or smaller than said initial slant angle θ
0 but is larger than −
90 angular degree.
-
2 is larger than said initial slant angle θ
-
31. An orientation angle detector as claimed in claim 22, further comprising a third accelerometer (406) disposed in parallel with Zs-axis of said detector co-ordinate to produce a third acceleration detection signal (Az), wherein said static angle calculator (405) further comprises an additional analogue-to-digital convertor (4061) coupled to said third accelerometer (406) for sampling said third acceleration detection signal (Az) every sampling intervals St to produce a third time-serial digital acceleration signal, said third time-serial acceleration signal being applied to said roll angle calculator (40504) for calculating said tentative roll angle R.
-
32. An orientation angle detector as claimed in claim 31, wherein said first, second and third time-serial digital acceleration signals are represented by Ax(t), Ay(t) and Az(t), where t=n, n−
- 1, . . . , 1, St=n−
(n−
1), tentative pitch angle calculator (40504) and said tentative roll angle calculator (40503) calculate the tentative pitch angle P and the tentative roll angle R according to the following equations, respectively;
- 1, . . . , 1, St=n−
-
33. An orientation angle detector as claimed in claim 2, which further comprises a static angle correction definer (50) coupled to said static angle calculator (405) and said orientation angle calculator (60) for defining accuracy of said static angle (P, R, Φ
- ) to produce a correction signal, and wherein said orientation angle calculator (60) is responsive to said correction signal to calculate a correction value and corrects said integrated value set by said correction value to produce a modified value set, said modified value set being delivered as said orientation angle (α
, β
, γ
).
- ) to produce a correction signal, and wherein said orientation angle calculator (60) is responsive to said correction signal to calculate a correction value and corrects said integrated value set by said correction value to produce a modified value set, said modified value set being delivered as said orientation angle (α
-
34. An orientation angle detector as claimed in claim 33, wherein said orientation angle calculator (60) calculates first through third integrated values Σ
- Δ
X, Σ
Δ
Y and Σ
Δ
Z according to the following equations;
- Δ
-
35. An orientation angle detector as claimed in claim 34, wherein said orientation angle calculator (60) calculates, upon receiving said correction signal, first through third angular errors Ex, Ey and Ez by the following equations:
-
36. An orientation angle detector as claimed in claim 35, wherein said C1, C2, and C3 are determined by k1×
- Ex, k2×
Ey, and k3×
Ez, respectively, where k1, k2 and k3 are constant values smaller than 1.
- Ex, k2×
-
37. An orientation angle detector as claimed in claim 35, which further comprises a correction coefficient generator (80) coupled to said static angle calculator (405) and said orientation angle calculator (60) for generating a set of first through third coefficients m1, m2 and m3, said first through third coefficients m1, m2 and m3 being selected from predetermined different values in response to angular values of said tentative pitch angle P and said tentative roll angle R received from said static angle calculator (405), and wherein said orientation angle calculator is responsive to said set of first through third coefficients m1, m2 and m3 to modify said first through third correction values C1, C2 and C3 by multiplying with said first through third coefficients m1, m2 and m3, respectively.
-
38. An orientation angle detector as claimed in claim 33, further comprising a third accelerometer (406) disposed in parallel with Zs-axis of said detector co-ordinate to produce a third acceleration detection signal (Az), wherein said static angle correction definer (50) receives said first, second and third acceleration detection signals (Ax, Ay, Az), and calculates an absolute value of a composite acceleration vector of acceleration of Xs-axis direction, Ys-axis direction, and Zs-axis direction, according to the following formula:
-
39. An orientation angle detector as claimed in claim 34, wherein said correction definer (50) is provided with a static angle memory (501) for storing said tentative pitch angle P and said tentative roll angle R received from said static angle calculator (405), and wherein said correction definer (50) compares the tentative pitch angle P(n) and tentative roll angle R(n) received at a time t=n with the earlier tentative pitch angle P(n−
- 1) and tentative roll angle R(n−
1) at t=n−
1 which are read from said static angle memory (501) to define said tentative pitch angle P(n) and tentative roll angle R(n) to be accurate when the following two equations (1) and (2) are fulfilled, respectively;
- 1) and tentative roll angle R(n−
-
40. An orientation angle detector as claimed in claim 39, wherein said B and Dare small values nearly equal to zero(0).
-
41. An orientation angle detector as claimed in claim 39, wherein said correction definer (50) is connected to said motion angle calculator (310) for receiving said X-moving angle Δ
- X(n) and said Y-moving angle Δ
Y(n), and said B and D are selected to be said X-moving angle Δ
X(n) and said Y-moving angle Δ
Y(n), respectively.
- X(n) and said Y-moving angle Δ
-
42. An orientation angle detector as claimed in claim 39, wherein said correction definer (50) is further received said tentative yaw angle Φ
- (t) and said Z-moving angle Δ
Z(t) from said static angle calculator (405) and motion angle calculator (310), respectively, said tentative yaw angle Φ
(t) being stored in said static angle memory (501), said correction angle definer (50) further processes the tentative yaw angle Φ
(n) and the Z-moving angle Δ
Z(n) at t=n and the yaw angle Φ
(n−
1) at t=n−
1 which is read from said orientation angle memory (501), according to the following equation (3);
- (t) and said Z-moving angle Δ
-
43. An orientation angle detector as claimed in claim 34, wherein said correction definer (50) is provided with a static and motion angle memory (501) for storing said tentative pitch angle P(n) and said tentative roll angle R(n) received from said static angle calculator (405), and said X-moving angle Δ
- X and said Y-moving angle Δ
Y received from said motion angle calculator (310), and wherein said correction definer (50) calculates a moving average of variation of said tentative pitch angle P(t), a moving average of variation of said tentative roll angle R(t), a moving average of said X-moving angle Δ
X(t), and a moving average of said Y-moving angle Δ
Y(t), and defines said tentative pitch angle P(n) and said tentative roll angle R(n) to be accurate when the following two equations (1) and (2) are fulfilled, respectively;
- X and said Y-moving angle Δ
-
44. An orientation angle detector as claimed in claim 43, where said correction definer (50) stores in said static and motion angle memory (501) said tentative yaw angle Φ
- (n) from said static angle calculator (405) and said Z-moving angle Δ
Z from said motion angle calculator (310), wherein said correction definer (50) further calculates a moving average of variation of said tentative yaw angle Φ
(t), a moving average of said Z-moving angle Δ
Z(t), and define said tentative yaw angle Φ
(n) to be accurate when the following equation (3) is fulfilled;
- (n) from said static angle calculator (405) and said Z-moving angle Δ
-
45. An orientation angle detector as claimed in claim 39 or 43, wherein said correction definer (50) produces said correction signal when at least one of said tentative pitch angle P(n) and said tentative roll angle R(n) is defined accurate, and said correction definer (50) produces a non-correction signal when none of said tentative pitch angle and said tentative roll angle R(n) is defined accurate, said orientation angle calculator (60) is responsive to said non-correction signal to produces said integrated value set Σ
- Δ
X(n), Σ
Δ
Y (n) and Σ
Δ
Z(n)) as said orientation angle (γ
, β
, α
).
- Δ
-
46. An orientation angle detector as claimed in claim 42 or 44, wherein said correction definer (50) produces said correction signal when said tentative yaw angle Φ
- (n) is defined accurate but in no relation to whether or not said tentative pitch angle and said tentative roll angle R(n) are defined accurate, and said correction definer (50) produces a non-correction signal when none of said tentative pitch angle, said tentative roll angle R(n) and said yaw angle Φ
(n) is defined accurate, said orientation angle calculator (60) is responsive to said non-correction signal to produces said integrated value set Σ
Δ
X(n), Σ
Δ
Y(n) and Σ
Δ
Z(n)) as said orientation angle (γ
, β
, α
).
- (n) is defined accurate but in no relation to whether or not said tentative pitch angle and said tentative roll angle R(n) are defined accurate, and said correction definer (50) produces a non-correction signal when none of said tentative pitch angle, said tentative roll angle R(n) and said yaw angle Φ
-
47. An orientation angle detector as claimed in any one of claims 39, 42, 43, and 44, wherein said correction definer (50), upon defining accuracy of particular ones of said tentative roll angle R, said tentative pitch angle P and said tentative yaw angle Φ
- produces, as said correction signal, an indication signal representative of that particular ones of said tentative roll angle R, said tentative pitch angle P and said tentative yaw angle Φ
which are defined accurate, and wherein said orientation angle calculator (60) is responsive to said indication signal to modify specific ones of said first through third integrated values Σ
Δ
X(n), Σ
Δ
Y(n) and Σ
Δ
Z(n) corresponding to said particular ones of said tentative roll angle R, said tentative pitch angle P and said tentative yaw angle Φ
by use of said tentative pitch angle P, said tentative roll angle R and said tentative yaw angle Φ
to produce specific modified values, said orientation angle calculator (60) delivers said specific modified values as that specified ones of said roll angle γ
, said pitch angle β and
said yaw angle α
, respectively, which are corresponding to said specific ones of said first through third integrated values Σ
Δ
X(n), Σ
Δ
(n) and Σ
Δ
Z(n), and said orientation angle calculator (60) delivers the remaining ones other than said specific ones of said first through third integrated values Σ
Δ
X(n), Σ
Δ
Y(n) and Σ
Δ
Z(n) as the remaining ones other than said specified ones of said roll angle β
, said pitch angle β and
said yaw angle α
.
- produces, as said correction signal, an indication signal representative of that particular ones of said tentative roll angle R, said tentative pitch angle P and said tentative yaw angle Φ
-
48. An orientation angle detector as claimed in claim 1, wherein said reference co-ordinate is a three-dimensional co-ordinate having a vertical Z axis and two horizontal Y and X axes, and said detector co-ordinate is another three-dimensional co-ordinates having three orthogonal axes Zs, Ys and Xs corresponding to said Z, Y and X axes, respectively, said orientation angle is represented by Z-Y-X Euler'"'"'s angle which is composed of three components of a yaw angle α
- being a rotational angle around Z axis, a pitch angle β
being a rotational angle around Y axis, and a roll angle γ
being a rotational angle around X axis, said orientation angle delivering only said yaw angle α and
said pitch angle β
as a 2-D orientation angle, wherein said plurality of gyroscopes are first and second gyroscopes (302, 303) disposed in parallel with said Ys and Zs axes for measuring first and second angular velocities (Jy, Jz) around said Ys and Zs axes, respectively, said motion angle calculator (310′
) calculating, from said first and second angular velocities (Jy, Jz), a Y-moving angle Δ
Y around the Y axis, and a Z-moving angle Δ
Z around Z axis as two factors of said motion angle, and wherein said two terrestrial magnetometers are first and second ones (401, 402) disposed in parallel with Xs and Ys axes for producing first and second magnetic detection signals (Mx, My), respectively, and said at least one accelerometer is one accelerometer (403) disposed in parallel with Xs axis for producing an acceleration detection signal (Ax), said static angle calculator (405′
) calculating a tentative pitch angle P and a tentative yaw angle Φ
as said azimuthal deviation angle from said first and second magnetic detection signals (Mx, My) and said acceleration detection signal (Ax), said tentative pitch angle P and said tentative yaw angle Φ
being factors of said static angle, and wherein said orientation angle calculator (60′
) calculates an integrated value set of first and second integrated values (Σ
Δ
Y, Σ
Δ
Z) by time integration of said Y-moving angle Δ
Y and Z-moving angle Δ
Z and produces said 2-D orientation angle (α
, β
) from said integrated value set (Σ
Δ
Y, Σ
Δ
Z) and said static angle, said 2-D orientation angle being stored in an orientation angle memory (70).
- being a rotational angle around Z axis, a pitch angle β
-
49. An orientation angle detector as claimed in claim 48, wherein said first and second gyroscopes (302, 303) are Coriolis vibratory gyroscopes each having a piezoelectric vibrator, said first and second gyroscopes (302, 303) produce first and second gyro outputs (Jy, Jz) representing the angular velocities around said Ys and Zs axes.
-
50. An orientation angle detector as claimed in claim 49, which further comprises first and second high-pass filters (305, 306) coupled to said first and second Coriolis vibratory gyroscopes (302, 303) for canceling offset voltages included in the first and second gyro outputs (Jy, Jz) of said first and second Coriolis vibratory gyroscopes to produce first and second filter outputs, respectively.
-
51. An orientation angle detector as claimed in claim 50, wherein said motion angle calculator (310′
- ) receiving said firsthand second filter outputs as first and second incoming signals to produce first and second time-serial moving angle signals (Δ
Y, Δ
Z) representing said motion angle, said orientation angle calculator (60′
) receiving said first and second time-serial moving angle signals (Δ
Y, Δ
Z) to time-serially produce said orientation angle, and said orientation angle memory (70) coupled to said orientation angle calculator (60′
) for storing said orientation angle time-serially delivered from said orientation angle calculator (60′
).
- ) receiving said firsthand second filter outputs as first and second incoming signals to produce first and second time-serial moving angle signals (Δ
-
52. An orientation angle detector as claimed in claim 51, wherein said first and second time-serial moving angle signals represent time-serial Y-moving angle Δ
- Y(t), t=n, (n−
1), (n−
2), . . , 1, n−
(n−
1)=St, and time-serial Δ
Z(t), said motion angle calculator (310′
) calculates, Y-moving angle Δ
Y(n) and Z-moving angle Δ
Z(n) at t=n according to the following equations;
where (n−
1) is the pitch angle β
at t=n−
1 which are read out from said orientation angle memory (70), Jy(n) and Jz(n) being said first and second incoming signals at t=n.
- Y(t), t=n, (n−
-
53. An orientation angle detector as claimed in claim 51, further comprising first and second oscillation removers (321, 322) connecting said first and second high-pass filters (305, 306) with said motion angle calculator (310′
- ), respectively, for removing a noise included in said first and second gyro outputs (Jy, Jz) due to oscillation of said detector itself.
-
54. An orientation angle detector as claimed in claim 53, wherein said first and second oscillation removers (321, 322) process to make first and second time averages of said first and second filter outputs over a predetermined time period, respectively, and to compare said first and second time averages with first and second predetermined threshold values, respectively, said first and second oscillation removers (321, 322) temporarily removing parts of said first and second filter outputs when said first and second time averages are smaller than said first and second threshold values, respectively, to produce first and second processed signals, said first and second processed signals being supplied to said motion angle calculator (310′
- ) as said first and second incoming signals.
-
55. An orientation angle detector as claimed in claim 48, wherein said static angle calculator (405′
- ) comprises a first analogue-to-digital convertor (40501) coupled to said accelerometer (403) for sampling said acceleration detection signals (Ax) every sampling intervals St to produce a time-serial digital acceleration signal, a tentative pitch angle calculator (40504) coupled to said first analogue convertor (40501) for calculating said tentative pitch angle P from said time-serial digital acceleration signal.
-
56. An orientation angle detector as claimed in claim 55, wherein said time-serial digital acceleration signal is represented by Ax(t), where t=n, n−
- 1, . . . , 1, St=n−
(n−
1), said tentative pitch angle calculator (40504) calculates the tentative pitch angle P according to the following equation;
- 1, . . . , 1, St=n−
-
57. An orientation angle detector as claimed in claim 55, wherein said static angle calculator (405′
- ) comprises second and third analogue-to-digital convertors (40505, 40506) coupled to said first and second terrestrial magnetometers (401, 402) for sampling said first and second magnetic detection signals Mx and My every sampling intervals St to produce first and second time-serial digital magnetic detection signals, an azimuth calculator (40508) coupled to said second and third analogue-to-digital convertors (40505, 40506) for calculating from said first and second time-serial digital magnetic detection signals an azimuth ψ
to produce a tentative yaw angle as said azimuthal deviation angle.
- ) comprises second and third analogue-to-digital convertors (40505, 40506) coupled to said first and second terrestrial magnetometers (401, 402) for sampling said first and second magnetic detection signals Mx and My every sampling intervals St to produce first and second time-serial digital magnetic detection signals, an azimuth calculator (40508) coupled to said second and third analogue-to-digital convertors (40505, 40506) for calculating from said first and second time-serial digital magnetic detection signals an azimuth ψ
-
58. An orientation angle detector as claimed in claim 57, which further comprises a static angle correction definer (50′
- ) coupled to said static angle calculator (405′
) and said orientation angle calculator (60′
) for defining accuracy of said static angle (P, Φ
) to produce a correction signal, and wherein said orientation angle calculator (60′
) is responsive to said correction signal to calculate a correction value and corrects said integrated value set by said correction value to produce a modified value set, said modified value set being delivered as said 2-D orientation angle (α
, β
).
- ) coupled to said static angle calculator (405′
-
59. An orientation angle detector as claimed in claim 58, wherein said orientation angle calculator (60′
- ) calculates first and second integrated values Σ
Δ
Y and Σ
Δ
Z according to the following equations;
- ) calculates first and second integrated values Σ
-
60. An orientation angle detector as claimed in claim 59, wherein said orientation angle calculator (60′
- ) calculates, upon receiving said correction signal, first and second angular errors Ey and Ez by the following equations;
- ) calculates, upon receiving said correction signal, first and second angular errors Ey and Ez by the following equations;
-
61. An orientation angle detector as claimed in claim 60, wherein said C1 and C2 are determined by k1×
- Ey and k2×
Ez, respectively, where k1 and k2 are constant values smaller than 1.
- Ey and k2×
-
62. An orientation angle detector as claimed in claim 59, wherein said correction definer (50′
- ) is coupled to said motion angle calculator (310′
), said correction definer (50′
) calculates a moving average of variation of said tentative pitch angle P(t), a moving average of said Y-moving angle Δ
Y(t), a moving average of variation of said tentative yaw angle Φ
(t), and a moving average of said Z-moving angle Δ
Z(t) and defines said tentative pitch angle P(n) and said tentative yaw angle Φ
(n) to be accurate when the following two equations (1) and (2) is fulfilled;
- ) is coupled to said motion angle calculator (310′
-
63. An orientation angle detector as claimed in claim 62, wherein said correction definer (50′
- ) produces said correction signal when at least one of said tentative pitch angle and said tentative yaw angle Φ
(n) is defined accurate, and said correction definer (50′
) produces a non-correction signal when none of said tentative pitch angle P(n) and said tentative yaw angles Φ
(n) is defined accurate, said orientation angle calculator (60′
) is responsive to said non-correction signal to produces said integrated value set (Σ
Δ
Y(n) and Σ
Δ
Z(n)) as said 2-D orientation angle (β
, α
).
- ) produces said correction signal when at least one of said tentative pitch angle and said tentative yaw angle Φ
-
64. An orientation angle detector as claimed in claim 62, wherein said correction definer (50′
- ), upon defining accuracy of a particular one of said tentative pitch angle P and said tentative yaw angle Φ
, produces, as said correction signal, an indication signal representative of that particular one of said tentative pitch angle P and said tentative yaw angle Φ
which is defined accurate, and wherein said orientation angle calculator (60′
) is responsive to said indication signal to modify a specific one of said first and second integrated values Σ
Δ
Y(n) and Σ
Δ
Z(n) corresponding to said particular one of said tentative pitch angle P and said tentative yaw angle Φ
by use of said tentative pitch angle P and said tentative yaw angle Φ
to produce a specific modified value, said orientation angle calculator (60′
) delivers said specific modified value as that specified one of said pitch angle β and
said yaw angle α
, respectively, which is corresponding to said specific one of said first and second integrated values Σ
Δ
Y(n) and Σ
Δ
Z(n), and said orientation angle calculator (60′
) delivers the remaining one other than said specific one of said first and second integrated values Σ
Δ
Y(n) and Σ
Δ
Z(n) as the remaining one other than said specified one of said pitch angle β and
said yaw angle α
.
- ), upon defining accuracy of a particular one of said tentative pitch angle P and said tentative yaw angle Φ
Specification