JP2013103319A - Device for calculating posture angle, and method for calculating posture angle, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To calculate a posture angle at low costs and with a high degree of accuracy.SOLUTION: A device for calculating a posture angel includes a gyro sensor for detecting an angle speed, and an acceleration sensor for detecting a rate of acceleration, and is configured to calculate a posture angel based on the angle speed detected by the gyro sensor and the rate of acceleration detected by the acceleration sensor. At least one of the gyro sensor and the acceleration sensor includes a plurality of sensors having different measurement ranges for measurement at the same location and in the same direction. The posture angle calculation device is provided with a posture angle calculation means for calculating a posture angel based on each output from the plurality of sensors having different measurement ranges.

Description

  The present invention relates to a posture angle calculation device, a posture angle calculation method, and a program that can calculate a posture angle of a robot or the like with high accuracy.

  For example, when a humanoid robot or the like is flexibly controlled to walk on an irregular road surface, it is required to calculate the posture angle of the robot with higher accuracy in order to stabilize the walking. On the other hand, a three-dimensional navigation device that calculates an attitude angle with high accuracy using an optical fiber gyro is known (see Patent Document 1).

Japanese Unexamined Patent Publication No. 7-072926

  However, when the optical fiber gyro according to Patent Document 1 is mounted on a humanoid robot or the like, its posture angle can be calculated with high accuracy, but on the other hand, its weight is large, which may lead to an increase in cost.

  The present invention has been made to solve such problems, and it is a main object of the present invention to provide an attitude angle calculation device, an attitude angle calculation method, and a program that can calculate an attitude angle with low cost and high accuracy. And

One aspect of the present invention for achieving the above object includes a gyro sensor that detects angular velocity and an acceleration sensor that detects acceleration, the angular velocity detected by the gyro sensor, the acceleration detected by the acceleration sensor, A posture angle calculation device for calculating a posture angle based on the at least one of the gyro sensor and the acceleration sensor, wherein at least one of the gyro sensor and the acceleration sensor is configured by a plurality of sensors having different measurement areas and measuring the same location and the same direction. A posture angle calculation device comprising posture angle calculation means for calculating a posture angle based on signals output from a plurality of sensors having different areas.
In this aspect, the apparatus further includes a combining unit that combines signals output from a plurality of sensors having different measurement areas, and the posture angle calculation unit is configured to output the posture angle based on the signal combined by the combining unit. May be calculated.
In this aspect, a plurality of posture angle calculation means that respectively use signals output from a plurality of sensors having different measurement areas may be provided.
In this aspect, the apparatus may further include a plurality of conversion means for converting signals output from the plurality of sensors from analog signals to digital signals, respectively.
In this aspect, the synthesizing unit may synthesize signals output from a plurality of sensors having different measurement areas so as to be continuous in the vicinity of the switching threshold of the measurement area.
In this embodiment, a plurality of calculation means for multiplying a plurality of posture angles respectively calculated by the plurality of posture angle calculation means by a predetermined coefficient based on a probability model or a composite ratio function model, and a calculation means respectively. Adding means for adding a plurality of calculated values;
May be further provided.
In this aspect, the different measurement areas may be a low speed area, a medium speed area, or a high speed area.
In this aspect, the plurality of gyro sensors include a first gyro sensor that detects an angular velocity in a low speed range, and a second gyro sensor that detects an angular velocity in a high speed range, and the plurality of acceleration sensors include a low speed range. The 1st acceleration sensor which detects the acceleration of this, and the 2nd acceleration sensor which detects the acceleration of a high-speed area may be included.
In this aspect, the plurality of conversion means include a first conversion means for converting an angular velocity in a low speed range detected by the first gyro sensor from an analog signal to a digital signal, and a high speed detected by the second gyro sensor. Second conversion means for converting the angular velocity of the region from an analog signal to a digital signal, third conversion means for converting the acceleration in the low speed range detected by the first acceleration sensor from an analog signal to a digital signal, and the second acceleration And fourth conversion means for converting the acceleration in the high speed range detected by the sensor from an analog signal to a digital signal.
In this aspect, the plurality of combining means combines the angular velocity signal converted by the first converting means and the angular velocity signal converted by the second converting means so as to be continuous in the vicinity of the switching threshold of the measurement area. First synthesizing means, the second synthesizing means for synthesizing the acceleration signal converted by the third converting means and the acceleration signal converted by the fourth converting means so as to be continuous in the vicinity of the switching threshold of the measurement area. And may be included.
In this aspect, the plurality of posture angle calculation means calculates a posture angle based on the angular velocity signal synthesized by the first synthesis means and the acceleration signal synthesized by the second synthesis means. Posture angle calculation means; and second attitude angle calculation means for calculating a posture angle based on the angular velocity signal synthesized by the first synthesis means and the acceleration signal synthesized by the second synthesis means. The plurality of calculation means include a first calculation means for multiplying the attitude angle calculated by the first attitude angle calculation means by a first predetermined coefficient based on a probability model or a composite ratio function model, and the second attitude Second calculating means for multiplying the attitude angle calculated by the angle calculating means by a second predetermined coefficient based on the probability model or the composite ratio function model, and the adding means is based on the first calculating means. Calculated calculation result with a calculation result calculated by the second calculation means may calculate the final attitude angle by adding.
In this aspect, the present invention may be a bipedal walking robot comprising a control device that controls walking based on the posture angle calculated by the posture angle calculation device.
On the other hand, one embodiment of the present invention for achieving the above object includes a gyro sensor that detects angular velocity and an acceleration sensor that detects acceleration, and the angular velocity detected by the gyro sensor and the acceleration detected by the acceleration sensor. And an attitude angle calculation method for calculating an attitude angle based on, wherein at least one of the gyro sensor and the acceleration sensor is configured by a plurality of sensors having different measurement areas that measure the same location and the same direction, The posture angle calculation method may include a step of calculating a posture angle based on signals output from a plurality of sensors having different measurement areas.
In this aspect, the method may further include a step of combining signals output from a plurality of sensors having different measurement areas, and calculating the posture angle based on the combined signal.
In this aspect, the method may further include a step of converting signals output from the plurality of sensors from analog signals to digital signals.
In this embodiment, signals output from a plurality of sensors having different measurement areas may be combined so as to be continuous in the vicinity of the switching threshold of the measurement area.
In this aspect, the step of multiplying the calculated plurality of posture angles by a predetermined coefficient based on a probability model or a combined ratio function model, respectively, and the step of adding the calculated plurality of calculated values are further performed. It may be included.
Another embodiment of the present invention for achieving the above object includes a gyro sensor that detects an angular velocity and an acceleration sensor that detects acceleration, and the angular velocity detected by the gyro sensor and the acceleration detected by the acceleration sensor. And at least one of the gyro sensor and the acceleration sensor is composed of a plurality of sensors having different measurement areas and measuring the same location and the same direction, the measurement area A program that causes a computer to execute a process of calculating a posture angle based on signals output from a plurality of different sensors.

  According to the present invention, it is possible to provide a posture angle calculation device, a posture angle calculation method, and a program that can calculate a posture angle with low cost and high accuracy.

