JP4753068B2 - POSITION DETECTION DEVICE FOR MOBILE BODY AND MOBILE ROBOT - Google Patents

Description

The present invention relates to posture detecting device that gives detects the posture of the moving body, in particular, for stable control of the posture of a mobile robot such as walking robots and wheel type moving robot, the body of the mobile robot If it of precisely detecting acceleration or posture rotational angular velocity and attitude angle, etc. relates posture detecting device of a moving body which can be suitably used for such.

  Conventionally, the posture control of a mobile robot as a moving body is performed by using one rate gyro or one acceleration sensor with respect to the uniaxial posture of the robot body, as shown in Patent Document 1, for example. The angle is detected and the signal is fed back to control the posture of the moving body.

  However, there is a problem that the posture control of the moving body cannot be accurately controlled because there is a drift due to temperature in the output of the posture sensor such as a rate gyroscope or an acceleration sensor. In addition, the drift amount becomes remarkable when the posture control of the moving body is performed for a long time, so that there is a problem that the mobile robot falls down especially in the walking robot.

  As a first method for solving such a problem, a method of correcting the output of the attitude sensor using a temperature sensor is considered.

  However, although this first solution is effective for a moving body having a system that can stably supply power for driving the attitude sensor, the driving power of the attitude sensor fluctuates, and as a result, the attitude sensor When the output also fluctuates, there is still a problem that the posture control of the moving body cannot be controlled with high accuracy. In particular, in the case of a walking robot that is compact and lightweight, and has a limited body space of the robot body with a control computer, sensor, drive power supply, etc., load fluctuations due to arithmetic processing in the control computer are output from the drive power supply. As a result, it also causes detection fluctuation and drift of the posture angle. Therefore, when the output of the posture sensor using the temperature sensor is corrected, there is still a problem that the walking robot falls.

  As another method for solving the drift problem, for example, as shown in Patent Document 2, drift is achieved using not only two differential circuits but also a synchronous detection circuit, a smoothing circuit, and a phase shift circuit. There is a method of detecting and correcting.

  However, this solution has a problem that the circuit becomes complicated. In addition, even if the sensor output is compensated for drift, the control signal when the control signal is taken into the attitude control device that performs control calculation processing due to fluctuations in the drive power supply of the attitude control device, that is, the reference voltage fluctuation of the analog / digital conversion circuit. As mentioned above, there was also a problem that drift occurred.

  As another solution to the rate gyro drift problem, for example, a combination of a rate gyro and an acceleration sensor is used to construct an attitude angle detection algorithm such as a Kalman filter and detect the attitude angle while correcting the drift of the attitude sensor. It has been.

  However, there has been a problem that the drift of the acceleration sensor itself that serves to eliminate the drift of the rate gyro cannot be corrected. Furthermore, in the posture angle detection algorithm such as the Kalman filter, since the posture angle is corrected statically (or in a low frequency region), the dynamic (or There is also a problem that the attitude angle (in the high frequency range) cannot be detected accurately. Due to these problems, when a posture angle is detected by a posture angle detection algorithm such as a Kalman filter, the walking robot still has a problem of falling.

The present inventors have previously proposed a solution for the case where there is a temperature drift of the posture detection sensor, fluctuations in the drive power supplied to the posture detection hand sensor, fluctuation drifts that occur when taken in by the control arithmetic processing mechanism, etc. As shown in Japanese Patent Application Laid-Open No. H10-228707, the mobile body includes a posture detection unit and another posture detection unit that outputs an inverted output with respect to the posture detection unit. There is a method of detecting the attitude of a moving body with high accuracy by a control arithmetic processing mechanism driven by a common drive power source.
JP 20019772 A JP 07-218269 A JP 2004-53530 A

  As shown in Patent Document 3, the present inventors have proposed a high-accuracy posture detection method for a moving body. Further, as a problem to be solved, a high-accuracy posture detection method for a mobile body described in Patent Document 3 In this case, when the mobile body itself performs sudden acceleration / deceleration, centrifugal acceleration acts on the mobile body due to turning movement, or when the mobile body is on a trolley that accelerates / decelerates, When inertial acceleration is acting, there is a problem that the posture angle cannot be detected with high accuracy. In order to eliminate the influence of inertial acceleration of the moving body, a method of calculating the posture angle by weighting the rate gyro in the posture detection calculation process can be considered, but the rotating environment (on the star: not only on the earth but on the moon When there is a moving object on all the stars including Mars, etc., there is a problem that the attitude angle cannot be detected with high accuracy because the rotational angular velocity of the rotation affects the rate gyro sensor itself.

The present invention has been made to solve such a problem, and an object of the present invention is, for example, even when the inertial acceleration of the moving body acts on the acceleration sensor itself . It is an object of the present invention to provide a moving body posture detecting apparatus capable of detecting a posture with high accuracy.

In order to achieve the above object, according to the first solving means of the present invention, an inertia detecting means for detecting the inertia of the moving body, an action generating means for generating an action of the moving body, a movement Arithmetic processing means for performing body posture calculation, wherein the arithmetic processing means corrects the output of the inertia detection means by subtracting the output of the motion generation means from the output of the inertia detection means, and the corrected inertia There is provided a mobile body posture detection apparatus that performs arithmetic processing for calculating a posture based on the output of the detection means and detects the posture of the mobile body.

  According to the first solving means of the present invention as described above, the motion generating means for generating the motion of the moving body from the output of the inertia detecting means mounted on the moving body, the inertia component unnecessary for the posture calculation of the moving body. Therefore, when the mobile body itself accelerates or decelerates suddenly, or when centrifugal acceleration acts on the mobile body due to turning movement, the mobile body gets on the trolley that accelerates or decelerates. Or in the environment where the moving object rotates (on the star: not only on the earth but on all stars including the moon and Mars), the posture of the moving object is detected accurately. can do.

According to the first specific method of the first solving means of the present invention, at least two acceleration sensors are provided in the inertia detection means so as to be orthogonal to each other , and at least the acceleration from the output of the motion generation means. The acceleration target value of the motion output with the same direction component as the sensor is output, and the arithmetic processing means calculates the same direction component of the acceleration target value of the motion generation means from the output of the acceleration sensor of the inertia detection means. the corrected output of the acceleration sensor of the inertial sensing means by subtracting performs division on the output of the acceleration sensor that the other correct output of the acceleration sensor which is corrected for one, the mobile division result arctan operation on Detect posture.

According to the second specific method of the first solving means of the present invention, the inertia detection means is provided with at least two acceleration sensors so as to be orthogonal to each other , and at least the acceleration from the output of the motion generation means. Centrifugal force due to rotation of the environment in which there is a moving body that is output in the same direction component as the sensor is output, and the arithmetic processing means uses the same centrifugal force of the motion generation means from the output of the acceleration sensor of the inertia detection means correcting the output of the acceleration sensor of the inertial sensing means by subtracting the directional components together, perform division with the other of the corrected acceleration sensor outputs an output of the acceleration sensor which is corrected for one, the division result to arctan calculation To detect the posture of the moving object.

According to a third specific method of the first aspect of the present invention, the said inertial sensing means, Les Tojairo is provided, is output in the same direction component as at least the rate gyro from the output of said motion generating means that rotation speed of the moving object is present environment are outputted, the processing means, the rate of the inertia detecting means by subtracting the rotation speed of said motion generating means from the rate gyro output of said inertial sensing means The posture of the moving body is detected by correcting the output of the gyro and integrating the output of the corrected rate gyro.

Further, according to the second solving means of the present invention, the mobile body includes an inertia detection means for detecting the inertia of the mobile body, an environment information means for detecting and storing information on an environment where the mobile body is located, Arithmetic processing means for performing body posture calculation, wherein the arithmetic processing means corrects the output of the inertia detection means by subtracting the output of the environment information means from the output of the inertia detection means, and the corrected inertia There is provided a mobile body posture detection apparatus that performs arithmetic processing for calculating a posture based on the output of the detection means and detects the posture of the mobile body.

  According to the second solving means of the present invention as described above, the inertia component unnecessary for the posture calculation of the moving object is detected from the output of the inertia detecting means mounted on the moving object, and the information on the environment where the moving object is present is detected. Since it can be removed using the output of the stored environmental information means, when the mobile body itself performs sudden acceleration / deceleration, when the centrifugal acceleration acts on the mobile body due to turning movement, the mobile body is mounted on the accelerating / decelerating carriage Or in the environment where the moving object rotates (on the stars: not only on the earth but on all stars including the moon and Mars) Can be detected.

According to the first specific method of the second solving means of the present invention, the inertia detection means is provided so that at least two acceleration sensors are orthogonal to each other , and the environment information means is at least a moving body. a force sensor for detecting a force acting between the environment, said force sensor and comprises a coordinate transformation means for performing coordinate conversion between the acceleration sensor Rutotomoni, the value of the known gravitational acceleration environment which the mobile is present The force sensor outputs a horizontal component and a vertical component of a force acting between the moving body and the environment, and the arithmetic processing means is provided between the moving body and the environment. An acceleration value that generates a motion of the moving object by dividing the horizontal component of the force acting on the object by the vertical component of the force acting between the moving object and the environment, and multiplying the division result by the known gravitational acceleration Calculate Converts the acceleration value for generating an operation of the moving body obtained by the acceleration value for generating an operation of the moving object are output in the same direction component as the acceleration sensor by the coordinate transforming means, the acceleration sensor of the inertial sensing means in the corrected output of the acceleration sensor of the inertial sensing means by the output by subtracting the same direction component between the converted acceleration value, the output of the acceleration sensors the other corrects the output of the acceleration sensor which is corrected for one Division is performed, and the division result is arctan calculated to detect the posture of the moving object.

According to a second specific method of the second solving means of the present invention, the inertia detection means is provided so that at least two acceleration sensors are orthogonal to each other , and the environment information means is at least a moving body. a GPS sensor for detecting latitude, Rutotomoni comprises a coordinate transformation means for performing coordinate transformation between the position and the acceleration sensor moving body is present, stores the rotation speed and the radius of the environment in which the mobile is present, the The arithmetic processing means calculates the centrifugal force at the position where the moving body is present using the output of the GPS sensor, the rotation speed, and the radius, and the coordinate conversion is performed on the centrifugal force at the position where the moving body is obtained by calculation. centrifugation through rotation at the position converted into the centrifugal force, the movable body converted from the output of the acceleration sensor of the inertial sensing means is present due to rotation of the mobile is present position in the same direction component as the acceleration sensor by means The same by subtracting the directional components together to correct the output of the acceleration sensor of the inertial sensing means performs division by the acceleration sensor output that is the other correct output of the acceleration sensor which is corrected for one, arctan this division result of The posture of the moving body is detected by calculation.

According to a third specific method of the second solving means of the present invention, the inertia detection means is provided with a rate gyro, and the environment information means includes at least a GPS sensor for detecting the latitude of the moving object; Rutotomoni comprises a coordinate transformation means for performing coordinate conversion between the position where the moving body is present and the rate gyro, stores the rotation speed of the environment in which the mobile is present, the processing means, the said GPS sensor The rotation angular velocity of the rotation speed at the position where the moving body is located is calculated from the rotation speed of the environment where the moving body is present, and the rotation angular velocity at the position where the moving body is found is the same as the rate gyro obtained by the coordinate conversion means. into a rotational angular velocity that is output by the directional component, the rotational angular velocity is subtracted child outputted in the same direction component rate gyro and output the converted rate gyroscopes of the inertial sensing means The corrected output of the rate gyro inertial detecting means for detecting the posture of the moving body by integrating the output of the corrected rate gyro by.

According to posture detecting device of the mobile body according to the present invention, if the mobile itself perform sudden acceleration or deceleration, centrifugal acceleration when acting on the moving body, the moving body is on board the truck accelerates or decelerates the pivotal movement In this case, in any case where the inertial acceleration of the moving body is acting on the acceleration sensor itself, it is possible to obtain a device that accurately detects the posture angle. In addition, even when the moving object is in an environment where the moving object rotates (on the star: not only on the earth but on all stars including the moon and Mars), the rate gyro sensor itself is affected by the rotational angular velocity of the rotation. A device for accurately detecting the attitude angle can be obtained.

Embodiments of a posture detection apparatus for a moving body according to the present invention will be described below. Posture detection device of the moving body to be described below, the detection of the attitude of the moving body from the inertial sensor output installed in the moving body by removing the inertial component becomes unnecessary, it can be accurately detect the posture of the moving body This is a posture detection device, and this posture detection device is referred to as a high-precision posture detection device. In the following description, “detect with high accuracy” and “detect with high accuracy” mean the same thing. FIG. 1 is a diagram schematically showing the configuration of a high-accuracy posture detection device according to a first embodiment of the present invention, and the posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2a. It is a figure explaining the structure of the apparatus of the Example to do. FIG. 1 shows a walking robot as an example of a moving body targeted by the present invention, but the moving body is not limited to a walking robot, and may be various moving bodies such as a wheeled mobile robot. .

  In FIG. 1, 3 is a robot body, 10 is inertia detection means, and 20 is motion generation means. 4a is a leg link of one leg, 4b is a leg link of one leg, 5 is a sole of a leg, 6a is a first joint motor, 6b is a second joint motor, and 6c is a third joint motor. They are also provided for the other one leg, not shown. Although not shown, the joint angle of each joint is detected by a joint angle sensor such as an encoder or a potentiometer. In addition, an actuator of each joint is controlled by a control device (not shown) so as to faithfully reproduce the motion generated by the motion generating means 20, and the illustrated walking robot moves.

  The high-accuracy posture detection device 2a includes an inertia detection unit 10 and a motion generation unit 20. The inertia detection means 10 is provided with an acceleration sensor (11a, 11b). The motion generation means 20 here does not include a gravitational acceleration component, but includes only an acceleration / deceleration component of the generated motion. A target value 21a is output. As will be described later, the acceleration target value 21a is preferably output with the same direction component as the acceleration sensors (11a, 11b) in order to simplify the arithmetic processing. When the acceleration sensor (11a, 11b) is not output in the same direction component, as will be described later, a coordinate conversion means is provided in the arithmetic processing unit, and the body in the target posture state with the output coordinates of the acceleration target value 21a. 3 is subjected to coordinate conversion with the output coordinates of the acceleration sensors (11a, 11b) installed at 3, and is calculated as the same direction component. Hereinafter, for simplification of explanation, it is assumed that the acceleration target value 21a is output with the same direction component as the acceleration sensors (11a, 11b).

  FIG. 2 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2a shown in FIG. 1, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. Specifically, the output of the acceleration sensor 11a is based on the acceleration target value “αa_ref1” which is a component in the same direction as the acceleration sensor 11a and does not include the gravity component in the acceleration target value 21a output from the motion generation unit 20. “Αa_sen” is corrected, and corrected acceleration “αa” is calculated. Similarly, based on the acceleration target value “αb_ref1”, which is a component in the same direction as the acceleration sensor 11b and out of the acceleration target value 21a output from the motion generation unit 20, the output of the acceleration sensor 11b is output. “Αb_sen” is corrected, and corrected acceleration “αb” is calculated. Finally, based on the corrected acceleration signals “αa” and “αb”, the posture of the moving body is detected with high accuracy.

In this case, the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal obtained by the calculation process of the high-accuracy posture detection device 2a are respectively αa = αa_sen−αa_ref1
αb = αb_sen−αb_ref1
Thus, by removing the acceleration / deceleration component accompanying the movement of the moving body itself, the acceleration (αa, αb) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG. 2 is specifically performed by posture angle = −tan ⁻¹ (αb / αa).
The posture angle is obtained by Thereby, even when the mobile body itself performs rapid acceleration / deceleration, or when centrifugal acceleration acts on the mobile body due to turning movement, the posture angle of the body 3 of the walking robot can be obtained with high accuracy.

  3-5 is a figure explaining operation | movement of the highly accurate attitude | position detection apparatus by this invention. As a specific example of the first embodiment of the present invention, as shown in FIG. 3, an outline will be described regarding a state in which the body 3 of the moving body is accelerating while maintaining the level.

  Conventionally, for example, in the high-accuracy posture detection device of Patent Document 3, since the acceleration accompanying the movement of the moving body is not considered in the calculation process of the posture angle, as shown in FIG. In the situation where acceleration is performed, as shown in FIG. 4, it may be erroneously recognized that the posture of the body 3 is inclined. Therefore, when the posture control of the moving body is performed using the posture detection information, there is a problem that the posture is controlled to be incorrect. In particular, the walking robot has a problem of falling.

  In contrast, in the high-accuracy posture detection apparatus according to the first embodiment of the present invention, in the posture angle calculation process, the acceleration accompanying the movement of the moving body included in the acceleration sensor is output from the motion generation means 20 (acceleration. 3), even in a situation where the body 3 is accelerating as shown in FIG. 3, the posture of the body 3 can be calculated with high accuracy as shown in FIG. It becomes. Therefore, when the posture control of the moving body is performed using this posture detection information, it is possible to control the target posture with high accuracy. Can be realized.

  FIG. 6 is a diagram schematically showing the configuration of the high-accuracy posture detection device according to the second embodiment of the present invention. The posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2b. It is a figure explaining the structure of the apparatus of the Example to do. The high-accuracy posture detection device 2b of the second embodiment is composed of the inertia detection means 10 and the motion generation means 20 in the same manner as the high-precision posture detection device 2a of the first embodiment. An acceleration sensor (11a, 11b) is provided. The difference is that the motion generation means 20 outputs the acceleration target value 21b obtained by adding the acceleration / deceleration component and the gravity component of the generated motion, and also outputs the known gravitational acceleration 22 and the target posture angle 23. It is a point. Using these data, the attitude angle of the moving body is detected with high accuracy. As with the high-accuracy posture detection device 2a of the first embodiment, the acceleration target value 21b is output with the same direction component as that of the acceleration sensors (11a, 11b) in order to simplify the following description. To do.

  FIG. 7 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2b of FIG. 6, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. Specifically, the acceleration target value “αa_ref2”, which is a component in the same direction as the acceleration sensor 11a in the acceleration target value 21b output from the motion generation means 20, and the motion acceleration / deceleration component and the gravity component are added together, Based on the known gravitational acceleration 22 and the target posture angle 23, the output “αa_sen” of the acceleration sensor 11a is corrected to calculate a corrected acceleration “αa”. Similarly, the acceleration target value “αb_ref2”, which is a component in the same direction as the acceleration sensor 11b in the acceleration target value 21b output from the motion generation means 20, and is the sum of the acceleration / deceleration component and the gravity component of the motion, is known. Based on the gravitational acceleration 22 and the target attitude angle 23, the output “αb_sen” of the acceleration sensor 11b is corrected to calculate the corrected acceleration “αb”. Finally, the posture of the moving body is detected with high accuracy based on the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal.

Here, based on the known gravitational acceleration 22 and the target posture angle 23, the gravity component when the posture angle of the body 3 of the moving body is the target posture angle 23 is calculated. That is, the component “αa_grav” in the same direction as the acceleration sensor 11a and the component “αb_grav” in the same direction as the acceleration sensor 11b are calculated. Therefore, the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal obtained by the arithmetic processing of the high-accuracy posture detection device 2b are αa = αa_sen−αa_ref2 + αa_grav, respectively.
αb = αb_sen−αb_ref2 + αb_grav
Thus, by removing the acceleration / deceleration component accompanying the movement of the moving body itself, the acceleration (αa, αb) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG. 7, specifically, the attitude angle = −tan ⁻¹ (αb / αa)
The posture angle is obtained by Thereby, even when the mobile body itself performs rapid acceleration / deceleration or when centrifugal acceleration acts on the mobile body due to turning movement, the posture angle of the body 3 of the walking robot can be obtained with high accuracy.

  FIG. 8 is a diagram schematically showing the configuration of the high-accuracy posture detection device according to the third embodiment of the present invention. The posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2c. It is a figure explaining the structure of the apparatus of the Example to do.

  Like the high-accuracy posture detection device 2a of the first embodiment and the high-precision posture detection device 2b of the second embodiment, the high-accuracy posture detection device 2c of the third embodiment includes inertia detection means 10 and motion generation means. The inertial detecting means 10 is provided with acceleration sensors (11a, 11b). The difference is that the motion generating means 20 outputs a centrifugal force 24 due to the rotation of the environment (star) in which the moving body is present. As will be described later, the centrifugal force 24 is a centrifugal force at a position where the moving body is present, and is output with the same direction component as the acceleration sensors (11a, 11b) in order to simplify the arithmetic processing by the arithmetic processing unit. Preferably it is. When the centrifugal force at the position where the moving body is present is not output, it is calculated from the known radius, the known rotation speed and the target latitude of the environment (star) where the moving body is present. In addition, when the same direction component as that of the acceleration sensor (11a, 11b) is not output, the coordinate conversion unit is provided, the target conversion direction is used by the coordinate conversion unit, and the output coordinate of the centrifugal force 24 and the target Coordinate conversion between the output coordinates of the acceleration sensors (11a, 11b) installed on the body 3 in the posture state is performed. Hereinafter, for simplification of explanation, it is assumed that the centrifugal force 24 is a centrifugal force at a position where the moving body is present and is output in the same direction component as the acceleration sensors (11a, 11b).

  FIG. 9 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2c of FIG. 8, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. Specifically, the output “αa_sen” of the acceleration sensor 11a is corrected based on the component “αa_cen1” in the same direction as the acceleration sensor 11a in the centrifugal force 24 output from the motion generation means 20, and the corrected acceleration “αa” Is calculated. Similarly, the output “αb_sen” of the acceleration sensor 11b is corrected based on the component “αb_cen1” in the same direction as the acceleration sensor 11b in the centrifugal force 24 output from the motion generation means 20, and the corrected acceleration “αb” is obtained. Calculated. Finally, based on the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal, the posture of the moving body is detected with high accuracy.

In this case, the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal obtained by the arithmetic processing of the high-accuracy posture detection device 2c are respectively αa = αa_sen−αa_cen1.
αb = αb_sen−αb_cen1
Thus, by removing the centrifugal force component by the rotation of the environment (star) where the moving body is present, the acceleration (αa, αb) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG. 9, specifically, the attitude angle = −tan ⁻¹ (αb / αa)
The posture angle is obtained by Thereby, even when the environment (star) in which the moving body is present rotates, and the centrifugal force component due to the rotation is mixed in the acceleration sensor output, the posture angle of the body 3 of the walking robot can be obtained with high accuracy. it can.

  FIG. 10 is a diagram schematically showing the configuration of the high-accuracy posture detection device according to the fourth embodiment of the present invention. The high-precision posture detection device 2d detects the posture angle of the body 3 of the walking robot with high accuracy. It is a figure explaining the structure of the apparatus of the Example to do.

  The high-accuracy posture detection device 2d of the fourth embodiment is composed of the inertia detection means 10 and the motion generation means 20 in the same manner as the high-precision posture detection devices (2a, 2b, 2c) of the first to third embodiments described above. ing. The difference from the high-accuracy posture detection devices of the first to third embodiments is that the inertia detection means 10 is provided with a rate gyro 12c, and the motion generation means 20 includes an environment (star). The rotation speed 25 is output. The rotation speed 25 is preferably output in the same direction component as that of the rate gyro 12c in order to simplify the arithmetic processing of the arithmetic processing unit described later. When the direction component is not output in the same direction as the rate gyro 12c, the coordinate conversion means is used to use the target latitude, target orientation, and target posture, and the output coordinates of the rotation speed 25 and the body 3 in the target posture state are used. Coordinate conversion between the output coordinates of the installed rate gyro 12c is performed. Hereinafter, for the sake of simplicity, it is assumed that the rotation speed 25 is output in the same direction component as the rate gyro 12c.

  FIG. 11 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2d in FIG. 10, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. Specifically, the output “ωc_sen” of the rate gyro 12c is corrected based on the component “ωc_rot1” in the same direction as the rate gyro 12c in the rotation speed 25 output from the motion generation means 20, and the corrected angular velocity “ωc”. Is calculated. Finally, based on the corrected angular velocity “ωc” of the corrected angular velocity signal, the posture of the moving body is detected with high accuracy.

により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。 In this case, the corrected angular velocity “ωc” of the corrected angular velocity signal obtained by the arithmetic processing of the arithmetic processing unit of the high-accuracy posture detecting device 2d is
ωc = ωc_sen−ωc_rot1
The angular velocity (ωc) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy by removing the rotation speed component due to the rotation of the environment (star) in which the moving body is present.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG.

The posture angle is obtained by Thereby, even when the environment (star) in which the moving body is present rotates, and the rotation speed component due to the rotation is mixed in the rate gyro output, the posture angle of the body 3 of the walking robot can be obtained with high accuracy. it can.

  FIG. 12 is a diagram schematically showing the configuration of the high-accuracy posture detection device according to the fifth embodiment of the present invention, and the posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2e. It is a figure explaining the structure of the apparatus of the Example to do.

  The high-accuracy posture detection device 2e of the fifth embodiment includes an inertia detection means 10 and an environment information means 30. The inertia detection means 10 is provided with two acceleration sensors (11a, 11b). In addition, the environment information means 30 is provided with a force sensor 31 that detects a force acting between the moving body and the environment. Although not shown in FIG. 12, coordinate conversion means 40 is provided for performing coordinate conversion between the output coordinates of the force sensor 31 and the output coordinates of the acceleration sensors (11a, 11b) installed on the body 3. . Coordinate conversion is performed from a joint angle sensor 32 such as an encoder or a potentiometer that detects the joint angle of each joint, or a joint angle command of each joint that is output from the motion generation means 20 (not shown), and necessary. The calculation is sequentially performed so as to extract the direction component.

  FIG. 13 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2e of FIG. 12, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. The contents of the arithmetic processing of the high accuracy posture detection device 2e of the fifth embodiment are similar to the arithmetic processing of the high accuracy posture detection device 2a of the first embodiment and the high accuracy posture detection device 2b of the second embodiment. The difference is that the calculation process is performed using the data from the environment information unit 30 without using the data output from the motion generation unit 20. Hereinafter, specific processing contents will be described.

  First, using the horizontal component “Fx_sen”, the vertical component “Fz_sen” and the known gravitational acceleration 22 which are outputs of the force sensor 31, an acceleration value “αx_mot” for generating a motion is obtained. When a lot of noise components appear in the vertical component “Fz_sen” due to landing impact, such as a walking robot, it is practical to use the known weight value without using the vertical component “Fz_sen” of the force sensor. No problem. Next, the acceleration value “αx_mot” that generates the motion is converted into acceleration values (αa_mot, αb_mot) having the same direction components as the acceleration sensors (11a, 11b) by coordinate conversion by the coordinate conversion means 40. Then, based on the acceleration value “αa_mot” and the acceleration value “αb_mot” of the direction component, the output (αa_sen, αb_sen) of the acceleration sensor (11a, 11b) is corrected, and the corrected acceleration “αa” and the corrected acceleration “αb” are corrected. Is calculated. Finally, based on the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal, the posture of the moving body is detected with high accuracy.

In this case, the acceleration value “αx_mot” that generates the motion calculated using the force sensor 31 is an acceleration value that generates acceleration / deceleration in the horizontal direction of the body 3 without including the gravity direction of the body 3.

On the other hand, when the body 3 is accelerated or decelerated in the horizontal direction as it moves, the component of the acceleration value “αx_mot” appears in the output of the acceleration sensor (11a, 11b). In order to calculate this influence, the coordinate conversion means 40 performs coordinate conversion “ba_R_xz” between the output coordinates of the force sensor 31 and the output coordinates of the acceleration sensors (11a, 11b), and converts the acceleration value “αx_mot” into the acceleration value. They are converted to the same direction components (αa_mot, αb_mot) as the sensors (11a, 11b).

Finally, the corrected acceleration “αa” and the corrected acceleration “αb” of the corrected acceleration signal obtained by the arithmetic processing of the high-accuracy posture detection device 2e are respectively αa = αa_sen−αa_mot
αb = αb_sen−αb_mot
Thus, by removing the acceleration / deceleration component accompanying the movement of the moving body itself, the acceleration (αa, αb) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG. 13, specifically, the attitude angle = −tan ⁻¹ (αb / αa)
The posture angle is obtained by Thereby, even when the mobile body itself performs rapid acceleration / deceleration or when centrifugal acceleration acts on the mobile body due to turning movement, the posture angle of the body 3 of the walking robot can be obtained with high accuracy.

により算出し、

のように、座標変換を行い、
αa＝αa_sen−αa_mot、
αb＝αb_sen−αb_mot、
αc＝αc_sen−αc_mot
のように、加速度センサを補償した後、
ｃ軸周りの姿勢角＝−ｔａｎ^−１（αb／αa）
ｂ軸周りの姿勢角＝＋ｔａｎ^−１（αc／αa）
のような計算により、３次元での胴体３の姿勢を高精度に検出することができる。更に、ノイズに対する信頼性を向上したい場合には、補正した加速度センサの加速度値（αa，αb，αc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢を高精度に信頼性良く検出することができる。 For simplification of description, FIG. 13 has been described by simplifying the description with reference to a two-dimensional drawing. However, when performing highly accurate posture detection in three dimensions, the two acceleration sensors (11a, 11b) are described. ) Is further provided, and the force sensor 31 also obtains the output “Fy_sen” in the y direction orthogonal to both the output “Fx_sen” in the x direction and the output “Fz_sen” in the z direction. It is easy to expand to three dimensions if it is equipped. Needless to say, such an extension is effective for all the embodiments of the present invention, but the following will describe the high-accuracy posture detection device 2e.
Specifically, the acceleration value that generates the motion calculated using the force sensor 31 is

Calculated by

As shown in Fig.
αa = αa_sen−αa_mot,
αb = αb_sen−αb_mot,
αc = αc_sen−αc_mot
After compensating the acceleration sensor,
Posture angle around c axis = -tan ^-1 (αb / αa)
Posture angle around b-axis = + tan ⁻¹ (αc / αa)
By such calculation, the posture of the body 3 in three dimensions can be detected with high accuracy. Furthermore, when it is desired to improve the reliability against noise, the corrected acceleration values (αa, αb, αc) of the acceleration sensor are processed using an existing Kalman filter, so that the posture of the body 3 in three dimensions can be highly accurate. Can be detected with high reliability.

  FIG. 14 is a diagram schematically showing the configuration of the high-accuracy posture detection device according to the sixth embodiment of the present invention. The posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2f. It is a figure explaining the structure of the apparatus of the Example to do.

  The high-accuracy posture detection device 2f of the sixth embodiment is composed of the inertia detection means 10 and the environment information means 30 like the high-precision posture detection device 2e of the fifth embodiment. An acceleration sensor (11a, 11b) and an acceleration sensor 11c (not shown) orthogonal to the acceleration sensor (11a, 11b) are provided. The difference from the high-accuracy posture detection device 2e of the fifth embodiment is that the environment information means 30 is composed of a GPS sensor 33 and a direction sensor 34, and at the same time, the environment information means 30 is used to provide an environment (star). ) Is stored.

  The latitude at which the moving body is located is detected by the GPS sensor 33 in the environment information means 30, and the direction in which the moving body is facing is detected by the direction sensor 34. Furthermore, the position where the moving body exists from the output of the GPS sensor 33 and the information on the rotation speed 25 and the radius 35 of the environment where the moving body exists (on the star: not only on the earth but on all stars including the moon and Mars). The centrifugal force at is sequentially calculated. In addition, coordinate conversion means 41 that performs coordinate conversion between the environment (floor) and the acceleration sensor is provided to perform necessary coordinate conversion. That is, the joint angle sensor 32 (not shown) such as an encoder or a potentiometer for detecting the joint angle of each joint, the joint angle command of each joint output from the motion generating means 20 (not shown), or the same Centrifugal force is sequentially calculated from the target posture angle of the body 3 and the output of the direction sensor 34 output from the motion generation means 20 (not shown), and necessary coordinate conversion is performed. The direction sensor 34 may be composed of two GPS sensors 33.

  FIG. 15 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2f shown in FIG. 14, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. The contents of the calculation process of the high-accuracy posture detection device 2f of the sixth embodiment are similar to the calculation processing of the high-precision posture detection device 2c. The difference is that the environment information means 30 is used instead of the motion generation means 20. Hereinafter, specific processing contents will be described.

  First, the centrifugal force (αx_cen2, αy_cen2, αz_cen2) at the position where the moving body is located from the output of the GPS sensor 33 and the information on the rotation speed 25 and the radius 35 which are known information of the environment (star) where the moving body is located. First, calculate. This centrifugal force (αx_cen2, αy_cen2, αz_cen2) is converted into a coordinate conversion means 41 from the posture angle between the sole 5 and the torso 3 calculated from the value of the joint angle sensor 32 and the azimuth angle detected by the azimuth sensor 34. Is converted into the same direction components (αb_cen2, αc_cen2, αa_cen2) as the acceleration sensors (11b, 11c, 11a). Based on the direction components (αb_cen2, αc_cen2, αa_cen2), the outputs (αb_sen, αc_sen, αa_sen) of the respective acceleration sensors (11b, 11c, 11a) are corrected to calculate corrected accelerations (αb, αc, αa). is doing. Finally, based on the corrected acceleration (αb, αc, αa) of the corrected acceleration signal, the posture of the moving body is detected with high accuracy.

