CN113034584B - Mobile robot visual positioning method based on object semantic road sign - Google Patents

Abstract

The invention discloses a mobile robot visual positioning method based on object semantic road signs, which comprises the steps of firstly constructing an off-line object road sign global map, then constructing an on-line object road sign local map, then carrying out mathematical expression on the object road sign local map and the object road sign global map by taking a map structure as a mathematical model, taking a sub-map matching problem as a map matching mathematical equation, realizing registration estimation of the object road sign local map in the object road sign global map by using a sub-map matching solving result, and finally realizing the visual positioning of a mobile robot. The invention carries out the visual positioning of the robot based on the semantic road signs of the object, utilizes the property that the geometric structure of the three-dimensional space has the visual angle invariance, gets rid of the limitation requirement of the positioning system on the motion posture of the robot, improves the robustness of the positioning system on the external environment conditions, and finally improves the autonomy and the flexibility of the mobile robot.

Description

A visual localization method for mobile robots based on object semantic landmarks

technical field

The invention relates to the technical field of mobile robots, in particular to a visual positioning method of mobile robots based on object semantic landmarks.

Background technique

Autonomous navigation is a core technical problem in the field of mobile robot technology. Autonomous navigation systems generally include three main modules: positioning, path planning and control. plays a vital role in autonomous navigation systems. The real-time, accuracy and robustness of the positioning module are decisive factors for the performance of the autonomous navigation system.

The existing visual sensor-based positioning systems mainly rely on the matching of visual feature key points, that is, the feature key points in the current frame image are matched and associated with the key points in the feature map database. In the case of successful matching, with the help of the three-dimensional coordinates of the key points in the map database, the three-dimensional pose of the camera coordinate system corresponding to the current frame image is estimated through the camera projection projection geometric model, which is equivalent to estimating the pose of the sensor mobile platform. So as to realize the positioning function. In the positioning process, the matching of visual feature key points is the premise of accurate pose estimation. The matching of feature key points is mainly based on the description vector extraction algorithm, which essentially depends on the local pixel intensity distribution characteristics of the image. Under the conditions that the texture information and lighting conditions of the external environment are strictly the same, the local pixel intensity distribution characteristics of the image will be affected by the angle of view captured by the camera, resulting in the failure of feature point matching in the same scene due to the inconsistency between the observation angle and the map construction process. , thus causing the robot positioning system to fail to work reliably.

SUMMARY OF THE INVENTION

Aiming at the problem of insufficient illumination invariance and viewing angle invariance in the extraction of visual feature points and the matching algorithm in the existing global positioning technology for mobile robots based on vision schemes, the present invention proposes a visual positioning method for mobile robots based on object semantic landmarks. The purpose is to get rid of the limitation of the positioning system on the robot's motion posture, improve the robustness of the positioning system to external environmental conditions, and finally improve the autonomy and flexibility of the mobile robot.

For achieving the above object, the technical scheme provided by the present invention is:

A mobile robot visual positioning method based on object semantic landmarks, comprising the following steps:

S1. Build an offline global map of object road signs;

S2. Build an online local map of object road signs;

S3. Mathematically express the local map of object landmarks and the global map of object landmarks using the graph structure as a mathematical model, and use the sub-map matching problem as a map matching mathematical equation, and use the sub-map matching solution result to realize the local map of object landmarks in the global map of object landmarks The registration estimation in the final realization of the visual localization of the mobile robot.

Further, the specific process of constructing an offline global map of object road signs in the step S1 is as follows:

S1-1. Acquire an input image, and preprocess the input image;

S1-2. Use the trained convolutional neural network to achieve multi-target object detection, and finally output it in the form of a 2D rectangular envelope, output the category vector c _i of all target objects in the image and its envelope at the pixel coordinates Position x _i , y _i and size information _wi , _hi in the system, namely { _ci , _xi , _yi , _wi , _hi ,i∈N}, _wi is height, _hi is height;

S1-3. Use the optical flow method to perform pixel-level registration between the current image and the previous frame image, and estimate the camera motion pose by minimizing the gray value cost function of the pixels registered between the two frames:

