State estimation device and state estimation method

Abstract

To improve accuracy of estimating a state including at least one of a position, a speed and an attitude of a movable body.SOLUTION: A state estimation device 10 estimates a state including at least one of a position, a speed and an attitude of a vehicle. The state estimation device 10 comprises: an input unit 22 which reads image data output by an imaging unit 2 and inertial data output by an inertial measurement device 3; and a pre-processing unit 24 which performs extraction and tracking of a feature point in the image data and calculates a state of the vehicle from the inertial data. The state estimation device 10 comprises: a calculation unit 26 which obtains a bias error of the inertial measurement device 3 by executing bundle adjustment to the position, the speed and the attitude of the vehicle based on the feature point in the image data and the inertial data; and a correction unit 28 which obtains correction data obtained by removing a bias error from the inertial data. The state estimation device 10 includes an estimation unit 30 which estimates the state including at least one of the position, the speed and the attitude of the vehicle on the basis of correction data.SELECTED DRAWING: Figure 1

Description

[Technical Field]

[0001] The present disclosure relates to a state estimation device and a state estimation method for estimating a state including at least one of the position, velocity, and attitude of a moving object. [Background technology]

[0002] Conventionally, a technique called visual inertial odometry (VIO) is known, which uses a camera and an inertial measurement unit (so-called IMU) to accurately estimate multiple parameters by a nonlinear least squares method called bundle adjustment. For example, Patent Document 1 discloses a technique for estimating the position, attitude, velocity, and bias error of a moving object, as well as the inertial measurement unit, by using visual inertial odometry (VIO) (see, for example, Patent Document 1). Note that "VIO" is an abbreviation for visual inertial odometry. Also, "IMU" is an abbreviation for inertial measurement unit. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] U.S. Patent No. 9,424,647 Summary of the Invention [Problem to be solved by the invention]

[0004] The inventors have been studying the use of visual inertial odometry (VIO) to estimate the position, attitude, and velocity of a moving body such as a vehicle, as well as bias errors in an inertial measurement unit. However, they have found that there is a large error in the estimation of attitude changes within a certain time after starting these estimations. The reason for this is thought to be that images output by a camera are easily affected by motion blur and the movement of surrounding objects, which results in a large error in the attitude calculated based on image data over a short period of time. These findings were discovered after extensive research by the inventors.

[0005] An object of the present disclosure is to provide a state estimation device and a state estimation method that can improve the accuracy of estimating a state including at least one of the position, velocity, and attitude of a moving body. [Means for solving the problem]

[0006] The invention described in claim 1 is A state estimation device that estimates a state including at least one of a position, a velocity, and an attitude of a moving body (1), an input unit (22) that reads image data output from an imaging unit (2) that captures images of the surroundings of the moving object and inertial data of the moving object output from an inertial measurement unit (3) installed on the moving object; a pre-processing unit (24) that extracts and tracks feature points included in the image data and calculates the position, velocity, and attitude of the moving object based on the inertial data; a calculation unit (26) that performs bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to determine a bias error of the inertial measurement unit; a correction unit (28) that obtains correction data by removing bias errors from the inertial data; an estimation unit (30) that estimates a state including at least one of a position, a velocity, and an attitude of the moving object based on the correction data; Equipped with The calculation unit is The reprojection error between the image coordinate system and the world coordinate system is defined as the residual error related to the image, the difference between the position and attitude measurement results of the inertial measurement unit and the position and attitude predicted from the image data is defined as the residual error related to the IMU, and the difference between the position and attitude of the moving body estimated from the most recent information and the position and attitude of the moving body estimated from prior information is defined as the residual error related to the prior information, The bias error is estimated by optimizing the residuals related to the image, the residuals related to the IMU, and the residuals related to prior information using bundle adjustment. .

[0007] According to the inventors' research, it has been found that the estimation error of the posture change becomes small after a certain time has passed since the start of the posture change estimation using the visual inertial odometry VIO. The reason for this is that the posture estimation result obtained from the analysis of image data does not include the bias error of the inertial measurement unit.

[0008] Taking these into consideration, the state estimation device of the present disclosure calculates a bias error using visual inertial odometry VIO, and estimates the state of the moving body based on corrected data obtained by removing the bias error from the inertial data.

[0009] In this way, by estimating the bias error using visual inertial odometry VIO and estimating the position, velocity, and orientation of the moving body based on the bias error and inertial data, it is possible to suppress the influence of errors caused by motion blur, surrounding moving objects, etc. Therefore, the state estimation device of the present disclosure can improve the accuracy of the state including at least one of the position, velocity, and orientation of the moving body.

[0010] Claim 9 The invention described in A state estimation method for estimating a state including at least one of a position, a velocity, and an attitude of a moving object (1), reading image data output from an imaging unit (2) that images the surroundings of the moving body and inertial data of the moving body output from an inertial measurement unit (3) installed on the moving body; extracting feature points contained in the image data and tracking the feature points, and calculating the position, speed, and attitude of the moving object based on the inertial data; Performing bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to determine the bias error of the inertial measurement unit; Obtaining correction data by removing bias errors from the inertial data; estimating a state including at least one of a position, a velocity, and an attitude of the moving object based on the correction data; Including, When calculating the bias error, The reprojection error between the image coordinate system and the world coordinate system is defined as the residual error related to the image, the difference between the position and attitude measurement results of the inertial measurement unit and the position and attitude predicted from the image data is defined as the residual error related to the IMU, and the difference between the position and attitude of the moving body estimated from the most recent information and the position and attitude of the moving body estimated from prior information is defined as the residual error related to the prior information, The bias error is estimated by optimizing the residuals related to the image, the residuals related to the IMU, and the residuals related to prior information using bundle adjustment. .

[0011] In this way, by estimating the bias error using visual inertial odometry VIO and estimating the position, velocity, and orientation of the moving body based on the bias error and inertial data, it is possible to suppress the influence of errors caused by motion blur, surrounding moving objects, etc. Therefore, according to the state estimation state of the present disclosure, it is possible to improve the accuracy of the state including at least one of the position, velocity, and orientation of the moving body.

[0012] The reference symbols in parentheses attached to each component indicate an example of the correspondence between the component and the specific components described in the embodiments described below. [Brief explanation of the drawings]

