Attitude estimation method, attitude estimation device, and movable device

Abstract

To provide a posture estimation method, a posture estimation device, and a movable device that can perform highly accurate measurement.SOLUTION: A posture estimation method includes the steps of: measuring a bias value BW and a dispersion value PWW of an angular velocity sensor 12 and a bias value BA and a dispersion value PVV of an acceleration sensor 14 in a predetermined movement of an object and storing the values in a storage unit; in resetting, reading out the bias value BW, dispersion value PWW, bias value BA, and dispersion value PVV from the storage unit and taking them as default values; measuring an angular velocity and acceleration by the angular velocity sensor 12 and the acceleration sensor 14 in a stationary state; updating the bias value BW and dispersion value PWW of the angular velocity sensor 12 and the dispersion value PVV of the acceleration sensor 14 from the default values; and estimating the posture of the object from output from the angular velocity sensor 12 and output from the acceleration sensor 14 by using a Kalman filter.SELECTED DRAWING: Figure 3

Description

[Technical field]

[0001] The present invention relates to a posture estimation method, a posture estimation device, and a movable device. [Background technology]

[0002] There are known devices and systems that attach an inertial measurement unit (IMU) to an object and calculate the position and attitude of the object using the output signal of the inertial measurement unit. Since the output signal of the inertial measurement unit has a bias error, which also causes an error in the attitude calculation, 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. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2020-20631 A Summary of the Invention [Problem to be solved by the invention]

[0004] However, inertial measurement units equipped with angular velocity sensors and acceleration sensors have different directions of the X-, Y-, and Z-axes 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. [Means for solving the problem]

[0005] The posture estimation method includes the steps of measuring a bias value BW and a variance value PWW of an angular velocity sensor and a bias value BA and a variance value PVV of an acceleration sensor during a predetermined movement of an object and storing them in a memory unit; reading out 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 as initial setting values; measuring 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 setting values; 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.

[0006] The posture estimation device has a memory unit that stores a bias value BW and a variance value PWW of an angular velocity sensor and a bias value BA and a variance value PVV of an acceleration sensor during a specified movement of an 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.

[0007] The movable device includes the above-described posture estimation device, and a control device that controls the posture of the object based on posture information of the object estimated by the posture estimation device. [Brief description of the drawings]

[0008] [Figure 1] FIG. 1 is a diagram showing an example of the configuration of a posture estimation device according to a first embodiment. [Diagram 2] FIG. 1 is a diagram for explaining a sensor coordinate system and a local coordinate system. [Diagram 3] FIG. 4 is a flowchart showing an example of a procedure of the posture estimation method according to the first embodiment. [Figure 4] FIG. 11 is a flowchart showing an example of a procedure of a posture estimation method according to the second embodiment. [Diagram 5] 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. [Figure 6] FIG. 2 is a block diagram showing a configuration example of a movable device. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0009] 1. First embodiment 1.1.Posture estimation device First, a posture estimation device 1 according to a first embodiment will be described with reference to FIGS. 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.

[0010] 1, the attitude estimation device 1 is separate from the inertial measurement unit 10, but the attitude estimation device 1 may include the inertial measurement unit 10. 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 a main body that houses the attitude estimation device 1. In the former case, the attitude estimation device 1 is attached to an object, and in the latter case, the inertial measurement unit 10 is attached to an object.

[0011] 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 a configuration in which some of these elements are changed or removed, or other elements are added.

[0012] The angular velocity sensor 12 measures the angular velocity in each of three axial directions that intersect with one another, ideally perpendicular to one another, and outputs an analog signal according to the magnitude and direction of the measured three-axis angular velocities.

[0013] The acceleration sensor 14 measures acceleration in each of three axial directions that intersect with one another, ideally perpendicular to one another, and outputs an analog signal according to the magnitude and direction of the measured three-axis acceleration.

[0014] 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ω of 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α of a digital value.

[0015] 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, and 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 attachment 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 a correction parameter calculated in advance according to the attachment 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.

[0016] The angular velocity sensor 12 and the acceleration sensor 14 may have built-in functions such as A / D conversion and temperature compensation.

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

[0018] 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 at an attachment position associated with a predetermined motion of the object. The predetermined motion is one motion that is most frequently performed by 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 types of motions.

