A robot visual recognition and positioning method and system

Abstract

The present invention discloses a robot visual recognition and positioning method, comprising S1, constructing a robot camera model and calibrating the camera; S2, obtaining an environmental image captured by the camera and performing preprocessing; S3, continuously tracking and detecting feature points in the preprocessed environmental image; S4, estimating the robot's posture according to the detected feature points. The present invention constructs a coordinate system conversion model under the robot's visual environment, continuously tracks and detects feature points in the environmental image, and finally estimates the robot's posture based on the detected feature points, thereby ensuring the positioning accuracy and accelerating the calculation efficiency of the robot positioning algorithm.

Description

Technical Field

[0001] The present invention relates to the technical field of robot self-perception and positioning, and in particular to a robot visual recognition and positioning method and system. Background Art

[0002] With the development of robotics technology, intelligent mobile robots with mobile walking functions, environmental perception capabilities and autonomous planning capabilities have received widespread attention from researchers in various countries, especially in the 1980s and 1990s, with the rapid development of computer technology, microelectronics technology, network technology, etc., the development of robotics technology has made great strides. The important characteristics of intelligent robot mobility are its autonomy and adaptability. Autonomy means that it can perform certain tasks completely autonomously in a certain environment without relying on external control; adaptability means that it can recognize and measure surrounding objects in real time, and adjust its own parameters, action strategies and handle emergencies according to environmental changes.

[0003] With the development of intelligent robot technology, it plays an important role in the military, medical, commercial and other fields. People's demand and expectations for intelligent mobile robots are getting higher and higher, and more and more urgent. Mobile robot research has entered a new stage of development. Positioning technology is the core of intelligent mobile robot research and also the key technology for its full automation. Only when the robot knows its own position, the position of obstacles in the workspace and the movement of obstacles accurately can it move safely and effectively. Therefore, autonomous positioning and environmental analysis are one of the most important capabilities of mobile robots. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention provides a robot visual recognition and positioning method and system.

[0005] In order to achieve the above-mentioned object of the invention, the technical solution adopted by the present invention is:

[0006] In a first aspect, the present invention provides a robot visual recognition and positioning method, comprising the following steps:

[0007] S1. Build a robot camera model and calibrate the camera;

[0008] S2, obtain the environment image collected by the camera and perform preprocessing;

[0009] S3, continuously tracking and detecting feature points in the preprocessed environment image;

[0010] S4. Estimate the robot's posture based on the detected feature points.

[0011] Furthermore, the step S1 specifically includes the following sub-steps:

[0012] S1-1, establish the world coordinate system and the image coordinate system respectively;

[0013] S1-2, establish a camera coordinate system with the optical center of the robot camera as the origin, the optical axis of the robot camera as the Z axis, and the x-axis and y-axis directions parallel to the image plane coordinate system as the x-axis and y-axis;

[0014] S1-3. According to the relationship between the world coordinate system, the camera coordinate system and the image coordinate system, a conversion model between the image coordinate system and the world coordinate system is established, which is expressed as:

[0015]

[0016] In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, (x, y, z) represents the three-dimensional coordinate value of the feature point in the world coordinate system, and (x c ,y c ,z c ) represents the three-dimensional coordinate value of the feature point in the camera coordinate system, f represents the focal length of the camera, u0, v0 represents the position offset of the image center of the camera when sampling in the x-axis and y-axis directions, and dx, dy represents the quantization factor of the camera's pixels when sampling in the x-axis and y-axis directions.

[0017] Furthermore, the step S2 specifically includes the following sub-steps:

[0018] S2-1, obtaining an environmental image captured by a camera;

[0019] S2-2, perform distortion and tangential error correction on the collected environment image, expressed as:

[0020]

[0021] In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, It represents the two-dimensional coordinate value of the feature point after correction in the image coordinate system, k1, k2, k3 represent the distortion coefficient, p1, p2 represent the Taylor series, and r represents the distance between the feature point and the optical center of the camera;

[0022] S2-3, performing denoising processing on the corrected environment image;

[0023] S2-4, grayscale the processed environment image.

[0024] Furthermore, the step S3 specifically includes the following sub-steps:

[0025] S3-1, performing feature detection on the preprocessed environment image;

[0026] S3-2, matching feature points in two adjacent environment images in the continuous environment images;

