Vision-based multi-agent distributed fast SLAM method

Abstract

The present invention relates to a multi-agent distributed fast SLAM method based on vision, belonging to the field of image processing technology. Before matching feature points, the picture collected by the camera is first segmented so that the feature points are evenly distributed in each area, so that the selected feature points are more representative; secondly, a "point-line" feature is designed to generate a new word bag, and loop detection is performed using reinforcement learning, which solves the problem of insufficient performance of point features in low-texture and low-contrast environments; since the convex optimization algorithm cannot be directly applied to the posture matrix, the duality principle is used to transform the original problem into a strongly convex dual problem, and the Lagrange multiplier method is used for iterative solution, and the equivalence of the original problem and the dual problem is proved; afterwards, since the convergence speed of the algorithm is slow, a posture averaging algorithm based on a random matrix is ​​designed, and the convergence speed of the multi-agent posture is significantly accelerated.

Description

Technical Field

[0001] The invention belongs to the technical field of image processing and relates to a multi-agent distributed fast SLAM method based on vision. Background Art

[0002] In actual SLAM applications, if you want to extract a map of a complex environment from a two-dimensional image, you need to extract feature points from the keyframes and match the feature points. Through the epipolar constraint and P2P algorithm, you can get the relative pose of the feature points and the camera and the camera motion trajectory. In addition, the robot pose is represented by a three-dimensional pose matrix, which satisfies the properties of Lie group elements. Therefore, the corresponding convex optimization algorithm cannot be directly applied in the process of multi-agent pose estimation. Therefore, in the robot SLAM problem, studying the multi-agent mapping and positioning problem has extremely important theoretical and application significance.

[0003] In current research, feature matching mainly includes two feature matching methods: point-based and point-line-based. Point features can be extracted through fast-orb, mainly to construct a binary brief descriptor, and select the two points with the closest descriptor distance for matching; since the point feature matching method cannot find enough feature points in a low-texture environment, the line feature method is needed, especially in areas with uneven feature distribution, which can well describe the texture of the environment. In addition, in loop detection, the traditional method is to use a pre-established word bag for loop detection, and it is necessary to traverse the entire word bag. When the number of entries is too large, the traversal takes a long time, and it is difficult to ensure the real-time performance of the system. Therefore, a new point-line word bag model is proposed to establish a dynamic word bag in the SLAM process, which does not require the preparation of the word bag in advance, and can store the word information of all images in an array, which can quickly perform loop detection.

[0004] In the target positioning problem, the main research is how to deal with the constraint problem of the pose matrix. There are usually two solutions. One is to convert the three-dimensional pose matrix into a one-dimensional vector through linear transformation, convert the distributed optimization problem into a minimum norm problem, and project the approximate solution onto the hyperplane with constraints to obtain the exact solution of the original problem. The other way is to convert the original problem into a strongly convex dual problem. The idea of ​​this method is to first define an adjoint operator to convert the pose matrix determinant constraint into the rank constraint of the dual problem. In the whole process, the non-negativity of the matrix needs to be guaranteed, and the dual problem and the original problem can be equivalent using the Slater condition.

[0005] In the current visual SLAM problem, feature matching mainly includes two feature matching methods: point feature and line feature. These two methods have better matching effects in environments with high contrast or clear textures. However, due to the large number of selected features and the long time consumption, it is difficult to achieve "real-time" matching in actual SLAM. Therefore, the present invention proposes a fast feature point matching method based on image segmentation, and uses deep learning to update the word bag online by combining points and lines within a certain range as an entry to achieve fast loop detection.

[0006] In addition, at this stage, single-agent visual SLAM is relatively mature, while multi-agent visual SLAM technology is in the initial development stage. Corresponding improved algorithms can be proposed in aspects such as robot communication and map fusion. The existing distributed multi-agent pose consistency algorithm uses the idea of ​​least squares method to iteratively solve the consistent pose, but the iterative convergence speed of this method is slow. Therefore, the present invention proposes an improved iterative algorithm. On the basis of the original algorithm, according to the communication topology of the agent, the perron matrix is ​​used to "weighted average" the pose of the agent, and then fast convergence is achieved. While achieving the consistency of the pose of the agent, the local point cloud map collected by a single agent is rotated, so that the maps of each group of robots can be simply and effectively fused to obtain a complete map. Summary of the invention