In this case, first, based on the output “θ_gps” of the GPS sensor 33, the information on the “ω_rot” of the rotation speed 25 and the “r” of the radius 35 of the environment (star) where the moving body is located, Calculate the centrifugal force.

On the other hand, if the moving body is in an environment (star) that rotates, a centrifugal force component due to the rotation appears in the output of the acceleration sensors (11a, 11b, 11c). In order to calculate this influence, the coordinate conversion means 41 performs coordinate conversion from the posture angle between the sole 5 and the torso 3 calculated from the value of the joint angle sensor 32 and the azimuth angle detected by the azimuth sensor 34. The centrifugal force (αx_cen2, αy_cen2, αz_cen2) is converted into the same direction component (αb_cen2, αc_cen2, αa_cen2) as the acceleration sensor (11b, 11c, 11a) using “bca_R_xyz”.

Finally, the corrected accelerations (αa, αb, αc) of the corrected acceleration signals obtained by the arithmetic processing of the high-accuracy posture detecting device 2f of the sixth embodiment are respectively αa = αa_sen−αa_cen2
αb = αb_sen−αb_cen2
αc = αc_sen−αc_cen2
Thus, by removing the centrifugal force associated with the rotation of the environment (star) in which the moving body is present, the acceleration (αa, αb, αc) resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG. 15, specifically, the attitude angle around the c axis = −tan ⁻¹ (αb / αa)
Posture angle around b-axis = + tan ⁻¹ (αc / αa)
The posture angle is obtained by Thereby, even when the environment (star) in which the moving body is present rotates, and the centrifugal force component due to the rotation is mixed in the acceleration sensor output, the posture angle of the body 3 of the walking robot can be obtained with high accuracy. it can. If it is desired to further improve the reliability with respect to noise, the corrected acceleration sensor values (αa, αb, αc) are processed using an existing Kalman filter, so that the posture of the body 3 in three dimensions can be improved with high accuracy. It can be detected with high reliability.

  FIG. 16 is a diagram schematically showing the configuration of a high-accuracy posture detection device according to the seventh embodiment of the present invention. The posture angle of the body 3 of the walking robot is detected with high accuracy by the high-precision posture detection device 2g. It is a figure explaining the structure of the apparatus of the Example to do.

  The high accuracy posture detection device 2g according to the seventh embodiment is similar to the high accuracy posture detection device 2e according to the fifth embodiment and the high accuracy posture detection device 2f according to the sixth embodiment from the inertia detection means 10 and the environment information means 30. It is configured. The difference from the high-accuracy posture detection devices (2e, 2f) of these embodiments is that the inertia detection means 10 includes a rate gyro 12c and a rate gyro (12a, 12b) orthogonal to the rate gyro 12c; ) Is provided, the environment information means 30 is composed of a GPS sensor 33 and a direction sensor 34, and the environment information means 30 stores information on the rotation speed 25 of the environment (star) where the moving object is located. It is a point that has been. The GPS sensor 33 detects the latitude at which the moving body is located. Further, the direction sensor 34 detects the direction in which the moving body is facing. In addition, necessary coordinate conversion is performed by the coordinate conversion unit 41, and although not shown, the joint angle sensor 32 such as an encoder or potentiometer that detects the joint angle of each joint, and each joint output from the motion generation unit 20. The joint angle command, the target posture angle of the body 3 output from the motion generation means 20, and the rotational angular velocity obtained by coordinate conversion from the output of the azimuth sensor 34 are sequentially calculated. The direction sensor 34 may be composed of two GPS sensors 33.

  FIG. 17 is a block diagram showing the configuration of the arithmetic processing unit of the high-accuracy posture detection device 2g of FIG. 15, and the configuration of the device of the embodiment that detects the posture angle of the body 3 of the walking robot with high accuracy will be described. FIG. The calculation processing contents of the high accuracy posture detection device 2g are similar to the calculation processing of the high accuracy posture detection device 2d, and the difference is that the environment information means 30 is used instead of the motion generation means 20. Hereinafter, specific processing contents will be described.

  First, rotation angular velocities (ωx_rot2, ωy_rot2, ωz_rot2) of the rotation speed at the position where the moving body exists are calculated from the output of the GPS sensor 33 and the information about the rotation speed 25 which is known information of the environment (star) where the moving body exists. To do. The rotation angular velocity (ωx_rot2, ωy_rot2, ωz_rot2) at the position where the moving body is present is subjected to coordinate conversion by the coordinate conversion means 41, and the posture between the sole 5 and the torso 3 calculated from the value of the joint angle sensor 32 By the coordinate conversion from the angle and the azimuth angle detected by the azimuth sensor 34, it is converted into the same direction component (ωb_rot2, ωc_rot2, ωa_rot2) as the rate gyroscope (12b, 12c, 12a). Based on the direction components (ωb_rot2, ωc_rot2, ωa_rot2), the output (ωb_sen, ωc_sen, ωa_sen) of the rate gyroscope (12b, 12c, 12a) is corrected, and the corrected angular velocity (ωb, ωc, ωa) is calculated. Yes. Finally, based on the corrected angular velocity signals (ωb, ωc, ωa), the posture of the moving body is detected with high accuracy.

