A feature selection SLAM method integrating attention and prediction

Abstract

The present invention relates to a feature selection SLAM method integrating attention and prediction. The method of the present invention comprises: establishing a mathematical model for quantifying the feasibility of feature points; establishing a nonlinear perspective projection model, and using its simplified linear measurement model to calculate the information matrix of the feature points; selecting k features (approximately) maximizing the logdet metric from the features detected by the SLAM front-end visual odometer through a greedy algorithm with delayed evaluation, and then placing the selected features into the SLAM back-end for nonlinear optimization.

Description

Technical Field

[0001] The present invention relates to the field of SLAM, in particular to a feature selection SLAM method integrating attention and prediction. Background Art

[0002] When faced with rapidly changing scenes, visual SLAM based on vision perception technology often fails due to the lack of trackable features.

[0003] Many international research institutions have invested a lot of manpower and material resources in research. The SLAM problem is an important part of robot research, and its focus is to enable the robot to obtain its own position in real time. In daily life, humans can continuously process a large amount of information obtained from the outside world, such as driving a car on a highway or walking on a crowded street. In cognitive science, it is generally agreed that humans can efficiently process the large amount of data they face because we can prioritize certain aspects of the visual scene and ignore other aspects. This is a visual attention mechanism of our human beings, a cognitive process that enables humans to parse large amounts of visual data by selecting relevant information and filtering out irrelevant stimuli, thereby maximizing performance with limited resources. Although robots cannot do this like humans, this visual attention mechanism is worth learning from for robot autonomous positioning and navigation. Summary of the invention

[0004] The main problem solved by the present invention is that visual SLAM often fails due to the lack of trackable features when facing sharp turns and rapidly changing scenes. The present invention introduces a prediction mechanism based on the feature utility generated by the interrelationship between the environment and the camera motion state, and uses attention and prediction to select features to reduce the loss of tracking features. By filtering out those feature points that will be lost in the field of view, the stability of the SLAM system is increased. The feature selector knows the robot's intention (future movement) and selects features that are more likely to appear in the future field of view accordingly, so that the optimization efficiency can be improved with fewer feature points containing more information.

[0005] A feature selection SLAM method integrating attention and prediction of the present invention comprises the following steps:

[0006] Step 1: Collect camera sensor data and input it into the front-end visual odometer SLAM to obtain feature points;

[0007] Step 2: Select the first K features that maximize the logdet metric among the acquired feature points;

[0008] Step 3: Put the selected features into the SLAM backend for nonlinear optimization to obtain the current pose of the camera sensor.

[0009] The sensor is a camera.

[0010] The first K features selected to maximize the logdet metric include:

[0011] Step 2.1: Establish a mathematical model to quantify the feasibility of feature points;

[0012] Step 2.2: Establish a nonlinear perspective projection model, and use its simplified linear measurement model to calculate the information matrix of the feature points;

[0013] Step 2.3: Bring the information matrix of the calculated feature points into the feasible mathematical model, calculate the logdet metric of each feature point and arrange them in descending order, and select the top K features.

[0014] The mathematical model for quantifying the feasibility of feature points adopts logdet measurement, including:

[0015] Step 2.1-1: The mathematical model is expressed as a feature point set S when the metric equation f is maximum:

[0016] And |S|≤κ

[0017] Among them, S is the feature set, κ is the number of feature points;

[0018] Step 2.1-2: The mathematical model measurement equation f adopts logdet measurement, so the above model is rewritten as:

[0019]

[0020] Among them, represents the state vector from time k to K+H, is the predicted state vector to be optimized from time k to K+H; in addition, let P k:k+H For The covariance matrix of the estimated error from time k to time K+H of the associated time is called the information matrix from time k to time K+H, which is expressed as Δ l is the information matrix of the lth feature; p l is the probability that the lth feature is tracked, which can be obtained by normalizing the feature detection score.

[0021] The method of establishing a nonlinear perspective projection model and calculating the information matrix of feature points using a linear measurement model includes:

[0022] Step 2.2-1: Establish a nonlinear perspective projection model;

[0023]

[0024] in, is the position of feature point l in the world coordinate system, is the position of the camera at time h;

[0025] Step 2.2-2: Simplify the above nonlinear perspective projection model into a linear measurement model and calculate the information matrix of the feature points:

[0026] I. Define the predicted posture of the sensor at any time h in the H time period as