[0027] S3-3. Use the extended Kalman filter method to calculate the coordinates of the matching feature points.

[0028] Furthermore, the step S3-1 specifically includes the following sub-steps:

[0029] S3-1-1, using a sliding window to calculate the maximum value of the weighted entropy of each window to obtain candidate feature points;

[0030] S3-1-2, respectively calculating the feature response value of each candidate feature point, and classifying according to the grayscale change of the image;

[0031] S3-1-3, using geometric thresholds to detect different candidate feature points of the classification;

[0032] S3-1-4. The detected feature points are screened using a threshold value to finally obtain the detected feature points.

[0033] Furthermore, the step S3-2 specifically includes the following sub-steps:

[0034] S3-2-1, match the feature points in two adjacent environment images in the continuous environment image using the following matching conditions:

[0035]

[0036] X i =Δ i20 +Δ i02

[0037] X j =Δ j20 +Δ j02

[0038]

[0039]

[0040] Where, d ij Indicates the matching value of feature points in two adjacent environment images in a continuous environment image, Δ ipq represents the (p+q)th order normalized central moment of feature point i;

[0041] S3-2-2, determine whether the matching values ​​of the feature points in two adjacent environment images in the continuous environment image are less than or equal to the matching threshold; if so, the two feature points match; otherwise, they do not match.

[0042] Furthermore, the step S3-3 specifically includes the following sub-steps:

[0043] S3-3-1. Construct the nonlinear difference model and observation model of the extended Kalman filter, expressed as:

[0044]

[0045]

[0046] In the formula, x(k) represents the true value of the feature point, each element is the rotation and translation vector of the feature point, and z(k) represents the observed value of the feature point;

[0047] S3-3-2, use the extended Kalman filter method to calculate the coordinates of the matching feature points, expressed as:

[0048]

[0049] In the formula, z j (k) represents the coordinates of the jth feature point in the image coordinate system among the n feature points in the environment image at time k that are successfully matched with the environment image at time k-1.

[0050] Furthermore, the step S4 specifically includes the following sub-steps:

[0051] S4-1, establish a posture matrix representing the posture of the robot;

[0052] S4-2, normalize all detected feature points, and build a positioning model based on the robot's posture matrix;

[0053] S4-3, expand all positioning models into vector form, construct positioning linear equations and solve them;

[0054] S4-4. Perform singular value decomposition on the solved posture matrix to obtain the robot's rotation matrix and translation vector.

[0055] Furthermore, the positioning model is expressed as:

[0056]

[0057] In the formula, (u1 v1 1), (u2 v2 1) represent the normalized coordinates of the two feature points. Represents the pose matrix, where each element is the rotation and translation vector of the robot.

[0058] In a second aspect, the present invention further provides a robot visual recognition and positioning system, comprising:

[0059] The calibration module is used to build a robot camera model and calibrate the camera;

[0060] The acquisition module is used to obtain the environmental image collected by the camera and perform preprocessing;

[0061] A detection module, used for continuously tracking and detecting feature points in the preprocessed environment image;

[0062] The positioning module is used to estimate the robot's posture based on the detected feature points.

[0063] The present invention has the following beneficial effects:

[0064] The present invention constructs a coordinate system conversion model in the robot's visual environment, continuously tracks and detects feature points in the environment image, and finally estimates the robot's posture based on the detected feature points. While ensuring the positioning accuracy, the present invention can speed up the calculation efficiency of the robot's positioning algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 It is a schematic diagram of the flow chart of the robot visual recognition and positioning method of the present invention;

[0066] Figure 2 It is a schematic diagram of the structure of the robot vision recognition and positioning system of the present invention. DETAILED DESCRIPTION

[0067] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0068] like Figure 1 As shown, the embodiment of the present invention provides a robot visual recognition and positioning method, comprising the following steps S1 to S4:

[0069] S1. Build a robot camera model and calibrate the camera;

[0070] In an optional embodiment of the present invention, step S1 specifically includes the following sub-steps:

[0071] S1-1, establish the world coordinate system and the image coordinate system respectively;

[0072] Specifically, the present invention establishes a world coordinate system based on the space where the robot is located, and establishes an image coordinate system based on the environment image captured by the robot camera.