1 is a block diagram showing a schematic system configuration of an attitude angle calculation device according to an embodiment of the present invention. It is a figure which shows an example of the relationship between the sensor output after a synthetic | combination process, and the behavior (attitude angle) of a robot. It is a figure which shows an example of the relationship between a synthetic | combination ratio and the behavior (attitude angle) of a robot. It is a block diagram which shows the schematic system configuration | structure of the 1st and 2nd attitude | position angle calculating part which concerns on one embodiment of this invention. It is a flowchart which shows an example of the processing flow by the attitude | position angle calculating apparatus which concerns on one embodiment of this invention. It is a block diagram which shows the schematic system configuration | structure of the attitude | position angle calculating apparatus which concerns on one modification of this invention. It is a block diagram which shows the schematic system configuration | structure of the attitude | position angle calculating apparatus which concerns on one modification of this invention.

  Embodiments of the present invention will be described below with reference to the drawings. The posture angle calculation apparatus 1 according to an embodiment of the present invention is mounted on, for example, a robot such as a biped robot or a humanoid robot, and can calculate the posture angle of the robot with high accuracy.

  FIG. 1 is a block diagram showing a schematic system configuration of an attitude angle calculation device according to an embodiment of the present invention. The posture angle calculation device 1 according to the present embodiment includes a first gyro sensor 2, a second gyro sensor 3, a first AD converter 4, a second AD converter 5, a first acceleration sensor 6, and a second gyro sensor 3. Acceleration sensor 7, third AD converter 8, fourth AD converter 9, first synthesis processor 10, second synthesis processor 11, first attitude angle calculator 12, and second attitude angle calculator 13, a first probability model calculation unit 14, a second probability model calculation unit 15, and an addition processing unit 16.

  The posture angle calculation device 1 includes, for example, a CPU (Central Processing Unit) that performs calculation processing, a ROM (Read Only Memory) that stores calculation programs executed by the CPU, calculation processing programs, and the like, and processing data A hardware configuration is mainly made of a microcomputer having a RAM (Random Access Memory) that temporarily stores and the like. The CPU, ROM, and RAM are connected to each other by a data bus or the like.

  The first gyro sensor 2 is, for example, a low speed MEMS (Micro Electro Mechanical Systems) gyro sensor capable of measuring only the angular velocity in the low speed range with high accuracy, and outputs the measured angular velocity to the first AD converter 4. .

  The first AD (Analog Digital) conversion unit 4 is a specific example of the first conversion unit, converts the analog angular velocity data output from the first gyro sensor 2 into digital data, and converts the converted digital angular velocity data into the first data. 1 is output to the synthesis processing unit 10.

  The second gyro sensor 3 is, for example, a MEMS gyro sensor for a high speed region that measures an angular velocity from a low speed region to a high speed region with low accuracy, and outputs the measured angular velocity to the second AD converter 5. In addition, the 1st gyro sensor 2 and the 2nd gyro sensor 3 measure the same location, the same direction, and one axis component (x-axis component, y-axis component, or z-axis component) with these two sensors. Thereby, as will be described later, even if a low-cost and lightweight MEMS gyro is used, for example, high accuracy close to that of a high-cost optical fiber gyro can be realized.

  The second AD conversion unit 5 is a specific example of the second conversion unit, converts analog angular velocity data output from the second gyro sensor 3 into digital data, and converts the converted digital angular velocity data into a first synthesis processing unit. 10 is output. As described above, the AD conversion processing is individually performed by the first and second AD conversion units 4 and 5, and the later-described synthesis processing is performed after the AD conversion processing, so that the quantization error can be reduced and the high-resolution AD conversion is performed. It does not require a vessel and leads to cost reduction.

  The first acceleration sensor 6 is, for example, a low speed MEMS acceleration sensor that can measure only low speed acceleration with high accuracy, and outputs the measured acceleration to the third AD converter 8.

  The third AD conversion unit 8 is a specific example of the third conversion means, converts analog acceleration data output from the first acceleration sensor 6 into digital data, and converts the converted digital acceleration data into a second synthesis processing unit. 11 is output.

  The second acceleration sensor 7 is, for example, a MEMS acceleration sensor for a high speed range that measures acceleration from a low speed range to a high speed range with low accuracy, and outputs the measured acceleration to the fourth AD conversion unit 9.

  The fourth AD conversion unit 9 is a specific example of the fourth conversion unit, converts analog acceleration data output from the second acceleration sensor 7 into digital data, and converts the converted digital acceleration data into a second synthesis processing unit. 11 is output.

  In this way, the output values of the first and second acceleration sensors 6 and 7 are individually subjected to AD conversion processing by the third and fourth AD conversion units 8 and 9, and after the AD conversion processing, a synthesis process described later is performed. The quantization error can be reduced, and further, a high resolution AD converter is not required, leading to cost reduction.

  Here, a synthesis process of the low-speed angular velocity data output from the first gyro sensor 2 and the high-speed angular velocity data output from the second gyro sensor 3 will be considered. For example, when a certain threshold value is set and the angular velocity from the gyro sensor is equal to or higher than the set threshold value, the angular velocity data for the high speed range output from the second gyro sensor 3 is used. A synthesis process using the angular velocity data for the low speed range output from 2 can be considered.

  However, the low-speed angular velocity data output from the first gyro sensor 2 and the high-speed angular velocity data output from the second gyro sensor 3 each include some non-linearity. For this reason, when the inclination and zero correction and the temperature correction that are generally performed are performed, the error remains, and when the synthesis process is performed, the error becomes discontinuous near the threshold value. This discontinuity adversely affects the processing described later, and causes an error in the finally calculated posture angle.

  Therefore, the first synthesis processing unit 10 uses the following equation (1) based on the synthesis ratio based on the digital angular velocity data from the first AD conversion unit 4 and the digital angular velocity data from the second AD conversion unit 5. Then, the angular velocity composition processing is performed (FIG. 2). As a result, even when the output values of the gyro sensor and acceleration sensor with different measurement areas are combined, they can be continued in the vicinity of the switching threshold of the measurement area, and the above error is suppressed. It can be calculated with high accuracy.