[0013] [Figure 1] 1 is a schematic configuration diagram of a state estimation device according to a first embodiment. [Figure 2] FIG. 2 is an explanatory diagram for explaining an imaging unit used in the state estimation device. [Figure 3] FIG. 2 is an explanatory diagram for explaining an inertial measurement unit used in the state estimation device. [Figure 4] FIG. 1 is an explanatory diagram for explaining an overview of visual inertial odometry. [Figure 5] FIG. 10 is an explanatory diagram for explaining residuals related to an image. [Figure 6] FIG. 10 is an explanatory diagram for explaining residuals related to an inertial measurement unit. [Figure 7] FIG. 10 is an explanatory diagram for explaining a residual related to prior information. [Figure 8] 1 is an explanatory diagram for explaining an angular velocity measured by a gyro sensor and an angular velocity estimated from an attitude obtained by visual inertial odometry; [Figure 9] FIG. 10 is an explanatory diagram for explaining a change over time in a bias error included in inertia data. [Figure 10] FIG. 1 is an explanatory diagram for explaining a state estimation device according to a first embodiment. [Figure 11] 10 is an explanatory diagram for explaining the estimation result of the vehicle attitude based on the bias error obtained by visual inertial odometry and correction data calculated from inertial data. FIG. [Figure 12] FIG. 10 is an explanatory diagram for explaining a state estimation device according to a second embodiment. [Figure 13] FIG. 10 is an explanatory diagram for explaining a state estimating device according to a third embodiment. [Figure 14]FIG. 11 is an explanatory diagram for explaining a method for estimating the attitude of a vehicle in a state estimation device according to a third embodiment. [Figure 15] 4 is an explanatory diagram for explaining a method of calculating and determining the attitude angle of the vehicle from the sensor output of the acceleration sensor; FIG. [Figure 16] FIG. 10 is an explanatory diagram for explaining a problem that occurs in an abnormal situation. [Figure 17] FIG. 10 is an explanatory diagram for explaining a state estimating device according to a fourth embodiment. [Figure 18] FIG. 10 is an explanatory diagram for explaining behavior under abnormal conditions. [Figure 19] FIG. 10 is an explanatory diagram for explaining a state estimating device according to a fifth embodiment. [Figure 20] FIG. 13 is an explanatory diagram for explaining a state estimating device according to a sixth embodiment. [Figure 21] FIG. 13 is an explanatory diagram for explaining a state estimating device according to a seventh embodiment. [Figure 22] FIG. 13 is an explanatory diagram for explaining a state estimating device according to an eighth embodiment. [Figure 23] FIG. 13 is an explanatory diagram for explaining a state estimating device according to a ninth embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0014] Hereinafter, embodiments of the present disclosure will be described with reference to the drawings. In the following embodiments, parts that are the same as or equivalent to those described in the preceding embodiments will be given the same reference numerals, and their description may be omitted. Furthermore, in the embodiments, when only some of the components are described, the components described in the preceding embodiments can be applied to the remaining components. The following embodiments can be partially combined with each other, even if not specifically stated, as long as there is no particular problem with the combination.

[0015] (First embodiment) This embodiment will be described with reference to Fig. 1 to Fig. 11. In this embodiment, an example will be described in which a state estimation device 10 of the present disclosure shown in Fig. 1 is applied to a vehicle 1, and the position p, velocity v, and attitude φ of the vehicle 1 are estimated and output to the outside. In this embodiment, the vehicle 1 corresponds to a "moving body."

[0016] A vehicle 1 is equipped with a state estimation device 10. In addition to the state estimation device 10, the vehicle 1 is also equipped with an imaging unit 2 and an inertial measurement unit 3. Note that a part of the state estimation device 10 may be installed outside the vehicle 1.

[0017] As shown in FIG. 2, the imaging unit 2 periodically captures images of the surroundings of the vehicle 1. The imaging unit 2 outputs image data of the captured image of the surrounding area of ​​the vehicle 1. The imaging unit 2 is configured, for example, by a camera equipped with a photoelectric conversion element such as a CCD or CMOS. CCD is an abbreviation for Charge Coupled Device. CMOS is an abbreviation for Complementary Metal Oxide Semiconductor. The imaging unit 2 of this embodiment is configured by a monocular camera. The imaging unit 2 may also be configured by a compound eye camera.

[0018] The inertial measurement unit 3 is a device that detects three-dimensional inertial motion of the vehicle 1. The inertial measurement unit 3 outputs the translational motion and rotational motion of the vehicle 1 in three orthogonal axial directions as inertial data. The inertial measurement unit 3 includes a gyro sensor 3a that detects angular velocities ωx, ωy, and ωz of the vehicle 1 as the rotational motion of the vehicle 1, and an acceleration sensor 3b that detects accelerations fx, fy, and fz of the vehicle 1 as the translational motion of the vehicle 1. The inertial measurement unit 3 of this embodiment is configured as a small MEMS-based IMU. MEMS is an abbreviation for Micro Electro Mechanical Systems.

[0019] As shown in FIG. 1, the state estimation device 10 estimates the state of the vehicle 1, such as the position p, velocity v, and attitude φ of the vehicle 1, based on the image data output by the imaging unit 2 and the inertial data output by the inertial measurement device 3, and outputs the estimation results to the outside.

[0020] The state estimation device 10 is a computer having a control unit 20 including a processor, a memory 50, etc. The memory 50 stores programs, data, etc. for executing various control processes. The control unit 20 executes the various programs stored in the memory 50.

[0021] The state estimation device 10 functions as various functional units through the execution of various programs by a control unit 20. The state estimation device 10 includes an input unit 22, a preprocessing unit 24, a calculation unit 26, a correction unit 28, and an estimation unit 30.

[0022] The input unit 22 is connected to the imaging unit 2 and the inertial measurement unit 3. The input unit 22 reads the image data output by the imaging unit 2 and the inertial data output by the inertial measurement unit 3.

[0023] The pre-processing unit 24 extracts feature points FP from the image data read by the input unit 22 and tracks the feature points FP, and calculates the position p, velocity v, and attitude φ of the vehicle 1 based on the inertial data read by the input unit 22.

[0024] The pre-processing unit 24 has an image processing unit 241 that extracts feature points FP from image data and tracks the feature points FP. The image processing unit 241 extracts feature points FP based on local feature amounts using, for example, SIFT or SURF, and associates the feature points FP extracted in the current image frame with the feature points FP extracted in the previous image frame using a nearest neighbor search or the like. Note that the extraction of feature points FP and the association of feature points FP in the image processing unit 241 may be achieved by means other than those described above.

