JP7619073B2 - Attitude estimation method, attitude estimation device, and movable device - Google Patents

Description

The present invention relates to a posture estimation method, a posture estimation device, and a movable device.

There are known devices and systems that attach an inertial measurement unit (IMU) to an object and use the output signal of the inertial measurement unit to calculate the position and attitude of the object. The output signal of the inertial measurement unit has bias errors, which also cause errors in attitude calculations, so a method has been proposed that uses a Kalman filter to correct these errors and estimate the accurate attitude of the object. For example, Patent Document 1 describes an attitude estimation method that corrects predicted attitude information of an object based on error information when an angular velocity sensor exceeds the effective measurement range, and estimates the attitude of the object.

JP 2020-20631 A

However, inertial measurement units equipped with angular velocity sensors and acceleration sensors have different directions for the X-axis, Y-axis, and Z-axis depending on the mounting position as the target object moves, and bias errors occur in each direction from the initial state, which reduces the accuracy of estimating the target object's posture and makes it impossible to perform high-precision measurements.

The posture estimation method includes the steps of measuring the bias value BW and variance value PWW of the angular velocity sensor and the bias value BA and variance value PVV of the acceleration sensor during a predetermined movement of the object and storing them in a memory unit; reading the bias value BW, the variance value PWW, the bias value BA, and the variance value PVV from the memory unit at the time of reset and setting them to initial settings; measuring the angular velocity and acceleration while the angular velocity sensor and the acceleration sensor are stationary; updating the bias value BW and the variance value PWW of the angular velocity sensor and the variance value PVV of the acceleration sensor from the initial settings; and estimating the posture of the object using a Kalman filter from the output of the angular velocity sensor and the output of the acceleration sensor.

The posture estimation device has a memory unit that stores the bias value BW and variance value PWW of the angular velocity sensor and the bias value BA and variance value PVV of the acceleration sensor during a specified movement of the object, and a processing unit that estimates the posture of the object using a Kalman filter from the output of the angular velocity sensor and the output of the acceleration sensor.

The movable device includes the posture estimation device described above and a control device that controls the posture of the object based on posture information of the object estimated by the posture estimation device.

FIG. 1 is a diagram showing an example of the configuration of a posture estimation device according to a first embodiment. FIG. 1 is a diagram for explaining a sensor coordinate system and a local coordinate system. FIG. 4 is a flowchart showing an example of a procedure of a posture estimation method according to the first embodiment. FIG. 11 is a flowchart showing an example of a procedure of a posture estimation method according to the second embodiment. FIG. 13 is a diagram showing an example of the configuration of a movable device including a posture estimation device according to a third embodiment. FIG. 2 is a block diagram showing a configuration example of a movable device.

1. First Embodiment 1.1. Posture Estimation Apparatus First, a posture estimation apparatus 1 according to a first embodiment will be described with reference to FIGS. 1 and 2. FIG.
1, the attitude estimation device 1 of this embodiment includes a processing unit 20, a ROM 30 as a storage unit, a RAM 40, a recording medium 50, and a communication unit 60, and estimates the attitude of a target object based on the output of an inertial measurement unit (IMU) 10. Note that the attitude estimation device 1 of this embodiment may have a configuration in which some of these elements are changed or deleted, or other elements are added.

In this embodiment, as shown in FIG. 1, the attitude estimation device 1 is separate from the inertial measurement unit 10, but the attitude estimation device 1 may also include the inertial measurement unit 10. Also, the inertial measurement unit 10 and the attitude estimation device 1 may be configured to be housed in a single housing, or the inertial measurement unit 10 may be configured to be separate or separable from the main body that houses the attitude estimation device 1. In the former case, the attitude estimation device 1 is attached to the target object, and in the latter case, the inertial measurement unit 10 is attached to the target object.

In this embodiment, the inertial measurement unit 10 includes an angular velocity sensor 12, an acceleration sensor 14, and a signal processing unit 16. However, the inertial measurement unit 10 of this embodiment may have some of these elements modified or removed, or other elements added.