In the following equation (1), ω _s is an angular velocity limit value in the low speed region of the first gyro sensor 2 (or an acceleration limit value in the low acceleration region of the first acceleration sensor 6), and ω _p is ω _s The switching start angle having a smaller absolute value, ω _{in_low} is angular velocity data output from the first AD converter (or acceleration data output from the third AD converter 8), and ω _{in_high} is the second AD converter. 5 is the angular velocity data (acceleration data output from the fourth AD conversion unit 9) output from 5, and ω _out is the angular velocity after synthesis processing (or second synthesis processing unit 12) output from the first synthesis processing unit 10. (The acceleration after the synthesis process output from).
(A) When ω _{in_low} ≦ ω _p ω _out = ω _{in_low}
(B) When ω _p ≦ ω _{in_low} ≦ ω _s (1) Expression ω _out = (ω _{in_low} −ω _p ) / (ω _s −ω _p ) × ω _{in_low} + (1− (ω _{in_low} −ω _p ) / (Ω _s -ω _p )) × ω _{in_high}
(C) When ω _s ≦ ω _{in_low} ω _out = ω _{in_high}

Further, for example, when a linear combination ratio function r as shown in FIG. 3 is set in advance, the first combination processing unit 10 receives the digital angular velocity data from the first AD conversion unit 4 and the second AD conversion unit 5. Based on the digital angular velocity data from, angular velocity synthesis processing may be performed using the following equation (2). r ₁ and r ₂ indicate the synthesis ratio, and are set so that r ₁ + r ₂ = 1.
ω _out = ω _{in_low} × r ₁ + ω _{in_high} × r ₂ (2)