[0007] In view of this, an object of the present invention is to provide a vision-based multi-agent distributed fast SLAM method.

[0008] In order to achieve the above object, the present invention provides the following technical solutions:

[0009] A multi-agent distributed fast SLAM method based on vision, the method comprises the following steps:

[0010] S1: Problem description;

[0011] S2: Establish feature matching algorithm;

[0012] S3: loop detection;

[0013] S4: Establish consistency algorithm and map fusion algorithm.

[0014] Optionally, the S1 is specifically:

[0015] A map is constructed by n agents, and the pose model of the agent is:

[0016]

[0017] R is a rotation matrix, which represents the posture of the agent and is an element of the Lie group, satisfying SO(3) = {R∈M 3×3|R T R=RR T =I, det(R) = 1}, the F-norm of R is denoted as ||R||=tr(RR T )=tr(R T R), and the matrix inner product is <A,B>=tr(A T B), where A,B∈M 3×3 ;

[0018] Given the initial pose T0 of the agent, the relative poses of n agents are obtained; the key frames captured by the camera are converted to grayscale, feature extracted and matched, and the motion of the agent between two key frames is solved using epipolar geometry and PNP. The pose of the agent at the previous moment is multiplied on the left to obtain the pose of the agent in the current key frame:

[0019]

[0020] Among them, T i is the pose of the agent in the ith key frame, is the relative motion between the ith key frame and the previous key frame;

[0021] After the agent group builds a local map, there are posture differences between the robot leaders, and the agent postures are normalized:

[0022]

[0023] Represent T in terms of R and t

[0024]

[0025] Where T i is the position of group i agents, R i is the i-group agent posture, It is a consistent posture. When the postures of all intelligent agents are consistent, the maps can be directly fused after the local maps are rotated accordingly.

[0026] Optionally, S2 is specifically:

[0027] S21: converting the collected image into a grayscale image and performing median filtering;

[0028] S22: evenly divide the processed image into 10×10 small areas;

[0029] S23: Calculate the contrast of each feature, expressed as variance, and exclude the area with contrast σ<ζ;

[0030] E=EP

[0031]

[0032] S24: In the remaining areas, extract the same number of point features and line features from each area;

[0033] S25: The sampling time is T, and the speed obtained by the agent is v, then x = kTv; k is the comparison washing, and x is the comparison distance;

[0034] S26: When matching the features of a certain area, the distance is less than x (2x+1) 2 -1 region, and select the feature with the closest descriptor distance for matching;

[0035] S27: After feature matching, the camera motion is calculated based on the matched feature point pairs; the pixels in the two images match correctly, indicating that they are projections of the same point on two surfaces, and the camera motion is solved using epipolar geometry constraints.

[0036] Optionally, the S3 is specifically:

[0037] After feature extraction, the features in each image are clustered into 5 terms and stored in a bag of words. The number of terms increases as the system runs.

[0038] The terms are represented by parameters, and the term information in each image is represented by vectors. The term information in the first two images is represented as follows:

[0039] P1=1·o1+1·o2+1·o3+1·o4+1·o5 (3-1)

[0040] P2=1·o3+1·o5+1·o6+1·o7+1·o8 (3-2)

[0041] Where P i Indicates the entry information of the i-th image, w i represents the i-th entry; assuming that m images are collected at the current moment, and there are n entries in the word bag, an m*n word bag matrix is ​​used to represent the entry information in the entire SLAM process;

[0042]

[0043] The entry information vector P of the image collected at time i i =1·o3+1·o5+1·o6+1·o7+1·o8, P i =P2, the robot is currently at the location of the second picture; comparing the entry information vector, there is no need to traverse the entire word bag, and its time complexity is O(5m).

[0044] Optionally, the S4 is specifically:

[0045] The posture of the i-th group of agents is T i , and get the consistent posture:

[0046]

[0047] Split the transfer matrix into two parts, R and t, and find the solution. We know that t is the translation vector, which is a three-dimensional column vector without constraints. The optimal solution of t is

[0048]

[0049] When only the rotation matrix is ​​considered, equation (4-1) is simplified to the following form