而exp(·)是对李代数se(3)的指数映射，即exp(ξ^)∈SE(3)为相邻帧的刚体运动位姿，SE(3)是特殊欧几里得群。P₁和P₂为对应相机的投影矩阵，X_i,1和X_i,2为空间中对应的三维点在对应相机坐标系下的表达，I₁(·)和I₂(·)为对应像素点的像素灰度值；In formula (1), the ( )^ symbol represents the transformation of the 6-dimensional vector in the Lie algebra se(3) into a 4×4 matrix form, that is

And exp( ) is the exponential mapping to the Lie algebra se(3), that is, exp(ξ^)∈SE(3) is the rigid motion pose of adjacent frames, and SE(3) is a special Euclidean group. P ₁ and P ₂ are the projection matrices of the corresponding cameras, X _i,1 and X _i,2 are the expressions of the corresponding three-dimensional points in the space in the corresponding camera coordinate system, and I ₁ ( ) and I ₂ ( ) are the corresponding The pixel gray value of the pixel point;

通过投影矩阵P_i＝K·[R_i t_i]具有如下关系：S1-4, use the dual ellipsoid surface to envelop the object as the object road sign map expression, first perform inscribed ellipse fitting on the image object detection frame and obtain the dual matrix form of the ellipse. For view i, in its camera When the pose (R _i , t _i ) and the camera internal parameter matrix K have been obtained, the projected contour of the dual ellipsoid surface Q ^* in the three-dimensional space in this view

Through the projection matrix P _i =K·[R _i t _i ] has the following relationship:

表明方程在相差一个尺度下具有等价性。In formula (2), the scalar

Show that the equations are equivalent up to one scale apart.

S1-5. In the homogeneous coordinate system, the dual form Q ^* of the ellipsoid is a 4X4 symmetric matrix, and the dual form C ^* of the ellipse is a 3X3 symmetric matrix, that is, Q ^* and C ^* have 10 and 6 independent elements; Equation (2) is a quadratic equation, expressing P _i as a quadratic vector, denoted as B _i , the original equation is linearized and expressed as:

将其恢复成对称矩阵可得到椭球原对偶形式：S1-6. For multiple views, multiple equations (3) are simultaneously constructed to construct a linear equation system. The solution problem for the over-constrained equation is a linear least squares problem, and the variables are solved by SVD.

Restoring it to a symmetric matrix gives the ellipsoid primordial form:

Complete the reconstruction of the object envelope ellipsoid to form a global map of object landmarks.

Further, in the step S1-1, the preprocessing process includes: filtering and denoising, grayscale image conversion and size scaling, so as to adapt to the input dimension of the neural network.

Further, the process of constructing an online local map of object road signs in the step S2 is as follows:

After the visual image is preprocessed, object detection and data association are performed; then the optical flow method is used for pose tracking, that is, the visual odometry function is realized. ; Then use multi-view to construct a local object landmark map, and adopt a sliding window type local map scale control strategy, and perform rigid body transformation on the coordinate system of the object landmark envelope surface, express it in the camera coordinate system, and finally form a camera coordinate system. A local map of the associated scale-controllable object landmarks.

Further, the specific process of the step S3 is as follows:

Match the local map of object landmarks with the global map of object landmarks. Mathematically, the matching problem is a subgraph matching problem. The global map of object landmarks is represented as a graph structure G=(V, E), where V is a node, including each The three-dimensional coordinates and labels of a landmark i, E is an edge, which represents the mutual three-dimensional relationship between the two landmarks; similarly, the local map is expressed as a graph structure H=(V′, E′), V′ is a node, E′ for the edge;

Then the map matching problem is specifically defined as: given a graph G=(V, E) with n nodes and a graph H=(V′, E′) with m nodes, find the corresponding relationship between nodes in graph G and graph H :

X∈{0,1} ^n×m

To maximize the consistency of graph properties and structure:

