MOTION ESTIMATION UTILIZING RANGE DETECTION-ENHANCED VISUAL ODOMETRY

• US 20140300732A1
• Filed: 04/05/2013
• Published: 10/09/2014
• Est. Priority Date: 04/05/2013
• Status: Active Grant

• Current Assignee: Caterpillar Incorporated
• Original Assignee: Caterpillar Incorporated
• Inventors: FRIEND, Paul Russell, CHEN, Qi, CHANG, Hong, MORRIS, Daniel Dale, GRAF, Jodi Seaborn
• Granted Patent: US 9,251,587 B2
• Time in Patent Office: Days
• US Class Current: 348/135
• CPC Class Codes: G06T 2200/04 involving 3D image data
• CPC Class Codes: G06T 2207/10016 Video; Image sequence
• CPC Class Codes: G06T 2207/10028 Range image; Depth image; 3D point clouds
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 2207/30252 Vehicle exterior; Vicinity of vehicle
• CPC Class Codes: G06T 7/73 using feature-based methods

Abstract

A motion determination system is disclosed. The system may receive a first and a second camera image from a camera, the first camera image received earlier than the second camera image. The system may identify corresponding features in the first and second camera images. The system may receive range data comprising at least one of a first and a second range data from a range detection unit, corresponding to the first and second camera images, respectively. The system may determine first positions and the second positions of the corresponding features using the first camera image and the second camera image. The first positions or the second positions may be determined by also using the range data. The system may determine a change in position of the machine based on differences between the first and second positions, and a VO-based velocity of the machine based on the determined change in position.

Claims

1. A motion determination system for determining motion of a machine, the system comprising:

one or more memories storing instructions; and

one or more processors configured to execute the instructions to perform operations, including;

receiving a first camera image and a second camera image from a camera affixed to the machine, wherein the first camera image is received earlier in time than the second camera image;

identifying corresponding features in the first camera image and the second camera image;

receiving range data, wherein the range data comprises at least one of a first range data and a second range data from a range detection unit affixed to the machine, and the first range data and the second range data correspond to the first camera image and the second camera image, respectively;

determining first positions of the corresponding features using the first camera image and determining second positions of the corresponding features using the second camera image, wherein the first positions or the second positions are determined by also using the range data;

determining a change in position of the machine based on differences between the first positions and the second positions; and

determining a VO-based velocity of the machine based on the determined change in position of the machine.

2. The system of claim 1, wherein:

the range data includes the first range data and the second range data;

the first positions are determined based on both the first camera image and the first range data; and

the second positions are determined based on both the second camera image and the second range data.

3. The system of claim 1, wherein the range data comprises the first range data and the second range data, and the one or more processors are further configured to execute the instructions to perform:

determining a range-based velocity of the machine based on changes between the first range data and the second range data; and

fusing the range-based velocity with the VO-based velocity to generate a fused-VO velocity.

4. The system of claim 3, wherein the one or more processors are further configured to execute the instructions to perform:

generate the fused-VO velocity based on a weighted average of the range-based velocity and the VO-based velocity, wherein the weighted average is based on uncertainty measures for the range-based velocity and the VO-based velocity.

5. The system of claim 3, wherein the one or more processors are further configured to execute the instructions to perform:

generate the fused-VO velocity based on an output of a Kalman filter, by;

utilizing the VO-based velocity in a propagation stage of the Kalman filter, and

utilizing the range-based velocity in a measurement update stage of the Kalman filter.

6. The system of claim 3, wherein the one or more processors are further configured to execute the instructions to perform:

fusing the range-based velocity and the VO-based velocity based on both velocities being valid, wherein the velocities are valid when associated uncertainty measures are below a predetermined threshold.

7. The system of claim 3, wherein the one or more processors are further configured to execute the instructions to perform:

receiving one or more IMU measurement from an IMU affixed to the machine;

determining one or more IMU-derived measurements derived from one or more IMU measurements; and

fusing at least one of the IMU measurements or at least one of the IMU-derived measurements with either the fused-VO velocity or a measurement derived from the fused-VO velocity to generate a fused-VO-IMU measurement.

8. The system of claim 7, wherein the one or more processors are further configured to execute the instructions to perform:

generating the fused-VO-IMU measurement based on a weighted average, wherein the weighted average is determined based on an uncertainty measure for each measurement that is being fused.

10. The system of claim 7, wherein the one or more processors are further configured to execute the instructions to perform:

fusing one or more derived measurements with one or more direct measurements to output the position of the machine using a Kalman filter, wherein

one or more derived measurements comprises the fused-VO-IMU measurement,

one or more direct measurements comprises an IMU measurement, the fused-VO velocity, or a relative measurement based on a key frame image obtained by at least one of the camera and the range detection device,

the one or more derived measurements are used in a propagation phase of the Kalman filter, and

the one or more direct measurements are used in a measurement update phase of the Kalman filter.

11. The system of claim 3, wherein the one or more processors are further configured to execute the instructions to perform:

correcting an IMU bias by utilizing a Kalman filter,

wherein an IMU-derived measurement derived from an IMU measurement is utilized in a propagation phase of the Kalman filter, and

a relative measurement based on a key frame image obtained by at least one of the camera and the range detection device is utilized in a measurement update phase of the Kalman filter.

9. The system of 7, wherein:

the fusion to generate a fused-VO-IMU measurement occurs when each measurement that is being fused is valid, wherein each measurement is valid when associated uncertainty measures are below a predetermined threshold.

12. A computer-implemented method for determining motion of a machine, the method comprising:

receiving, by one or more computer processors, a first camera image and a second camera image from a camera affixed to the machine, wherein the first camera image is received earlier in time than the second camera image;

identifying, by the one or more computer processors, corresponding features in the first camera image and the second camera image;

receiving, by the one or more computer processors, range data, wherein the range data comprises at least one of a first range data and a second range data from a range detection unit affixed to the machine, and the first range data and the second range data correspond to the first camera image and the second camera image, respectively;

determining first positions of the corresponding features using the first camera image and determining second positions of the corresponding features using the second camera image, wherein the first positions or the second positions are determined by also using the range data;

determining, by the one or more computer processors, a change in position of the machine based on differences between the first positions and the second positions; and

determining, by the one or more computer processors, a VO-based velocity of the machine based on the determined change in position of the machine.

13. The method of claim 12, wherein:

the range data includes the first range data and the second range data;

the first positions are determined based on both the first camera image and the first range data; and

the second positions are determined based on both the second camera image and the second range data.

14. The method of claim 12, wherein the range data comprises the first range data and the second range data, further including:

determining a range-based velocity of the machine based on changes between the first range data and the second range data; and

fusing the range-based velocity with the VO-based velocity to generate a fused-VO velocity, wherein

the fused-VO velocity is generated based on a weighted average of the range-based velocity and the VO-based velocity, and the weighted average is based on uncertainty measures for the range-based velocity and the VO-based velocity, or

the fused-VO velocity is generated based on an output of a Kalman filter, by;

utilizing the VO-based velocity in a propagation stage of the Kalman filter, and

utilizing the range-based velocity in a measurement update stage of the Kalman filter.

15. The method of claim 14, further including:

receiving one or more IMU measurement from an IMU affixed to the machine;

determining one or more IMU-derived measurements derived from one or more IMU measurements; and

fusing at least one of the IMU measurements or at least one of the IMU-derived measurements with either the fused-VO velocity or a measurement derived from the fused-VO velocity to generate a fused-VO-IMU measurement, wherein

the fused-VO-IMU measurement is based on a weighted average, wherein the weighted average is determined based on an uncertainty measure for each measurement that is being fused.

16. The method of claim 15, further including:

fusing one or more derived measurements with one or more direct measurements to output the position of the machine using a Kalman filter, wherein

one or more derived measurements comprises the fused-VO-IMU measurement,

one or more direct measurements comprises an IMU measurement, the fused-VO velocity, or a relative measurement based on a key frame image obtained by at least one of the camera and the range detection device,

the one or more derived measurements are used in a propagation phase of the Kalman filter, and

the one or more direct measurements are used in a measurement update phase of the Kalman filter.

17. The method of claim 14, further including:

correcting an IMU bias utilizing a Kalman filter,

wherein an IMU-derived measurement derived from an IMU measurement is utilized in a propagation phase of the Kalman filter, and

a relative measurement based on a key frame image obtained by at least one of the camera and the range detection device is utilized in a measurement update phase of the Kalman filter.

18. The method of claim 14, wherein:

the range-based velocity and the VO-based velocity are fused based on both velocities being valid, wherein the velocities are valid when associated uncertainty measures are below a predetermined threshold.

19. The method of claim 15, wherein:

the fusion to generate the fused-VO-IMU measurement occurs when each measurement that is being fused is valid, wherein each measurement is valid when associated uncertainty measures are below a predetermined threshold.

20. A machine, comprising:

a camera affixed to the machine;

a range detection unit affixed to the machine; and

a controller in communication with the camera and the range detection unit, configured to;

receive a first camera image and a second camera image from the camera, wherein the first camera image is received earlier in time than the second camera image;

identify corresponding features in the first camera image and the second camera image;

receive range data, wherein the range data comprises at least one of a first range data and a second range data from the range detection unit, and the first range data and the second range data corresponding to the first camera image and the second camera image, respectively,

determine first positions of the corresponding features using the first camera image and determine second positions of the corresponding features using the second camera image, wherein the first positions or the second positions are determined by also using the range data;

determine a change in position of the machine based on differences between the first positions and the second positions; and

determine a VO-based velocity of the machine based on the determined change in position of the machine.

21. The machine of claim 20, wherein:

the range data includes the first range data and the second range data;

the first positions are determined based on both the first camera image and the first range data; and

the second positions are determined based on both the second camera image and the second range data.

22. The machine of claim 20, wherein the range data comprises the first range data and the second range data, and the controller is further configured to:

determine a range-based velocity of the machine based on differences between the first range data and the second range data; and

fuse the range-based velocity with the VO-based velocity to generate a fused-VO velocity, wherein

the fused-VO velocity is generated based on a weighted average of the range-based velocity and the VO-based velocity, wherein the weighted average is determined based on uncertainty measures for the range-based velocity and the VO-based velocity, or

the fused-VO velocity is generated based on an output of a Kalman filter, by;

utilizing the VO-based velocity in a propagation stage of the Kalman filter, and

utilizing the range-based velocity in a measurement update stage of the Kalman filter.

23. The machine of claim 22, wherein the controller is further configured to:

receive one or more IMU measurement from an IMU affixed to the machine;

determine one or more IMU-derived measurement derived from one or more IMU measurements; and

fuse an IMU measurement or an IMU-derived measurement with either the fused-VO velocity or a measurement derived from the fused-VO velocity to generate a fused-VO-IMU measurement, wherein

the fused-VO-IMU measurement is generated based on a weighted average, and the weighted average is determined based on an uncertainty measure for each measurement that is being fused.