[0019] The RAM 40 is a storage unit that is used as a working area for the processing unit 20 and temporarily stores programs and data read from the ROM 30, calculation results executed by the processing unit 20 in accordance with various programs, and the like.

[0020] The recording medium 50 is a non-volatile storage unit that stores data that needs to be stored for a long period of time among data generated by the processing of 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, and the like.

[0021] 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, an attitude change amount calculation unit 24, a velocity change amount calculation unit 26, and an attitude 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 intervals of Δt to estimate the attitude of the target object. 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 and the bias value BA and variance value PVV of the acceleration sensor 14 from the ROM 30, and sets them as initial settings. 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.

[0022] 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-axis, y-axis, and z-axis, and a local space coordinate system, which is a coordinate system of a space in which an object exists, for example, an XYZ coordinate system consisting of mutually orthogonal X-axis, Y-axis, and Z-axis, 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.

[0023] 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 initial values ​​of the bias error.

[0024] 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 a process of calculating the posture change amount of the target object by approximating the three-axis angular velocities from which bias errors have been removed by the bias removal unit 22 with a polynomial having the sampling interval Δt as a variable.

[0025] 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 a process of calculating the velocity change amount of the object using the three-axis angular velocities and three-axis accelerations from which the bias error has been removed by the bias removal unit 22.

[0026] 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 uses an extended Kalman filter to calculate a state vector x and its error covariance matrix Σ x 2 A process is performed to estimate the following.

[0027] Integral calculation unit 101 performs integration processing to integrate the posture change amount calculated by posture change amount calculation unit 24 with the estimate value of the immediately previous posture corrected by posture information correction unit 105 and normalized by normalization unit 106. In addition, integral calculation unit 101 performs integration processing to integrate the velocity change amount calculated by velocity change amount calculation unit 26 with the estimate value of the immediately previous velocity corrected by posture information correction unit 105 and normalized by normalization unit 106.

[0028] The orientation information prediction unit 102 performs a process of predicting an orientation quaternion q, which is orientation information of the target object, using the orientation change amount calculated by the orientation change amount calculation unit 24. The orientation 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 orientation information prediction unit 102 performs a process of predicting a state vector x that includes the orientation quaternion q and the motion velocity vector v as elements.

[0029] The error information update unit 103 updates the error covariance matrix Σ x 2 Specifically, the error information update unit 103 updates the attitude error of the target object using the three-axis angular velocities from which the bias error has been removed by the bias removal unit 22. In practice, the error information update unit 103 updates the error covariance matrix Σ x 2 The process of updating is performed.

[0030] The rotation error component remover 108 removes the error covariance matrix Σ 2 Specifically, the rotation error component remover 108 removes the rotation error component around the reference vector from the error covariance matrix Σ x 2 In practice, the rotation error component remover 108 removes the azimuth error component contained in the attitude error in the error covariance matrix Σ x 2 For the pose error covariance matrix Σ q 2 The error covariance matrix Σ with rank restriction and removal of the azimuth error component x 2 The process of generating the

[0031] The error information adjustment unit 110 determines whether the output of the angular velocity sensor 12 is within the valid range, and when it is determined that the output of the angular velocity sensor 12 is not within the valid range, adjusts the error information, that is, the error covariance matrix Σ x 2and the error covariance matrix Σ x 2 In addition, the error information adjustment unit 110 performs a process of reducing the correlation components between the attitude error components in and the error components other than the attitude error components, for example, to zero. In addition, the error information adjustment unit 110 determines whether the output of the acceleration sensor 14 is within a valid range, and when 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 performs a process of reducing the correlation components between the attitude error components in and the error covariance matrix Σ x,k 2 and increase the motion speed error component in the error covariance matrix Σ x,k 2 In particular, the error information adjustment unit 110 performs a process of reducing the correlation components between the motion velocity error components and error components other than the motion velocity error components in the error covariance matrix Σ generated by the rotation error component removal unit 108 during the 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. x 2 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 movement velocity error components and error components other than the movement velocity error components. In addition, the error information adjustment unit 110 performs processing to reduce, for example to zero, the error covariance matrix Σ generated by the rotation error component removal unit 108 during the 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. x 2 and reduces, for example, sets to zero, the correlation component between the movement velocity error component and an error component other than the movement velocity error component.