The angular velocity sensor 12 measures the angular velocity in each of three mutually intersecting, ideally orthogonal, axial directions, and outputs an analog signal corresponding to the magnitude and direction of the measured angular velocity along the three axes.

The acceleration sensor 14 measures the acceleration in three mutually intersecting, ideally perpendicular, directions, and outputs an analog signal corresponding to the magnitude and direction of the measured three-axis acceleration.

The signal processing unit 16 samples the output signal of the angular velocity sensor 12 at a predetermined sampling interval Δt and converts it into angular velocity data dω, which is a digital value. The signal processing unit 16 also samples the output signal of the acceleration sensor 14 at a predetermined sampling interval Δt and converts it into acceleration data dα, which is a digital value.

Ideally, the angular velocity sensor 12 and the acceleration sensor 14 are attached to the inertial measurement unit 10 so that their three axes coincide with the three axes (x-axis, y-axis, z-axis) of the sensor coordinate system, which is an orthogonal coordinate system defined for the inertial measurement unit 10; however, in reality, an error in the mounting angle occurs. Therefore, the signal processing unit 16 also performs a process of converting the angular velocity data dω and the acceleration data dα into data in the xyz coordinate system using correction parameters calculated in advance according to the mounting angle error. Furthermore, the signal processing unit 16 also performs a process of temperature correction of the angular velocity data dω and the acceleration data dα according to the temperature characteristics of the angular velocity sensor 12 and the acceleration sensor 14.

The angular velocity sensor 12 and the acceleration sensor 14 may also have A/D conversion and temperature compensation functions built in.

The inertial measurement unit 10 outputs the angular velocity data dω and acceleration data dα after processing by the signal processing unit 16 to the processing unit 20 of the posture estimation device 1.

The ROM 30 stores programs for the processing unit 20 to perform various processes, and various programs and data for implementing application functions. The ROM 30 also stores the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 when the inertial measurement unit 10 is placed in an attachment position associated with a predetermined motion of the object. The predetermined motion is the most frequently performed motion of the object. The predetermined motion may also be multiple motions. In this case, the ROM 30 stores the bias values BW, BA and variance values PWW, PVV according to the multiple motions, i.e., the type of motion.

RAM 40 is used as a working area for the processing unit 20, and is a storage unit that temporarily stores programs and data read from ROM 30, the results of calculations performed by the processing unit 20 according to various programs, etc.

The recording medium 50 is a non-volatile storage unit that stores data that needs to be stored long-term among the data generated by the processing unit 20. The recording medium 50 may also store programs for the processing unit 20 to perform various processes, various programs and data for implementing application functions, etc.

The processing unit 20 performs various processes according to a program stored in the ROM 30 or the recording medium 50, or a program received from a server via a network and stored in the RAM 40 or the recording medium 50. In particular, in this embodiment, the processing unit 20 executes the program to function as a bias removal unit 22, a posture change amount calculation unit 24, a velocity change amount calculation unit 26, and a posture estimation unit 28, and performs a predetermined calculation on the angular velocity data dω and the acceleration data dα output by the inertial measurement unit 10 at Δt intervals to estimate the posture of the target object. In addition, when the processing unit 20 receives a reset instruction from the user, it reads out the bias value BW and variance value PWW of the angular velocity sensor 12 from the ROM 30, and the bias value BA and variance value PVV of the acceleration sensor 14, and sets them as initial settings. Note that if the predetermined motion is a plurality of motions, the processing unit 20 reads out the bias values BW, BA and variance values PWW, PVV corresponding to the motion selected by the user from the plurality of motions from the ROM 30, and sets them as initial settings.