where v represents the consistency measure of the i ₁ node of graph G and the i ₂ node of graph H, which is derived from the 3D scale consistency of landmarks and the consistency of semantic labels; d represents the i ₁ -j ₁ edge of graph G and the graph The consistency measure of the i ₂ -j ₂ edge of H, which comes from the consistency of the three-dimensional position distance between different landmarks in the map; uses semantic information to match nodes with the same label in the two graphs, reducing the number of optimization iterations, Finally, the optimal matching solution is obtained, and the one-to-one correspondence between the road signs between the two maps is determined;

After completing the matching between the local map of the object landmark and the global map of the object landmark, the three-dimensional information of the landmark in the global map of the object landmark is used as the benchmark to propagate the pose, that is, the three-dimensional information of the matching global map landmark - the three-dimensional information of the local map landmark - the local map. Reference coordinate system - the transformation of the camera coordinate system in the world coordinate system T _wc ∈ SE (3), and finally obtain the pose of the robot relative to the map world coordinate system at the current moment:

T _wr =T _wc T _cr ∈SE(3)

In the formula, T _CR ∈ SE(3) is the transformation matrix of the robot relative to the camera coordinate system, and T _WC ∈ SE(3) is the transformation matrix of the camera coordinate system relative to the world coordinate system. According to the transformation matrix chain rule, the two are multiplied together. The transformation matrix T _WR ∈ SE (3) of the robot relative to the world coordinate system can be obtained by doing matrix operations, that is, the positioning and attitude information of the robot.

Compared with the prior art, the principle and advantages of this scheme are as follows:

This scheme firstly builds an offline global map of object landmarks, and then builds an online local map of object landmarks. Then, the local map of object landmarks and the global map of object landmarks are mathematically expressed using the graph structure as a mathematical model, and the subgraph matching problem is used as the map. Match mathematical equations, and realize the registration estimation of the local map of object landmarks in the global map of object landmarks based on the sub-map matching solution results, and finally realize the visual positioning of mobile robots. This scheme performs robot visual positioning based on object semantic landmarks. The geometric structure of three-dimensional space has the property of invariance of viewing angle, which gets rid of the restriction requirements of the positioning system on the robot's motion posture, improves the robustness of the positioning system to external environmental conditions, and finally improves the movement. Robotic autonomy and flexibility.

Description of drawings

In order to illustrate the embodiments of the present invention or the technical solutions in the prior art more clearly, the following briefly introduces the services required in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only For some embodiments of the present invention, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without creative efforts.

Fig. 1 is the principle flow chart of a kind of mobile robot visual positioning method based on object semantic landmarks of the present invention;

2 is a global map of object landmarks represented by an ellipsoid envelope surface in an embodiment;

FIG. 3 is a partial map of object landmarks under the sliding window strategy in the embodiment.

Detailed ways

Below in conjunction with specific embodiment, the present invention will be further described:

As shown in FIG. 1 , a method for visual positioning of a mobile robot based on object semantic landmarks described in this embodiment includes the following steps:

S1. Build an offline global map of object road signs. The process is as follows:

S1-1. Acquire an input image, and perform preprocessing on the input image, specifically including: filtering and denoising, grayscale image conversion, and size scaling, so as to adapt to the input dimension of the neural network;

S1-2. Use the trained convolutional neural network to achieve multi-target object detection, and finally output it in the form of a 2D rectangular envelope, output the category vector c _i of all target objects in the image and its envelope at the pixel coordinates Position x _i , y _i and size information _wi , _hi in the system, namely { _ci , _xi , _yi , _wi , _hi ,i∈N}, _wi is height, _hi is height;

S1-3. Use the optical flow method to perform pixel-level registration between the current image and the previous frame image, and estimate the camera motion pose by minimizing the gray value cost function of the pixels registered between the two frames:

而exp(·)是对李代数se(3)的指数映射，即exp(ξ^)∈SE(3)为相邻帧的刚体运动位姿，SE(3)是特殊欧几里得群。P₁和P₂为对应相机的投影矩阵，X_i,1和X_i,2为空间中对应的三维点在对应相机坐标系下的表达，I₁(·)和I₂(·)为对应像素点的像素灰度值；In formula (1), the ( )^ symbol represents the transformation of the 6-dimensional vector in the Lie algebra se(3) into a 4×4 matrix form, that is

And exp( ) is an exponential mapping to the Lie algebra se(3), that is, exp(ξ^)∈SE(3) is the rigid motion pose of adjacent frames, and SE(3) is a special Euclidean group. P ₁ and P ₂ are the projection matrices of the corresponding cameras, X _i,1 and X _i,2 are the expressions of the corresponding three-dimensional points in the space in the corresponding camera coordinate system, and I ₁ (·) and I ₂ (·) are the corresponding The pixel gray value of the pixel point;

通过投影矩阵P_i＝K·[R_i t_i]具有如下关系：S1-4, use the dual ellipsoid surface to envelop the object as the object road sign map expression, first perform inscribed ellipse fitting on the image object detection frame and obtain the dual matrix form of the ellipse. For view i, in its camera When the pose (R _i , t _i ) and the camera internal parameter matrix K have been obtained, the projected contour of the dual ellipsoid surface Q ^* in the three-dimensional space in this view

Through the projection matrix P _i =K·[R _i t _i ] has the following relationship:

表明方程在相差一个尺度下具有等价性。In formula (2), the scalar

Show that the equations are equivalent up to one scale apart.

S1-5. In the homogeneous coordinate system, the dual form Q ^* of the ellipsoid is a 4X4 symmetric matrix, and the dual form C ^* of the ellipse is a 3X3 symmetric matrix, that is, Q ^* and C ^* have 10 and 6 independent elements; Equation (2) is a quadratic equation, expressing P _i as a quadratic vector, denoted as B _i , the original equation is linearized and expressed as:

将其恢复成对称矩阵可得到椭球原对偶形式：S1-6. For multiple views, multiple equations (3) are simultaneously constructed to construct a linear equation system. The solution problem for the over-constrained equation is a linear least squares problem, and the variables are solved by SVD.

Restoring it to a symmetric matrix gives the ellipsoid primordial form:

After completing the reconstruction of the object envelope ellipsoid, a global map of object landmarks is formed, as shown in Figure 2.

S2. Build an online local map of object road signs;

This step is the same as step S1. After the visual image is preprocessed, object detection and data association are performed; then the optical flow method is used to track the pose, that is, the visual odometry function is realized. To eliminate the accumulated error to the greatest extent; then use multi-view to build a local object landmark map, and adopt a sliding window type local map scale control strategy, and perform rigid body transformation on the coordinate system of the object landmark envelope, and express it in the camera coordinate system, Finally, a scale-controllable local map of object landmarks associated with the camera coordinate system is formed, as shown in Figure 3.

S3, the local map of the object road sign is matched with the global map of the object road sign, and finally the visual positioning of the mobile robot is realized.

The specific process of this step is as follows:

Match the local map of object landmarks with the global map of object landmarks. Mathematically, the matching problem is a subgraph matching problem. The global map of object landmarks is represented as a graph structure G=(V, E), where V is a node, including each The three-dimensional coordinates and labels of a landmark i, E is an edge, which represents the mutual three-dimensional relationship between the two landmarks; similarly, the local map is expressed as a graph structure H=(V′, E′), V′ is a node, E′ for the edge;

Then the map matching problem is specifically defined as: given a graph G=(V, E) with n nodes and a graph H=(V′, E′) with m nodes, find the corresponding relationship between nodes in graph G and graph H :

X∈{0,1} ^n×m

To maximize the consistency of graph properties and structure:

where v represents the consistency measure of the i ₁ node of graph G and the i ₂ node of graph H, which is derived from the 3D scale consistency of landmarks and the consistency of semantic labels; d represents the i ₁ -j ₁ edge of graph G and the graph The consistency measure of the i ₂ -j ₂ edge of H, which comes from the consistency of the three-dimensional position distance between different landmarks in the map; uses semantic information to match nodes with the same label in the two graphs, reducing the number of optimization iterations, Finally, the optimal matching solution is obtained, and the one-to-one correspondence between the road signs between the two maps is determined;