[0050]

[0051] Expand the matrix norm in the above formula to get

[0052]

[0053] Solving the consistent solution of the rotation matrix is ​​to obtain the optimal linear function on the convex hull SO(3); replace convex{SO(3)} with a 2 m-1 ×2 m-1 The semi-positive definite matrix P represents

[0054]

[0055] Where P ≥ 0, trace(P) = 1, and the above transformation is expressed by A(P), we get

[0056] SO(3)={A(P)|P≥0,tr(P)=1} (4-6)

[0057] Transform A(P) to convert 2 m-1 ×2 m-1 The semi-positive definite matrix is ​​converted to an m×m-dimensional rotation matrix. Given the matrix

[0058]

[0059] make

[0060] in

[0061]

[0062] but

[0063]

[0064]

[0065] Define a matrix inner product adjoint operator A*(R) to realize the m×m-dimensional rotation matrix to 2 m-1 ×2 m-1 Transformation of a dimensional positive semidefinite matrix,

[0066]

[0067] The original problem is expressed as

[0068]

[0069] Its optimal solution

[0070]

[0071] Where α is the eigenvector of the largest eigenvalue of A*(R);

[0072] Divide all agents into three groups and write the consensus problem as follows:

[0073]

[0074] Or written as parameter P:

[0075]

[0076] The equality constraints in the above formula can be written in Lagrangian form, as

[0077]

[0078] Where P is a semi-positive definite matrix with a trace of 1, Y is a symmetric matrix, and the Lagrangian function is solved by the gradient descent method, which is written in the form of a distributed algorithm:

[0079]

[0080] in P i ≥0,trace(P i )=1,(i,j)∈V;

[0081] In the above Lagrange iterative formula, the rotation matrix can only be updated once in each iteration, and the convergence rate is low. A gradient descent algorithm based on a random matrix is ​​proposed:

[0082] According to the communication topology of the intelligent agent, set the corresponding weights and establish the random matrix W;

[0083] S41: Update weights based on current posture

[0084]

[0085]

[0086] S42: Select the average coefficient k and use the gradient descent method to solve the Lagrangian function

[0087]

[0088]

[0089] S43: Estimation based on random matrices

[0090]

[0091]

[0092] When performing the above iterations, each time an iteration is performed, use Get the posture of the i-th group of agents in each iteration Then the rotation matrix from the last iteration to the current posture is During iteration, the local map constructed by each group is rotated at the same time, and its rotation matrix is

[0093] Using the above random matrix gradient descent algorithm, each group of agents quickly converges to the consistency value P*, using the formula Get the consistent posture of the agent When the iteration converges, the fusion of each local map is achieved.

[0094] The beneficial effects of the present invention are:

[0095] 1. The algorithm designed in the present invention can be used for multi-agent distributed posture consistency control, projecting the non-convex posture model onto a convex plane and solving it iteratively using the least squares method.

[0096] 2. The present invention proposes a fast map fusion algorithm, which can realize fast map fusion by simply projecting the local maps established by each intelligent agent into the same coordinate system.

[0097] 3. The present invention proposes an iterative least squares algorithm based on a random matrix. On the basis of the Lagrangian iterative algorithm, the current node posture is updated with the average posture of the neighboring nodes, which can achieve rapid convergence of the intelligent body posture.

[0098] 4. The present invention proposes a block-based feature matching method. According to the sampling time and camera speed, feature retrieval and matching only need to be performed in adjacent blocks, which can shorten the feature matching time and avoid traversing the entire image.

[0099] 5. The present invention proposes a loop detection algorithm for online updating of word bags. In the process of mapping, the entries of key frames are generated online and the word bags are updated, avoiding the process of establishing word bags separately.

[0100] 6. The present invention stores the entire word bag in the form of an array. When performing loop detection, it is only necessary to detect whether the term vector of the current frame matches a row of the word bag array, thereby reducing the time complexity of loop detection.

[0101] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0103] Figure 1 This is a comparison chart of feature matching algorithms;

[0104] Figure 2 Schematic diagram of loop detection;

[0105] Figure 3 This is the loop detection flow chart;

[0106] Figure 4 To improve the algorithm convergence speed comparison chart;