In this embodiment, as shown in FIG. 2, a sensor coordinate system, which is the coordinate system of the inertial measurement unit 10, for example, an xyz coordinate system consisting of mutually orthogonal x-, y-, and z-axes, and a local space coordinate system, which is the coordinate system of the space in which the object exists, for example, an XYZ coordinate system consisting of mutually orthogonal x-, y-, and z-axes, are considered. The processing unit 20 estimates the orientation of the object in the local space coordinate system from the three-axis angular velocity and three-axis acceleration of the sensor coordinate system output from the inertial measurement unit 10 attached to the object. The orientation of the object can also be said to be the orientation of the inertial measurement unit 10.

The bias removal unit 22 reads out the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 from the ROM 30, and performs a process of calculating the three-axis angular velocity with the bias error removed from the output of the angular velocity sensor 12 and a process of calculating the three-axis acceleration with the bias error removed from the output of the acceleration sensor 14, using these as the initial values of the bias error.

The posture change amount calculation unit 24 calculates the posture change amount of the target object based on the output of the angular velocity sensor 12. Specifically, the posture change amount calculation unit 24 performs processing to calculate the posture change amount of the target object by approximating the three-axis angular velocities from which the bias error has been removed by the bias removal unit 22 with a polynomial whose variable is the sampling interval Δt.

The velocity change amount calculation unit 26 calculates the velocity change amount of the object based on the output of the angular velocity sensor 12 and the output of the acceleration sensor 14. Specifically, the velocity change amount calculation unit 26 performs processing to calculate the velocity change amount of the object using the three-axis angular velocity and three-axis acceleration from which the bias error has been removed by the bias removal unit 22.

Attitude estimation unit 28 functions as an integral calculation unit 101, an attitude information prediction unit 102, an error information update unit 103, a correction coefficient calculation unit 104, an attitude information correction unit 105, a normalization unit 106, an error information correction unit 107, a rotation error component removal unit 108, a bias error limiting unit 109, and an error information adjustment unit 110. Then, attitude estimation unit 28 performs a process of estimating the attitude of the target object using the attitude change amount calculated by attitude change amount calculation unit 24 and the velocity change amount calculated by velocity change amount calculation unit 26. In practice, attitude estimation unit 28 performs a process of estimating state vector x and its error covariance matrix Σ _x ² by an extended Kalman filter.

The integral calculation unit 101 performs integral processing to integrate the posture change amount calculated by the posture change amount calculation unit 24 with the estimate of the immediately preceding posture corrected by the posture information correction unit 105 and normalized by the normalization unit 106. The integral calculation unit 101 also performs integral processing to integrate the velocity change amount calculated by the velocity change amount calculation unit 26 with the estimate of the immediately preceding velocity corrected by the posture information correction unit 105 and normalized by the normalization unit 106.

The posture information prediction unit 102 performs a process of predicting a posture quaternion q, which is posture information of the target object, using the posture change amount calculated by the posture change amount calculation unit 24. The posture information prediction unit 102 also performs a process of predicting a motion velocity vector v, which is velocity information of the target object, based on the velocity change amount calculated by the velocity change amount calculation unit 26. In practice, the posture information prediction unit 102 performs a process of predicting a state vector x that includes the posture quaternion q and the motion velocity vector v as elements.

The error information updating unit 103 performs processing to update the error covariance matrix Σ _x ² , which is error information, based on the output of the angular velocity sensor 12. Specifically, the error information updating unit 103 performs processing to update the attitude error of the target object by using the three-axis angular velocities from which the bias error has been removed by the bias removing unit 22. In practice, the error information updating unit 103 performs processing to update the error covariance matrix Σ _x ² by an extended Kalman filter.

The rotation error component removal unit 108 performs a process of removing rotation error components around the reference vector in the error covariance matrix Σ ² that is error information. Specifically, the rotation error component removal unit 108 performs a process of removing the azimuth error components included in the attitude error in the error covariance matrix Σ _x ² updated by the error information update unit 103. In practice, the rotation error component removal unit 108 performs a process of generating an error covariance matrix Σ _x ² by limiting the rank of the error covariance matrix Σ _q ² of the attitude and removing the azimuth error components from the error covariance matrix Σ _x ² .