[0025] The pre-processing unit 24 also has an inertia processing unit 242 that calculates the position p, velocity v, and attitude φ of the vehicle 1 based on the inertial data. The inertia processing unit 242, for example, integrates the angular velocity ω that is the sensor output of the gyro sensor 3a to obtain the attitude φ and rotation matrix Cb of the vehicle 1. The inertia processing unit 242 also integrates the product of the acceleration f that is the sensor output of the acceleration sensor 3b and the rotation matrix Cb to obtain the velocity v of the vehicle 1, and also integrates the obtained velocity v of the vehicle 1 to obtain the position p of the vehicle 1. The inertia processing unit 242 calculates three attitude angles, namely, the roll angle, pitch angle, and yaw angle, as the attitude φ.

[0026] The calculation unit 26 estimates various parameters including the bias error of the inertial measurement unit 3 using the visual inertial odometry VIO. In this embodiment, the calculation unit 26 performs a nonlinear least squares method called bundle adjustment on the feature points FP of the image data and the position p, velocity v, and attitude φ of the vehicle 1 based on the inertial data to estimate the position p, attitude φ, velocity v, and bias error of the inertial measurement unit 3. The calculation unit 26 estimates the bias error of the gyro sensor 3a and the bias error of the acceleration sensor 3b as the bias error of the inertial measurement unit 3.

[0027] Specifically, as shown in FIG. 4, the calculation unit 26 estimates the position p, attitude φ, velocity v of the vehicle 1, and bias error of the inertial measurement unit 3 by optimizing the residuals related to the image, the residuals related to the IMU, and the residuals related to the prior information by bundle adjustment.

[0028] The calculation unit 26 optimizes the reprojection error between the image coordinate system and the world coordinate system as a residual error related to the image by bundle adjustment. For example, as shown in Fig. 5, the calculation unit 26 transforms the position of the feature point FP in the i-th image frame into the world coordinate system, and then optimizes the difference between the position of the feature point FP reprojected onto the image coordinate system of the j-th image frame and the position of the feature point FP in the j-th image frame as a residual error related to the image.

[0029] Furthermore, the calculation unit 26 optimizes, for example, the difference between the measurement results of the position p and attitude φ by the inertial measurement unit 3 and the prediction results of the position p and attitude φ predicted from the image data as a residual related to the IMU by bundle adjustment. The sampling time of the inertial data in the inertial measurement unit 3 is shorter than the sampling time of the image data in the imaging unit 2. For this reason, for example, as shown in FIG. 6, the calculation unit 26 optimizes, as a residual related to the IMU, the difference between the prediction of changes in the position p and attitude φ of the vehicle 1 between the ith and jth image frames and the measurement results of the position p and attitude φ obtained by integrating the inertial data acquired between each image frame.

[0030] Furthermore, the calculation unit 26 performs bundle adjustment using not only the most recent information of the image data and inertial data but also prior information, as shown in Fig. 7. For example, the calculation unit 26 optimizes the difference between the position p and attitude φ of the vehicle 1 estimated from the most recent information and the position p and attitude φ of the vehicle 1 estimated from the prior information as a residual related to the prior information.

[0031] When new information is added, the calculation unit 26 reduces the load of calculation processing, etc. by deleting part of the prior information or performing marginalization processing. Note that a method for determining the position, velocity v, and attitude φ in the visual inertial odometry VIO is also disclosed in, for example, the following document 1.

[0032] [Reference 1] T. Qin, P. Li and S. Shen, "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator," in IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, Aug. 2018, doi: 10.1109 / TRO.2018.2853729.

[0033] 8 shows an analysis of the angular velocity ω measured by the gyro sensor 3a and the angular velocity ω estimated from the attitude φ determined by the visual inertial odometry VIO using Allan variance. In FIG. 8, the analysis result of the angular velocity ω measured by the gyro sensor 3a is shown by a two-dot chain line, and the analysis result of the angular velocity ω estimated from the attitude φ determined by the visual inertial odometry VIO is shown by a one-dot chain line.

[0034] 8, it was found that the estimation of the angular velocity ω by the visual inertial odometry VIO has a larger error within a predetermined time from the start of estimation compared to the measurement results by the gyro sensor 3a. The reason for this is thought to be that the image data output by the imaging unit 2 is easily affected by motion blur and surrounding moving objects, which results in a large error in the attitude φ obtained from the analysis of the image data over a short period of time.

[0035] On the other hand, the error in the estimation of the angular velocity ω by the visual inertial odometry VIO decreases over time. This is thought to be because the estimation of the angular velocity ω by the visual inertial odometry VIO is not affected by the bias error of the gyro sensor 3a.

[0036] In contrast, the measurement results of the gyro sensor 3a have small errors until a certain time has passed since the start of measurement, but the errors gradually increase as time passes, as shown in Figures 8 and 9. This is thought to be due to the accumulation of bias errors over time.

[0037] Taking these characteristics into consideration, the state estimation device 10 calculates a bias error using visual inertial odometry VIO and estimates the state of the vehicle 1 based on correction data obtained by removing the bias error from the inertial data. The state estimation device 10 of this embodiment includes a correction unit 28 that calculates correction data obtained by removing the bias error from the inertial data, and an estimation unit 30 that estimates at least one of the position p, velocity v, and attitude φ of the vehicle 1 based on the correction data.

[0038] The correction unit 28, for example, as shown in FIG. 10, subtracts the bias error of the inertial measurement unit 3 obtained by the visual inertial odometry VIO from the inertial data output by the inertial measurement unit 3, and outputs the result as correction data.

[0039] The estimation unit 30 calculates the position p, velocity v, and attitude φ of the vehicle 1 based on the correction data and outputs the calculation results. The estimation unit 30 integrates the angular velocity ω corrected by the correction unit 28 to obtain the attitude φ and rotation matrix Cb of the vehicle 1. The estimation unit 30 also integrates the product of the acceleration f corrected by the correction unit 28 and the rotation matrix Cb to obtain the velocity v of the vehicle 1, and also integrates the obtained velocity v of the vehicle 1 to obtain the position p of the vehicle 1.

[0040] 11 shows an analysis using Allan variance of the angular velocity ω measured by the gyro sensor 3a, the angular velocity ω estimated from the attitude φ determined by the visual inertial odometry VIO, and the angular velocity ω determined by removing the bias error from the angular velocity ω measured by the gyro sensor 3a. In FIG. 11, the analysis result of the angular velocity ω measured by the gyro sensor 3a is shown by a two-dot chain line, and the analysis result of the angular velocity ω estimated from the attitude φ determined by the visual inertial odometry VIO is shown by a single-dot chain line. Also in FIG. 11, the analysis result of the angular velocity ω determined by removing the bias error from the angular velocity ω measured by the gyro sensor 3a is shown by a solid line.