[0073] S1-2, establish a camera coordinate system with the optical center of the robot camera as the origin, the optical axis of the robot camera as the Z axis, and the x-axis and y-axis directions parallel to the image plane coordinate system as the x-axis and y-axis;

[0074] S1-3. According to the relationship between the world coordinate system, the camera coordinate system and the image coordinate system, a conversion model between the image coordinate system and the world coordinate system is established, which is expressed as:

[0075]

[0076] In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, (x, y, z) represents the three-dimensional coordinate value of the feature point in the world coordinate system, and (x c ,y c ,z c ) represents the three-dimensional coordinate value of the feature point in the camera coordinate system, f represents the focal length of the camera, u0, v0 represents the position offset of the image center of the camera when sampling in the x-axis and y-axis directions, and dx, dy represents the quantization factor of the camera's pixels when sampling in the x-axis and y-axis directions.

[0077] Specifically, according to the camera imaging principle, the relationship between the camera coordinate system and the image coordinate system is expressed as

[0078]

[0079] The relationship between the world coordinate system and the camera coordinate system is expressed as

[0080]

[0081] In the formula, represents the rotation matrix and t represents the translation vector.

[0082] Therefore, a transformation model between the image coordinate system and the world coordinate system is established based on the above relationship.

[0083] S2, obtain the environment image collected by the camera and perform preprocessing;

[0084] In an optional embodiment of the present invention, step S2 specifically includes the following sub-steps:

[0085] S2-1, obtaining an environmental image captured by a camera;

[0086] S2-2, perform distortion and tangential error correction on the collected environment image, expressed as:

[0087]

[0088] In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, It represents the two-dimensional coordinate value of the feature point after correction in the image coordinate system, k1, k2, k3 represent the distortion coefficient, p1, p2 represent the Taylor series, and r represents the distance between the feature point and the optical center of the camera;

[0089] S2-3, performing denoising processing on the corrected environment image;

[0090] Specifically, the denoising process for the corrected environment image may be performed by using denoising methods commonly used in the art, such as median filtering, bilateral filtering, and Gaussian filtering, which will not be described in detail here.

[0091] S2-4, grayscale the processed environment image.

[0092] Specifically, the processed environment image may be gray-scaled using a weighted average method or other processing method commonly used in the art, which will not be described in detail here.

[0093] S3, continuously tracking and detecting feature points in the preprocessed environment image;

[0094] In an optional embodiment of the present invention, step S3 specifically includes the following sub-steps:

[0095] S3-1, performing feature detection on the preprocessed environment image;

[0096] Specifically, step S3-1 includes the following sub-steps:

[0097] S3-1-1, using a sliding window to calculate the maximum value of the weighted entropy of each window to obtain candidate feature points;

[0098] Specifically, the present invention selects a pixel point in the environmental image in turn, uses a window of a set size centered on the pixel point to detect the point with the maximum image entropy in its field, and compares each point in the detection window with the sample value in the field. If the image entropy within the field of a certain point is greater than that of other points, then the point is a candidate feature point.

[0099] S3-1-2, respectively calculating the feature response value of each candidate feature point, and classifying according to the grayscale change of the image;

[0100] Specifically, the present invention uses a circular window to detect the minimum brightness change in the window, that is, the characteristic response value. If the characteristic response value is greater than a set threshold value, the detection point is a candidate characteristic point obtained by screening, otherwise the detection point is eliminated.

[0101] The present invention classifies the candidate feature points according to the size of the feature response value to characterize the grayscale change trend of the image. The classification model is expressed as:

[0102]

[0103] In the formula, R max ,R min Indicates the maximum and minimum values ​​of the characteristic response value.

[0104] S3-1-3, using geometric thresholds to detect different candidate feature points of the classification;

[0105] Specifically, when M=I, the grayscale variation range of the point to be detected is small, and the grayscale variation of this type of point is the smallest. The present invention uses a geometric threshold d=7 to process it; when M=II, the present invention uses a geometric threshold d=5 to process it; when M=III, the point has a larger grayscale variation range, and the present invention uses a geometric threshold d=3 to process it.

[0106] S3-1-4. The detected feature points are screened using a threshold value to finally obtain the detected feature points.

