VISUAL POSITIONING AND NAVIGATION DEVICE AND METHOD THEREOF

• US 20180297207A1
• Filed: 04/14/2017
• Published: 10/18/2018
• Est. Priority Date: 04/14/2017
• Status: Active Grant

• Current Assignee: Suzhou Panxi Artificial Intelligence Technology Company Limited
• Original Assignee: Twoantz, Inc.
• Inventors: HUANG, Chi-Min, HUANG, Cheng
• Granted Patent: US 10,593,060 B2
• Time in Patent Office: Days
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/73 using feature-based methods
• CPC Class Codes: G06V 10/757 Matching configurations of points or features
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59
• CPC Class Codes: Y10S 901/01 Mobile robot

Abstract

The present invention discloses a visual positioning and navigation device, comprising: a motion module, configured to drive a robot accordingly, and acquire a current pose information of the robot in real time; a camera module, configured to capture an environmental image during the movement of the robot; an image processing module, configured to perform the feature extraction and the feature description for the environmental image; and a pose estimation module, configured to match the feature point description of the environmental image, build a feature database, calculate the pose correction of the robot, and obtain the corrected robot pose based on the robot current pose and the pose correction. The visual positioning and navigation device and method thereof can build the scene map by detecting and tracking the feature information of the ORB feature points of the indoor ceiling, so as to achieve the accurate positioning and navigation of the robot.

Claims

1. A visual positioning and navigation device, comprising:

a motion module, configured to drive a robot and acquire current pose information of the robot in real time;

a camera module comprising an image sensor that captures an environmental image during movement of the robot;

an image processing module, configured to perform feature extraction and feature description for the environmental image captured by the image sensor of the camera module; and

a pose estimation module, configured to build a feature database comprising a plurality of features and compare the feature point description of the environmental image to the features in the feature database, calculate a pose correction of the robot, and obtain a corrected robot pose based on the current pose information and the pose correction.

2. The visual positioning and navigation device according to claim 1, wherein the motion module is configured to obtain a deflection angle θ

of the robot by a gyroscope, and obtain a movement distance d of the robot by a photoelectric pulse counter on the robot wheel, and wherein the current pose (Rx, Ry, Rth) of the robot is calculated by the equation set;

Rth2=Rth1+θ

;

Rx2=Rx1+d*cos(θ

);

Ry2=Ry1+d*sin(θ

).

3. The visual positioning and navigation device according to claim 1, wherein the camera module is mounted on top of the robot, and wherein the environmental image is a ceiling photo.

4. The visual positioning and navigation device according to claim 1, wherein the image processing module comprises:

an image pre-processing unit, configured to pre-process the environmental image to remove a lens distortion effect; and

a feature description unit, configured to use ORB feature point detection method to extract feature points in the environmental image and prepare a description of the feature points.

5. The visual positioning and navigation device according to claim 1, wherein the pose estimation module comprises:

a match unit, configured to compare the feature point description of the environmental image with the features in the feature database, wherein if the feature point description of the environmental image does not match at least one of the features in the feature database, the related data of the environmental image is added into the feature database; and

a pose correction unit, configured to calculate the pose correction of the robot if the match unit indicates that the feature of the environmental image matches at least one of the features in the feature database, and to obtain the corrected robot pose based on the robot current pose and the pose correction.

6. The visual positioning and navigation device according to claim 5, wherein the match unit is configured to use the Euclidean distance as the similarity measurement of the feature points, and obtain the matched feature points between two image frames.

7. The visual positioning and navigation device according to claim 5, wherein the pose correction unit is configured to:

calculate a ratio between a pixel distance of the image and the actual distance of the camera module;

for the deflection angle θ and

a pixel shift pd between two image frames, calculate a set of estimated current poses of the robot;

use a k-means clustering method to calculate a final-estimated pose of the robot;

subtract the final-estimated pose from the current pose information of the robot, to obtain the pose correction of the robot; and

add the current pose information and the correction, to obtain the corrected pose.

8. The visual positioning and navigation device according to claim 7, wherein the pose correction unit is configured to calculate a set of estimated poses of the robot by the equation set:

Rth=Dth+θ

Rx=Dx+pd*k*cos(Rth)

Ry=Dy+pd*k*sin(Rth)

wherein k represents the ratio between the pixel distance of the image and the actual distance of the camera, and Dx, Dy, Dth represents the current pose of the robot.

9. A visual positioning and navigation method, comprising

driving a robot and acquiring a current pose information of the robot in real time during said driving;

capturing an environmental image during the movement of the robot;

performing feature extraction and feature description for the environmental image captured;

building a feature database including a plurality of features;

comparing the feature point description of the environmental image with the features included in the feature database that was built, calculating a pose correction of the robot, and obtaining a corrected robot pose based on the current pose information and the pose correction.

10. The visual positioning and navigation method according to claim 9, wherein the step of acquiring the current pose information of the robot in real time comprises:

obtaining a deflection angle θ

of the robot by a gyroscope, and obtaining a movement distance d of the robot by a photoelectric pulse counter on a robot wheel, and wherein the current pose information (Rx, Ry, Rth) of the robot is calculated by the equation set;

Rth2=Rth1+θ

;

Rx2=Rx1+d*cos(θ

);

Ry2=Ry1+d*sin(θ

).

11. The visual positioning and navigation method according to claim 9, wherein the environmental image is a ceiling photo.

12. The visual positioning and navigation method according to claim 9, wherein the step of performing the feature extraction and the feature description for the environmental image comprises:

pre-processing the environmental image before performing the feature extraction and the feature description to remove a lens distortion effect; and

use ORB feature point detection method to extract feature points in the environmental image and describe the feature points.

13. The visual positioning and navigation method according to claim 9, wherein the step of comparing the feature point description of the environmental image to the features in the database, calculating the pose correction of the robot, and obtaining the corrected robot pose based on the robot current pose and the pose correction comprises:

if the feature description of the environmental image is not included in the feature database, the related data of the environmental image is added into the feature database;

calculating the pose correction of the robot if the match unit indicates that the feature description of the environmental image is in the feature database, and

obtaining the corrected robot pose based on the current pose information and the pose correction.

14. The visual positioning and navigation method according to claim 13, wherein the step of comparing the feature point description of the environmental image comprises:

using the Euclidean distance as a similarity measurement of feature points, and

obtaining the matched feature points between two image frames.

15. The visual positioning and navigation method according to claim 13, wherein the step of obtaining the corrected robot pose based on the current pose information and the pose correction comprises:

calculating a ratio between a pixel distance of the image and an actual distance of the camera module;

for the deflection angle θ and

the pixel shift pd between two image frames, calculating a set of estimated current poses of the robot;

using a k-means clustering method to calculate a final-estimated pose of the robot;

subtracting the final-estimated pose from an uncorrected pose of the robot, to obtain the pose correction of the robot; and

adding the current pose and the correction, to obtain the corrected pose.

16. The visual positioning and navigation method according to claim 15, wherein the step of calculating a set of estimated current poses of the robot comprises:

calculating the set of estimated poses of the robot by the equation set;

Rth=Dth+θ

Rx=Dx+pd*k*cos(Rth)

Ry=Dy+pd*k*sin(Rth)

wherein k represents the ratio between the pixel distance of the image and the actual distance of the camera, and Dx, Dy, Dth represents the current pose of the robot.