In this case, the rotation speed component at the position where the moving body exists is calculated from the output “θ_gps” of the GPS sensor 33 and the information of “ω_rot” of the rotation speed 25 of the environment (star) where the moving body exists.

On the other hand, if the mobile body is in an environment (star) that rotates, this rotation speed component appears in the output of the rate gyro (12a, 12b, 12c). In order to calculate this influence, the coordinate conversion means 41 performs coordinate conversion from the posture angle between the sole 5 and the torso 3 calculated from the value of the joint angle sensor 32 and the azimuth angle detected by the azimuth sensor 34. Using “bca_R_xyz”, the rotation speed components (ωx_ro2t, ωy_rot2, ωz_rot2) are converted into the same direction components (ωb_rot2, ωc_rot2, ωa_rot2) as the rate gyros (12b, 12c, 12a).

により姿勢角を求める。これにより、移動体が居る環境（星）が自転しており、レートジャイロ出力に自転による自転速度成分が混入するような場合においても、高精度に歩行ロボットの胴体３の姿勢角を求めることができる。なお、より正確に姿勢角を求めたい場合には、補正した角速度値（ωa，ωb，ωc）を既存のカルマンフィルタを用いて処理することにより、３次元での胴体３の姿勢をより正確で高精度に検出することができる。 Finally, the corrected angular velocity signals (ωb, ωc, ωa) obtained by the arithmetic processing of the high-accuracy posture detection device 2g are respectively ωa = ωa_sen−ωa_rot2
ωb = ωb_sen−ωb_rot2
ωc = ωc_sen−ωc_rot2
By removing the rotation speed component accompanying the rotation of the environment (star) in which the moving body is present, the angular velocities ωa, ωb, and ωc resulting from the posture of the body 3 of the walking robot can be obtained with high accuracy.
Therefore, the arithmetic processing of the arithmetic processing unit shown in FIG.