The error information adjustment unit 110 determines whether the output of the angular velocity sensor 12 is within the valid range, and if it determines that the output of the angular velocity sensor 12 is not within the valid range, it increases the attitude error component in the error covariance matrix Σ _x ² which is the error information, and reduces, for example, zeroes the correlation component between the attitude error component and the error component other than the attitude error component in the error covariance matrix Σ _x ^2. The error information adjustment unit 110 also determines whether the output of the acceleration sensor 14 is within the valid range, and if it determines that the output of the angular velocity sensor 12 or the output of the acceleration sensor 14 is not within the valid range, it increases the motion velocity error component in the error covariance matrix Σ x _,k ² , and reduces, for example, zeroes the correlation component between the motion velocity error component and the error component other than the motion velocity error component in the error covariance matrix Σ _x,k ² . Specifically, during an angular velocity off-scale recovery period after it is determined that the output of the angular velocity sensor 12 is not within the valid range, the error information adjustment unit 110 increases the attitude error components and motion velocity error components in the error covariance matrix Σ _x ² generated by the rotation error component removal unit 108, and reduces, for example, to zero, the correlation components between the attitude error components and error components other than the attitude error components and the correlation components between the motion velocity error components and error components other than the motion velocity error components. Also, during an acceleration off-scale recovery period after it is determined that the output of the angular velocity sensor 12 is within the valid range and the output of the acceleration sensor 14 is not within the valid range, the error information adjustment unit 110 increases the motion velocity error components in the error covariance matrix Σ _x ² generated by the rotation error component removal unit 108, and reduces, for example, to zero, the correlation components between the motion velocity error components and error components other than the motion velocity error components.

The bias error limiting unit 109 performs processing to limit the bias error component of the angular velocity around the reference vector in the error covariance matrix Σ _x ² , which is error information. Specifically, the bias error limiting unit 109 performs processing to limit the vertical component of the bias error of the angular velocity in the error covariance matrix Σ _x ² generated by the error information adjusting unit 110. In practice, the bias error limiting unit 109 determines whether or not the vertical component of the bias error of the angular velocity exceeds an upper limit, and if the vertical component exceeds the upper limit, performs processing to generate an error covariance matrix Σ _x ² in which the vertical component is limited to the upper limit.

The correction coefficient calculation unit 104 performs a process of calculating a correction coefficient that determines the amount of correction of the attitude quaternion q, which is the attitude information of the target object, and the motion velocity vector v, which is the velocity information, by the attitude information correction unit 105, and the amount of correction of the error covariance matrix Σ _x ^, which is the error information, by the error information correction unit 107, based on the error covariance matrix Σ _{x 2,} which is the error information generated by the bias error limiting unit 109. In practice, the correction coefficient calculation unit 104 performs a process of calculating the observation residual Δz, the Kalman coefficient K, and the transformation matrix H.

The attitude information correction unit 105 performs a process of correcting the attitude quaternion q, which is the attitude information of the object predicted by the attitude information prediction unit 102, based on the error covariance matrix Σ _x, which is the error information. Specifically, the attitude information correction unit 105 performs a process of correcting the attitude quaternion q using the error covariance matrix Σ _x generated by the bias error limiting unit 109, and the Kalman coefficient K and the observation residual error Δz _a of the gravitational acceleration calculated by the correction coefficient calculation unit 104 based on the gravitational acceleration vector g, which is the reference vector, and the acceleration vector α obtained from the output of the acceleration sensor 14. In practice, the attitude information correction unit 105 performs a process of correcting the state vector x predicted by the attitude information prediction unit 102, using an extended Kalman filter.

The normalization unit 106 performs a process of normalizing the orientation quaternion q, which is the orientation information of the target object corrected by the orientation information correction unit 105, so that its magnitude does not change. In practice, the normalization unit 106 performs a process of normalizing the state vector x corrected by the orientation information correction unit 105.