[0107] Specifically, through the above classification, the grayscale changes in the area where the feature points in the image are located are divided. In the area with small grayscale changes, a smaller detection threshold is used, which makes it easier to find the feature points and prevents missed detection of feature points; in the area with large grayscale changes, a larger threshold is used to remove some false feature points. Such a threshold setting not only avoids missed detection in the feature point extraction process, but also avoids false detection of feature points. The threshold value used in the present invention is expressed as:

[0108]

[0109] Where δ represents the threshold parameter.

[0110] S3-2, matching feature points in two adjacent environment images in the continuous environment images;

[0111] Specifically, step S3-2 includes the following sub-steps:

[0112] S3-2-1, match the feature points in two adjacent environment images in the continuous environment image using the following matching conditions:

[0113]

[0114] X i =Δ i20 +Δ i02

[0115] X j =Δ j20 +Δ j02

[0116]

[0117]

[0118] Where, d ij Indicates the matching value of feature points in two adjacent environment images in a continuous environment image, Δ ipq represents the (p+q)th order normalized central moment of feature point i;

[0119] S3-2-2, determine whether the matching values ​​of the feature points in two adjacent environment images in the continuous environment image are less than or equal to the matching threshold; if so, the two feature points match; otherwise, they do not match.

[0120] S3-3. Use the extended Kalman filter method to calculate the coordinates of the matching feature points.

[0121] Specifically, step S3-3 includes the following sub-steps:

[0122] S3-3-1. Construct the nonlinear difference model and observation model of the extended Kalman filter, expressed as:

[0123]

[0124]

[0125] In the formula, x(k) represents the true value of the feature point, and each element is the rotation and translation vector of the feature point, that is, t u (k),t v (k) represents the translation vector, represents the rotation matrix, z(k) represents the observation value of the feature point;

[0126] S3-3-2, use the extended Kalman filter method to calculate the coordinates of the matching feature points, expressed as:

[0127]

[0128] In the formula, z j (k) represents the coordinates of the jth feature point in the image coordinate system among the n feature points in the environment image at time k that are successfully matched with the environment image at time k-1.

[0129] S4. Estimate the robot's posture based on the detected feature points.

[0130] In an optional embodiment of the present invention, step S4 specifically includes the following sub-steps:

[0131] S4-1, establish a posture matrix representing the posture of the robot;

[0132] Specifically, the present invention uses the essential matrix to characterize the robot posture. The essential matrix is ​​a 3x3 matrix containing 9 unknowns and is defined by epipolar constraints. The essential matrix has a total of 6 degrees of freedom, including 3 rotation matrices and 3 translation vectors, expressed as

[0133]

[0134] In the formula, Represents a rotation matrix and a translation vector.

[0135] S4-2, normalize all detected feature points, and build a positioning model based on the robot's posture matrix;

[0136] Specifically, the coordinates of the matched feature points are normalized to x1 = [u1, v1, 1] T , x2=[u2,v2,1] T ; According to the epipolar constraint, the positioning model is expressed as:

[0137]

[0138] In the formula, (u1 v1 1), (u2 v2 1) represent the normalized coordinates of the two feature points. Represents the pose matrix, where each element is the rotation and translation vector of the robot.

[0139] S4-3, expand all positioning models into vector form, construct positioning linear equations and solve them;

[0140] S4-4. Perform singular value decomposition on the solved posture matrix to obtain the robot's rotation matrix and translation vector.

[0141] like Figure 2 As shown, the embodiment of the present invention also provides a robot visual recognition and positioning system, including:

[0142] The calibration module is used to build a robot camera model and calibrate the camera;

[0143] The acquisition module is used to obtain the environmental image collected by the camera and perform preprocessing;

[0144] A detection module, used for continuously tracking and detecting feature points in the preprocessed environment image;

[0145] The positioning module is used to estimate the robot's posture based on the detected feature points.

[0146] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0147] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0148] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0149] The present invention uses specific embodiments to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

[0150] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the present invention.

Claims