The posture angle is obtained by Thereby, even when the environment (star) in which the moving body is present rotates, and the rotation speed component due to the rotation is mixed in the rate gyro output, the posture angle of the body 3 of the walking robot can be obtained with high accuracy. it can. If it is desired to obtain the attitude angle more accurately, the corrected angular velocity values (ωa, ωb, ωc) are processed using an existing Kalman filter, so that the attitude of the torso 3 in three dimensions is more accurate and higher. It can be detected with accuracy.

The high-accuracy posture detection device of the present invention can detect postures with higher accuracy by combining the high-precision posture detection devices of the first to seventh embodiments described above.
Specifically, the corrected acceleration signal (αb, αc, αa) and the corrected angular velocity signal (ωb, ωc, ωa) are obtained as follows.

αa = αa_sen− {Wa1, αa_ref1
+ Wa2 · (αa_ref2−αa_grav) + (1−Wa1−Wa2) · αa_mot}
αb = αb_sen− {Wb1 ・ αb_ref1
+ Wb2 · (αb_ref2−αb_grav) + (1−Wb1−Wb2) · αb_mot}
αa = αa_sen− {Wc1 ・ αc_ref1
+ Wc2 · (αc_ref2−αc_grav) + (1−Wc1−Wc2) · αc_mot}