The synthesis ratio function is an example, and is not limited to this. For example, a synthesis ratio function of a quadratic expression or a cubic expression may be used, and any function can be used as long as r ₁ + r ₂ = 1 Functions can be applied.
Further, for example, the angular velocity composition processing may be performed using the following equation (2-1) using a low-pass filter (LPF) and a high-pass filter (HPF).
ω _out = ω _{in_low} × LPF + ω _{in_high} × HPF (2-1) Here, LPF = ω _c / (s + ω _c ) and HPF = s / (s + ω _c ) can be considered as an example. An order filter may be used, and an LPF and an HPF that are 1 over the entire frequency band may be used.

  The first synthesis processing unit 10 is a specific example of the first synthesis unit, and outputs the angular velocity obtained by the synthesis process to the first attitude angle calculation unit 12 and the second attitude angle calculation unit 13.

  The second synthesis processing unit 11 is a specific example of the second synthesis unit, and includes digital acceleration data output from the third AD conversion unit 8 and digital acceleration data output from the fourth AD conversion unit 9. Based on the above formula (1), formula (2) or formula (2-1), the same synthesis processing as the first synthesis processing unit 10 is performed. The second synthesis processing unit 11 outputs the synthesized acceleration to the first attitude angle calculation unit 12 and the second attitude angle calculation unit 13.

  Here, when a plurality of sensors (first and second gyro sensors 2, 3, first and second acceleration sensors 6, 7) are switched and used as in the present embodiment, as described above, It is more accurate and preferable to calculate a posture angle by setting a parameter corresponding to the sensor. Therefore, the posture angles are calculated using the first and second posture angle calculation units 12 and 13 having parameters suitable for each sensor as follows.

  The first posture angle calculation unit 12 is a specific example of the first posture angle calculation unit, and angular velocity data output from the first synthesis processing unit 10, acceleration data output from the second synthesis processing unit 11, Based on the above, a temporary posture angle is calculated. The first attitude angle calculation unit 12 includes a first integration processing unit 121, a first calculation processing unit 122, a first high-pass filter unit 123, a first low-pass filter unit 124, and a first addition unit 125. (FIG. 4).

  The first integration processing unit 121 performs integration processing on the angular velocity data output from the first synthesis processing unit 10, calculates a posture angle, and outputs the posture angle to the first high-pass filter unit 123.

  The first arithmetic processing unit 122 calculates an attitude angle from the acceleration data output from the second synthesis processing unit 11 and outputs the attitude angle to the first low-pass filter unit 124. Note that the posture angle calculated by the first integration processing unit 121 includes a cumulative error due to the integration processing, and the posture angle based on the acceleration of the first and second acceleration sensors 6 and 7 is a delay of the response speed. Will be included. Therefore, as described later, the above-mentioned error is corrected by adding the posture angle based on the acceleration of the first and second acceleration sensors 6 and 7 to the posture angle calculated by the first integration processing unit 121. The above delay is eliminated.

  The first high-pass filter unit 123 extracts a posture angle in a high frequency band equal to or higher than a predetermined frequency from the posture angles calculated by the first integration processing unit 121 and outputs the posture angle to the first addition unit 125. As described above, with respect to the attitude angle based on the angular velocities from the first and second gyro sensors 2 and 3, the high frequency band is regarded as important, and data in the high frequency band is extracted. The first high-pass filter unit 123 and the first low-pass filter unit 124 are set to be 1 when the respective outputs are added.

  On the other hand, the first low-pass filter unit 124 extracts a posture angle in a low frequency band equal to or lower than a predetermined frequency from the posture angles calculated by the first arithmetic processing unit 122 and outputs the posture angle to the first adding unit 125. . As described above, regarding the posture angle based on the acceleration from the first and second acceleration sensors 6 and 7, the low frequency band is regarded as important, and data in the low frequency band is extracted.

  The first adding unit 125 adds the posture angle output from the first high-pass filter unit 123 and the posture angle output from the first low-pass filter unit 124 to calculate a temporary posture angle, The result is output to the one probability model calculation unit 14.

  The second attitude angle calculation unit 13 is a specific example of the second attitude angle calculation means, has substantially the same configuration as the first attitude angle calculation unit 12, and performs substantially the same processing. The second posture angle calculation unit 13 calculates a temporary posture angle based on the angular velocity data output from the first synthesis processing unit 10 and the acceleration data output from the second synthesis processing unit 11. The second attitude angle calculation unit 13 includes a second integration processing unit 131, a second calculation processing unit 132, a second high-pass filter unit 133, a second low-pass filter unit 134, and a second addition unit 135. (FIG. 4).

  The second integration processing unit 131 performs integration processing on the angular velocity data output from the first synthesis processing unit 10, calculates a posture angle, and outputs the posture angle to the second high-pass filter unit 133.

  The second arithmetic processing unit 132 calculates an attitude angle from the acceleration data output from the second synthesis processing unit 11 and outputs the attitude angle to the second low-pass filter unit 134.

  The second high pass filter unit 133 extracts a posture angle in a high frequency band equal to or higher than a predetermined frequency from the posture angles calculated by the second integration processing unit 131 and outputs the posture angle to the second adding unit 135. On the other hand, the second low-pass filter unit 134 extracts a posture angle in a low frequency band equal to or lower than a predetermined frequency from the posture angles calculated by the second arithmetic processing unit 132 and outputs the posture angle to the second adding unit 135. .

  Here, the predetermined frequency, which is a frequency at which the frequency characteristics of the high-pass filter unit 133 and the low-pass filter unit 134 cross, is, for example, the first and second gyro sensors 2 and 3 and the first and second acceleration sensors 6 and 7. Is set based on the frequency characteristics, resolution, and the like.

  The second addition unit 135 adds the posture angle output from the second high-pass filter unit 133 and the posture angle output from the second low-pass filter unit 134 to calculate a temporary posture angle, This is output to the 2-probability model calculation unit 15.

  Note that the first and second posture angle calculation units 12 and 13 may calculate the posture angle using the following equation (3) based on the Kalman filter.