The error information correction unit 107 performs a process of correcting the error covariance matrix _Σx, which is error information. Specifically, the error information correction unit 107 performs a process of correcting the error covariance matrix Σx generated by the bias error limiting unit 109 by an extended Kalman filter, using the transformation matrix H and Kalman coefficient _K calculated by the correction coefficient calculation unit 104.

The orientation quaternion q, which is the orientation information of the object estimated by the processing unit 20, can be transmitted to other devices via the communication unit 60.

As described above, the posture estimation device 1 of this embodiment can estimate the posture of the object using a Kalman filter from the output of the angular velocity sensor 12 and the output of the acceleration sensor 14 based on the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 during a specific movement of the stored object. Therefore, it is possible to correct bias errors in the initial state that arise due to differences in the mounting position associated with a specific movement of the object, and therefore it is possible to measure the posture of the object with high accuracy.

1.2 Posture Estimation Method Next, a posture estimation method of the posture estimation device 1 according to the first embodiment will be described with reference to FIG.

As shown in FIG. 3, the posture estimation method of the posture estimation device 1 according to this embodiment includes a measurement process, a storage process, an initial value setting process, an angular velocity and acceleration measurement process, an initial value update process, and a posture estimation process.

1.2.1. Measurement process First, in step S101, the inertial measurement unit 10 is installed at an attachment position associated with one of the most frequently performed predetermined movements of the object, and the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 are measured.

1.2.2. Storage Step In step S102, the processing unit 20 stores the measured bias value BW and variance value PWW of the angular velocity sensor 12, and the measured bias value BA and variance value PVV of the acceleration sensor 14 in the ROM 30 serving as a storage unit.

1.2.3. Initial Value Setting Process In step S103, upon receiving a reset instruction from the user, the processing unit 20 reads out from the ROM 30 the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14, and sets them as initial bias error values.

In step S104, the angular velocity and acceleration are measured while the angular velocity sensor 12 and the acceleration sensor 14 are in a stationary state. The measurement time is 200 msec.

In step S105, the processing unit 20 adds the bias value BW and variance value PWW of the angular velocity sensor 12 to the measured angular velocity data dω to update the bias error from the initial setting value. Also, the processing unit 20 adds the variance value PVV of the acceleration sensor 14 to the acceleration data dα to update the bias error from the initial setting value. Note that the bias value BA of the acceleration sensor 14 is not updated from the initial setting value.

In step S106, the processing unit 20 estimates the attitude of the object using a Kalman filter from the output of the angular velocity sensor 12 and the output of the acceleration sensor 14, based on the updated bias value BW, variance value PWW, and variance value PVV, and the non-updated bias value BA. The attitude quaternion q, which is the estimated attitude information of the object, is transmitted to another device via the communication unit 60.

As described above, the posture estimation method of this embodiment can estimate the posture of the object using a Kalman filter from the output of the angular velocity sensor 12 and the output of the acceleration sensor 14 based on the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 during a specific movement of the stored object. Therefore, it is possible to correct bias errors in the initial state that arise due to differences in the mounting position associated with a specific movement of the object, and therefore it is possible to measure the posture of the object with high accuracy.

2. Second Embodiment 2.1. Posture Estimation Method Next, a posture estimation method according to a second embodiment will be described with reference to FIG.

The posture estimation method of this embodiment is similar to the posture estimation method of the first embodiment, except that the predetermined motion of the target object is a plurality of motions, and the ROM 30 stores the corresponding bias values BW, BA and variance values PWW, PVV according to the type of motion. Note that the following description will focus on the differences from the first embodiment described above, and a description of similar matters will be omitted.

The posture estimation method according to the second embodiment will be described with reference to FIG. 4.

As shown in FIG. 4, the posture estimation method according to this embodiment includes a measurement process, a storage process, an initial value selection process, an initial value setting process, an angular velocity and acceleration measurement process, an initial value update process, and a posture estimation process.