ωa = ωa_sen− {Va · ωa_rot1 + (1−Va) ωa_rot2}
ωb = ωa_sen− {Vb · ωb_rot1 + (1−Vb) ωb_rot2}
ωc = ωa_sen− {Vc · ωc_rot1 + (1−Vc) ωc_rot2}

However, Wa1, Wa2, Wb1, Wb2, Wc1, Wc2, Va, Vb, Vc are
It is a weighting coefficient used by selecting 2g from the high-accuracy posture detection device 2a.

  As a result, by removing the acceleration / deceleration component accompanying the movement of the moving body itself and the centrifugal force accompanying the rotation of the environment (star) where the moving body is present from the output of the acceleration sensor, the posture of the body 3 of the walking robot can be obtained with high accuracy. Acceleration (αa, αb, αc) resulting from can be obtained. Also, by removing the rotation speed component accompanying the rotation of the environment (star) in which the moving body is present from the output of the rate gyro sensor, the angular velocities (ωa, ωb, ωc) resulting from the posture of the body 3 of the walking robot with high accuracy are removed. ). By processing the corrected acceleration (αb, αc, αa) of the corrected acceleration signal and the corrected angular velocity (ωb, ωc, ωa) of the corrected angular velocity signal using an existing Kalman filter, the three-dimensional body 3 It is possible to detect the posture with high accuracy and reliability.