[0041] 11, the angular velocity ω calculated by removing the bias error from the angular velocity ω measured by the gyro sensor 3a was different from the angular velocity ω estimated from the attitude φ calculated by the visual inertial odometry VIO in that the error was small immediately after the start of estimation. Furthermore, the angular velocity ω calculated by removing the bias error from the angular velocity ω measured by the gyro sensor 3a was different from the measurement result of the gyro sensor 3a in that the error was small even after a certain amount of time had passed.

[0042] The state estimation device 10 and state estimation method described above calculate a bias error using the visual inertial odometry VIO, and estimate the position p, velocity v, and attitude φ of the vehicle 1 based on corrected data obtained by removing the bias error from the inertial data. In this way, by estimating the bias error using the visual inertial odometry VIO and estimating the position p, velocity v, and attitude φ of the vehicle 1 based on the bias error and the inertial data, the influence of errors caused by motion blur, surrounding moving objects, and the like can be suppressed. Therefore, the state estimation device 10 and state estimation method disclosed herein can improve the accuracy of estimating a state including at least one of the position p, velocity v, and attitude φ of the vehicle 1.

[0043] Unlike a Kalman filter, which sequentially estimates the current state from past and current measurement results, the visual inertial odometry VIO minimizes error using a nonlinear least-squares method such as bundle adjustment. Although bundle adjustment requires a high computational load, it is characterized by high accuracy because it iteratively calculates an estimate that minimizes error using multiple data from the past to the present. In particular, bundle adjustment has superior performance to a Kalman filter in terms of resistance to external noise and state estimation using nonlinear functions. The state estimation device 10 and state estimation method disclosed herein use bias errors estimated with high accuracy by the visual inertial odometry VIO, allowing appropriate correction of inertial data. This is effective in improving the accuracy of state estimation, including at least one of the position p, velocity v, and attitude φ of the vehicle 1.

[0044] (Second embodiment) Next, a second embodiment will be described with reference to Fig. 12. In this embodiment, differences from the first embodiment will be mainly described.

[0045] When the imaging unit 2 is configured with a monocular camera as in the first embodiment, the error in scale estimation becomes larger than when a compound camera is used. If this error is large, the accuracy of estimating the bias error of the acceleration f included in the inertial data may decrease, and even if the bias error is removed from the acceleration f included in the inertial data and then integrated to estimate the velocity v and position p of the vehicle 1, there is a risk that a sufficient improvement in accuracy may not be achieved.

[0046] Taking this into consideration, the state estimation device 10 of this embodiment is configured to determine the position p and velocity v of the vehicle 1 not only by removing the bias error from the inertial data but also by using the sensor output of the wheel speed sensor 4, as shown in Fig. 12. The wheel speed sensor 4 is configured, for example, by a rotary encoder. The wheel speed sensor 4 outputs a signal corresponding to the rotation speed of the wheels of the vehicle 1 to the outside as a sensor output.

[0047] The estimation unit 30 is directly or indirectly connected to the wheel speed sensor 4 so as to be able to read the sensor output of the wheel speed sensor 4. The estimation unit 30 estimates the speed v and position p of the vehicle 1 based on the correction data obtained by the correction unit 28 and the sensor output of the wheel speed sensor 4 as well.

[0048] Specifically, the estimation unit 30 obtains the attitude φ and rotation matrix Cb of the vehicle 1 by integrating the angular velocity ω corrected by the correction unit 28. Then, the estimation unit 30 obtains the velocity v of the vehicle 1 by integrating the product of the acceleration f of the vehicle 1 estimated from the output of the wheel speed sensor 4 and the rotation matrix Cb, rather than the acceleration f corrected by the correction unit 28, and also obtains the position p of the vehicle 1 by integrating the obtained velocity v of the vehicle 1.

[0049] The rest of the configuration is the same as that of the first embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the first embodiment that are achieved by a configuration common to or equivalent to that of the first embodiment.

[0050] Moreover, the state estimating device 10 of this embodiment has the following features.

[0051] (1) The estimation unit 30 of the state estimation device 10 calculates the speed v of the vehicle 1 based on the correction data and the sensor output of the wheel speed sensor 4 installed on the vehicle 1, and estimates the position p of the vehicle 1 based on the calculated speed v of the vehicle 1. This makes it possible to estimate the speed v and position p of the vehicle 1 with sufficient accuracy even when the imaging unit 2 is configured with a monocular camera. The configuration of this proposal is suitable for a configuration in which a monocular camera is used for the imaging unit 2 or a configuration in which it is difficult to reduce the error in camera scale estimation.

[0052] (Third embodiment) Next, a third embodiment will be described with reference to Figures 13 to 15. In this embodiment, differences from the first embodiment will be mainly described.

[0053] In the inertial measurement unit 3, the acceleration sensor 3b has a simpler sensor structure such as MEMS than the gyro sensor 3a, and bias changes in the acceleration sensor 3b tend to be smaller than bias changes in the gyro sensor 3a. Given this background, the bias error estimated by the visual inertial odometry VIO tends to be less accurate for the gyro sensor 3a than for the acceleration sensor 3b.

[0054] In light of these considerations, the state estimation device 10 of this embodiment estimates the attitude φ of the vehicle 1 using the sensor output of the wheel speed sensor 4 in addition to the corrected data obtained by correcting the inertial data using the bias error obtained by the visual inertial odometry VIO, as shown in FIG. 13.

[0055] As shown in Figure 14, the estimation unit 30 determines the bias error of the gyro sensor 3a using the visual inertial odometry VIO, and determines the first attitude angle φ1 indicating the attitude φ of the vehicle 1 based on the sensor output of the gyro sensor 3a corrected using the bias error.

[0056] The estimation unit 30 also obtains a bias error of the acceleration sensor 3b using the visual inertial odometry VIO. The estimation unit 30 then obtains a second attitude angle φ2 indicating the attitude φ of the vehicle 1 based on the sensor output of the acceleration sensor 3b corrected using the bias error of the acceleration sensor 3b and the gravitational acceleration obtained from the sensor output of the wheel speed sensor 4.

[0057] Specifically, the estimation unit 30 removes the translational acceleration from the sensor output of the acceleration sensor 3b using the differential value of the sensor output of the wheel speed sensor 4, extracts only the gravitational acceleration, and then calculates the attitude angle shown in FIG. 15 to obtain the second attitude angle φ2 that indicates the attitude φ of the vehicle 1.