2.1.1. Measurement Process First, in step S201, the inertial measurement unit 10 is installed at an attachment position associated with a first movement of the object, and the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 are measured.

In step S202, the measured bias value BW and variance value PWW of the angular velocity sensor 12 and the measured bias value BA and variance value PVV of the acceleration sensor 14 are stored in the ROM 30 as the bias values BW, BA and variance values PWW, PVV of the first operation.

Next, the inertial measurement unit 10 is placed at a mounting position associated with the second motion of the object, and steps S201 and S202 are repeated. Steps S201 and S202 are repeated for multiple motions of the object, i.e., for each type of motion.

2.1.3. Initial Value Selection Step After acquiring the bias values BW, BA and variance values PWW, PVV of the sensors 12, 14 corresponding to a plurality of motions of the object, in step S203, the processing unit 20 receives an instruction to select a motion from the user.

In step S204, upon receiving a reset instruction from the user, the processing unit 20 reads out from the ROM 30 the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 associated with the selected operation, and sets these as initial bias error values.

2.1.5. Angular Velocity and Acceleration Measurement Step S205 is the same as step S104 in the first embodiment, and therefore a description thereof will be omitted.

2.1.6. Initial Setting Value Update Process Step S206 is the same as step S105 in the first embodiment, and therefore a description thereof will be omitted.

2.1.7. Posture Estimation Step Step S207 is the same as step S106 in the first embodiment, and therefore a description thereof will be omitted.

With this configuration, even if the inertial measurement unit 10 is installed at a mounting position corresponding to multiple movements of the object, the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 can be selected according to the type of movement, so that the bias error in the initial state can be corrected and the posture of the object can be measured with high accuracy.

3. Third embodiment 3.1. Movable device Next, a movable device 600 including a posture estimation device 1 according to a third embodiment will be described with reference to FIGS. 5 and 6.

The posture estimation device 1 of the above embodiment can be effectively used in posture control of construction machinery, and Figures 5 and 6 show a hydraulic excavator, which is an example of construction machinery, as the movable device 600.

As shown in FIG. 5, the movable device 600 is configured with a vehicle body including a lower running body 612 and an upper rotating body 611 rotatably mounted on the lower running body 612, and a working mechanism 620 composed of multiple members that can rotate in the vertical direction is provided on the front side of the upper rotating body 611. A driver's seat (not shown) is provided on the upper rotating body 611, and an operating device (not shown) for operating each member that constitutes the working mechanism 620 is provided on the driver's seat. An inertial measurement unit 10d that functions as an inclination sensor that detects the inclination angle of the upper rotating body 611 is also provided on the upper rotating body 611.

The working mechanism 620 includes, as multiple components, a boom 613 attached to the front side of the upper rotating body 611 so as to be movable up and down, an arm 614 attached to the tip side of the boom 613 so as to be movable up and down, a bucket link 616 rotatably attached to the tip side of the arm 614, a bucket 615 rotatably attached to the tip sides of the arm 614 and the bucket link 616, a boom cylinder 617 that drives the boom 613, an arm cylinder 618 that drives the arm 614, and a bucket cylinder 619 that drives the bucket 615 via the bucket link 616.

The base end of the boom 613 is supported by the upper rotating body 611 so that it can rotate up and down, and the boom 613 is rotated relative to the upper rotating body 611 by the extension and contraction of the boom cylinder 617. An inertial measurement unit 10c is disposed on the boom 613 and functions as an inertial sensor that detects the state of movement of the boom 613.

One end of an arm 614 is rotatably supported at the tip of the boom 613, and the arm 614 is rotated relative to the boom 613 by the extension and contraction of an arm cylinder 618. An inertial measurement unit 10b is disposed on the arm 614, functioning as an inertial sensor that detects the state of movement of the arm 614.