Here, the first and second gyro sensors 2, 3, the first and second acceleration sensors 6, 7, noise, and the calculated attitude angle are expressed as a state equation and an output equation as follows.
x _k = Ax _k−1 + w _k−1
z _k = Hx _k + v _k
State prediction (posture angle estimation) is performed using the following equation.
x _k = Ax _k-1 + Bd _{k-1
x k = x k + K (} z k -Hx k)
The covariance equation and Kalman gain are calculated by the following formula. (3) Formula _Pk = APk _-1 ^AT + Q
P _k = (I−KH) P _k
K = P _k H ^T (HP _k H ^T + R) ⁻¹

In the above equation (3), x _k is a true value of the state variable including the posture angle, and x _k is an estimated value of the state variable including the posture angle (a value actually obtained from the above equation (3)). Yes, z _k is an attitude angle (a value calculated based on the acceleration measured by the first and second acceleration sensors 6, 7) obtained using the output values of the first and second acceleration sensors 6, 7. V _k is observation noise, d _k is an angular velocity (sensor value) measured by the first and second gyro sensors 2 and 3, A is a state transition matrix, B is a drive matrix, H is an observation matrix and K is a Kalman gain.

In the above equation (3), Q is a process noise covariance matrix, R is a measurement noise covariance matrix, and these Q and R are, for example, the first and second gyro sensors 2, 3 and the first And the frequency characteristics and resolution of the second acceleration sensors 6 and 7 are set.
In the above description, k is used as a subscript representing time. In the following description, i is used as a subscript indicating one of the first and second attitude angle calculation units 12 and 13. Further, i is also used as a subscript indicating which of the first and second probability model calculation units 14 and 15 is.

  As described above, using the first and second attitude angle calculators 12 and 13 having parameters suitable for each sensor, the calculated attitude angles are added to the first and second probability model calculators 14 and 15 and added. Are integrated using the unit 16.

  The first probability model calculation unit 14 is a specific example of the first calculation means, and the posture angle output from the first posture angle calculation unit 12 is multiplied by a first probability model coefficient (an example of a first predetermined coefficient). Then, the multiplication value is output to the addition processing unit 16.

The second probability model calculation unit 15 is a specific example of the second calculation means. Like the first probability model calculation unit 14, the second probability model calculation unit 15 sets the second probability model to the posture angle output from the second posture angle calculation unit 13. The coefficient (an example of the second predetermined coefficient) is multiplied, and the multiplied value is output to the addition processing unit 16. The first and second probability model coefficients can be set using, for example, the combination ratio in the combination process described above. More specifically, a first probability model coefficient is r _1, the second probability model coefficients may be set to r _2.

On the other hand, when the first and second attitude angle calculation units 12 and 13 calculate the attitude angle using the Kalman filter, since the probability model assumes a normal distribution, the following equation (4) is used. First and second probability model coefficients can be calculated. This probability model is based on the idea that the smaller the difference between the estimated value and the actual value, the more likely it is. As described above, the attitude angle can be calculated with higher accuracy by using the probability model.

Furthermore, from the above equation (4), since the first and second attitude angle calculation units 12 and 13 are provided in the present embodiment, the i-th probability model coefficient p_coef _i (first and second probability model coefficients: i = 1, 2) is determined by the following equation (5). Thereby, each probability model coefficient is set so that it becomes 1 when the first probability model coefficient and the second probability model coefficient are added.

また、下記（７）式を用いて合成比率関数のみによって、より少ない計算量によって姿勢角を算出することもできる。さらに、上記段落（００２６）に記載のように、ＬＰＦとＨＰＦとを合成するようにしてもよい。

Furthermore, when the composite ratio function using the threshold value as described above and the probability model based on the normal distribution are used in combination, the first and second probability model coefficients can be calculated using the following equation (6). In this case, since information that is known in advance can be used, the posture angle can be calculated with higher accuracy.