[0058] Here, since the wheel speed sensor 4 has a large quantization noise, it is preferable to limit the band using a low-pass filter. For example, it is preferable to smooth the second attitude angle φ2 calculated by the estimation unit 30 using a moving average filter so that only the low-frequency components are used. In this case, high-frequency components will be insufficient, but the first attitude angle φ1 can be used to make up for the insufficient high-frequency components.

[0059] Taking these factors into consideration, the estimation unit 30 passes the first attitude angle φ1 through a high-pass filter of a complementary filter, passes the second attitude angle φ2 through a low-pass filter of a complementary filter, and then combines the two to estimate the attitude φ of the vehicle 1. It is desirable that the orders of the cutoff frequencies of the low-pass filter and the high-pass filter of the complementary filters match.

[0060] The rest of the configuration is the same as the embodiments described above. The state estimation device 10 and the state estimation method of this embodiment can obtain the same effects as those of the embodiments described above, which are achieved by configurations common to or equivalent to those of the embodiments described above.

[0061] Moreover, the state estimating device 10 of this embodiment has the following features.

[0062] (1) The estimation unit 30 passes the first attitude angle φ1 through a high-pass filter of a complementary filter, passes the second attitude angle φ2 through a low-pass filter of a complementary filter, and then combines the two to estimate the attitude φ of the vehicle 1. In this way, if the configuration is such that the attitude φ of the vehicle 1 is estimated using the second attitude angle φ2 estimated from the sensor output of the acceleration sensor 3b in addition to the first attitude angle φ1 estimated from the sensor output of the gyro sensor 3a, it is possible to achieve a sufficient improvement in the accuracy of the estimation of the attitude φ of the vehicle 1.

[0063] (Fourth embodiment) Next, a fourth embodiment will be described with reference to Figures 16 to 18. In this embodiment, differences from the first embodiment will be mainly described.

[0064] When there is a change in the environment around the vehicle 1 (for example, backlight, a tunnel, etc.), it becomes difficult for the imaging unit 2 to perform its intended function. When such an abnormal situation occurs, for example, as shown in Fig. 16, the estimation of the bias error by the visual inertial odometry VIO becomes unstable, making it difficult to estimate the position p, velocity v, and attitude φ of the vehicle 1 with high accuracy.

[0065] In view of this, the state estimation device 10 is provided with an abnormality determination unit 31, as shown in FIG. 17, that determines whether or not an abnormal situation exists in which it is difficult for the imaging unit 2 to perform its intended function.

[0066] The abnormality determination unit 31 determines whether or not the imaging unit 2 can perform the intended function, such as extracting feature points FP, based on, for example, image data output by the imaging unit 2. If the imaging unit 2 can perform the intended function, the abnormality determination unit 31 determines that there is no abnormal situation. If the imaging unit 2 cannot perform the intended function, the abnormality determination unit 31 determines that there is an abnormal situation.

[0067] The correction unit 28 of this embodiment calculates correction data for the inertial data in consideration of the determination result of the abnormality determination unit 31. Specifically, when the imaging unit 2 is in a normal state where it can perform its intended function, the correction unit 28 calculates the correction data by removing the bias error calculated by the visual inertial odometry VIO from the inertial data.

[0068] On the other hand, when the imaging unit 2 is not in an abnormal situation where it cannot perform its intended function, the correction unit 28 calculates correction data by removing the bias estimate value previously stored in the memory 50 from the inertial data instead of the bias error obtained by the visual inertial odometry VIO.

[0069] Here, the bias error changes depending on stress, temperature, etc., but these do not change much over a short period of time (for example, about 10 seconds). For this reason, for example, it is desirable that the correction unit 28 stores in the memory 50, as a bias estimate, the bias error calculated by the visual inertial odometry VIO immediately before an abnormal situation occurs in which the imaging unit 2 cannot perform its intended function.

[0070] The rest of the configuration is the same as the embodiments described above. The state estimation device 10 and the state estimation method of this embodiment can obtain the same effects as those of the embodiments described above, which are achieved by configurations common to or equivalent to those of the embodiments described above.

[0071] Moreover, the state estimating device 10 of this embodiment has the following features.

[0072] (1) The state estimation device 10 includes an abnormality determination unit 31 that determines whether an abnormal situation exists that makes it difficult for the imaging unit 2 to perform its intended function. If the determination by the abnormality determination unit 31 indicates that the situation is not abnormal, the correction unit 28 calculates correction data by removing a bias error from the inertia data. If the determination by the abnormality determination unit 31 indicates that the situation is abnormal, the correction unit 28 calculates correction data by removing a bias estimate value that was previously stored in the memory 50, instead of the bias error calculated by the calculation unit 26, from the inertia data. This allows the state of the vehicle 1 to continue to be appropriately estimated, for example, as shown in FIG. 18 , even if an abnormal situation occurs that makes it difficult for the imaging unit 2 to perform its intended function.

[0073] (2) The abnormality determination unit 31 determines whether or not an abnormal situation exists based on the image data output by the imaging unit 2. This eliminates the need for additional sensor equipment dedicated to abnormality determination in the imaging unit 2, and allows the state of the vehicle 1 to be continuously estimated in a simple manner.

[0074] (Fifth embodiment) Next, a fifth embodiment will be described with reference to Fig. 19. In this embodiment, differences from the fourth embodiment will be mainly described.

[0075] In an abnormal situation where it is difficult for the imaging unit 2 to perform its intended function, there is a high possibility that the pre-processing unit 24 will be unable to extract the feature points FP from the image data or will be unable to track the feature points FP.

[0076] In view of this, the abnormality determination unit 31 of this embodiment does not acquire image data from the imaging unit 2, but acquires the analysis result of the image data from the pre-processing unit 24 as shown in Fig. 19, and determines whether or not an abnormal situation exists based on the analysis result. For example, the abnormality determination unit 31 determines that an abnormal situation exists when the pre-processing unit 24 becomes completely unable to extract the feature points FP of the image data or when it suddenly becomes unable to track the feature points FP.

[0077] The rest of the configuration is the same as in the fourth embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the fourth embodiment, which are achieved by a configuration common to or equivalent to the fourth embodiment.

[0078] (Sixth embodiment) Next, a sixth embodiment will be described with reference to Fig. 20. In this embodiment, differences from the fourth embodiment will be mainly described.

[0079] In an abnormal situation where it is difficult for the imaging unit 2 to perform its intended function, the accuracy of estimation by the visual inertial odometry VIO of the attitude φ of the vehicle 1 is likely to decrease. Conversely, if the accuracy of estimation by the visual inertial odometry VIO of the attitude φ of the vehicle 1 is decreased, there is a possibility that an abnormal situation has occurred where it is difficult for the imaging unit 2 to perform its intended function.