A bucket link 616 and a bucket 615 are rotatably supported at the tip of the arm 614, and the bucket link 616 is rotated relative to the arm 614 in response to the expansion and contraction of the bucket cylinder 619, and in conjunction with this, the bucket 615 is rotated relative to the arm 614. An inertial measurement unit 10a that functions as an inertial sensor that detects the state of movement of the bucket link 616 is disposed on the bucket link 616.

The inertial measurement units 10a, 10b, 10c, and 10d can detect at least one of the angular velocity and acceleration acting on each member of the working mechanism 620 and the upper rotating body 611. In addition, the inertial measurement units 10a, 10b, 10c, and 10d can correct bias errors in the initial state because the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 are actually measured before installation according to the installation positions of the upper rotating body 611, boom 613, arm 614, and bucket 615, which operate differently. In addition, the inertial measurement units 10a, 10b, and 10c are connected in series as shown in FIG. 6, and can transmit detection signals to the calculation device 630. In this way, by connecting the inertial measurement units 10a, 10b, and 10c in series, the number of wirings for transmitting detection signals within the movable area can be reduced, and a compact wiring structure can be obtained. The compact wiring structure makes it easier to select the wiring installation method, and reduces the occurrence of deterioration and damage to the wiring.

Furthermore, the movable device 600 is provided with a calculation device 630 that calculates the tilt angle of the upper rotating body 611 and the position and attitude of the boom 613, arm 614, and bucket 615 that constitute the working mechanism 620, as shown in FIG. 5. As shown in FIG. 6, the calculation device 630 includes the attitude estimation device 1 of the above embodiment and a control device 632. The attitude estimation device 1 estimates attitude information of the movable device 600 based on the output signals of the inertial measurement units 10a, 10b, 10c, and 10d. The control device 632 controls the attitude of the movable device 600 based on the attitude information of the movable device 600 estimated by the attitude estimation device 1. Specifically, the calculation device 630 inputs various detection signals from each inertial measurement unit 10a, 10b, 10c, and 10d, and calculates the position, attitude, or attitude angle of the boom 613, arm 614, and bucket 615 and the tilt state of the upper rotating body 611 based on the various detection signals. The calculated position and attitude signals including the attitude angles of the boom 613, arm 614, and bucket 615, and the tilt signal including the attitude angle of the upper rotating body 611, for example the position and attitude signal of the bucket 615, are used for display on a monitor device in the driver's seat (not shown), or as feedback information for controlling the operation of the working mechanism 620 and the upper rotating body 611.

In addition, examples of construction machinery (construction machines) that can be used with the posture estimation device 1 of the above embodiment include, in addition to the hydraulic excavators (backhoes, backhoes, and power shovels) exemplified above, rough terrain cranes (crane trucks), bulldozers, excavators/loaders, wheel loaders, and high-altitude work vehicles (lift trucks).

According to this embodiment, the attitude estimation device 1 can obtain attitude information with high accuracy, thereby realizing appropriate attitude control of the movable device 600. In addition, according to the movable device 600, the inertial measurement units 10a, 10b, 10c, and 10d having the bias value BW and variance value PWW of the angular velocity sensor 12 and the bias value BA and variance value PVV of the acceleration sensor 14 according to the mounting position are attached to the upper rotating body 611, boom 613, arm 614, and bucket 615, which perform different operations, so that the bias error in the initial state can be corrected, and the attitude of the target object can be measured with high accuracy.

In this embodiment, four-wheeled automobiles such as agricultural machinery (farm machinery) and construction machinery (construction machinery) are used as examples of movable devices in which the posture estimation device 1 is used. In addition to these, motorcycles, bicycles, trains, airplanes, bipedal robots, remote-controlled or autonomous flying objects such as radio-controlled airplanes, radio-controlled helicopters, and drones, rockets, artificial satellites, ships, and AGVs (automated guided vehicles) can also be used. The above-described predetermined motion may be the lifting motion of the bucket 615, the lowering motion of the bucket 615, or rotation or movement. The above-described multiple motions may be any two of the lifting motion of the bucket 615, the lowering motion of the bucket 615, rotation or movement, etc. The predetermined motion may also be the predetermined motion of a robot arm or a drone. Furthermore, the predetermined motion may be the walking motion of a bipedal robot. In each of the inertial measurement units 10a, 10b, 10c, and 10d, the bias value BW and the variance value PWW of the angular velocity sensor 12 may be different values or may be the same values for each of the inertial measurement units 10a, 10b, 10c, and 10d. The movable device 600 has four inertial measurement units 10a, 10b, 10c, and 10d, but may have two, three, or five or more.

