The present invention relates to a method and apparatus for accurately detecting the attitude of a moving body, and in particular to a method and apparatus for accurately detecting, for example, the attitude of a moving body which can be preferably used for measuring the attitude information such as the attitude rotation angular velocity and attitude angle of a walking robot, in order to control the stable attitude of the walking robot.
As described in Japanese Patent Laid-Open No. 2001-9772, attitude control of a moving body has been made by detecting the attitude angle of a single axis attitude of a robot main body by the use of a single rate gyroscope and a single acceleration sensor and feedback of a corresponding signal.
The attitude sensor output of a rate. gyroscope or an acceleration sensor involves a temperature drift. This prevents accurate attitude control of a moving body. In particular, in case the attitude control of a moving body is made for a prolonged duration, the drift amount is remarkable so that a walking robot could fall to the ground.
A first exemplary method to solve these problems is to correct the output of an attitude sensor by using a temperature sensor.
Although the first solution is effective for a moving body equipped with a system providing stable output of a power supply to drive an attitude sensor, it cannot control the attitude of a moving body with high accuracy in case the output of the attitude sensor also varies. In particular, in the case of a lightweight and compact walking robot including a control computer, a sensor, a driving power supply and the like in a limited space in the main body, a variation in the load caused by arithmetic operation by the control computer triggers a variation in the output of the driving power supply, thereby causing a detection variation and a drift in the attitude angle. Thus, the walking robot could fall to the ground even when the output of an attitude sensor using a temperature sensor is corrected.
Another method to solve the problem of drift, as described in Japanese Patent Laid-Open No. Hei07-0218269, is to detect and correct a drift by using two differential circuits as well as a synchronization detector circuit, smoothing circuit and a phase shifter circuit.
The solution requires a complicated circuit design. The solution compensates for a drift in the sensor output and inputs the resulting output as a control signal to an attitude controller for performing control arithmetic operation and the like. This approach, however, cannot prevent occurrence of a drift as a control signal input to the attitude controller due to a variation in the driving power supply for the attitude controller, that is, a variation in the reference voltage of an analog-to-digital converter circuit.
Another solution of a problem with drift is for example a method for using a combination of a rate gyroscope and an acceleration sensor to configure an attitude angle detection algorithm such as a Kalman filter and detecting an attitude angle while correcting a drift in the output of an attitude sensor.
This approach has a problem that the drift in the output of an acceleration sensor serving to eliminate the drift in the output of a rate gyroscope is left uncorrected. Further, in an attitude detection algorithm such as a Kalman filter, correction of a static (or low-frequency range) attitude angle is made, so that it is impossible to accurately detect a dynamic (or high-frequency range) attitude angle in case a moving body is in dynamic motion. Due to such problems, a walking robot could fall to the ground even when an attitude angle is detected by an attitude angle detection algorithm such as a Kalman filter.
The invention has been accomplished in order to solve the problems. A first object of the invention is to provide a high-accuracy attitude detection method and apparatus for accurately detecting the attitude information such as the rotation angular velocity and acceleration of a moving body.
A second object of the invention is to provide a high-accuracy attitude detection method and apparatus for accurately detecting the attitude information such as the rotation angle and acceleration of a moving body even when a power supply for driving attitude detection section such as a rotation angular velocity and acceleration also serves as a power supply which drives section to perform arithmetic operation of the attitude.
In order to solve the above problems, especially the first problem, the invention provides a method for accurately detecting an attitude of a moving body, the moving body including a high-accuracy attitude detection mechanism having a pair of a first attitude detection section for detecting the attitude of the moving body and a second attitude detection section which provides an output inverted from that of the first attitude detection section, the method comprising the steps of: performing arithmetic processing which mutually compensates for a variation in the output of both of the attitude detection section, and accurately detecting the attitude information of the moving body.
In order to solve the above problems, the invention provides an apparatus for accurately detecting an attitude of a moving body comprising a high-accuracy attitude detection mechanism having a pair of a first attitude detection section for detecting the attitude of the moving body and a second attitude detection section which provides an output inverted from that of the first attitude detection section, the apparatus being capable of accurately detecting the attitude information of the moving body by incorporating into the high-accuracy attitude detection mechanism an arithmetic processing feature which mutually compensates for a variation in the output of both of the attitude detection section.
According to the method and apparatus of the invention, a variation and a drift in the output of attitude detection section can be mutually compensated for by the arithmetic processing in a pair of attitude detection section forming a high-accuracy attitude detection mechanism. Even in case a temperature drift or the variation of the driving power supply for the attitude detection section is present, it is possible to configure a method and apparatus for accurately detecting the attitude of a moving body.
A specific aspect of the high-accuracy attitude detection apparatus of the invention is characterized in that the attitude detection section are rate gyroscopes, that a pair of rate gyroscopes forming a high-accuracy attitude detection mechanism is mounted on the moving body in opposite directions to each other, and that the attitude information, or to be more specific, the rotation angular velocity, attitude angle or rotation angular acceleration of the moving body is detected by a differential signal obtained from the output signals of the rate gyroscopes.
Another specific aspect of the invention is characterized in that the attitude detection section are acceleration sensors, that a pair of acceleration sensors forming a high-accuracy attitude detection mechanism is mounted on the moving body in opposite directions to each other, and that the attitude information, or to be more specific, the acceleration, translation speed, translation travel amount or attitude angle of the moving body is detected by a differential signal obtained from the output signals of the acceleration sensors.
Another specific aspect of the high-accuracy attitude detection apparatus of the invention is characterized in that the attitude detection section are inclination sensors, that a pair of inclination sensors forming a high-accuracy attitude detection mechanism is mounted on the moving body in opposite directions to each other, and that the attitude information, or to be more specific, the inclination angle, rotation angular velocity, or rotation angular acceleration of the moving body is detected by a differential signal obtained from the output signals of the inclination sensors.
Another specific aspect of the high-accuracy attitude detection apparatus of the invention is characterized in that the attitude detection section is formed by a pair of rate gyroscopes and a pair of acceleration sensors, that each of the pair of rate gyroscopes and acceleration sensors forming a high-accuracy attitude detection mechanism is mounted on the moving body in opposite directions to each other, and that the attitude information, or to be more specific, the inclination angle, rotation angular velocity, or rotation angular acceleration of the moving body is detected by of a differential signal obtained from the differential signal of output signals of the rate gyroscopes and that of output signals of the acceleration sensors.
Another specific aspect of the high-accuracy attitude detection apparatus of the invention is characterized in that the attitude detection section is formed by a pair of rate gyroscopes and a pair of inclination sensors, that each of the pair of rate gyroscopes and the pair of inclination sensors forming a high-accuracy attitude detection mechanism is mounted on the moving body in opposite directions to each other, and that the attitude information, or to be more specific, the inclination angle, rotation angular velocity, or rotation angular acceleration of the moving body is detected by a differential signal obtained from the differential signal of output signals of the rate gyroscopes and that of output signals of the inclination sensors.
In order to solve the above problems, especially the second problem, the invention provides a method for accurately detecting an attitude of a moving body, characterized in that the moving body includes a control arithmetic processing mechanism for detecting the attitude of the moving body by using the output of the attitude detection section as an input signal, and that the attitude detection section and the control arithmetic processing mechanism are driven by a common driving power supply. The invention also provides an apparatus for accurately detecting an attitude of the moving body comprising: at least a pair of moving body attitude detection section, and a control arithmetic processing mechanism for detecting the attitude of the moving body by using the output of the attitude detection section as an input signal, characterized in that the attitude detection section and the control arithmetic processing mechanism are connected to a common driving power supply.
According to the high-accuracy attitude detection method and apparatus, it is possible to mutually compensate for, using a control arithmetic processing mechanism, a variation or a drift input to the control arithmetic processing mechanism as well as a variation or drift in the output of attitude detection section. Even in the presence of a temperature drift, the variation of a driving power to feed power to the attitude detection section, or a variation or a drift in the input to the control arithmetic processing mechanism.
According to the invention, a high-accuracy attitude detection apparatus for accurately detecting an attitude of a moving body is provided, characterized in that the moving body is a walking robot.
According to the invention, it is possible to accurately detect the attitude of a walking robot, so that it is possible to control the stable attitude of a walking robot.
In order to describe the invention in more detail, the invention is described referring to the attached drawings.
In this case, the output from each of the rate gyroscope 2a, 2b is respectively
ω1=+ω+ωoffset
ω2=−ω+ωoffset
where ω is the actual rotation angular velocity of the upper body 3 and ωoffset is a temperature drift or an output variation caused by the variation of the driving power supply to feed power to the rate gyroscopes 2a, 2b.
A high-accuracy attitude detection mechanism 1A in
In this case, the output: Vout obtained through the arithmetic processing by the high-accuracy attitude detection mechanism 1A is
Vout=ω
and it is possible to correct the temperature drift of the rate gyroscopes 2a, 2b and the output variation (ωoffset) caused by the variation of the driving power supply 10 to feed power to the rate gyroscopes 2a, 2b and accurately obtain the rotation angular velocity of the upper body 3 of the walking robot.
In this case, the output: Vout obtained through the arithmetic processing by the high-accuracy attitude detection mechanism 1B is
Vout=ƒ ω dt
and it is possible to correct the temperature drift of the rate gyroscopes 2a, 2b and the output variation (ωoffset) caused by the variation of the driving power supply 10 to feed power to the rate gyroscopes 2a, 2b and accurately obtain the attitude angle of the upper body 3 of the walking robot.
In case arithmetic processing of differentiation is made in a differentiating element 8C instead of the integrating element in
Even in case the attitude detection section in
Even in case the attitude detection section in
The embodiment in
The configuration of the walking robot in this embodiment is the same as that in
For the above embodiment, the following outputs are obtained from the acceleration sensors 12a, 12b:
α1=+α+αoffset
α2=−α+αoffset
where α is the actual translation acceleration of the upper body 3 and α offset is an output variation caused by a variation in the driving power supply to feed power to the acceleration sensors 12a, 12b.
A high-accuracy attitude detection mechanism 11A in
In this case, the output: Vout obtained through the arithmetic processing by the high-accuracy attitude detection mechanism 11A is
Vout=α
and it is possible to correct the temperature drift of the acceleration sensors 12a, 12 and the output variation caused by the variation of the driving power supply 10 to feed power to the acceleration sensors 12a, 12b and accurately obtain the acceleration of the upper body 3 of walking robot.
In this case, the output: Vout obtained through the arithmetic processing by the high-accuracy attitude detection mechanism 11B is
Vout=ƒƒα dt dt
and it is possible to correct the temperature drift of the acceleration sensors 12a, 12b and the output variation caused by the variation of the driving power supply 10 to feed power to the acceleration sensors 12a, 12b and accurately obtain the translation amount of the upper body 3 of the walking robot.
In case the integrating element 15 in
The embodiment in
On the high-accuracy attitude detection mechanism 21a is provided the pair of acceleration sensors 22a, 22b with the acceleration sensors arranged in opposite directions to each other. On the high-accuracy attitude detection mechanism 2b is provided the pair of acceleration sensors 22c, 22d with the acceleration sensors arranged in opposite directions to each other. Both of the high-accuracy attitude detection mechanisms 21a, 21b are arranged orthogonally to each other to form a high-accuracy attitude detection mechanism 21.
To be more specific, the high-accuracy attitude detection mechanism 21 comprises a single driving power supply 10 which drives the high-accuracy attitude detection mechanisms 21a, 21b including the acceleration sensors 22a through 22d. The high-accuracy attitude detection mechanism 21 adds (subtracts) the output signals of the acceleration sensors 22a, 22b respectively in adding elements 23a, 23b and compensates for the variations in the outputs or drifts then performs arithmetic processing of multiplying the result by ½ in multiplying elements 24a, 24b, thereby accurately detecting the acceleration signals Vout-1 and Vout-2 of the upper body 3 of the walking robot in different orientations.
In this case, the acceleration signal Vout-1 obtained from the difference between the output signals of the acceleration sensors 22a and 22b and the acceleration signal Vout-2 obtained from the difference between the output signals of the acceleration sensors 22c and 22d are respectively
Vout-1=αz
Vout-2=αz
and, from the difference, it is possible to correct the temperature drift of the acceleration sensors 22a, 22b, 22c, 22d and the output variation of the driving power supply to feed power to the acceleration sensors, thereby accurately obtaining the acceleration αz, αx of the upper body of the walking robot.
The high-accuracy attitude detection mechanisms 2a, 2b are arranged orthogonally to each other so that the square root of the sum of squares of outputs of the high-accuracy attitude detection mechanisms statically matches the gravitational acceleration. In the two dimension, the arithmetic processing shown in
Attitude angle (Vout)=tan−1 (Vout-2/Vout-1)
by arithmetic elements 25 and 26 is used to accurately obtain the static attitude angle signal of the upper body 3 of the walking robot. It goes without saying that the attitude angle signal has undergone correction of the temperature drift of the acceleration sensors 22a, 22b, 22c, 22d and the output variation caused by the variation of the driving power supply 10 to feed power to the acceleration sensors.
The embodiment in
On the high-accuracy attitude detection mechanism 31b is provided the pair of acceleration sensors 33a, 33b with the acceleration sensors arranged in opposite directions to each other. On the high-accuracy attitude detection mechanism 31c is provided the pair of acceleration sensors 33c, 33d with the acceleration sensors arranged in opposite directions to each other. Both of the high-accuracy attitude detection mechanisms 31b, 31c are arranged orthogonally to each other. On the high-accuracy attitude detection mechanism 31a is provided a pair of rate gyroscopes 32a, 32b with the acceleration sensors arranged in opposite directions to each other. The high-accuracy attitude detection mechanism 31a, 31b, 31c form the high-accuracy attitude detection mechanism 31.
Even in case an existing Kalman filter is configured aster a high-accuracy attitude sensor output is calculated in the above high-accuracy attitude detection mechanisms 31a, 31b, 31c, it is possible to form apparatus which accurately detects the attitude angle of the upper body of the walking robot.
Further, by adding a differentiating element to the processing in the high-accuracy attitude detection mechanism 31 in
Inclination sensors can be used instead of the acceleration sensors 33a through 33d in
The embodiment in
In this case, from the rate gyroscopes 42a, 42b are respectively obtained the outputs
ω1=+ω+ωoffset
ω2=−ω+ωoffset
where ω is the actual rotation angular velocity of the upper body 3 and ωoffset is an output variation caused by a temperature drift or a variation in the driving power supply 40 to feed power to the rate gyroscopes 22a, 22b and a control arithmetic processing mechanism 43.
To be more specific, the output signals of the rate gyroscopes 42a, 42b pass through D/A converters 44a, 44b and are added (subtracted) in an adding element 45, and then multiplied by 1/(2×DAGain) in an arithmetic element 46 in order to accurately detect the rotation angular velocity of the upper body 3 of the walking robot.
Signals obtained when the outputs of the rate gyroscopes 42a, 42b are input to the control arithmetic processing mechanism 43 are respectively
DAω1=DAGain×(+ω+ωoffset)+DAoffset
DAω2=DAGain×(−ω+ωoffset)+DAoffset
where DAGain is an input gain at the control arithmetic processing mechanism 43, DAω offset is an output variation which takes place at the input to the control arithmetic processing mechanism 43 due to the reference voltage variation of a DA converter caused by the variation of the common driving power supply 40.
The output: DAout obtained by the internal arithmetic operation processing by the control arithmetic processing mechanism 43 is
DAout32 ω
and it is possible to correct the temperature drift of the rate gyroscopes 42a, 42b, the variation of the driving power supply to feed power to the rate gyroscopes 42a, 42b and a variation/drift which takes place at the input of the output signals to the control arithmetic processing mechanism 43, and accurately obtain the rotation angular velocity of the upper body 3 of the walking robot.
In the walking robot in
Further, the internal arithmetic processing in the control arithmetic processing mechanism 43 may include a differentiation feature as shown in
Even in case the attitude detection section in
In case the attitude detection section in
The embodiment in
To be more specific, the high-accuracy attitude detection mechanism 51 comprises the pair of acceleration sensors 52a, 52b and the pair of acceleration sensors 52c, 52d in the internal of the upper body 3 of the walking robot. It also comprises a control arithmetic processing mechanism 53 and a common driving power supply 50 which drives the acceleration sensors and the control arithmetic processing mechanism 53. The pair of acceleration sensors 52a, 52b is provided with the acceleration sensors arranged in opposite directions to each other. The pair of acceleration sensors 52c, 52d is provided with the acceleration sensors arranged in opposite directions to each other. The high-accuracy attitude detection mechanisms 51a, 51b composed of theses components are provide orthogonally to each other.
To be more specific, in the control arithmetic processing mechanism 53, the output signals of the acceleration sensors 52a, 52b respectively pass through D/A converters 54a, 54b and are added (subtracted) in an adding element 55a. The output signals of the acceleration sensors 52c, 52d respectively pass through D/A converters 54c, 54d and are added (subtracted) in an adding element 55b. In arithmetic element 56a, 56b, the resulting values are multiplied by 1/(2×DAGain) to obtain the high-accuracy acceleration signals DAout-1 and DAout-2.
In this case, the high-accuracy acceleration signal DAout-1 obtained by inputting the outputs of the acceleration sensors 52a, 52b to the control arithmetic processing mechanism 53 and obtaining the difference is
DAout-1=αz
and the high-accuracy acceleration signal DAout-2 obtained by inputting the outputs of the acceleration sensors 52c, 52d to the control arithmetic processing mechanism 53 and obtaining the difference is
VAout-2=αx.
It is thus possible to correct the temperature drift of the acceleration sensors 52a through 52d, the variation of a driving power supply to feed power to the acceleration sensors, and the variation/drift in the control arithmetic processing mechanism 53 which is caused by the variation of the driving power supply 50 common to the acceleration sensors 52a through 52d and the control arithmetic processing mechanism 53, thereby obtaining the acceleration αz, αx of the upper body 3 of the walking robot.
The pair of acceleration sensors 52a, 52b and the pair of acceleration sensors 52c, 52d is arranged orthogonally to each other, so that the square root of the sum of squares of outputs of the high-accuracy acceleration sensor signals DAout-1 and the high-accuracy acceleration sensor signals DAout-2 operated in the control arithmetic processing mechanism 53 statically matches the gravitational acceleration. In the two dimension, the arithmetic processing shown in
Attitude angle (DAout)=tan−1 (DAout-2/DAout-1)
by arithmetic elements 57 and 58 is used correct the temperature drift of the acceleration sensors 52a through 52d, the sensor output variation caused by the variation of a common driving power supply 50 to feed power to the acceleration sensors, and the variation/drift in the control arithmetic processing mechanism 53 which is caused by the variation of the common driving power supply 50, thereby obtaining the static attitude angle of the upper body 3 of the walking robot.
In the case of the third dimension also, it is possible to accurately obtain the static attitude angle of the walking robot by an existing calculation method.
As mentioned in detail hereinabove, according to the invention, even in the presence of a temperature drift in attitude detection section or an output variation caused of the variation of a driving power supply to feed power to the attitude detection section, the output variations in first moving body attitude detection section and second attitude detection section which obtains an output inverted from that of the first attitude detection section are mutually compensated for, which assures high-accuracy detection of the attitude information of the moving body.
Even in case a temperature drift, variation of a driving power to feed the attitude detection section, or variation/drift caused when signal is input to the control arithmetic mechanism is present on the attitude detection section, the first moving body attitude detection section and the second attitude detection section which obtains an output inverted from that of the first attitude detection section are provided and the attitude detection section and the control arithmetic processing mechanism are driven by a common driving power supply, which assures high-accuracy detection of the attitude of the moving body by the control arithmetic processing mechanism.
As mentioned hereinabove, a method and apparatus for accurately detecting the attitude of a moving body according to the invention is suited for use in an application where the attitude information such as the attitude rotation angular velocity or attitude angle of a walking robot is accurately measured in order to assure stable control of the walking robot.
Number | Date | Country | Kind |
---|---|---|---|
2002-214401 | Jul 2002 | JP | national |
Filing Document | Filing Date | Country | Kind | 371c Date |
---|---|---|---|---|
PCT/JP03/09201 | 7/18/2003 | WO | 2/8/2006 |