[0080] 20, the abnormality determination unit 31 determines whether or not an abnormal situation exists in which it is difficult for the imaging unit 2 to perform its intended function, based on the sensor output of the steering angle sensor 5 installed in the vehicle 1 and the attitude φ of the vehicle 1 calculated by the calculation unit 26. For example, the abnormality determination unit 31 determines that an abnormal situation exists when the attitude φ of the vehicle 1 calculated from the sensor output of the steering angle sensor 5 and the attitude φ of the vehicle 1 calculated by the calculation unit 26 deviate by more than a reference value.

[0081] The rest of the configuration is the same as in the fourth embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the fourth embodiment, which are achieved by a configuration common to or equivalent to the fourth embodiment.

[0082] Moreover, the state estimating device 10 of this embodiment has the following features.

[0083] (1) The abnormality determination unit 31 determines whether or not an abnormal situation exists in which it is difficult for the imaging unit 2 to perform its intended function, based on the sensor output of the steering angle sensor 5 and the attitude φ of the vehicle 1 calculated by the calculation unit 26. In this way, if the sensor output of the steering angle sensor 5 already installed in the vehicle 1 is used to determine whether or not an abnormal situation exists, there is no need to add a sensor device dedicated to determining abnormalities in the imaging unit 2, and therefore it is possible to continue estimating the state of the vehicle 1 in a simple manner.

[0084] (Seventh embodiment) Next, a seventh embodiment will be described with reference to Fig. 21. In this embodiment, differences from the fourth embodiment will be mainly described.

[0085] In an abnormal situation where it is difficult for the image capturing unit 2 to perform its intended function, the accuracy of estimating the velocity v by the visual inertial odometry VIO is likely to decrease. Conversely, if the accuracy of estimating the velocity v of the vehicle 1 by the visual inertial odometry VIO decreases, there is a possibility that an abnormal situation has occurred where it is difficult for the image capturing unit 2 to perform its intended function.

[0086] 21, the abnormality determination unit 31 determines whether or not an abnormal situation exists in which it is difficult for the imaging unit 2 to perform its intended function, based on the sensor output of the wheel speed sensor 4 installed on the vehicle 1 and the speed v of the vehicle 1 calculated by the calculation unit 26. For example, the abnormality determination unit 31 determines that an abnormal situation exists when the speed v of the vehicle 1 calculated from the sensor output of the wheel speed sensor 4 and the speed v of the vehicle 1 calculated by the calculation unit 26 deviate by more than a reference value.

[0087] The rest of the configuration is the same as in the fourth embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the fourth embodiment, which are achieved by a configuration common to or equivalent to the fourth embodiment.

[0088] Moreover, the state estimating device 10 of this embodiment has the following features.

[0089] (1) The abnormality determination unit 31 determines whether or not an abnormal situation exists that makes it difficult for the imaging unit 2 to perform its intended function, based on the sensor output of the wheel speed sensor 4 and the speed v of the vehicle 1 calculated by the calculation unit 26. In this way, if the sensor output of the wheel speed sensor 4 already installed in the vehicle 1 is used to determine whether or not an abnormal situation exists, there is no need to add a sensor device dedicated to determining abnormalities in the imaging unit 2, and therefore it is possible to continue estimating the state of the vehicle 1 in a simple manner.

[0090] (Eighth embodiment) Next, an eighth embodiment will be described with reference to Fig. 22. In this embodiment, differences from the fourth embodiment will be mainly described.

[0091] In an abnormal situation where it is difficult for the imaging unit 2 to perform its intended function, the accuracy of estimation of the position p, velocity v, and attitude φ by the visual inertial odometry VIO is likely to decrease. Therefore, in an abnormal situation where it is difficult for the imaging unit 2 to perform its intended function, the position p, velocity v, and attitude φ of the vehicle 1 calculated by the calculation unit 26 are likely to diverge from the position p, velocity v, and attitude φ of the vehicle 1 estimated by the estimation unit 30.

[0092] 22, the abnormality determination unit 31 determines whether or not an abnormal situation exists in which it is difficult for the imaging unit 2 to perform its intended function, based on the position p, speed v, and attitude φ of the vehicle 1 calculated by the calculation unit 26 and the position p, speed v, and attitude φ estimated by the estimation unit 30. For example, the abnormality determination unit 31 determines that an abnormal situation exists when at least one of the position p, speed v, and attitude φ of the vehicle 1 calculated by the calculation unit 26 deviates from the position p, speed v, and attitude φ of the vehicle 1 estimated by the estimation unit 30 by more than a reference value.

[0093] The rest of the configuration is the same as in the fourth embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the fourth embodiment, which are achieved by a configuration common to or equivalent to the fourth embodiment.

[0094] Moreover, the state estimating device 10 of this embodiment has the following features.

[0095] (1) The abnormality determination unit 31 determines whether or not an abnormal situation exists that makes it difficult for the imaging unit 2 to perform its intended function, based on the position p, speed v, and attitude φ of the vehicle 1 calculated by the calculation unit 26 and the position p, speed v, and attitude φ of the vehicle 1 estimated by the estimation unit 30. This eliminates the need to add a sensor device dedicated to abnormality determination for the imaging unit 2, making it possible to continue estimating the state of the vehicle 1 in a simple manner.

[0096] (Ninth embodiment) Next, a ninth embodiment will be described with reference to Fig. 23. In this embodiment, differences from the fourth embodiment will be mainly described.

[0097] The bias error of the inertial measurement unit 3 has the characteristic of changing depending on the temperature of the inertial measurement unit 3. For this reason, it is desirable that the bias estimation value used when calculating the correction data in the correction unit 28 is not a fixed value, but a variable value that changes depending on the temperature of the inertial measurement unit 3.

[0098] Taking this into consideration, the correction unit 28 corrects the bias estimate value in accordance with the measurement result of the temperature of the inertial measurement unit 3, and calculates the correction data by removing the corrected bias estimate value from the inertial data, as shown in Fig. 23. For example, if the bias error is likely to increase as the temperature of the inertial measurement unit 3 increases, the correction unit 28 corrects the bias estimate value stored in memory 50 by adding a predetermined value to it when the temperature of the inertial measurement unit 3 increases.

[0099] Here, the means for measuring the temperature of the inertial measurement unit 3 may be a temperature sensor 6 attached to the inertial measurement unit 3, or may be estimated from the outside air temperature and the usage conditions of the inertial measurement unit 3. Furthermore, the correction of the bias estimate value is not limited to the above, and may be performed by other methods.

