Visual positioning and navigation device and method thereof

• US 10,593,060 B2
• Filed: 04/14/2017
• Issued: 03/17/2020
• 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
• Primary Examiner(s): Mancho, Ronnie M
• Application Number:
  US15/487,461
• Publication Number:
  US 20180297207A1
• Time in Patent Office: 1,068 Days
• Field of Search: 700245, 700259
• 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, wherein the camera module is mounted on top of the robot, and wherein the environment image is a ceiling photo;

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 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,

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 a wheel of the robot, wherein the current pose information of the robot is calculated by the equation set;

R<sub>θ

2</sub>=R<sub>θ

1</sub>+θ

;

R_x2=R_x1+d*cos(θ

); and

R_y2=R_y1+d*sin(θ

), and

wherein R<sub>θ

2</sub> is an angle in a polar coordinate system of the robot in a current pose, R<sub>θ

1</sub> is an angle in the polar coordinate system of the robot in a previous pose, R_x2 is an x-component in the polar coordinate system of the robot in the current pose, R_x1 is an x-component in the polar coordinate system of the robot in the previous pose, R_y2 is a y-component in the polar coordinate system of the robot in the current pose, R_y1 is a y-component in the polar coordinate system of the robot in the previous pose.

2. 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 an oriented fast and rotated brief (ORB) feature point detection method to extract feature points in the environmental image and prepare a description of the feature points.

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

a match unit, configured to compare the feature description of the environmental image with the features in the feature database, wherein when the feature description of the environmental image does not match at least one of the features in the feature database, 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 when the match unit indicates that the feature description 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 current pose information and the pose correction.

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

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

calculate a ratio between a pixel distance between two image frames and an actual distance of the camera module catching the two image frames;

for the deflection angle θ and

a pixel shift p_d between the two image frames, calculate an estimated current pose 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 pose correction, to obtain the corrected robot pose.

6. The visual positioning and navigation device according to claim 5, wherein the pose correction unit is configured to calculate the estimated current pose of the robot by the equation set:

R_θ

=R<sub>θ

2</sub>+θ

;

R_x=R_x2+p_d*k*cos(R_θ

); and

R_y=R_y2p_d*k*sin(R_θ

),

wherein k represents the ratio between the pixel distance between the two image frames and the actual distance of the camera module, wherein R_θ

is an angle in the polar coordinate system of the robot in the estimated current pose, R_x is an x-component in the polar coordinate system of the robot in the estimated current pose, and R_y is a y-component in the polar coordinate system of the robot in the estimated current pose.

7. A visual positioning and navigation method, comprising

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

capturing an environmental image during movement of the robot, wherein the environmental image is a ceiling photo;

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

building a feature database comprising a plurality of features; and

comparing the feature description of the environmental image to the features 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,

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 wheel of the robot, wherein the current pose information of the robot is calculated by the equation set;

R<sub>θ

2</sub>=R<sub>θ

1</sub>+θ

;

R_x2=R_x1+d*cos(θ

); and

R_y2=R_y1+d*sin(θ

), and

wherein R<sub>θ

2</sub> is an angle in a polar coordinate system of the robot in a current pose, R<sub>θ

1</sub> is an angle in the polar coordinate system of the robot in a previous pose, R_x2 is an x-component in the polar coordinate system of the robot in the current pose, R_x1 is an x-component in the polar coordinate system of the robot in the previous pose, is a y-component in the polar coordinate system of the robot in the current pose, and R_y1 is a y-component in the polar coordinate system of the robot in the previous pose.

8. The visual positioning and navigation method according to claim 7, 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

using an oriented fast and rotated brief (ORB) feature point detection method to extract feature points in the environmental image and prepare a description of the feature points.

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

when the feature description of the environmental image does not match at least one of the features in the feature database, adding related data of the environmental image into the feature database;

calculating the pose correction of the robot when the feature description of the environmental image matches at least one of the features in the feature database; and

obtaining the 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 comparing the feature description of the environmental image comprises:

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

obtaining matched feature points between two image frames.

11. The visual positioning and navigation method according to claim 9, 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 between two image frames and an actual distance of a camera module catching the two image frames;

for the deflection angle θ and

a pixel shift p_d between the two image frames, calculating an estimated current pose of the robot;

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

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

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

12. The visual positioning and navigation method according to claim 11, wherein the step of calculating the estimated current pose of the robot comprises:

calculating the estimated current pose of the robot by the equation set;

R_θ

=R<sub>θ

2</sub>+θ

;

R_x=R_x2p_d*k*cos(R_θ

); and

R_y=R_y2p_d*k*sin(R_θ

),

wherein k represents the ratio between the pixel distance between the two image frames and the actual distance of the camera module, wherein R_θ

is an angle in the polar coordinate system of the robot in the estimated current pose, R_x is an x-component in the polar coordinate system of the robot in the estimated current pose, and R_y is a y-component in the polar coordinate system of the robot in the estimated current pose.