[0107] Figure 5 This is the flow chart of the SLAM system;

[0108] Figure 6 It is the system communication topology diagram;

[0109] Figure 7 It is a schematic diagram of the local map of agent 1;

[0110] Figure 8 It is a schematic diagram of the local map of Agent 2;

[0111] Fig. 9 It is a schematic diagram of the local map of agent 3;

[0112] Fig.10 This is the global map after fusion. DETAILED DESCRIPTION

[0113] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0114] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0115] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "back" and the like indicate directions or positional relationships, they are based on the directions or positional relationships shown in the drawings, which are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0116] The present invention consists of three parts: problem description, feature matching algorithm and loop detection, consistency algorithm and map fusion algorithm, which will be introduced in detail below:

[0117] 1. Problem Description

[0118] In the present invention, consider building a map by n agents, and assume that the pose model of the agent is:

[0119]

[0120] R is a rotation matrix that can be used to represent the posture of the agent. It is an element of the Lie group and satisfies SO(3) = {R∈M 3×3 |R T R=RR T =I, det(R) = 1}, the F-norm of R is denoted as ||R||=tr(RR T )=tr(R T R), and the matrix inner product is <A,B>=tr(AT B), where A,B∈M 3×3 .

[0121] Given the initial pose T0 of the agent, the relative poses of n agents can be obtained. The key frames captured by the camera are converted to grayscale, feature extracted, and matched. The motion of the agent between two key frames is solved using epipolar geometry and PNP. The pose of the agent at the previous moment is multiplied on the left to obtain the pose of the agent in the current key frame.

[0122] T n =T0T1 0 ...T n n-1 (1-2)

[0123] Among them, T i is the pose of the agent in the i-th key frame, T i i-1 is the relative motion between the ith key frame and the previous key frame.

[0124] After the agent group has established a local map, due to the differences in posture between the robot leaders, they cannot be directly fused and the posture of the agents needs to be normalized.

[0125]

[0126] Represent T in terms of R and t

[0127]

[0128] Where T i is the position of group i agents, R i is the i-group agent posture, It is a consistent posture. When the postures of all intelligent agents are consistent, the maps can be directly fused after the local maps are rotated accordingly.

[0129] 2. Feature Matching Algorithm

[0130] The key point of visual SLAM is to estimate the motion of the camera based on the image. The color image itself is expressed as a matrix of pixel colors, or it is converted into a grayscale image and then represented by a matrix of pixel grayscale values. The feature point matching method is to select representative pixels (feature points) as references, and then compare the positions of the corner points in the image at different times to obtain the motion of the camera.

[0131] Feature points are composed of key points and descriptors. The descriptors mainly describe the information around the key points. The more commonly used ones are ORB features, which are composed of fast corner points and brief descriptors.

[0132] For line segments in an image, the endpoints of the line segments can usually be used as ORB feature points. By combining point features and line features, a new graph feature can be generated for loop detection. When detecting and matching line segments in an image, there may be partial occlusion or disconnection of the line segments. In this case, the graph features formed by the points and lines can be compared with the features in the image library. A threshold ξ∈[0,1] is set. When the similarity is greater than or equal to ξ, the graph feature match is considered successful. In addition, in a highly structured environment, two line segments can be matched by comparing the similarity of the line segment lengths.

[0133] In addition, in order to improve the representativeness of feature points, the entire image is divided into 10×10 small blocks, and the same number of ORB key points and line features are extracted in each small block. The brightness and contrast of each area are analyzed, and the areas with low contrast are excluded. In addition, in the traditional feature matching algorithm, it is necessary to scan the features in all areas and select two features with a close descriptor distance for matching. This method requires traversing the entire image. Therefore, in order to improve the efficiency of feature matching, when matching features in a certain area, only the (2x+1) distance of x is matched. 2 -1 region, where x increases with the increase of sampling time and speed. Figure 1 The figure shows a comparison of feature matching algorithms, with the conventional algorithm on the left and the improved algorithm on the right. If there is a feature point in the (6,5) area of ​​the current key frame, the corresponding feature point can be found in the (7,4) area of ​​the previous key frame based on the camera motion speed and sampling time without traversing the entire image. Since the entire image does not need to be traversed, the feature matching time can be effectively shortened.