[0100] The rest of the configuration is the same as in the fourth embodiment. The state estimation device 10 and the state estimation method of the present embodiment can obtain the same effects as those of the fourth embodiment, which are achieved by a configuration common to or equivalent to the fourth embodiment.

[0101] Moreover, the state estimating device 10 of this embodiment has the following features.

[0102] (1) The correction unit 28 corrects the bias estimate value according to the temperature of the inertial measurement unit 3 and calculates the correction data by removing the corrected bias estimate value from the inertial data. This allows the state of the vehicle 1 to be continued in an appropriate manner even if an abnormal situation occurs in which the imaging unit 2 cannot perform its intended function.

[0103] (Other embodiments) Representative embodiments of the present disclosure have been described above, but the present disclosure is not limited to the above-described embodiments and can be modified in various ways, for example, as follows.

[0104] Although the state estimation device 10 of the above embodiment has been described as being applied to the vehicle 1, the state estimation device 10 is not limited to this. The state estimation device 10 can be applied to moving bodies other than the vehicle 1.

[0105] The state estimation device 10 in the above embodiment is exemplified as estimating the position p, velocity v, and attitude φ of the vehicle 1, but is not limited to this. The state estimation device 10 may be configured to estimate a state including part of the position p, velocity v, and attitude φ of the vehicle 1.

[0106] In the above-described embodiments, it goes without saying that the elements constituting the embodiments are not necessarily essential unless they are specifically stated as essential or are clearly considered essential in principle.

[0107] In the above-described embodiments, when numerical values ​​such as the number, values, amounts, ranges, etc. of components of the embodiments are mentioned, they are not limited to the specific numbers unless they are specifically stated as essential or are clearly limited to a specific number in principle.

[0108] In the above-described embodiments, when referring to the shapes, positional relationships, etc. of components, etc., the shapes, positional relationships, etc. are not limited to those unless otherwise specified or when they are fundamentally limited to specific shapes, positional relationships, etc.

[0109] The controller and method of the present disclosure may be implemented on a special-purpose computer by configuring a processor and memory programmed to perform one or more functions embodied in a computer program. The controller and method of the present disclosure may be implemented on a special-purpose computer by configuring a processor with one or more dedicated hardware logic circuits. The controller and method of the present disclosure may be implemented on one or more special-purpose computers configured with a processor and memory programmed to perform one or more functions in combination with a processor configured with one or more hardware logic circuits. The computer program may also be stored on a computer-readable non-transitory tangible storage medium as instructions executed by a computer.

[0110] (Features of the present disclosure) [Disclosure 1] A state estimation device that estimates a state including at least one of a position, a velocity, and an attitude of a moving body (1), an input unit (22) that reads image data output from an imaging unit (2) that images the surroundings of the moving body and inertial data of the moving body output from an inertial measurement unit (3) installed on the moving body; a pre-processing unit (24) that extracts feature points included in the image data, tracks the feature points, and calculates the position, velocity, and attitude of the moving object based on the inertial data; a calculation unit (26) that performs bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to determine a bias error of the inertial measurement unit; a correction unit (28) that obtains correction data by removing the bias error from the inertial data; an estimation unit (30) that estimates a state including at least one of a position, a velocity, and an attitude of the moving body based on the correction data; A state estimation device comprising:

[0111] [Disclosure 2] The state estimation device according to Disclosure 1, wherein the estimation unit calculates a speed of the moving body based on the correction data and a sensor output of a wheel speed sensor (4) installed on the moving body, and estimates a position of the moving body based on the calculated speed of the moving body.

[0112] [Disclosure 3] the inertial measurement unit includes a gyro sensor (3a) that detects the angular velocity of the moving body and an acceleration sensor (3b) that detects the acceleration of the moving body; The estimation unit determining a first attitude angle indicating the attitude of the moving body based on the sensor output of the gyro sensor corrected by the correction unit; calculating a second attitude angle indicating the attitude of the moving body based on the sensor output of the acceleration sensor corrected by the correction unit and the gravitational acceleration calculated from the sensor output of the wheel speed sensor of the moving body; the state estimation device according to Disclosure 1 or 2, wherein the posture of the moving body is estimated by passing the first posture angle through a high-pass filter of a complementary filter, passing the second posture angle through a low-pass filter of the complementary filter, and then combining the two.

[0113] [Disclosure 4] an abnormality determination unit (31) that determines whether or not an abnormal situation exists in which it is difficult for the imaging unit to perform its intended function; The correction unit If the abnormal situation is not present, the correction data is calculated by removing the bias error from the inertial data; the state estimation device according to any one of Disclosures 1 to 3, wherein, when the abnormal situation occurs, the correction data is calculated by removing a bias estimation value previously stored in a memory from the inertia data, instead of the bias error calculated by the calculation unit.

[0114] [Disclosure 5] The state estimation device according to Disclosure 4, wherein the abnormality determination unit determines whether or not the abnormal situation exists based on the image data.

[0115] [Disclosure 6] The state estimation device according to Disclosure 4 or 5, wherein the abnormality determination unit determines whether or not the abnormal situation exists based on a sensor output of a steering angle sensor (5) installed in the moving body and the attitude of the moving body calculated by the calculation unit.

[0116] [Disclosure 7] The state estimation device according to any one of Disclosures 4 to 6, wherein the abnormality determination unit determines whether or not the abnormal situation exists based on a sensor output of a wheel speed sensor (4) installed on the moving body and the speed of the moving body calculated by the calculation unit.

[0117] [Disclosure 8] The state estimation device according to any one of Disclosures 4 to 7, wherein the abnormality determination unit determines whether or not the abnormal situation exists based on the position, speed, and attitude of the moving body calculated by the calculation unit and the position, speed, and attitude of the moving body estimated by the estimation unit.

[0118] [Disclosure 9] 9. The state estimation device according to any one of Disclosures 4 to 8, wherein the correction unit corrects the bias estimation value in accordance with a temperature of the inertial measurement unit, and calculates the correction data by removing the corrected bias estimation value from the inertial data.

[0119] [Disclosure 10] A state estimation method for estimating a state including at least one of a position, a velocity, and an attitude of a moving object (1), reading image data output from an imaging unit (2) that images the surroundings of the moving body and inertial data of the moving body output from an inertial measurement unit (3) installed on the moving body; extracting feature points included in the image data and tracking the feature points, and calculating the position, velocity, and attitude of the moving object based on the inertial data; performing bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to obtain a bias error of the inertial measurement unit; obtaining correction data by removing the bias error from the inertial data; estimating a state including at least one of a position, a velocity, and an attitude of the moving body based on the correction data; A state estimation method including: [Explanation of symbols]

