A robot visual odometry method in dynamic scenes

Through multi-model fitting technology and locally sensitive hashing algorithms, segmentation and classification of motion models, the problem of large positioning errors of robot visual odometers in dynamic scenarios is solved, and higher positioning accuracy and robustness are achieved.

CN114782802BActive Publication Date: 2024-11-01THE 54TH RESEARCH INSTITUTE OF CHINA ELECTRONICS TECHNOLOGY GROUP CORPORATION
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210280765.X
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-03-22
Publication Date
2024-11-01
Estimated Expiration
2042-03-22

AI Technical Summary

Technical Problem

In dynamic scenarios, the position estimation error of robot visual odometer positioning is large due to interference from moving objects, which is difficult to effectively solve the problem of the prior art.

Method used

Multi-model fitting technology is adopted to select frameworks through stereoscopic image sequence acquisition and feature point extraction and matching, combining local sensitive hashing algorithms and iterative model, segmenting and classifying motion models, eliminating interference from moving objects, and improving positioning accuracy.

Benefits of technology

It effectively eliminates interference from moving objects, improves the accuracy and robustness of robot positioning and navigation, and improves the performance of robot visual odometer in dynamic scenarios.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114782802B_ABST
    Figure CN114782802B_ABST
Patent Text Reader

Abstract

The present invention discloses a robot visual odometer method in a dynamic scene, and belongs to the field of robot positioning and navigation technology. There are generally multiple moving objects in a real scene. In such a dynamic scene, since the traditional visual odometer assumes that there are no moving objects in the scene, it causes a large error in the robot's positioning in the dynamic scene. The present invention integrates multi-model fitting technology into visual means, which can realize the solution of motion parameters of each motion model in the scene, further identify the robot's motion model, eliminate the interference of moving objects, and thus achieve more accurate robot self-positioning. The present invention improves the accuracy of robot positioning in dynamic scenes and enhances the accuracy and robustness of the robot positioning and navigation system.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robot positioning and navigation, in particular to the field of robot visual odometer positioning and navigation, and more particularly to the field of robot visual odometer positioning and navigation in dynamic scenes. Background Art

[0002] Visual odometry is an important solution for robot positioning and navigation. It uses the camera installed on the robot platform to perform self-positioning of the robot, that is, by analyzing the rotation and translation between two adjacent moments, and then multiplying the rotation and translation of each moment, the transformation relationship between the robot and the initial moment is calculated to obtain the robot's pose estimation. Given that there are inevitably moving objects in the robot's driving scene, most visual odometry methods are more suitable for positioning and navigation in static scenes. In dynamic scenes, the interference of moving objects will cause large errors in the robot's pose estimation. In addition, with the increasing application of robots in search and rescue, agricultural plant protection, space exploration, object grasping and other fields, the requirements for robot positioning capabilities are also getting higher and higher.

[0003] In view of this, a method is needed to achieve good robot positioning in dynamic scenes. Multi-model fitting is a basic data fitting method that aims to effectively estimate the number of model instances and corresponding parameters from the observed data. It has been widely used in many computer vision tasks. Multi-model fitting has excellent performance in terms of robustness to noise and outliers, and estimation of multi-model parameters. Summary of the invention

[0004] The present invention introduces multi-model fitting technology, fits multiple motion models in the input data, divides the interior points of each motion model, and estimates the parameters of each motion model, thereby eliminating the interference of moving objects and solving the positioning problem of the robot in dynamic scenes. In addition, the present invention adopts an iterative strategy in the multi-model fitting framework to further enhance the accuracy and robustness of multi-model fitting and improve the positioning and navigation performance of the robot under the interference of multiple moving objects.

[0005] The technical solution adopted by the present invention is:

[0006] A robot visual odometer method in a dynamic scene includes the following steps:

[0007] (1) The robot collects stereoscopic image sequences and performs image preprocessing while moving;

[0008] (2) For the collected stereo images at the initial moment and the next moment, feature points are extracted and matched for the left and right images at each moment, and feature points are extracted and matched for the left images at the previous and next moments, and the intersection of the feature points extracted and matched twice is taken to obtain the three-dimensional coordinates of the feature matching points;

[0009] (3) Perform motion segmentation on the feature matching points at the previous and next moments to estimate the robot motion model;

[0010] (4) Estimate the parameters of the motion model to obtain the basic matrix, tap the basic matrix to obtain the rotation matrix R and the translation matrix t, and multiply the pose to calculate the pose of the current frame;