[0134] 1. Convert the collected image into a grayscale image and perform median filtering.

[0135] 2. Divide the processed image evenly into 10×10 small areas.

[0136] 3. Calculate the contrast of each feature, expressed as variance, and exclude areas where the contrast σ < ζ.

[0137] E=EP

[0138]

[0139] 4. In the remaining regions, extract the same number of point features and line features from each region.

[0140] 5. The sampling time is T, and the speed obtained by the agent is v, then x = kTv (k is the comparison coefficient, x is the comparison distance)

[0141] 6. When matching features of a certain area, the distance is less than x (2x+1)2 -1 region, and select the feature with the closest descriptor distance for matching.

[0142] After feature matching, the camera motion needs to be calculated based on the matched feature point pairs. If the pixels in the two images match correctly, it means that they are the projections of the same point on two surfaces, and the camera motion can be solved using epipolar geometry constraints.

[0143] 3. Loop Detection

[0144] In SLAM mapping, one of the most important aspects is loop detection, that is, to verify whether the robot has passed the current position through the algorithm. Since only the estimation between two adjacent frames is given during pose estimation, the estimation result is prone to drift. i and x2, the accumulated error can be eliminated by using the pose after loop closure detection.

[0145] like Figure 2 As shown, conventional loop detection algorithms require a word bag containing various clusters to be given in advance, but when a key frame contains a new entry, the entry cannot be matched; in addition, all the entries containing a certain project are usually in the millions, and it takes a lot of time to traverse the entire word bag. Therefore, the present invention proposes a fast loop detection method based on an online word bag, which does not require the word bag required for the project to be provided in advance, can generate the word bag online during the mapping process, and store the entry information of the key frame in the form of an array, which can quickly realize loop detection and avoid traversing the entire word bag.

[0146] After feature extraction, the features in each image are clustered into 5 terms and stored in a word bag. During the entire SLAM process, when there are similar terms in two images, only new terms are added to the word bag, so the size of the word bag is variable, that is, the number of terms increases as the system runs.

[0147] If the terms are represented by parameters, the term information in each image can be represented by a vector. For example, the term information in the first two images can be represented as:

[0148] P1=1·o1+1·o2+1·o3+1·o4+1·o5 (3-1)

[0149] P2=1·o3+1·o5+1·o6+1·o7+1·o8 (3-2)

[0150] Where P i Indicates the entry information of the i-th image, w iRepresents the i-th term. Assuming that m images are collected at the current moment and there are n terms in the bag of words, an m*n bag of words matrix can be used to represent the term information in the entire SLAM process.

[0151]

[0152] The entry information vector P of the image collected at time i i =1·o3+1·o5+1·o6+1·o7+1·o8. By comparison, we can see that P i =P2, the robot is currently at the location of the second picture. For the given P matrix, the conventional loop detection algorithm needs to traverse the entire word bag, and its time complexity is O(m×n), while the improved algorithm only needs to compare the word information vectors without traversing the entire word bag, and its time complexity is O(5m).

[0153] Figure 3 This is the loop detection flow chart, the left side is the conventional algorithm, and the right side is the improved algorithm.

[0154] 4. Map Fusion Algorithm

[0155] From the above analysis, we can see that each group of agents can independently construct a local map, but because the postures of each group of robots are different, the maps cannot be simply added together. When the postures of the agents are the same, all local maps can be regarded as constructed by one agent and can be superimposed. This map fusion algorithm is simple in principle, has high accuracy, and has a good fusion effect.

[0156] It is known that the posture of the i-th group of agents is T i , we can get the consistent posture

[0157]

[0158] Split the transfer matrix into two parts, R and t, and solve it. It is known that t is a translation vector, which is a three-dimensional column vector without constraints. The optimal solution of t is

[0159]

[0160] Therefore, considering only the rotation matrix, equation (4-1) is simplified to the following form:

[0161]

[0162] Expand the matrix norm in the above formula to get

[0163]

[0164] Solving the consistent solution of the rotation matrix is ​​to obtain the optimal linear function on the convex hull SO(3). m-1 ×2 m-1 The semi-positive definite matrix P represents

[0165]

[0166] Where P ≥ 0, trace(P) = 1, and the above transformation is expressed by A(P), we can get