The present invention is not limited to this embodiment, and various modifications are possible within the scope of the present invention.

The above-described embodiment and modifications are merely examples, and the present invention is not limited to these. For example, the embodiments and modifications can be appropriately combined.
The present invention includes configurations that are substantially the same as the configurations described in the embodiments, for example, configurations with the same functions, methods, and results, or configurations with the same purpose and effects. The present invention also includes configurations in which non-essential parts of the configurations described in the embodiments are replaced. The present invention also includes configurations that achieve the same effects as the configurations described in the embodiments, or configurations that can achieve the same purpose. The present invention also includes configurations in which publicly known technology is added to the configurations described in the embodiments.

1...attitude estimation device, 10...inertial measurement unit, 12...angular velocity sensor, 14...acceleration sensor, 16...signal processing unit, 20...processing unit, 22...bias removal unit, 24...attitude change amount calculation unit, 26...velocity change amount calculation unit, 28...attitude estimation unit, 30...ROM as storage unit, 40...RAM, 50...recording medium, 60...communication unit, 101...integral calculation unit, 102...attitude information prediction unit, 103...error information update unit, 104...correction coefficient calculation unit, 105...attitude information correction unit, 106...normalization unit, 10 7...Error information correction unit, 108...Rotation error component removal unit, 109...Bias error limiting unit, 110...Error information adjustment unit, 600...Movable device, 611...Upper rotating body, 612...Lower traveling body, 613...Boom, 614...Arm, 615...Bucket, 616...Bucket link, 617...Boom cylinder, 618...Arm cylinder, 619...Bucket cylinder, 620...Work mechanism, 630...Calculation unit, 632...Control unit, BA, BW...Bias value, PVV, PWW...Dispersion value.

Claims (4)

A bias value BW and a variance value PWW of the angular velocity sensor in a given motion of the object;
measuring a bias value BA and a variance value PVV of the acceleration sensor and storing them in a storage unit;
At the time of reset, the bias value BW, the variance value PWW, the bias value BA,
A step of reading out the variance value PVV from the storage unit and setting it as an initial setting value;
measuring an angular velocity and an acceleration while the angular velocity sensor and the acceleration sensor are in a stationary state;
updating the bias value BW and the variance value PWW of the angular velocity sensor and the variance value PVV of the acceleration sensor from the initial setting values;
estimating a posture of the object using a Kalman filter from the output of the angular velocity sensor and the output of the acceleration sensor;
Including,
In the updating step, the bias voltage as the initial setting value is applied to the measured angular velocity.
The bias error of the angular velocity sensor is updated by adding the value BW and the variance value PWW.
, generate an updated bias value BW and an updated variance value PWW,
The variance value PVV as the initial setting value is added to the acceleration to obtain the balance of the acceleration sensor.
update the bias error to generate an updated variance value PVV;
In the step of estimating, the updated bias value BW, the updated variance value PWW
Based on the bias value BA as an initial setting value and the updated variance value PVV,
and estimating the posture of the object . The predetermined motion is one of the most frequently performed motions of the object.
The pose estimation method of claim 1 . The predetermined operation is a plurality of operations, and the bias value BW, the variance value PWW, the bias value BA, and the variance value PVV corresponding to any one of the plurality of operations according to the type of the operation are read out from the storage unit.
The pose estimation method of claim 1 . The measurement time in the stationary state is 200 msec.
The method of pose estimation according to any one of claims 1 to 3.

Images