[0011] (5) Take the next moment as the initial moment, return to step (2), and perform local bundle adjustment to correct the robot's position and posture at each moment.

[0012] Furthermore, in step (1), the robot acquires a stereoscopic image sequence through a binocular camera, and the binocular camera is installed on the robot platform.

[0013] Furthermore, the specific process of step (3) is as follows: first, at N feature matching points X = [x1, x2, ..., x N ] to generate M model hypotheses, and then calculate the residual matrix from all points to all model hypotheses:

[0014]

[0015] Among them, the i-th row r of R i =[r i (1) ,r i (2) ,…,r i (M) ] is point x i To all model assumptions residuals, the vector r i Sort in ascending order from small to large to form the permutation preference vector Make

[0016] Match the feature point x i The permutation preference vector σ i and feature matching point x j The permutation preference vector σ j The correlation between them is defined as:

[0017]

[0018] Where k = M / 10, M is the number of model assumptions;

[0019] Then, by defining the semi-positive definite kernel matrix S∈[0,1] N×N Calculate feature matching point x i and feature matching point x j The similarity S between (i,j) :

[0020]

[0021] ε(i,j)=1-f(σ i ,σ j )

[0022] Among them, ε(i,j) is the point x i and point x j The spatial distance between

[0023] Then the similarity matrix S (i,j) As the input of the local sensitive hash retrieval algorithm based on P-stable distribution, the feature matching points are grouped and divided into multiple clusters; then, through the designed model selection iterative framework, the method of constructing an energy function and finding the global minimum is adopted, combined with the GRIC criterion, redundant clusters are eliminated from the obtained clusters, and the model closest to the true model is selected; finally, based on the residual information corresponding to the true model, hierarchical clustering is performed on the remaining feature matching points to re-divide the points into the correct motion model, thereby realizing the internal point classification of different motion models.

[0024] Furthermore, the specific process of step (4) is as follows:

[0025] First, use RANSAC to estimate the parameters of the obtained robot motion model to obtain the basic matrix, and then convert the basic matrix into the essential matrix. Then decompose the essential matrix E into the rotation matrix R and the translation matrix t, that is, the robot posture at the current moment is obtained. Then, the robot coordinate system at the initial moment is set as the unified coordinate system, and the calculated robot posture at the current moment is combined with the robot posture calculated at the previous moment. The posture of the robot in the current frame relative to the unified coordinate system is obtained through cumulative transformation.

[0026] Further, the specific method of the local bundle adjustment in the described step (5) is: use a sliding window to intercept the nearest h frame images around the current moment, construct a reprojection error equation, and correct the posture of the robot at each moment, where h is a set value.

[0027] The advantages of the method of the present invention are as follows:

[0028] 1. This paper proposes a robot visual odometer method in dynamic scenes, which organically integrates visual odometer and multi-model fitting technology. Compared with traditional visual odometer, the core of this method lies in the motion segmentation part. The proposed method effectively eliminates the interference of moving objects, makes up for the defect that traditional visual odometer is easy to fall into local optimum, solves the problem of large positioning error of robots in dynamic scenes, and improves the accuracy and applicability of robot positioning and navigation system.

[0029] 2. The present invention uses the idea of ​​multi-model fitting to complete motion segmentation, innovatively projects data points into similarity space and uses local sensitive hashing algorithm for grouping, and designs an iterative model selection strategy to select high-quality model hypotheses. The proposed motion segmentation method is robust to noise and outliers, and can complete the segmentation of multiple motions more robustly and accurately. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 The present invention is an overall implementation flow chart of the method.

[0031] Figure 2 This is a flow chart for implementing the motion segmentation part of the present invention. DETAILED DESCRIPTION

[0032] The technical solution of the present invention is further described below in conjunction with the accompanying drawings.

[0033] like Figure 1 As shown, a robot visual odometer method in a dynamic scene includes the following steps:

[0034] 1) Data acquisition. Fix the binocular camera horizontally on the top of the robot platform and ensure that the binocular camera's line of sight is not blocked. Calibrate the binocular camera. Then operate the robot to start moving. During the robot's movement, the binocular camera collects stereo image sequences and inputs them into the processor for image correction and other processing.

