Autonomous Navigation using Visual Odometry

• US 20170277197A1
• Filed: 03/22/2016
• Published: 09/28/2017
• Est. Priority Date: 03/22/2016
• Status: Active Grant

• Current Assignee: Sharp Kabushiki Kaisha (Hon Hai Precision Industry Co., Ltd.)
• Original Assignee: Sharp Laboratories of America Incorporated (Hon Hai Precision Industry Co., Ltd.)
• Inventors: Liao, Miao, Li, Ming, Hong, Soonhac
• Granted Patent: US 9,946,264 B2
• Time in Patent Office: Days
• CPC Class Codes: G05D 1/0251 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision stereoscopic image analysis H04N13/00; depth recovery from images G06T7/593
• CPC Class Codes: G05D 1/0253 extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow determining position or orientation from images G06T7/70
• CPC Class Codes: G06T 2207/20228 Disparity calculation for image-based rendering
• CPC Class Codes: G06T 2207/30261 Obstacle
• CPC Class Codes: G06T 7/20 Analysis of motion motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51
• CPC Class Codes: G06T 7/246 using feature-based methods, e.g. the tracking of corners or segments
• CPC Class Codes: G06T 7/55 from multiple images
• CPC Class Codes: G06T 7/593 from stereo images
• CPC Class Codes: G06V 10/757 Matching configurations of points or features
• CPC Class Codes: G06V 20/58 Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
• CPC Class Codes: H04L 12/40 Bus networks
• CPC Class Codes: H04L 12/40013 Details regarding a bus controller
• CPC Class Codes: H04L 2012/40273 the transportation system being a vehicle

Abstract

A system and method are provided for autonomously navigating a vehicle. The method captures a sequence of image pairs using a stereo camera. A navigation application stores a vehicle pose (history of vehicle position). The application detects a plurality of matching feature points in a first matching image pair, and determines a plurality of corresponding object points in three-dimensional (3D) space from the first image pair. A plurality of feature points are tracked from the first image pair to a second image pair, and the plurality of corresponding object points in 3D space are determined from the second image pair. From this, a vehicle pose transformation is calculated using the object points from the first and second image pairs. The rotation angle and translation are determined from the vehicle pose transformation. If the rotation angle or translation exceed a minimum threshold, the stored vehicle pose is updated.

Claims

1. A method for autonomously navigating a vehicle, the method comprising:

capturing a sequence of image pairs using a stereo camera;

a navigation application embedded in a non-transitory memory including a sequence of processor executable instructions for;

storing a vehicle pose, where a vehicle pose is defined as a position and orientation with respect to a coordination frame;

detecting a plurality of matching feature points in a first matching image pair;

determining a plurality of corresponding object points in three-dimensional (3D) space from the first image pair;

tracking the plurality of feature points from the first image pair to a second image pair;

determining a plurality of corresponding object points in 3D space from the second image pair;

calculating a vehicle pose transformation using the object points from the first and second image pairs;

determining rotation angle and translation from the vehicle pose transformation; and

,

when the rotation angle or translation exceed a minimum threshold, updating the stored vehicle pose.

2. The method of claim 1 wherein determining the rotation angle and translation from the vehicle pose transformation includes:

converting a rotation matrix to a rotation vector;

computing a length of the rotation vector to obtain a rotation angle; and

,

computing a length of a 3×

1 translation vector to obtain vehicle movement.

3. The method of claim 1 wherein updating the stored vehicle pose when the rotation angle or translation exceed the minimum threshold includes minimizing the accumulation of errors in vehicle pose transformations calculated between image pairs.

4. The method of claim 3 wherein updating the stored vehicle pose when the rotation angle or translation exceed the minimum threshold includes discarding a vehicle transformation when the rotation angle and translation do not exceed the minimum threshold.

5. The method of claim 4 wherein updating the stored vehicle pose when the rotation angle or translation exceed the minimum threshold includes:

discarding a current vehicle pose transformation, and failing to update the stored vehicle pose, when the rotation angle and translation do not exceed the minimum threshold; and

,

updating the stored vehicle pose with a subsequent vehicle pose transformation in response to the rotation angle or translation exceeding the minimum threshold.

6. The method of claim 1 further comprising:

using a plane model, finding a dominant plane in the 3D space that fits the maximum number of points in an initial image frame; and

,

defining the dominant plane as an estimated ground plane in a world coordinate system.

7. The method of claim 6 further comprising:

in response to defining the estimated ground plane, determining vehicle camera height with respect to the estimated ground plane and camera pitch with respect to the estimated ground plane.

8. The method of claim 7 further comprising:

converting the vehicle pose into the world coordinate system having a ground plane.

9. The method of claim 7 wherein estimating the ground plane includes continuously redefining the estimated ground plane over a plurality of sequential image pairs as the vehicle changes position;

wherein determining the camera height and camera pitch includes continuously redetermining the camera height and the camera pitch; and

,

the method further comprising;

in response to redetermining the camera height and camera pitch, avoiding the definition of a changing ground plane slope as an obstacle.

10. The method of claim 9 wherein continuously redetermining the camera height and the camera pitch includes defining the estimated ground plane as invalid with the pitch angle exceeds a pitch angle maximum value.

11. The method of claim 9 wherein continuously redetermining the camera height and the camera pitch includes finding the pitch angle sign as follows:

sign=((NM)·

V);

where N is a vector (0, 1,

     0) normal to a plane in the camera coordinate system with y=0;

where M is a vector normal to the estimated ground plane in the camera coordinate system;

where V is a vector along the x axis (1, 0,

     0) of the camera coordinate system; and

,

where is a symbol representing a cross product operation.

12. The method of claim 6 further comprising:

removing all points on the estimated ground plane from the 3D space; and

,

defining remaining point in the 3D space as potential obstacles.

13. The method of claim 6 wherein defining the estimated ground plane includes defining an estimated ground plane as points falling within a predetermined translation error and a predetermined rotation error of the points found in the dominant plane.

14. A system for autonomously navigating a vehicle, the system comprising:

a vehicle;

a stereo camera mounted on the vehicle having an output to supply image pairs, each image pair including a right image and a left image;

a processor;

a non-transitory memory comprising;

a stored vehicle pose, where a vehicle pose is defined as a position and orientation with respect to a coordination frame;

a navigation application embedded in the non-transitory memory including a sequence of processor executable instructions, the navigation application detecting a plurality of matching feature points in a first matching image pair, determining a plurality of corresponding object points in three-dimensional (3D) space from the first image pair, tracking the plurality of feature points from the first image pair to a second image pair, determining the plurality of corresponding object points in 3D space from the second image pair, calculating a vehicle pose transformation using the objects points from the first and second image pairs, determining rotation angle and translation from the vehicle pose transformation, and updating the stored vehicle pose when the rotation angle and translation exceed a minimum threshold.

15. The system of claim 14 wherein the navigation application determines the rotation angle and translation by converting a rotation matrix to a rotation vector, computing a length of the rotation vector to obtain a rotation angle, and computing a length of a 3×

1 translation vector to obtain vehicle movement.

16. The system of claim 14 wherein the navigation application minimizes the accumulation of errors in vehicle pose transformations calculated between image pairs.

17. The system of claim 16 wherein the navigation application discards a current vehicle pose transformation without updating the stored vehicle pose, when the rotation angle or translation do not exceed the minimum threshold, and updates the stored vehicle pose with a subsequent vehicle pose transformation in response to the rotation angle and translation exceeding the minimum threshold.

18. The system of claim 14 wherein the navigation application converts the vehicle pose into a world coordinate system having a ground plane.

19. The system of claim 18 wherein the navigation application uses a plane model to find a dominant plane in the 3D space that fits the maximum number of points in an initial image frame, and defines the dominant plane as an estimated ground plane in the world coordinate system.

20. The system of claim 19 wherein the navigation application determines vehicle camera height with respect to the estimated ground plane, and camera pitch with respect to the estimated ground plane.

21. The system of claim 19 wherein the navigation application continuously redefines the estimated ground plane over a plurality of sequential image pairs as the vehicle changes position, continuously redetermines the camera height and the camera pitch, and in response to redetermining the camera height and camera pitch, avoids the definition of a changing ground plane slope as an obstacle.

22. The system of claim 19 wherein the navigation application defines the estimated ground plane as invalid when the pitch angle exceeds a pitch angle maximum value.

23. The system of claim 19 wherein the navigation application finds the pitch angle sign as follows:

sign=((NM)·

V);

where N is a vector (0, 1,

     0) normal to a plane in the camera coordinate system with y=0;

where M is a vector normal to the estimated ground plane in the camera coordinate system;

where V is a vector along the x axis (1, 0,

     0) of the camera coordinate system; and

,

where is a symbol representing a cross product operation.

24. The system of claim 18 wherein the navigation application removes all points on the estimated ground plane from the 3D space, and defines remaining points in the 3D space as potential obstacles.

25. The system of claim 18 wherein the navigation application defines the estimated ground plane as points falling within a predetermined translation error and a predetermined rotation error of the points found in the dominant plane.