[0120] 1. Vehicle (moving object) 2. Imaging unit 3 Inertial Measurement Unit 10 State Estimation Device 22 Input section 24 Pretreatment section 26 Arithmetic section 28 Correction section 30 Estimation part

Claims

1. A state estimation device that estimates a state including at least one of a position, a velocity, and an attitude of a moving body (1), an input unit (22) that reads image data output from an imaging unit (2) that images the surroundings of the moving body and inertial data of the moving body output from an inertial measurement unit (3) installed on the moving body; a pre-processing unit (24) that extracts feature points included in the image data, tracks the feature points, and calculates the position, velocity, and attitude of the moving object based on the inertial data; a calculation unit (26) that performs bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to determine a bias error of the inertial measurement unit; a correction unit (28) that obtains correction data by removing the bias error from the inertial data; an estimation unit (30) that estimates a state including at least one of the position, velocity, and attitude of the moving body based on the correction data, The calculation unit a residual error relating to the image is defined as a reprojection error between the image coordinate system and the world coordinate system; a residual error relating to the IMU is defined as a difference between the measurement results of the position and attitude by the inertial measurement unit and the prediction results of the position and attitude predicted from the image data; and a residual error relating to prior information is defined as a difference between the position and attitude of the moving body estimated from the most recent information and the position and attitude of the moving body estimated from prior information. a state estimation device that estimates the bias error by optimizing a residual related to the image, a residual related to the IMU, and a residual related to the prior information using the bundle adjustment;

2. 2. The state estimation device according to claim 1, wherein the estimation unit calculates a speed of the moving body based on the correction data and a sensor output of a wheel speed sensor (4) installed on the moving body, and estimates a position of the moving body based on the calculated speed of the moving body.

3. the inertial measurement unit includes a gyro sensor (3 a) that detects the angular velocity of the moving body and an acceleration sensor (3 b) that detects the acceleration of the moving body; The estimation unit determining a first attitude angle indicating the attitude of the moving body based on the sensor output of the gyro sensor corrected by the correction unit; calculating a second attitude angle indicating the attitude of the moving body based on the sensor output of the acceleration sensor corrected by the correction unit and the gravitational acceleration calculated from the sensor output of the wheel speed sensor of the moving body; 3. The state estimation device according to claim 2, wherein the attitude of the moving body is estimated by passing the first attitude angle through a high-pass filter of a complementary filter, passing the second attitude angle through a low-pass filter of the complementary filter, and then combining the two.

4. an abnormality determination unit (31) for determining whether or not an abnormal situation exists in which it is difficult for the imaging unit to perform its intended function; The correction unit If the abnormal situation is not present, the correction data is calculated by removing the bias error from the inertial data; When the abnormal situation occurs, the correction data is calculated by removing a bias estimate value previously stored in a memory from the inertia data instead of the bias error calculated by the calculation unit; 4. The state estimation device according to claim 1, wherein the abnormality determination unit determines that the abnormal situation exists when it becomes completely unable to extract feature points from the image data or when it suddenly becomes unable to track the feature points.

5. an abnormality determination unit (31) for determining whether or not an abnormal situation exists in which it is difficult for the imaging unit to perform its intended function; The correction unit If the abnormal situation is not present, the correction data is calculated by removing the bias error from the inertial data; When the abnormal situation occurs, the correction data is calculated by removing a bias estimate value previously stored in a memory from the inertia data instead of the bias error calculated by the calculation unit; 4. The state estimation device according to claim 1, wherein the abnormality determination unit determines that the abnormal situation exists when the attitude of the moving body based on a sensor output of a steering angle sensor (5) installed on the moving body and the attitude of the moving body calculated by the calculation unit deviate by more than a reference value.

6. an abnormality determination unit (31) for determining whether or not an abnormal situation exists in which it is difficult for the imaging unit to perform its intended function; The correction unit If the abnormal situation is not present, the correction data is calculated by removing the bias error from the inertial data; When the abnormal situation occurs, the correction data is calculated by removing a bias estimate value previously stored in a memory from the inertia data instead of the bias error calculated by the calculation unit; 4. The state estimation device according to claim 1, wherein the abnormality determination unit determines that the abnormal situation exists when a deviation between a speed of the moving body based on a sensor output of a wheel speed sensor (4) installed on the moving body and a speed of the moving body calculated by the calculation unit exceeds a reference value.

7. an abnormality determination unit (31) for determining whether or not an abnormal situation exists in which it is difficult for the imaging unit to perform its intended function; The correction unit If the abnormal situation is not present, the correction data is calculated by removing the bias error from the inertial data; When the abnormal situation occurs, the correction data is calculated by removing a bias estimate value previously stored in a memory from the inertia data instead of the bias error calculated by the calculation unit; 4. The state estimation device according to claim 1, wherein the abnormality determination unit determines that the abnormal situation exists when the position, velocity, and attitude of the moving body calculated by the calculation unit deviate from the position, velocity, and attitude of the moving body estimated by the estimation unit by more than a reference value.

8. The state estimation device according to claim 4 , wherein the correction unit corrects the bias estimate value in accordance with a temperature of the inertial measurement unit, and calculates the correction data by removing the corrected bias estimate value from the inertial data.

9. A state estimation method for estimating a state including at least one of a position, a velocity, and an attitude of a moving body (1), comprising: reading image data output from an imaging unit (2) that images the surroundings of the moving body and inertial data of the moving body output from an inertial measurement unit (3) installed on the moving body; extracting feature points included in the image data and tracking the feature points, and calculating the position, velocity, and attitude of the moving object based on the inertial data; performing bundle adjustment on the feature points of the image data and the position, velocity, and attitude of the moving body based on the inertial data to obtain a bias error of the inertial measurement unit; obtaining correction data by removing the bias error from the inertial data; and estimating a state including at least one of a position, a velocity, and an attitude of the moving body based on the correction data; When calculating the bias error, a residual error relating to the image is defined as a reprojection error between the image coordinate system and the world coordinate system; a residual error relating to the IMU is defined as a difference between the measurement results of the position and attitude by the inertial measurement unit and the prediction results of the position and attitude predicted from the image data; and a residual error relating to prior information is defined as a difference between the position and attitude of the moving body estimated from the most recent information and the position and attitude of the moving body estimated from prior information. A state estimation method comprising: estimating the bias error by optimizing a residual related to the image, a residual related to the IMU, and a residual related to the prior information using the bundle adjustment.

Images