[0035] 2) Feature point extraction and matching. For the collected stereo images at the initial moment and the next moment, the SIFT algorithm is used to extract and match feature points for the left and right images at each moment, and the SIFT algorithm is used to extract and match feature points for the left images at the previous and next moments. Then the intersection of the two is taken to obtain the three-dimensional coordinates of the feature matching points to achieve feature association between the previous and next moments.

[0036] 3) Motion segmentation before and after, such as Figure 2 As shown. First, N feature matching points X = [x1, x2, ..., x N ] Obtain M model hypotheses H=[h1,h2,…,h M ], calculate the residuals from all points to the model assumptions:

[0037]

[0038] Where R N×M The i-th row r i =[r i (1) ,r i (2) ,…,r i (M) ] is point x i To all model assumptions residuals, the vector r i Sort in ascending order from small to large to form the permutation preference vector Make Now point x i The permutation preference vector σ i and point x j The permutation preference vector σ j The correlation between them is defined as:

[0039]

[0040] Each data point is represented by its first k (k = M / 10) permutation preferences, then σ i and σ j The correlation f(σ i ,σ j ) is larger, then point x i and point x j The greater the probability of belonging to the same motion model.

[0041] Next, we use a semi-positive definite kernel matrix S∈[0,1] N×N Define point x i and point x j The similarity between:

[0042]

[0043] ε(i,j)=1-f(σ i ,σ j )

[0044] Where ε(i,j) is the point x i and point x j The spatial distance between them, the similarity arrangement vector Represents point x i The similarity to all points.

[0045] Next, the P-stable distribution-based locality sensitive hashing (P-stableLSH) algorithm is used to map the data points into several clusters, where the hash function of P-stableLSH is defined as:

[0046]

[0047] in is a floor function, x represents the original input data, and each entry in the random variable a is generated independently from the p-stable distribution. g is a constant greater than 0, and it is the most important parameter in the algorithm. If g is set too large, all data points will be easily mapped to one bucket. If it is set too small, the local sensitivity effect will not be achieved. c is a real number randomly generated by a uniform distribution, where the uniform distribution range is between [0, g].

[0048] Since P-stableLSH only performs approximate queries, we then perform further model selection operations to remove some redundant clusters and select the model that is closest to the true model. The model selection framework combines energy function minimization and GRIC criteria to continuously optimize the initial model in an iterative manner.

[0049] First, construct an energy function G on the dataset:

[0050] G=G d +G s

[0051] The data item G d and the spatial smoothness term G s Used to penalize the neighborhood midpoint x i and point x j The label inconsistency is defined as follows:

[0052]

[0053]

[0054] Among them, f i and f j Represents point x i and point x j The label of point x is used to measure i Function of the distance to the model, V(f i ,f j ) comes from the Potts model:

[0055]

[0056] Then, the α expansion algorithm is used to solve the local minimum of the energy function E, and the GRIC criterion (Geometric Robust Information Criterion) is used to further select the model. In order to solve the global minimum of the energy function E, the model selection framework adopts an iterative approach, alternately executing the α expansion algorithm and the GRIC criterion, and continuously optimizing the model until the model cluster remains basically unchanged. Since the GRIC criterion contains model complexity constraints, it enables the energy to converge to the minimum value quickly. Finally, hierarchical clustering is performed to re-divide the remaining data points into the correct motion model to achieve internal point classification of different motion models.

[0057] 4) Current frame motion estimation. In this step, RANSAC is first used to estimate the parameters of the robot motion model, and then the basic matrix F and the camera intrinsic parameters K1 and K2 are used to estimate the essential matrix E:

[0058] E=K2 T FK1

[0059] Essential matrix E = R[T] × =R[R T t] × =[t] × R, after SVD decomposition, is E = U*diag(1,1,0)*V T , where U and V are unitary matrices, that is, they satisfy U T U=I,V T V=I, for any E, there are two possible solutions for t, R:

[0060]

[0061]

[0062] in = represents the rotation matrix obtained by rotating 90° along the Z axis. Since -E and E are equivalent, if the negative sign is taken for any t, the same result will be obtained. Therefore, there are four solutions for t and R, and then the solution with positive depth of the feature point in both camera poses is selected.

[0063] Then, the robot coordinate system at the initial moment is set as the unified coordinate system, and the robot posture calculated at the current moment is combined with the robot posture calculated at the previous moment to calculate the robot posture relative to the unified coordinate system through cumulative multiplication.