[0032] The bias error limiting unit 109 calculates the error covariance matrix Σ x 2 Specifically, the bias error limiting unit 109 performs a process of limiting the bias error component of the angular velocity around the reference vector in the error covariance matrix Σ x 2In practice, the bias error limiting unit 109 determines whether the vertical component of the bias error of the angular velocity exceeds an upper limit, and if the vertical component exceeds the upper limit, it limits the error covariance matrix Σ x 2 The process of generating the

[0033] The correction coefficient calculation unit 104 calculates the error covariance matrix Σ x 2 Based on this, the amount of correction of the orientation quaternion q, which is the orientation information of the object, and the motion velocity vector v, which is the velocity information, by the orientation information correction unit 105, and the error covariance matrix Σ, which is the error information by the error information correction unit 107, are calculated. x In practice, the correction coefficient calculation unit 104 performs processing to calculate the observation residual Δz, the Kalman coefficient K, and the transformation matrix H.

[0034] The posture information correction unit 105 calculates the error covariance matrix Σ x Based on the above, the orientation information correcting unit 105 performs a process of correcting the orientation quaternion q, which is the orientation information of the target object predicted by the orientation information predicting unit 102. Specifically, the orientation information correcting unit 105 performs a process of correcting the error covariance matrix Σ x , and the Kalman coefficient K and the observation residual Δz 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. a In practice, the attitude information correcting unit 105 performs a process of correcting the state vector x predicted by the attitude information predicting unit 102 using an extended Kalman filter.

[0035] The normalization unit 106 performs a process of normalizing the orientation quaternion q, which is the orientation information of the object corrected by the orientation information correction unit 105, so that the magnitude of the orientation quaternion q 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.

[0036] The error information correction unit 107 corrects the error covariance matrix Σ x Specifically, the error information correction unit 107 uses the conversion matrix H and the Kalman coefficient K calculated by the correction coefficient calculation unit 104 to correct the error covariance matrix Σ generated by the bias error limiting unit 109 using an extended Kalman filter. x The process of correcting the above is performed.

[0037] The orientation quaternion q, which is the orientation information of the target object estimated by the processing unit 20, can be transmitted to another device via the communication unit 60.

[0038] As described above, the posture estimation device 1 of this embodiment can estimate the posture of the object by 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 stored predetermined movement of the object. Therefore, it is possible to correct bias errors in the initial state that occur due to differences in the mounting position associated with a predetermined movement of the object, and therefore it is possible to measure the posture of the object with high accuracy.

[0039] 1.2.Pose estimation method Next, a posture estimation method of posture estimation device 1 according to the first embodiment will be described with reference to FIG.

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

[0041] 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 movements of the object, which is a predetermined 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.

[0042] 1.2.2. Storage process 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.

[0043] 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.

[0044] 1.2.4. Angular velocity and acceleration measurement process In step S104, the angular velocity sensor 12 and the acceleration sensor 14 measure the angular velocity and acceleration in a stationary state for a measurement time of 200 msec.

[0045] 1.2.5. Initial setting value update process 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.

[0046] 1.2.6. Posture estimation process 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.

[0047] 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 stored predetermined movement of the object. Therefore, it is possible to correct bias errors in the initial state that occur due to differences in the mounting position associated with the predetermined movement of the object, and therefore it is possible to measure the posture of the object with high accuracy.

[0048] 2. Second embodiment 2.1.Pose estimation method Next, a posture estimation method according to the second embodiment will be described with reference to FIG.

[0049] 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 object is a plurality of motions, and corresponding bias values ​​BW, BA and variance values ​​PWW, PVV are stored in ROM 30 according to the type of motion. Note that the following description will focus on the differences from the first embodiment, and description of similar matters will be omitted.

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

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

[0052] 2.1.1.Measurement process First, in step S201, the inertial measurement unit 10 is installed at a mounting 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.

[0053] 2.1.2. Storage process 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 a storage unit as the bias values ​​BW, BA and variance values ​​PWW, PVV of the first operation.

[0054] Next, the inertial measurement unit 10 is placed at a mounting position associated with a second motion of the object, and the processes of steps S201 and S202 are repeated. The processes of steps S201 and S202 are repeated for a plurality of motions of the object, that is, according to the types of motions.