As described in detail above, posture detecting device of the mobile body according to the present invention, the movement if the mobile itself perform sudden acceleration or deceleration, if the centrifugal acceleration by pivotal movement acts on the movable body, the carriage accelerates or decelerates If the body is on board, in either case when the inertial acceleration of the moving object in the acceleration sensor itself is acting, a posture detecting device you accurately detected attitude angle, also moved This is a posture detection device that accurately detects a posture angle even when a moving body is in an environment (on the star) where the body rotates and the rotational angular velocity of the rotation affects the rate gyro sensor itself. For this reason, in order to perform stable control of the posture of the walking robot, which is a moving body, it can be suitably used when detecting the acceleration, posture rotation angular velocity, posture angle, and the like of the main body of the walking robot with high accuracy .

1 is a diagram schematically illustrating a configuration of a high-accuracy posture detection apparatus according to a first embodiment of the present invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2a. It is a 1st figure explaining operation | movement of the highly accurate attitude | position detection apparatus by this invention. It is a 2nd figure explaining operation | movement of the highly accurate attitude | position detection apparatus by this invention. It is a 3rd figure explaining operation | movement of the highly accurate attitude | position detection apparatus by this invention. It is a figure which shows schematically the structure of the highly accurate attitude | position detection apparatus of 2nd Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2b. It is a figure which shows schematically the structure of the highly accurate attitude | position detection apparatus of 3rd Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2c. It is a figure which shows schematically the structure of the highly accurate attitude | position detection apparatus of 4th Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2d. It is a figure which shows schematically the structure of the highly accurate attitude | position detection apparatus of 5th Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2e. It is a figure which shows schematically the structure of the highly accurate attitude | position detection apparatus of 6th Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the high precision attitude | position detection apparatus 2f. It is a figure which shows roughly the structure of the highly accurate attitude | position detection apparatus of 7th Example of this invention. It is a block diagram which shows the structure of the arithmetic processing part of the highly accurate attitude | position detection apparatus 2g.