[0064] 5) Continuous frame motion segmentation and motion estimation. Because the binocular camera continuously collects stereo image sequences during the robot's movement, for the next moment of the collected stereo image, step 2) to step 4) are executed in sequence, and the robot's position at that moment is calculated through motion segmentation and further motion estimation. At the same time, local bundle adjustment is performed. The nearest k frames of images around the current moment are intercepted, and the reprojection error equation is constructed:

[0065]

[0066] Among them, P j represents the coordinates of the object point, represents the position of the left camera of the i-frame image, represents the pose of the right camera of the i-frame image, and is the left and right image matching points corresponding to the i-th frame image, n represents the number of views taken by the camera, m represents the number of 3D points, and h represents the h-frame image closest to the current frame. Represents point P j Is there a pixel on image i? If so, otherwise When the coordinate system of the calculated camera pose is set to the left camera coordinate system, s=[b,0,0] T , b is the length of the baseline. Next, the LM (Levenberg-Marquardt) algorithm is used to optimize the posture and three-dimensional point coordinates to correct the posture of the robot at each moment.

Claims

1. A robot visual odometry method in dynamic scenes, It is characterized in that The following steps are involved: (1) The robot collects stereo image sequences and performs image preprocessing while moving; (2) For the collected stereo images at the initial moment and the next moment, feature points are extracted and matched for the left and right images at each moment, and feature points are extracted and matched for the left images at the previous and next moments, and the intersection of the feature points extracted and matched twice is taken to obtain the three-dimensional coordinates of the feature matching points; (3) Perform motion segmentation on the feature matching points at the previous and next moments to estimate the robot motion model; (4) Estimate the parameters of the motion model to obtain the basic matrix, tap the basic matrix to obtain the rotation matrix R and the translation matrix t, and multiply the pose to calculate the pose of the current frame; (5) Taking the next moment as the initial moment, returning to step (2), and performing local bundle adjustment at the same time to correct the position and posture of the robot at each moment; The specific process of step (3) is as follows: first, at N feature matching points X = [x 1 ,x 2 ,…,x N ] to generate M model hypotheses, and then calculate the residual matrix from all points to all model hypotheses: Among them, the i-th row r of R i =[r i (1) ,r i (2) ,…,r i (M) ] is point x i To all model assumptions residuals, the vector r i Sort in ascending order from small to large to form the permutation preference vector Make Match the feature point x i The permutation preference vector σ i and feature matching point x j The permutation preference vector σ j The correlation between them is defined as: Where k = M / 10, M is the number of model assumptions; Then, by defining the semi-positive definite kernel matrix S∈[0,1] N×N Calculate feature matching point x i and feature matching point x j The similarity S between (i,j) : ε(i,j)=1-f(σ i ,s j ) Among them, ε(i,j) is the point x i and point x j The spatial distance between Then the similarity matrix S (i,j) As the input of the local sensitive hash retrieval algorithm based on P-stable distribution, the feature matching points are grouped and divided into multiple clusters; then, through the designed model selection iterative framework, the method of constructing an energy function and finding the global minimum is adopted, combined with the GRIC criterion, redundant clusters are eliminated from the obtained clusters, and the model closest to the true model is selected; finally, based on the residual information corresponding to the true model, hierarchical clustering is performed on the remaining feature matching points to re-divide the points into the correct motion model, thereby realizing the internal point classification of different motion models.

2. According to the robot visual odometer method in a dynamic scene described in claim 1, It is characterized in that In step (1), the robot acquires a stereoscopic image sequence through a binocular camera, and the binocular camera is installed on the robot platform.

3. According to the robot visual odometer method in a dynamic scene described in claim 1, It is characterized in that The specific process of step (4) is as follows: First, use RANSAC to estimate the parameters of the obtained robot motion model to obtain the basic matrix, and then convert the basic matrix into the essential matrix. Then decompose the essential matrix E into the rotation matrix R and the translation matrix t, that is, the robot posture at the current moment is obtained. Then, the robot coordinate system at the initial moment is set as the unified coordinate system, and the calculated robot posture at the current moment is combined with the robot posture calculated at the previous moment. The posture of the robot in the current frame relative to the unified coordinate system is obtained through cumulative transformation.

4. According to the robot visual odometer method in a dynamic scene described in claim 1, It is characterized in that The specific method of local bundle adjustment in the described step (5) is: use sliding window to intercept the nearest h frame image around the current moment, construct reprojection error equation, correct the posture of the robot at each moment, and h is a set value.