State estimation device and state estimation method

Patent metadata

Field	Value
Publication number	US-12584747-B2
Application number	US-202318309225-A
Country	US
Kind code	B2
Filing date	Apr 28, 2023
Priority date	Jun 16, 2022
Publication date	Mar 24, 2026
Grant date	Mar 24, 2026

Abstract

By a state estimation device or a state estimation method, image data is read, a feature point included in the image data is extracted, the feature point is tracked, a position, a velocity, or an attitude of a mobile object is calculated based on inertia data, and a bias error of an inertial measurement unit is calculated. Correction data is calculated by removing the bias error from the inertia data, and a state including at least one of the position, the velocity, or the attitude of the mobile object is estimated based on the correction data.

First claim

The invention claimed is:

1 . A state estimation device for estimating a state including at least one of a position, a velocity, or an attitude of a mobile object, the state estimation device comprising:
at least one processor configured to
read image data which is output by a camera configured to capture an image of a peripheral area of the mobile object and inertia data of the mobile object, the inertia data being output from an inertial measurement unit installed on the mobile object;
perform preprocessing to
extract a feature point included in the image data,
track the feature point, and
calculate the position, the velocity, or the attitude of the mobile object by performing inertia processing on the inertia data;

calculate a bias error of the inertial measurement unit by performing bundle adjustment on the feature point of the image data, the position, the velocity, or the attitude of the mobile object based on the inertia data;
calculate correction data by removing the bias error from the inertia data on which the inertia processing is not performed; and
estimate a state including at least one of the position, the velocity, or the attitude of the mobile object based on the correction data.

2 . The state estimation device according to claim 1 , wherein
the at least one processor is configured to
estimate the velocity of the mobile object based on the correction data and a sensor output of a vehicle wheel sensor installed on the mobile object, and
estimate the position of the mobile object based on an estimated speed of the mobile object.

3 . The state estimation device according to claim 2 , wherein
the inertial measurement unit includes a gyro sensor that detects an angular velocity of the mobile object and an acceleration sensor that detects an acceleration of the mobile object, and the at least one processor is configured to
calculate a first attitude angle indicating an attitude angle of the mobile object based on a result obtained by correction of a sensor output of the gyro sensor, the correction being performed by the at least one processor,
calculate a second attitude angle indicating an attitude of the mobile object based on a gravitational acceleration calculated using a result obtained by correction of a sensor output of the acceleration sensor by the at least one processor and a sensor output of a wheel speed sensor of the mobile object,
pass the first attitude angle through a high-pass filter of a complementary filter and passes the second attitude angle through a low-pass filter of the complementary filter, and
after passing the first attitude angle and the second attitude angle, synthesize the first attitude angle and the second attitude angle to estimate the attitude of the mobile object.

4 . The state estimation device according to claim 1 , the at least one processor is further configured to
determine whether an abnormal situation where the camera has difficulty performing a predetermined function has occurred, wherein the at least one processor
calculates the correction data by removing the bias error from the inertia data on which the inertia processing is not performed when the abnormal situation has not occurred, and
calculates the correction data by removing, instead of the bias error which was calculated by the at least one processor, a bias error estimation value stored in a memory in advance from the inertia data on which the inertia processing is not performed when the abnormal situation has occurred.

5 . The state estimation device according to claim 4 , wherein
the at least one processor is configured to determine whether the abnormal situation has occurred based on the image data.

6 . The state estimation device according to claim 4 , wherein
the at least one processor is configured to determine whether the abnormal situation has occurred based on a sensor output of a steering angle sensor installed on the mobile object and the attitude of the mobile object, the attitude being calculated by the at least one processor.

7 . The state estimation device according to claim 4 , wherein
the at least one processor is configured to determine whether the abnormal situation has occurred based on a sensor output of a vehicle wheel speed sensor installed on the mobile object and a speed of the mobile object, the speed being calculated by the at least one processor.

8 . The state estimation device according to claim 4 , wherein
the at least one processor is configured to determine whether the abnormal situation has occurred based on the position, the velocity, and the attitude calculated by the at least one processor and the position, the velocity, and the attitude estimated by the at least one processor.

9 . The state estimation device according to claim 4 , wherein
the at least one processor is configured to
correct the bias error estimation value according to a temperature of the inertial measurement unit, and
calculate the correction data by removing the bias error estimation value which is corrected from the inertia data on which the inertia processing is not performed.

10 . The state estimation device according to claim 1 , wherein the bundle adjustment is a nonlinear least-squares method, and the at least one processor is configured to optimize a residual of the image data which is output from the camera and a residual of the image data by the bundle adjustment to calculate the bias error.

11 . The state estimation device according to claim 1 ,
wherein the inertia processing integrates the inertia data, and the inertia processing is performed before the bias error is calculated.

12 . A state estimation method for estimating a state including at least one of a position, a velocity, or an attitude of a mobile object, the state estimation method comprising:
reading image data which is output by a camera configured to capture an image of a peripheral area of the mobile object and inertia data of the mobile object, the inertia data being output from an inertial measurement unit installed on the mobile object; extracting a feature point included in the image data; tracking the feature point; calculating the position, the velocity, or the attitude of the mobile object by performing inertia processing on the inertia data; calculating a bias error of the inertial measurement unit by performing bundle adjustment on the feature point of the image data and the position, the velocity, and the attitude of the mobile object based on the inertia data; calculating correction data by removing the bias error from the inertia data on which the inertia processing is not performed; and estimating a state including at least one of the position, the velocity, or the attitude of the mobile object based on the correction data.

13 . The state estimation method according to claim 12 , further comprising
estimating the velocity of the mobile object based on the correction data and a sensor output of a vehicle wheel sensor installed on the mobile object, and estimating the position of the mobile object based on an estimated speed of the mobile object.

14 . The state estimation method according to claim 13 , wherein
the inertial measurement unit includes
a gyro sensor that detects an angular velocity of the mobile object and
an acceleration sensor that detects an acceleration of the mobile object, the state estimation method further comprising
calculating a first attitude angle indicating an attitude angle of the mobile object based on a result obtained by correction of a sensor output of the gyro sensor,
calculating a second attitude

Assignees

• Denso Corp
• Toyota Motor Co Ltd
• MIRISE Technologies Corporation

Inventors

• Matsudaira Nobuaki
• Xu Hui

Classifications

• G06T7/246

  using feature-based methods, e.g. the tracking of corners or segments · CPC title

• G06V10/44

  Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components · CPC title

• G06T2207/30244

  Camera pose · CPC title

• G06T7/73

  using feature-based methods · CPC title

• G01C25/005

  initial alignment, calibration or starting-up of inertial devices · CPC title