[0027] II. Order but exp(φ^) is the rotation matrix ΔR, Converted into the form of Lie group Lie algebra.

[0028] III. Rotate the matrix Translation Vector Expressed as R and t, these quantities are then used to parameterize the nonlinear perspective projection model, and the left side of the nonlinear perspective projection model equation is expressed as And linearize it using the total differential formula, as follows:

[0029]

[0030] in,

[0031] IV. Then, the contribution of the lth feature in time period H is Δ l , which can be expressed as:

[0032] Δ l =A T A

[0033]

[0034] in, is at time k It is k+l moment U kl is the equation of the characteristic point l at time k The partial derivative with respect to t, U (k+H)l is the equation of the characteristic point l at time k+H The partial derivative with respect to t.

[0035] The proposed attention and prediction mechanism retains the most informative visual features detected by the front end and feeds them back to the estimation back end. The mechanism has the following characteristics: (1) It is predictive in nature, able to sense the robot's future motion and select features accordingly. It ensures accurate positioning under sharp turns and rapid changes; (2) It is greedy, using a greedy algorithm with delayed evaluation to ensure the suboptimality of the logdet metric. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is the overall flow chart of the present invention;

[0037] Figure 2 A timeline diagram of the present invention;

[0038] Figure 3 It is the vector collinear diagram of the present invention; DETAILED DESCRIPTION

[0039] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation method of the present invention is described in detail below in conjunction with the accompanying drawings. In the following description, many specific details are set forth to facilitate a full understanding of the present invention. However, the present invention can be implemented in many other ways different from those described herein, and those skilled in the art can make similar improvements without violating the connotation of the invention, so the present invention is not limited by the specific implementation disclosed below.

[0040] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used in the specification of the invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0041] A feature selection SLAM method integrating attention and prediction of the present invention comprises the following steps:

[0042] Step 1: Establish a mathematical model to quantify the feasibility of feature points;

[0043] Step 2: Establish a nonlinear perspective projection model and use its simplified linear measurement model to calculate the information matrix of feature points;

[0044] Step 3: Select k features from the features detected by the SLAM front-end visual odometer through a greedy algorithm with delayed evaluation to maximize the logdet metric. Finally, put the selected features into the SLAM back-end for nonlinear optimization.

[0045] like Figure 1The figure shows the overall flow chart, which solves the problem of SLAM positioning failure in the face of sharp turns and fast-moving scenes by integrating a feature selection method based on attention and prediction into the classic framework of SLAM.

[0046] The feasibility of quantifying feature points in step 1 includes the following specific processes:

[0047] (1): To maximize the performance of motion estimation, the feature subset should have at most κ features that are most effective in reducing the uncertainty of visual state estimation. This problem can be simply stated as

[0048]

[0049] Among them, S is the feature set and κ is the number of feature points.

[0050] (2): The solution to this problem depends on the choice of the metric equation f, which will quantify the feasibility of the feature subset. The present invention adopts the logdet metric, which can represent the sub-modularity and quantify the uncertainty of the state estimation. More importantly, it should be able to obtain the most relevant feature points.

[0051]

[0052] like Figure 2 The timeline diagram shown is represents the state vector from time k to K+H, where is the predicted state vector to be optimized from time k to K+H. In addition, let P k:k+H For The covariance matrix of the estimated error from the associated k moment to the K+H time period, its inverse matrix is ​​called the information matrix, expressed as Δ l is the information matrix of the lth feature, p l is the probability that the lth feature is tracked, which can be obtained by normalizing the feature detection score.

[0053] The specific steps for calculating the feature point information matrix in step 2 are as follows:

[0054] (1): Through Figure 3 The vector collinearity diagram shown establishes a nonlinear perspective projection model;

[0055]

[0056] where u hl is the position of feature point l in the pixel coordinate system, is the position of feature point l in the world coordinate system, It is the position of the camera at time h in the time period from time k to K+H.

[0057] (2): Simplify the above nonlinear perspective projection model into a linear measurement model and calculate the information matrix of the feature points;