[0167] SO(3)={A(P)|P≥0,tr(P)=1} (4-6)

[0168] Transformation A(P) can transform 2 m-1 ×2 m-1 The semi-positive definite matrix is ​​converted to an m×m-dimensional rotation matrix. Given the matrix

[0169]

[0170] make

[0171] in

[0172]

[0173] but

[0174]

[0175]

[0176] Define a matrix inner product adjoint operator A*(R) to realize the m×m-dimensional rotation matrix to 2 m-1 ×2 m-1 Transformation of a dimensional positive semidefinite matrix,

[0177]

[0178] Then the original problem can be expressed as

[0179]

[0180] Its optimal solution

[0181]

[0182] where α is the eigenvector of the largest eigenvalue of A*(R).

[0183] Since all agents are divided into three groups, the consensus problem can be written as follows:

[0184]

[0185] Or written as parameter P:

[0186]

[0187] The equality constraints in the above formula can be written in Lagrangian form, as

[0188]

[0189] Where P is a semi-positive definite matrix with a trace of 1, and Y is a symmetric matrix. By solving the Lagrangian function using the gradient descent method, the above formula can be written as a distributed algorithm:

[0190]

[0191] in P i ≥0,trace(P i )=1,(i,j)∈V.

[0192] In the above Lagrange iterative formula, the rotation matrix can only be updated once in each iteration, and the convergence rate is low, so a gradient descent algorithm based on random matrices is proposed:

[0193] According to the communication topology of the intelligent agent, the corresponding weights are set and the random matrix W is established.

[0194] 1. Update weights based on current posture

[0195]

[0196]

[0197] 2. Select the average coefficient k and use the gradient descent method to solve the Lagrangian function

[0198]

[0199]

[0200] 3. Estimation based on random matrix

[0201]

[0202]

[0203] When performing the above iterations, each time an iteration is performed, use The posture of the i-th group of agents in each iteration can be obtained Then the rotation matrix from the last iteration to the current posture is During iteration, the local map constructed by each group is rotated at the same time, and its rotation matrix is

[0204] Using the above random matrix gradient descent algorithm, each group of agents can quickly converge to the consistency value P*. The consistent posture of the intelligent agent can be obtained When the above iterations converge, the fusion of local maps can be achieved. The principle of this fusion algorithm is simple and the accuracy is high, which is of great significance in multi-agent distributed SLAM systems. Figure 4 Comparison chart of convergence speed of improved algorithms. Figure 5 This is the flow chart of the SLAM system; Figure 6 It is the system communication topology diagram; Figure 7 It is a schematic diagram of the local map of agent 1; Figure 8 It is a schematic diagram of the local map of Agent 2; Fig. 9 It is a schematic diagram of the local map of agent 3; Fig.10 This is the global map after fusion.