[0055] 2.1.3. Initial value selection process After acquiring the bias values ​​BW, BA and variance values ​​PWW, PVV of the sensors 12, 14 corresponding to a plurality of movements of the object, in step S203, the processing unit 20 receives an instruction to select a movement from the user.

[0056] 2.1.4. Initial value setting process 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 associated with the selected operation, and the bias value BA and variance value PVV of the acceleration sensor 14, and sets them as initial setting values ​​for the bias error.

[0057] 2.1.5. Angular velocity and acceleration measurement process Step S205 is the same as step S104 in the first embodiment, and therefore a description thereof will be omitted.

[0058] 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.

[0059] 2.1.7. Posture estimation process Step S207 is the same as step S106 in the first embodiment, and therefore a description thereof will be omitted.

[0060] With this configuration, even if the inertial measurement unit 10 is installed in an attachment 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.

[0061] 3. Third embodiment 3.1. Movable Devices Next, a movable device 600 including a posture estimation device 1 according to a third embodiment will be described with reference to FIGS.

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

[0063] 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 including a plurality of members rotatable 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 constituting 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.

[0064] The working mechanism 620 comprises, as multiple components, a boom 613 attached to the front side of the upper rotating body 611 so as to be able to move up and down, an arm 614 attached to the tip side of the boom 613 so as to be able to move up and down, a bucket link 616 rotatably attached to the tip side of the arm 614, a bucket 615 rotatably attached to the tips 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.

[0065] A base end side of the boom 613 is supported by the upper rotating body 611 so as to be rotatable in the vertical direction, and the boom 613 is rotated relative to the upper rotating body 611 by the extension and contraction of a boom cylinder 617. An inertial measurement unit 10c that functions as an inertial sensor that detects the state of movement of the boom 613 is disposed on the boom 613.

[0066] One end of an arm 614 is rotatably supported on the tip side 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 that functions as an inertial sensor that detects the movement state of the arm 614 is disposed on the arm 614.

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

[0068] 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, the boom 613, the arm 614, and the 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 wires 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 a wiring installation method, and makes it possible to reduce the occurrence of deterioration and damage to the wiring.

[0069] 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 positions and attitudes of the boom 613, arm 614, and bucket 615 that configure 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 output signals from 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 the inertial measurement units 10a, 10b, 10c, and 10d, and calculates the positions, attitudes, or attitude angles 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 work mechanism 620 and the upper rotating body 611.

[0070] In addition, examples of construction machines (construction machinery) in which the posture estimation device 1 of the above embodiment can be used 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 aerial work vehicles (lift trucks).

[0071] 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. Furthermore, 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 positions are attached to the upper rotating body 611, the boom 613, the arm 614, and the bucket 615, which perform different movements, so that the bias error in the initial state can be corrected, thereby making it possible to measure the attitude of the target object with high accuracy.

[0072] In the present 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. However, 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) may also be used. The above-described predetermined motion may be a lifting motion of the bucket 615, a lowering motion of the bucket 615, or a 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, the rotation or movement, and the like. The predetermined motion may also be a predetermined motion of a robot arm or a drone. Furthermore, the predetermined motion may also be a walking motion of a bipedal robot. In each of the inertial measurement units 10a, 10b, 10c, and 10d, the bias value BW and the dispersion value PWW of the angular velocity sensor 12 may be different or the same 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, five or more inertial measurement units.

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

[0074] 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. [Explanation of symbols]

[0075] 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 a 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, 1 07...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 running 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...variance value.

Claims

1. A bias value BW and a variance value PWW of the angular velocity sensor in a given motion of the object; A step of measuring the bias value BA and the variance value PVV of the acceleration sensor and storing them in a memory unit. and, 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; The angular velocity sensor and the acceleration sensor measure the angular velocity and the acceleration in a stationary state. and The bias value BW and the variance value PWW of the angular velocity sensor, and the acceleration sensor updating the variance value PVV from the initial setting value; A Kalman filter is used to calculate the output of the angular velocity sensor and the output of the acceleration sensor. estimating a pose of the object; 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.

2. The predetermined motion is one of the most frequently performed motions of the object. The pose estimation method of claim 1 .

3. The predetermined motion is a plurality of motions, and any one of the plurality of motions is selected according to the type of motion. The bias value BW, the variance value PWW, the bias value BA, and the The variance value PVV is read from the storage unit. The pose estimation method of claim 1 .

4. 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