1. A robot visual recognition and positioning method, characterized in that: The following steps are involved: S1. Build a robot camera model and calibrate the camera; The following steps are included: S1-1, establish the world coordinate system and the image coordinate system respectively; S1-2, establish a camera coordinate system with the optical center of the robot camera as the origin, the optical axis of the robot camera as the Z axis, and the x-axis and y-axis directions parallel to the image plane coordinate system as the x-axis and y-axis; S1-3. According to the relationship between the world coordinate system, the camera coordinate system and the image coordinate system, a conversion model between the image coordinate system and the world coordinate system is established, which is expressed as: In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, (x, y, z) represents the three-dimensional coordinate value of the feature point in the world coordinate system, and (x c ,y c ,z c ) represents the three-dimensional coordinate value of the feature point in the camera coordinate system, f represents the focal length of the camera, u0, v0 represents the position offset of the camera's image center when sampling in the x-axis and y-axis directions, dx, dy represents the quantization factor of the camera's pixels when sampling in the x-axis and y-axis directions; S2, obtain the environment image collected by the camera and perform preprocessing; The following steps are included: S2-1, obtaining an environmental image captured by a camera; S2-2, perform distortion and tangential error correction on the collected environment image, expressed as: In the formula, (u, v) represents the two-dimensional coordinate value of the feature point in the image coordinate system, It represents the two-dimensional coordinate value of the feature point after correction in the image coordinate system, k1, k2, k3 represent the distortion coefficient, p1, p2 represent the Taylor series, and r represents the distance between the feature point and the optical center of the camera; S2-3, performing denoising processing on the corrected environment image; S2-4, graying the processed environment image; S3, continuously tracking and detecting the feature points in the preprocessed environment image; The following steps are included: S3-1. Perform feature detection on the preprocessed environment image; including: S3-1-1, using a sliding window to calculate the maximum value of the weighted entropy of each window to obtain candidate feature points; S3-1-2, respectively calculating the feature response value of each candidate feature point, and classifying according to the grayscale change of the image; S3-1-3, using geometric thresholds to detect different candidate feature points of the classification; S3-1-4, screening the detected feature points using a threshold value, and finally obtaining the detected feature points; S3-2, matching feature points in two adjacent environment images in the continuous environment image; including: S3-2-1, match the feature points in two adjacent environment images in the continuous environment image using the following matching conditions: X i =D i20 +D i02 X j =D j20 +D j02 Where, d ij Indicates the matching value of feature points in two adjacent environment images in a continuous environment image, Δ ipq represents the (p+q)th order normalized central moment of feature point i; S3-2-2, determining whether the matching values ​​of feature points in two adjacent environment images in the continuous environment image are less than or equal to the matching threshold; if so, the two feature points match; otherwise, they do not match; S3-3, using the extended Kalman filter method to calculate the coordinates of the matching feature points; S4. Estimate the robot's posture based on the detected feature points.

2. The robot visual recognition and positioning method according to claim 1, characterized in that: The step S3-3 specifically includes the following sub-steps: S3-3-1. Construct the nonlinear difference model and observation model of the extended Kalman filter, expressed as: In the formula, x(k) represents the true value of the feature point, each element is the rotation and translation vector of the feature point, and z(k) represents the observed value of the feature point; S3-3-2, use the extended Kalman filter method to calculate the coordinates of the matching feature points, expressed as: In the formula, z j (k) represents the coordinates of the jth feature point in the image coordinate system among the n feature points in the environment image at time k that are successfully matched with the environment image at time k-1.

3. The robot visual recognition and positioning method according to claim 1, characterized in that: The step S4 specifically includes the following sub-steps: S4-1, establish a posture matrix representing the posture of the robot; S4-2, normalize all detected feature points, and build a positioning model based on the robot's posture matrix; S4-3, expand all positioning models into vector form, construct positioning linear equations and solve them; S4-4. Perform singular value decomposition on the solved posture matrix to obtain the robot's rotation matrix and translation vector.

4. The robot visual recognition and positioning method according to claim 3, characterized in that: The positioning model is expressed as: In the formula, (u1v11), (u2v21) represent the normalized coordinates of two feature points. Represents the pose matrix, where each element is the rotation and translation vector of the robot.

5. A robot visual recognition and positioning system using the method of claim 1, characterized in that: include: The calibration module is used to build the robot camera model and calibrate the camera; The acquisition module is used to obtain the environmental image collected by the camera and perform preprocessing; A detection module, used for continuously tracking and detecting feature points in the preprocessed environment image; The positioning module is used to estimate the robot's posture based on the detected feature points.

Images