After completing the matching of the local map of the object landmark to the global map of the object landmark, the three-dimensional information of the landmark in the global map of the object landmark is used as the benchmark to propagate the pose, that is, the three-dimensional information of the matching global map landmark - the three-dimensional information of the local map landmark - the local map. Reference coordinate system - the transformation of the camera coordinate system in the world coordinate system T _wc ∈ SE (3), and finally obtain the pose of the robot relative to the map world coordinate system at the current moment:

T _wr =T _wc T _cr ∈SE(3)

In the formula, T _CR ∈ SE(3) is the transformation matrix of the robot relative to the camera coordinate system, and T _WC ∈ SE(3) is the transformation matrix of the camera coordinate system relative to the world coordinate system. According to the transformation matrix chain rule, the two are multiplied together. The transformation matrix T _WR ∈ SE (3) of the robot relative to the world coordinate system can be obtained by doing matrix operations, that is, the positioning and attitude information of the robot.

This embodiment performs robot visual positioning based on object semantic landmarks, uses the property of invariant viewing angle of the geometric structure of three-dimensional space, gets rid of the restriction requirements of the positioning system on the robot's motion posture, improves the robustness of the positioning system to external environmental conditions, and finally improves the Autonomy and flexibility of mobile robots.

The above-mentioned embodiments are only preferred embodiments of the present invention, and are not intended to limit the scope of implementation of the present invention. Therefore, any changes made according to the shape and principle of the present invention should be included within the protection scope of the present invention.

Claims (4)

1. A mobile robot visual positioning method based on object semantic road signs is characterized by comprising the following steps:

s1, constructing an offline object landmark global map;

s2, constructing an online object road sign local map;

s3, performing mathematical expression on the object landmark local map and the object landmark global map by taking the graph structure as a mathematical model, taking a sub-map matching problem as a map matching mathematical equation, realizing registration estimation of the object landmark local map in the object landmark global map by using a sub-map matching solution result, and finally realizing visual positioning of the mobile robot;

the specific process of constructing the offline global map of object landmarks in step S1 is as follows:

s1-1, acquiring an input image, and preprocessing the input image;

s1-2, realizing multi-target object detection by using the trained convolutional neural network, finally outputting in the form of a 2D rectangular envelope frame, and outputting the category vectors c of all target objects in the image _i And the position x of its envelope frame in the pixel coordinate system _i ，y _i And size information w _i ，h _i I.e. { c _i ,x _i ,y _i ,w _i ,h _i ,i∈N}，w _i Is height, h _i Is the height;

s1-3, performing pixel level registration on the current image and the previous frame image by using an optical flow method, and estimating the motion pose of the camera by minimizing the gray value cost function of the registered pixels between two frames:

in equation (1) (. cndot.). Lambda-symbology transforms a 6-dimensional vector in lie algebra se (3) into a 4x4 matrix form, i.e.

Exp (-) is an exponential mapping of lie algebra SE (3), namely exp (xi ^ epsilon SE (3) is a rigid motion pose of an adjacent frame, and SE (3) is a special Euclidean group; p is ₁ And P ₂ For projection matrices of corresponding cameras, X _i,1 And X _i,2 For the representation of the corresponding three-dimensional points in space in the corresponding camera coordinate system, I ₁ (. cndot.) and I ₂ (·) is the pixel gray value of the corresponding pixel point;

s1-4, enveloping the object by using the dual ellipsoid curve as object road sign map expression, firstly carrying out inscribed ellipse fitting on the image object detection frame and obtaining the dual matrix form of ellipse, and regarding the view i, at the camera attitude (R) of the view i _i ，t _i ) Dual ellipsoid curved surface in three-dimensional space under the condition that the sum camera internal reference matrix K is obtainedQ ^* Projected contour in this view

By projecting a matrix P _i ＝K·[R _i t _i ]Has the following relationship:

in the formula (2), scalar quantity

Showing that the equations are equivalent at one scale of phase difference;

s1-5, in the homogeneous coordinate system, the dual form Q of ellipsoid ^* Is a symmetric matrix of 4X4, and the dual form C of the ellipse ^* Is a symmetrical matrix of 3X3, i.e. Q ^* And C ^* There are 10 and 6 independent elements, respectively; equation (2) is a quadratic equation with P _i Performing quadratic form vector expression, and recording as B _i The original equation is linearly expressed as:

s1-6, for multiple views, simultaneously establishing multiple equations (3) to construct a linear equation set, solving the overconstrained equation to be a linear least squares problem, and solving through SVD to obtain variables

Restoring the ellipsoid into a symmetric matrix to obtain an ellipsoid primal-dual form:

and reconstructing an object envelope ellipsoid to form an object landmark global map.

2. The method for visually positioning a mobile robot based on semantic landmarks of objects as recited in claim 1, wherein said preprocessing step S1-1 comprises: filtering and denoising, converting a gray scale image and scaling the size to adapt to the input dimension of the neural network.

3. The visual positioning method for mobile robot based on semantic object signposts of claim 1, wherein the step S2 comprises the following steps:

after the visual image is preprocessed, object detection and data association are carried out; then, a position and pose tracking is carried out by using an optical flow method, namely, the function of a visual odometer is realized, and the visual odometer integrates the position and pose optimized at the last moment to eliminate the accumulated error to the maximum extent; and then constructing a local object road sign map by using multiple views, adopting a sliding window type local map scale control strategy, carrying out rigid body transformation on an object road sign envelope surface coordinate system, expressing under a camera coordinate system, and finally forming the scale-controllable object road sign local map related to the camera coordinate system.

4. The visual positioning method for mobile robot based on semantic object road sign of claim 1, wherein the specific process of step S3 is as follows:

matching an object landmark local map and an object landmark global map, wherein the matching problem is a sub-graph matching problem in mathematics, and the object landmark global map is represented as a graph structure G (V, E), wherein V is a node and comprises a three-dimensional coordinate and a label of each landmark i, and E is an edge and represents the mutual three-dimensional relationship between two landmarks; in the same way, the local map is represented as a graph structure H ═ (V ', E'), where V 'is a node and E' is an edge;

the map matching problem is specifically defined as: given a graph G with n nodes (V, E) and a graph H with m nodes (V ', E'), node correspondences are sought in graph G and graph H:

X∈{0,1} ^n×m

to maximize consistency of graph attributes and structure:

wherein v represents i of graph G ₁ Node and i of graph H ₂ A consistency measure of the nodes, the measure derived from the three-dimensional scale consistency of the signposts and the consistency of the semantic tags; d represents i of diagram G ₁ -j ₁ Edge and i of graph H ₂ -j ₂ A consistency measure of the edge, the measure derived from three-dimensional positional distance consistency between different landmarks in the map; matching nodes with the same label in the two maps by utilizing semantic information, reducing the number of optimization iterations, finally obtaining an optimal matching solution, and determining the one-to-one correspondence of the road signs between the two maps;

after the matching of the object landmark local map to the object landmark global map is finished, the pose propagation is carried out by taking the landmark three-dimensional information in the object landmark global map as a reference, namely the transformation T of the global map landmark three-dimensional information-local map reference coordinate system-camera coordinate system in the world coordinate system on the matching _wc E, SE (3), and finally obtaining the pose of the robot at the current moment relative to a map world coordinate system:

T _WR ＝T _WC T _CR ∈SE(3)

in the formula, T _CR E SE (3) is the transformation matrix of the robot with respect to the camera coordinate system, T _WC E SE (3) is a transformation matrix of the camera coordinate system relative to the world coordinate system, and the transformation matrix T of the robot relative to the world coordinate system can be obtained by multiplying the two according to a transformation matrix chain rule and performing matrix operation _WR And E, SE (3) is the positioning attitude information of the robot.

Images