Further, the posture angle can be calculated with a smaller amount of calculation by using only the composite ratio function using the following equation (7). Furthermore, as described in the paragraph (0026) above, LPF and HPF may be synthesized.

  The addition processing unit 16 is a specific example of addition means, and adds the multiplication value output from the first probability model calculation unit 12 and the multiplication value output from the second probability model calculation unit 13 to finally The correct posture angle is calculated.

  Next, the attitude angle calculation method by the attitude angle calculation apparatus 1 according to the present embodiment will be described in detail. FIG. 5 is a flowchart showing an example of a processing flow by the posture angle calculation apparatus according to the present embodiment. The arithmetic processing shown in FIG. 5 is repeatedly executed, for example, every predetermined minute time.

  First, the first gyro sensor 2 measures the angular velocity in the low speed region and outputs the measured angular velocity to the first AD converter 4. The second gyro sensor 3 measures the angular velocity from the low speed region to the high speed region, and outputs the measured angular velocity to the second AD conversion unit 5 (step S101). Thus, the angular velocity is measured using a plurality of gyro sensors having different measurement areas.

  At the same time, the first acceleration sensor 6 measures the acceleration in the low speed range, and outputs the measured acceleration to the third AD converter 8. The second acceleration sensor 7 measures the acceleration from the low speed range to the high speed range, and outputs the measured acceleration to the fourth AD conversion unit 9 (step S102). As described above, acceleration is measured using a plurality of acceleration sensors having different measurement areas.

  Next, the first AD converter 4 converts the analog angular velocity data output from the first gyro sensor 2 into digital data, and outputs the converted digital angular velocity data to the first synthesis processor 10. The second AD converter 5 converts the analog angular velocity data output from the second gyro sensor 3 into digital data, and outputs the converted digital angular velocity data to the first synthesis processor 10. The third AD converter 8 converts the analog acceleration data output from the first acceleration sensor 6 into digital data, and outputs the converted digital acceleration data to the second synthesis processor 11. The fourth AD converter 9 converts the analog acceleration data output from the second acceleration sensor 7 into digital data, and outputs the converted digital acceleration data to the second synthesis processor 11 (step S103). In this manner, the AD conversion is performed on the sensor values output from each sensor individually.

  Thereafter, the first synthesis processing unit 10 uses the digital angular velocity data from the first AD conversion unit 4 and the digital angular velocity data from the second AD conversion unit 5 to formula (1), ( The angular velocity synthesis process is performed using the formula (2) or formula (2-1), and the synthesized angular velocity is output to the first attitude angle calculation unit 12 and the second attitude angle calculation unit 13. Based on the digital acceleration data output from the third AD conversion unit 8 and the digital acceleration data output from the fourth AD conversion unit 9, the second synthesis processing unit 11 represents the above formula (1), (2 ) Expression or (2-1) expression is performed, and the combined acceleration is output to the first attitude angle calculator 12 and the second attitude angle calculator 13 (step S104). As described above, the synthesis process is performed so that sensor values output from a plurality of sensors in different measurement areas are continuous in the vicinity of the switching threshold of the measurement area.

  Further, the first posture angle calculation unit 12 calculates a temporary posture angle based on the angular velocity data output from the first synthesis processing unit 10 and the acceleration data output from the second synthesis processing unit 11. And output to the first probability model calculation unit 14. The second attitude angle calculation unit 13 calculates a temporary attitude angle based on the angular velocity data output from the first synthesis processing unit 10 and the acceleration data output from the second synthesis processing unit 11, It outputs with respect to the 2nd probability model calculating part 14 (step S105). In this way, the posture angle is calculated using the angular acceleration and acceleration after the synthesis process.

  Then, the first probability model calculation unit 14 multiplies the posture angle output from the first posture angle calculation unit 12 by the first probability model coefficient, and outputs the multiplication value to the addition processing unit 16. The second probability model calculation unit 15 multiplies the posture angle output from the second posture angle calculation unit 13 by the second probability model coefficient, and outputs the multiplication value to the addition processing unit 16 (step S106). . In this way, the posture angle calculated by the plurality of posture angle calculation units is multiplied by the coefficient obtained by the probability model.

  Finally, the addition processing unit 16 adds the multiplication value output from the first probability model calculation unit 12 and the multiplication value output from the second probability model calculation unit 13 to calculate a final posture angle. (Step S107). As described above, the result obtained by multiplying each posture angle by the coefficient obtained by the probability model can be added to obtain a highly accurate posture angle.

  As described above, in the posture angle calculation device 1 according to the present embodiment, measurement is performed using a plurality of gyro sensors and acceleration sensors having different measurement areas, and the measured values are combined to calculate the posture angle. It is possible to calculate a posture angle with high accuracy while being cost and light weight.

  In addition, by separately performing AD conversion processing on signals output from a plurality of gyro sensors and acceleration sensors, it is possible to reduce quantization errors and calculate posture angles with higher accuracy.

  Furthermore, even when the output values of a plurality of gyro sensors and acceleration sensors with different measurement areas are combined, the above-described error is suppressed by continuing the vicinity of the switching threshold of the measurement area. It can be calculated with higher accuracy.

  After synthesizing the output values of a plurality of gyro sensors and acceleration sensors, the posture angle is calculated by a plurality of posture angle calculation units in which optimum parameters are set for each sensor, and the calculated posture angle is a probability. The final posture angle is calculated by multiplying the coefficients obtained by the models and adding the results. Thereby, the posture angle can be calculated with higher accuracy.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.

  For example, in the above-described embodiment, the first gyro sensor for the low speed range, the second gyro sensor for the high speed range, the acceleration sensor for the low speed range, and the acceleration sensor for the high speed range are provided. However, the present invention is not limited to this, for example, a first gyro sensor for a low speed range, a second gyro sensor for a high speed range, a third gyro sensor for a medium speed range, an acceleration sensor for a low speed range, and a high speed range And a third acceleration sensor for medium speed range, and further includes a fifth gyro sensor for low and medium speed range and a fifth acceleration sensor for low and medium speed range. A configuration may be used, and a combination of arbitrarily set speed ranges is applicable.

  Further, in the above-described embodiment, a configuration including a single gyro sensor and the first and second acceleration sensors, a configuration including the first and second gyro sensors and a single acceleration sensor, As long as at least one of the gyro sensor and the acceleration sensor is composed of a plurality of sensors, a combination of any number of sensors can be applied. When one of the gyro sensor and the acceleration sensor is configured by a single sensor, the synthesis processing unit may have a single configuration corresponding to the configuration.

  Furthermore, in the above-described embodiment, the configuration includes a plurality of posture angle calculation units. However, the configuration may include a single posture angle calculation unit. (The structure which has only any one among the 1st and 2nd attitude angle calculating parts 12 and 13 may be sufficient.).

  In the said embodiment, the structure which does not have the 1st and 2nd synthetic | combination process parts 10 and 11 may be sufficient (FIG. 6). In the posture angle calculation device 20 according to this configuration, the first and third AD conversion units 4 and 8 are connected to the first posture angle calculation unit 12, and the second and fourth conversion units 5 and 9 are second posture angle calculation units. 13 is connected. Thereby, the configuration is further simplified, and the overall calculation speed can be shortened. Further, in this configuration, by connecting the first synthesis processing unit 10 to the first and second AD conversion units 4 and 5 and connecting the second synthesis processing unit 11 to the third and fourth AD conversion units 8 and 9, The angular velocity may be output from the first synthesis processing unit 10 and the acceleration may be output from the second synthesis processing unit 11 (FIG. 7).

  The present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. In the above-described embodiments, the present invention has been described as a hardware configuration, but the present invention is not limited to this. In the present invention, for example, the processing shown in FIG. 5 can be realized by causing a CPU to execute a computer program.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer readable media are magnetic recording media (eg flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (eg magneto-optical disks), CD-ROM, CD-R, CD-R / W. Semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM).

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

  The present invention includes, for example, the posture angle calculation device 1 described above and a control device that controls walking based on the posture angle calculated with high accuracy by the posture angle calculation device 1, and performs higher-precision walking. It can be applied to a biped walking robot.

DESCRIPTION OF SYMBOLS 1 Attitude angle calculating device 2 1st gyro sensor 3 2nd gyro sensor 4 1st AD conversion part 5 2nd AD conversion part 6 1st acceleration sensor 7 2nd acceleration sensor 8 3rd AD conversion part 9 4th AD conversion part 10 1st synthetic | combination process Unit 11 second composition processing unit 12 first posture angle computing unit 13 second posture angle computing unit 14 first probability model computing unit 15 second probability model computing unit 16 addition processing unit

Claims (18)

A posture angle calculation device that includes a gyro sensor that detects angular velocity and an acceleration sensor that detects acceleration, and calculates a posture angle based on the angular velocity detected by the gyro sensor and the acceleration detected by the acceleration sensor. And
At least one of the gyro sensor and the acceleration sensor is composed of a plurality of sensors having different measurement areas that measure the same location and the same direction,
A posture angle calculation device comprising posture angle calculation means for calculating a posture angle based on signals output from a plurality of sensors having different measurement areas. The posture angle calculation device according to claim 1,
Further comprising synthesis means for synthesizing signals respectively output from a plurality of sensors having different measurement areas;
The posture angle calculation device, wherein the posture angle calculation means calculates the posture angle based on the signal synthesized by the synthesis means. The posture angle calculation device according to claim 2,
A posture angle calculation device comprising a plurality of posture angle calculation means that respectively use signals output from a plurality of sensors having different measurement areas. The posture angle calculation device according to claim 3,
A posture angle calculation device further comprising a plurality of conversion means for converting signals output from the plurality of sensors from analog signals to digital signals, respectively. The attitude angle calculation device according to claim 4,
The posture angle calculation device characterized in that the synthesizing unit synthesizes signals output from a plurality of sensors in different measurement areas so as to be continuous in the vicinity of the switching threshold of the measurement area. The attitude angle calculation device according to claim 5,
A plurality of calculation means for multiplying the plurality of attitude angles respectively calculated by the plurality of attitude angle calculation means by a predetermined coefficient based on a probability model or a composite ratio function model;
Adding means for adding a plurality of calculated values respectively calculated by the calculating means;
An attitude angle calculation device characterized by further comprising: The posture angle calculation device according to any one of claims 1 to 6,
The posture angle calculation device characterized in that the different measurement areas are a low speed area, a medium speed area or a high speed area. The posture angle calculation device according to claim 6,
The plurality of gyro sensors include a first gyro sensor that detects an angular velocity in a low speed region, and a second gyro sensor that detects an angular velocity in a high speed region,
The plurality of acceleration sensors include a first acceleration sensor that detects an acceleration in a low speed region and a second acceleration sensor that detects an acceleration in a high speed region. The posture angle calculation device according to claim 8,
The plurality of conversion means include
First conversion means for converting the angular velocity in the low speed range detected by the first gyro sensor from an analog signal to a digital signal;
Second conversion means for converting the angular velocity in the high speed range detected by the second gyro sensor from an analog signal to a digital signal;
Third conversion means for converting low-speed acceleration detected by the first acceleration sensor from an analog signal to a digital signal;
Fourth conversion means for converting acceleration in the high speed range detected by the second acceleration sensor from an analog signal to a digital signal;
A posture angle calculation device comprising: The posture angle calculation device according to claim 9,
The plurality of synthesizing means include
First synthesizing means for synthesizing the angular velocity signal converted by the first converting means and the angular velocity signal converted by the second converting means so as to be continuous in the vicinity of the switching threshold of the measurement area;
Second combining means for combining the acceleration signal converted by the third converting means and the acceleration signal converted by the fourth converting means so as to be continuous in the vicinity of the switching threshold of the measurement area;
A posture angle calculation device comprising: The posture angle calculation device according to claim 10,
The plurality of posture angle calculation means include
First attitude angle calculation means for calculating an attitude angle based on the angular velocity signal synthesized by the first synthesis means and the acceleration signal synthesized by the second synthesis means;
Second attitude angle calculation means for calculating an attitude angle based on the angular velocity signal synthesized by the first synthesis means and the acceleration signal synthesized by the second synthesis means,
The plurality of calculation means include
First computing means for multiplying the attitude angle calculated by the first attitude angle computing means by a first predetermined coefficient based on a probability model or a composite ratio function model;
Second calculating means for multiplying the attitude angle calculated by the second attitude angle calculating means by a second predetermined coefficient based on a probability model or a composite ratio function model,
The adding means includes
A posture angle calculation device, wherein a final posture angle is calculated by adding the calculation result calculated by the first calculation means and the calculation result calculated by the second calculation means. An attitude angle calculation device according to any one of claims 1 to 11,
A control device that controls walking based on the posture angle calculated by the posture angle calculation device;
A biped walking robot characterized by comprising: A posture angle calculation method that includes a gyro sensor that detects angular velocity and an acceleration sensor that detects acceleration, and calculates a posture angle based on the angular velocity detected by the gyro sensor and the acceleration detected by the acceleration sensor. And
At least one of the gyro sensor and the acceleration sensor is composed of a plurality of sensors having different measurement areas that measure the same location and the same direction,
A posture angle calculation method comprising a step of calculating a posture angle based on signals respectively output from a plurality of sensors having different measurement areas. The attitude angle calculation method according to claim 13,
Further comprising the step of synthesizing signals respectively output from a plurality of sensors having different measurement areas;
A posture angle calculation method, wherein the posture angle is calculated based on the synthesized signal. The attitude angle calculation method according to claim 14,
An attitude angle calculation method further comprising the step of converting signals output from the plurality of sensors from analog signals to digital signals, respectively. The posture angle calculation method according to claim 15,
A posture angle calculation method characterized in that signals output from a plurality of sensors in different measurement areas are combined so as to be continuous in the vicinity of the switching threshold of the measurement area. The attitude angle calculation method according to claim 16, wherein
Multiplying the calculated plurality of posture angles by respective predetermined coefficients based on a probability model or a composite ratio function model;
Adding the calculated plurality of calculated values;
A posture angle calculation method characterized by further comprising: A program for calculating a posture angle based on an angular velocity detected by the gyro sensor and an acceleration detected by the acceleration sensor, comprising a gyro sensor for detecting angular velocity and an acceleration sensor for detecting acceleration;
At least one of the gyro sensor and the acceleration sensor is composed of a plurality of sensors having different measurement areas that measure the same location and the same direction,
A program for causing a computer to execute a process of calculating an attitude angle based on signals output from a plurality of sensors having different measurement areas.

Images