[0205] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A vision-based multi-agent distributed fast SLAM method, characterized by: The method comprises the following steps: S1: Problem description; S2: Establish feature matching algorithm; S3: loop detection; S4: Establish consistency algorithm and map fusion algorithm; The S1 is specifically: A map is constructed by n agents, and the pose model of the agent is: R is a rotation matrix, which represents the posture of the agent and is an element of the Lie group, satisfying SO(3) = {R∈M 3×3 |R T R=RR T =I, det(R) = 1}, the F-norm of R is denoted by ||R||=tr(RR T )=tr(R T R), and the matrix inner product is <A,B>=tr(A T B), where A,B∈M 3×3 ; Given the initial pose T0 of the agent, the relative poses of n agents are obtained; the key frames captured by the camera are converted to grayscale, feature extracted and matched, and the motion of the agent between two key frames is solved using epipolar geometry and PNP. The pose of the agent at the previous moment is multiplied on the left to obtain the pose of the agent in the current key frame: Among them, T i is the pose of the agent in the i-th key frame, T i i-1 is the relative motion between the i-th key frame and the previous key frame; After the agent group builds a local map, there are posture differences between the robot leaders, and the agent postures are normalized: Represent T in terms of R and t Where T i is the position of group i agents, R i is the i-group agent posture, It is a consistent posture. When the postures of all agents are consistent, the maps can be directly fused after the local maps are rotated accordingly. The S2 is specifically: S21: converting the collected image into a grayscale image and performing median filtering; S22: evenly divide the processed image into 10×10 small areas; S23: Calculate the contrast of each feature, expressed as variance, and exclude the area with contrast σ<ζ; E=EP S24: In the remaining areas, extract the same number of point features and line features from each area; S25: The sampling time is T, and the speed obtained by the agent is v, then x = kTv; k is the comparison coefficient, and x is the comparison distance; S26: When matching the features of a certain area, the distance is less than x (2x+1) 2 -1 region, and select the feature with the closest descriptor distance for matching; S27: After feature matching, the camera motion is calculated based on the matched feature point pairs; the pixels in the two images are matched correctly, indicating that they are projections of the same point on two surfaces, and the camera motion is solved using epipolar geometry constraints; The S3 is specifically: After feature extraction, the features in each image are clustered into 5 terms and stored in a bag of words. The number of terms increases as the system runs. The terms are represented by parameters, and the term information in each image is represented by vectors. The term information in the first two images is represented as: P1=1·o1+1·o2+1·o3+1·o4+1·o5 (3-1) P2=1·o3+1·o5+1·o6+1·o7+1·o8 (3-2) Where P i Represents the entry information of the i-th image; assuming that a total of m images are collected at the current moment, and there are n entries in the word bag, an m*n word bag matrix is ​​used to represent the entry information in the entire SLAM process; The entry information vector P of the image collected at time i i =1·o3+1·o5+1·o6+1·o7+1·o8, P i =P2, the robot is currently at the location of the second picture; comparing the entry information vector, there is no need to traverse the entire word bag, and its time complexity is O(5m); The S4 is specifically: The posture of the i-th group of agents is T i , and get the consistent posture: Split the transfer matrix into two parts, R and t, and find the solution. We know that t is the translation vector, which is a three-dimensional column vector without constraints. The optimal solution of t is When only the rotation matrix is ​​considered, equation (4-1) is simplified to the following form Expand the matrix norm in the above formula to get Solving the consistent solution of the rotation matrix is ​​to obtain the optimal linear function on the convex hull SO(3); replace convex{SO(3)} with a 2 m-1 ×2 m-1 The semi-positive definite matrix P represents Where P ≥ 0, trace(P) = 1, and the above transformation is expressed by A(P), we get SO(3)={A(P)|P≥0,tr(P)=1} (4-6) Transform A(P) to convert 2 m-1 ×2 m-1 The semi-positive definite matrix is ​​converted to an m×m-dimensional rotation matrix. Given the matrix make in but Define a matrix inner product adjoint operator A*(R) to realize the m×m-dimensional rotation matrix to 2 m-1 ×2 m-1 Transformation of a dimensional positive semidefinite matrix, The original problem is expressed as Its optimal solution Where α is the eigenvector of the largest eigenvalue of A*(R); Divide all agents into three groups and write the consensus problem as follows: s.t.R i ∈SO(3) Or written as parameter P: s.t.P i ≥0,trace(P)=1 The equality constraints in the above formula can be written in Lagrangian form, as Where P is a semi-positive definite matrix with a trace of 1, Y is a symmetric matrix, and the Lagrangian function is solved by the gradient descent method, which is written in the form of a distributed algorithm: P i t+1 =max tr(Tmp T P) in P i ≥0,trace(P i )=1,(i,j)∈V; In the above Lagrange iterative formula, the rotation matrix can only be updated once in each iteration, and the convergence rate is low. A gradient descent algorithm based on a random matrix is ​​proposed: According to the communication topology of the intelligent agent, set the corresponding weights and establish the random matrix W; S41: Update weights based on current posture S42: Select the average coefficient k and use the gradient descent method to solve the Lagrangian function S43: Estimation based on random matrices When performing the above iterations, each time an iteration is performed, use Get the posture of the i-th group of agents in each iteration Then the rotation matrix from the last iteration to the current posture is During iteration, the local map constructed by each group is rotated at the same time, and its rotation matrix is Using the above random matrix gradient descent algorithm, each group of agents quickly converges to the consistency value P*, using the formula Get the consistent posture of the agent When the iteration converges, the fusion of each local map is achieved.

Images