Explanation of symbols

2a to 2g High accuracy posture detection device 3 Robot body 4a Single leg thigh link 4b Single leg leg link 5 Single leg sole 6a First joint motor 6b Second joint motor 6c Third joint motor 10 Inertial detection Means 11a Acceleration sensor 11b Acceleration sensor 11c Acceleration sensor 12a Rate gyro 12b Rate gyro 12c Rate gyro 20 Motion generation means 25 Rotational speed 30 Environment information means 32 Joint angle sensor 33 GPS sensor 34 Direction sensor 35 Radius 40 Coordinate conversion means 41 Coordinate conversion means

Claims (8)

The moving body includes an inertia detection unit that detects inertia of the moving body, an operation generation unit that generates an operation of the moving body, and an arithmetic processing unit that performs posture calculation of the moving body .
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the motion generation unit from the output of the inertia detection unit, and calculates the posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection unit includes at least two acceleration sensors so as to be orthogonal to each other, and outputs an acceleration target value of an operation output in at least the same direction component as the acceleration sensor from the output of the operation generation unit. ,
The arithmetic processing means corrects the output of the acceleration sensor of the inertia detection means by subtracting the same direction component of the acceleration target value of the motion generation means from the output of the acceleration sensor of the inertia detection means, and corrects one of the corrections. An apparatus for detecting a posture of a moving body that divides the output of the acceleration sensor by the output of the other corrected acceleration sensor and detects the posture of the moving body by performing an arctan calculation on the division result. The moving body includes an inertia detection unit that detects inertia of the moving body, an operation generation unit that generates an operation of the moving body, and an arithmetic processing unit that performs posture calculation of the moving body .
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the motion generation unit from the output of the inertia detection unit, and calculates the posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection means includes at least two acceleration sensors so as to be orthogonal to each other, and the centrifugal force due to the rotation of the environment in which there is a moving body output from the output of the motion generation means with at least the same direction component as the acceleration sensor. Is output,
The arithmetic processing unit corrects the output of the acceleration sensor of the inertia detection unit by subtracting the same direction components of the centrifugal force of the motion generation unit from the output of the acceleration sensor of the inertia detection unit, and corrects one of the corrections. The acceleration sensor output is divided by the other corrected acceleration sensor output, and this division result is subjected to arctan calculation to correct the acceleration sensor output of the inertia detection means based on the centrifugal force for detecting the posture of the moving body, and the correction is made. A moving body posture detection apparatus that detects the posture of a moving body based on an output of an acceleration sensor. The moving body includes an inertia detection unit that detects inertia of the moving body, an operation generation unit that generates an operation of the moving body, and an arithmetic processing unit that performs posture calculation of the moving body .
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the motion generation unit from the output of the inertia detection unit, and calculates the posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection means is provided with a rate gyro, and the rotation speed of the environment in which there is a moving body that is output at least in the same direction component as the rate gyro is output from the output of the motion generation means,
The arithmetic processing means corrects the rate gyro output of the inertia detection means by subtracting the rotation speed of the motion generation means from the rate gyro output of the inertia detection means, and integrates the corrected rate gyro output. An apparatus for detecting a posture of a moving body, wherein the posture of the moving body is detected. The moving body is provided with inertia detecting means for detecting the inertia of the moving body, environment information means for detecting and storing information on the environment in which the moving body is present, and arithmetic processing means for calculating the posture of the moving body ,
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the environment information unit from the output of the inertia detection unit, and calculates a posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection means includes at least two acceleration sensors so as to be orthogonal to each other,
The environment information means includes at least a force sensor that detects a force acting between the moving body and the environment, and a coordinate conversion means that performs coordinate conversion between the force sensor and the acceleration sensor. Remembers the value of the known gravitational acceleration of the environment you are in,
From the force sensor, the horizontal component and the vertical component of the force acting between the moving body and the environment are output or only the horizontal component of the force acting between the moving body and the environment is output,
When only the horizontal component of the force acting between the moving body and the environment is output from the force sensor, the environmental information means also stores the value of the nominal weight of the moving body,
The arithmetic processing means divides a horizontal component of a force acting between the mobile body and the environment by a vertical component of a force acting between the mobile body and the environment, or between the mobile body and the environment. The horizontal component of the force acting on the mobile body is divided by the value of the nominal weight of the moving body, and the result of multiplication is multiplied by the known gravitational acceleration to calculate the acceleration value that generates the movement of the moving body, and the calculation is obtained. The acceleration value that generates the motion of the moving body is converted into the acceleration value that generates the motion of the moving body that is output in the same direction component as the acceleration sensor by the coordinate conversion means, and the output of the acceleration sensor of the inertia detection means By subtracting the same direction components of the converted acceleration values, the output of the acceleration sensor of the inertia detection means is corrected, and the output of the corrected acceleration sensor on one side is corrected with the corrected acceleration sensor on the other side. Of performed divided by output, the division result arctan operation on the moving body posture detecting device and detects the posture of the moving body. The moving body is provided with inertia detecting means for detecting the inertia of the moving body, environment information means for detecting and storing information on the environment in which the moving body is present, and arithmetic processing means for calculating the posture of the moving body ,
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the environment information unit from the output of the inertia detection unit, and calculates a posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection means includes at least two acceleration sensors so as to be orthogonal to each other,
The environment information means includes at least a GPS sensor that detects the latitude of the moving body, and coordinate conversion means that performs coordinate conversion between the position where the moving body is located and the acceleration sensor, and the rotation speed of the environment where the moving body exists. And remember the radius,
The calculation processing means calculates the centrifugal force at the position where the moving body is present using the output of the GPS sensor, the rotation speed, and the radius, and calculates the centrifugal force at the position where the moving body is obtained by the calculation as the coordinates. The centrifugal force due to the rotation at the position where the moving body is converted from the output of the acceleration sensor of the inertia detection means, which is converted by the converting means into the centrifugal force due to the rotation at the position where the moving body has the same direction component as the acceleration sensor. The output of the acceleration sensor of the inertia detection means is corrected by subtracting the same direction components of each other, and the output of the corrected acceleration sensor on one side is divided by the output of the corrected acceleration sensor on the other side, and this division result is arctan. An apparatus for detecting a posture of a moving body, wherein the posture of the moving body is detected by calculation. The moving body is provided with inertia detecting means for detecting the inertia of the moving body, environment information means for detecting and storing information on the environment in which the moving body is present, and arithmetic processing means for calculating the posture of the moving body ,
The calculation processing unit corrects the output of the inertia detection unit by subtracting the output of the environment information unit from the output of the inertia detection unit, and calculates a posture based on the corrected output of the inertia detection unit. In the mobile body posture detection device that performs and detects the posture of the mobile body,
The inertia detection means includes a rate gyro,
The environment information means includes at least a GPS sensor for detecting the latitude of the moving body, coordinate conversion means for performing coordinate conversion between the position where the moving body is located and the rate gyro, and the rotation speed of the environment where the moving body is located Remember
The arithmetic processing means calculates a rotation angular velocity of a rotation speed at a position where the moving body is present from a rotation speed of an environment where the GPS sensor and the moving body are present, and calculates a rotation angular velocity at the position where the moving body exists. Is converted into a rotational angular velocity output in the same direction component as the rate gyro by the coordinate conversion means, and a rotational angular velocity output in the same direction component as the converted rate gyro is output from the rate gyro output of the inertia detection means. An apparatus for detecting a posture of a moving body which corrects the output of the rate gyro of the inertia detection means by subtraction and detects the attitude of the moving body by integrating the output of the corrected rate gyro. A plurality of output values of the moving body posture detecting device according to any one of claim 1 to 6 by adding by adding a weighting factor, of a moving body and detects the posture of the moving body Attitude detection device. Mobile robot posture detection device is mounted in the moving body according to any one of claims 1-7.

Images