[0058] The state at each moment from time k to K+H is defined as (the robot's predicted pose), where For the convenience of expressing the formula later, the rotation matrix Translation Vector Denoted as R and t, these quantities are then used to parameterize the nonlinear perspective projection model. The left side of the nonlinear perspective projection model equation is expressed as And linearize it using the total differential formula

[0059]

[0060] in,

[0061] The contribution of the lth feature in the time period H from time k to K+H is Δ l It can be expressed as

[0062] Δ l =A T A

[0063]

[0064] in, is at time k It is k+l moment U kl is the equation of the characteristic point l at time k The partial derivative with respect to t, U (k+H)l is the equation of the characteristic point l at time k+H The partial derivative with respect to t.

[0065] Selecting k features to maximize the logdet metric through a greedy algorithm with delayed evaluation includes the following specific process:

[0066] In step three:

[0067] (1): As input, a certain number of features F are required for the system to select. The prediction information matrix {Δ l} l∈F , and the information matrix for the future H time period The pseudocode of the greedy algorithm with delayed evaluation is as follows

[0068] illustrate:

[0069] 1. Input information matrix Δ l ,The maximum number of features k that a feature point l and a feature subset should have.

[0070] 2. Initialize the feature subset to S # =φ.

[0071] 3. For loop i=1,...,κ, in each loop

[0072] 1) First, calculate the upper bound f(S) for each feature point in each frame # ∪{l}), l=1,...,N.

[0073] 2) Get

[0074] 3) Sort the feature points in descending order according to the upper limit value F ↓ =sort(U1,...,U N ).

[0075] 4) Initialize the best features.

[0076] Define the initial value f max =-1, l max = -1. For loop each feature point l = 1, ..., N, if the upper bound U of the feature point l <f max Then jump out of the loop; if f(S # ∪l)>f max Then f max =f(S # ∪l), l max =l.

[0077] 5) max Put it into feature subset S # In, S # =S # ∪l max .

[0078]

[0079] (2): Put the selected features into the SLAM backend for nonlinear optimization.

[0080] The above is a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should be regarded as within the scope of protection of the present invention.

Claims

1. A feature selection SLAM method incorporating attention and prediction, characterized in that: The following steps are involved: Step 1: Collect camera sensor data and input it into the front-end visual odometer SLAM to obtain feature points; Step 2: Select the first K features that maximize the logdet metric from the acquired feature points; including: Step 2.1: Establish a mathematical model for quantifying the feasibility of feature points; the mathematical model for quantifying the feasibility of feature points adopts logdet measurement, including: Step 2.1-1: The mathematical model is expressed as a feature point set S when the metric equation f is maximum: And |S|≤κ Among them, S is the feature set, κ is the number of feature points; Step 2.1-2: The mathematical model measurement equation f adopts logdet measurement, so the above model is rewritten as: Among them, represents the state vector from time k to K+H, is the predicted state vector to be optimized from time k to K+H; in addition, let P k:k+H For The covariance matrix of the estimated error from time k to time K+H of the associated time is called the information matrix from time k to time K+H, which is expressed as Δ l is the information matrix of the lth feature; p l is the probability that the lth feature is tracked, which can be obtained by normalizing the feature detection score; Step 2.2: Establishing a nonlinear perspective projection model, and using its simplified linear measurement model to calculate the information matrix of the feature points; said establishing a nonlinear perspective projection model, and using the linear measurement model to calculate the information matrix of the feature points, includes: Step 2.2-1: Establish a nonlinear perspective projection model; in, is the position of feature point l in the world coordinate system, is the position of the camera at time h; Step 2.2-2: Simplify the above nonlinear perspective projection model into a linear measurement model and calculate the information matrix of the feature points: Ⅰ. Define the predicted posture of the sensor at any time h in the H time period as II. Order but exp(φ^) is the rotation matrix ΔR, Converted into the form of Lie group Lie algebra; III. Rotate the matrix Translation Vector Expressed as R and t, these quantities are then used to parameterize the nonlinear perspective projection model, and the left side of the nonlinear perspective projection model equation is expressed as And linearize it using the total differential formula, as follows: in, IV. Then, the contribution of the first feature in time period H is Δ l , which can be expressed as: D l =A T A in, is at time k It is k+l moment U kl is the equation of the characteristic point l at time k The partial derivative with respect to t, U (k+H)l is the equation of the characteristic point l at time k+H The partial derivative with respect to t; Step 2.3: Bring the information matrix of the calculated feature points into the feasible mathematical model, calculate the logdet metric of each feature point and arrange them in descending order, and select the top K features; Step 3: Put the selected features into the SLAM backend for nonlinear optimization to obtain the current pose of the camera sensor.

2. A feature selection SLAM method incorporating attention and prediction according to claim 1, characterized in that: The sensor is a camera.

Images