Multi-machine monocular vision elastic triangularization map fusion method

Abstract

The invention discloses a multi-machine monocular vision elastic triangulation map fusion method, which is characterized in that two robots carrying monocular cameras and IMUs do not need to keep the same direction and maintain a baseline, can autonomously plan a track based on a mapping efficiency maximization principle, and when the common-view features of the two robots are matched, the two robots can automatically plan the track according to the mapping efficiency maximization principle. Combined observation of multi-machine visual angles is elastically expanded to a single-machine independent triangulation equation, and multi-machine combined triangulation is achieved. Through 3D-3D pose estimation of common-view map points, coordinate system conversion of two robot vision inertial odometers is realized, so that a multi-robot global coordinate system is established under the condition that a new sensor is not introduced and relative observation motion is not maintained. And constructing a global fusion map composed of the independent map points of the robots and the common-view map points of the multiple robots. According to the method, the motion condition of pure rotation can be robustly adapted, the depth estimation precision of the long-distance map point is effectively improved, and meanwhile, the restriction on multi-robot track planning is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of map fusion, and in particular to a map fusion method based on multi-machine monocular vision elastic triangulation. Background Art

[0002] In the monocular visual-inertial simultaneous localization and mapping (SLAM) system, triangulation of feature points is a technology that uses the matching features of multiple viewpoints to estimate the three-dimensional spatial position of the corresponding map point. It is an important step in the Structure from Motion (SfM) process. Single-robot monocular visual-inertial SLAM systems usually select the previous and next keyframes in the time sliding window during the robot's motion for triangulation. For example, VINS-Mono (Qin T, Li P, Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator [J]. IEEE Transactions on Robotics, 2018, 34 (4): 1004-1020.) traverses all keyframes in the sliding window and triangulates each group of two keyframes. OpenVINS (Geneva P, Eckenhoff K, Lee W, et al. OpenVINS: A Research Platform for Visual-Inertial Estimation [C] / / 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris, France: IEEE, 2020: 4666-4672.) All key frames in the sliding window are combined into a triangulated linear equation system for solution. Such a system selects multi-view images between frames with small camera perspective differences. Even for binocular vision systems, the length of the two-view geometric baseline is short, so it is difficult to accurately estimate the depth of distant map points. In addition, in order to obtain the scale, the monocular vision system requires the translation observation information of the inertial measurement unit (IMU) between the image frames involved in the triangulation. Therefore, when the robot performs "pure rotation" motion, the three-dimensional space points obtained by triangulation lack scale information. In order to solve these problems, it is feasible to use multiple robots equipped with monocular visual inertial sensors to form a multi-machine system.Omni-Swarm (XuH, Zhang Y, Zhou B, et al. Omni-Swarm: A Decentralized Omnidirectional Visual–Inertial–UWB State Estimation System for Aerial Swarms[J]. IEEE Transactions on Robotics, 2022, 38(6): 3374-3394.) estimates the relative pose between two drones through the common view features of the fisheye cameras of two drones. However, it does not triangulate the features using the observation data of multiple drones. Instead, it uses the binocular fisheye cameras on a single drone to achieve separate triangulation, which increases the cost of the single drone and reduces the application space. Some scholars use cameras from multiple machines to form a virtual stereo camera to achieve triangulation of long-distance feature points: Karrer et al. (Karrer M, Chli M. Distributed Variable-Baseline Stereo SLAM from two UAVs[C] / / 2021IEEE International Conference on Robotics and Automation(ICRA).Xi'an,China:IEEE,2021:82-88.) used two UAVs equipped with downward-looking monocular cameras and IMUs to form a virtual stereo camera, and formed an actively adjustable variable baseline based on the relative pose estimation of ultra-wideband (Ultra-Wide Band, UWB), thereby achieving high-precision depth estimation of ground points when the UAV is flying at high altitude. Wang et al. (Wang Z, Dong W. A Collaborative Stereo Camera with Two UAVs for Long-distance Mapping of Urban Buildings [C] / / 2024 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). 2024: 7944-7951.) used the forward-facing cameras of two UAVs to form a wide-baseline collaborative stereo camera. Based on mutual observations between IMUs, UWBs, and infrared cameras, they achieved relative pose estimation, enabling grid map generation of distant buildings. However, both approaches rely on adding multi-source mutual observation sensors to VIO to achieve relative state estimation and provide relative pose for joint triangulation. The two robots require strict trajectory planning to maintain a stable baseline virtual stereo camera, which limits the operational freedom of the multi-robot system.Moreover, both methods only process the common view features and discard the independent features obtained by each robot camera, which reduces the mapping efficiency of the multi-machine system. Summary of the Invention

[0003] During the SfM process, SLAM systems need to triangulate image features to obtain three-dimensional map points, which are then used as input to solve the perspective-n-point problem (PnP). However, monocular visual-inertial SLAM systems perform triangulation using the preceding and following keyframes in a temporal sliding window during motion. The difference in camera perspective between the two frames is small, and the length of the two-view geometric baseline is short. Therefore, it is difficult to accurately estimate the depth of distant map points, which limits the robot's perception ability. Moreover, when the robot only has posture changes without translational motion, that is, in the case of "pure rotational" motion, the system has difficulty recovering the correct three-dimensional map points due to the lack of scale information provided by the IMU during translational motion, which limits the robot's maneuverability. The present invention aims to utilize the multi-view joint observation of multiple robot cameras to flexibly access the observation information of multi-view common view features for single-machine vision to achieve joint triangulation, enabling the system to robustly adapt to pure rotational motion conditions and effectively improve the accuracy of depth estimation of distant map points. At the same time, it reduces the constraints on multi-robot trajectory planning and gives multiple robots sufficient degrees of freedom to increase the efficiency of map exploration. Through the joint initialization of multi-machine vision of common view map points, a multi-robot global coordinate system is established without introducing new sensors or maintaining relative observation motion, providing a multi-robot global coordinate benchmark for flexible independent-joint triangulation, and realizing global map construction. Therefore, the present invention proposes a map fusion method for multi-machine monocular vision elastic triangulation. Two robots equipped with monocular cameras and IMUs do not need to keep the same direction and maintain the baseline. They can autonomously plan the trajectory based on the principle of maximizing mapping efficiency. When the common view features of the two robots are matched, the joint observation of the multi-machine perspective is flexibly extended to the independent triangulation equation of the single machine, realizing multi-machine joint triangulation. By performing 3D-3D pose estimation on the common view map points to realize the coordinate system conversion of the visual-inertial odometry (VIO) of the two robots, a multi-robot global coordinate system is established without introducing new sensors or maintaining relative observation motion, and a global fusion map composed of the independent map points of each robot and the common view map points of multiple robots is constructed.

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

[0005] A map fusion method for multi-machine monocular vision elastic triangulation, comprising:

[0006] Step 1: Robot A and robot B use SuperPoint to extract features from each key frame and obtain the feature point sets f of robot A and robot B respectively. A 、f B ;

[0007] Step 2: Robot B transmits the time, feature point number, feature point position and its descriptor, as well as the pose of robot B in the visual inertial odometry (VIO) coordinate system of the key frame to robot A through network communication;

[0008] Step 3: Robot A uses brute force matching to match the descriptor, and uses the matching algorithm to match the feature point set f A 、f B Segmentation is performed, and the matched feature points are the common view features under the camera coordinate system of robot A and robot B The unmatched feature points are the independent features of the camera systems of robot A and robot B. Complete the management of multiple machine features;

[0009] Step 4: Perform 3D-3D pose estimation on the common view map points obtained through independent triangulation through multi-machine vision joint initialization, obtain the transformation matrix from the robot B coordinate system to the robot A coordinate system, and establish the global coordinate system g;

[0010] Step 5: Use the elastic ridge estimation triangulation method to and The feature points in the image are processed to complete the global map construction.

[0011] Furthermore, the elastic ridge estimation triangulation method includes:

[0012] The camera poses of robot A and robot B are used to align the The feature points in the image are independently triangulated to obtain the independent map points of robot A and robot B, and the obtained independent map points are unified into the global coordinate system g through multi-machine vision joint initialization;

[0013] Through multi-machine vision joint initialization, the coordinate systems of robots A and B are unified into the global coordinate system g. The common view features in the global coordinate system are jointly triangulated using the camera poses of robots A and B in motion to obtain the common view map points of robots A and B.

[0014] Furthermore, for robot A, during the independent triangulation process, it is necessary to construct the corresponding independent triangulation equation:

[0015]

[0016] in is the two-dimensional feature f Ci The coordinates of the normalized plane pass through the camera frame C i The posture matrix in the VIO coordinate system of robot A is converted to the representation in the VIO coordinate system of robot A, i∈[m,n], [m,n] is a sliding window, p A represents the 3D map points visible only to robot A, is the camera frame C i The position in the VIO coordinate system of robot A, s represents the number of camera poses of robot A;

[0017] Then solve the independent triangulated equations constructed for robot A and obtain:

[0018]

[0019] Where T represents transpose.

[0020] Furthermore, during the joint triangulation process, it is necessary to construct a joint triangulation equation:

[0021]

[0022] in represents the common view map points of robot A and robot B in the g system, Two-dimensional features The coordinates of the normalized plane pass through the camera frame C i The posture matrix of robot A in the VIO coordinate system is converted to the representation in the g system. Two-dimensional feature f Ci The coordinates of the normalized plane pass through the camera frame C i′ The posture matrix of robot B in the VIO coordinate system is converted to the representation in the g system. Represents the camera frame C i Convert the position of robot A's VIO coordinate system to the position of g system. Represents the camera frame C i′ The position in the VIO coordinate system of robot B is converted to the position in the g system, and s′ represents the number of camera poses of robot B.

[0023] Furthermore, the joint triangulated equation is solved as follows:

[0024] make Express the joint triangulated equation as a standard linear equation Ax = b, and construct the normal equation of the standard linear equation:

[0025] A T Ax=A T b

[0026] For the normal equation coefficient matrix A T A, whose condition number is defined as:

[0027]

[0028] Among them, ||·||2 is the matrix norm, σ max (·),σ min (·) are the maximum and minimum singular values ​​of the matrix;

[0029] Set the lower threshold n and upper threshold N of the condition number. When the condition number is less than the lower threshold n, the least squares solution is calculated directly; when the condition number is greater than the upper threshold N, it is discarded as an invalid solution; when the condition number is between the lower threshold and the upper threshold, the solution is obtained through ridge estimation.

[0030] Furthermore, the solving by ridge estimation includes:

[0031] In the normal equation coefficient matrix A T Add a ridge estimation parameter α to the main diagonal elements of A, as follows:

[0032] (A T A+αI)x=A T b

[0033] Then the ridge estimate solution of the linear equation is:

[0034]

[0035] Where I is the identity matrix, That is

[0036] Furthermore, when the multi-machine vision is jointly initialized, the VIO coordinate system of robot A is used as the global coordinate system g, and 3D-3D pose solution is performed through the visual common view map points between robot A and robot B to obtain the relative pose between the two robots, thereby obtaining the transformation matrix between robot A and robot B, and the robot pose and map points in the VIO coordinate system of robot B are converted to the VIO coordinate system of robot A through the transformation matrix.

[0037] Furthermore, the conversion matrix is ​​obtained in the following manner:

[0038] Robot A and robot B establish their own VIO coordinate systems with the camera position at the initial time t0 as the origin and the posture as the axis, and the three-dimensional common view map point set in the VIO coordinate system of robot B is the source point set, and the three-dimensional common view map point set in the VIO coordinate system of robot A is the target point set Since the point set P A 、P BThe correspondence between them has been established through 2D feature matching, so the optimal Euclidean transformation from the VIO coordinate system of robot B to the VIO coordinate system of robot A satisfies:

[0039]

[0040] Where R and t represent the optimal rotation matrix and translation vector between robot A and robot B at time t0 respectively;

[0041] Define the centroid of two sets of common view map points:

[0042]

[0043] μ A and μ B Substituting into the optimal Euclidean transformation equation, we get:

[0044]

[0045] Among them, the cross terms The objective function is simplified to:

[0046]

[0047] Solve for R and t separately:

[0048] Let the decentralized point set of the two point sets be

[0049]

[0050] Will Substitute the simplified first term of the objective function and expand it to obtain the error term about R:

[0051]

[0052] Since only the third term is related to R, the actual optimization objective function is:

[0053]

[0054] The optimal solution of R is obtained by SVD decomposition. R is substituted into the second term of the simplified objective function, and when the second term is equal to 0, the solution of the translation vector is obtained:

[0055] t=μ A -Rμ B

[0056] Finally, the transformation matrix T = [R|t] between robot A and robot B is obtained.

[0057] Compared with the prior art, the present invention has the following beneficial effects:

[0058] (1) Compared with the monocular vision triangulation of a single robot, the proposed elastic ridge estimation triangulation method can robustly adapt to pure rotation motion conditions, and the long baseline epipolar geometry formed by the joint triangulation can effectively improve the depth estimation accuracy of long-distance map points and increase the richness of the map. Compared with the long baseline binocular multi-machine system composed of multiple monocular robots, the flexible access and access processing method of the common view features in this method reduces the constraints on the trajectory planning of multiple robots and gives multiple robots sufficient freedom to increase the efficiency of map exploration. At the same time, the ridge estimation method reduces the elimination of effective data and improves the utilization rate of multi-machine joint visual measurement data. Finally, the observation information of single and multi-machines is fully utilized to jointly establish a global map with independent map points of single machines and common view map points.

[0059] (2) Compared with the relative pose solution method based on UWB and camera mutual observation, the proposed multi-machine vision joint initialization method of common view map points can establish the global coordinate system of multiple robots without introducing new sensors or maintaining relative observation motion. The solution is only performed in the initialization stage, and this global coordinate system is always maintained during operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0060] Figure 1 A flowchart of a multi-machine monocular vision elastic triangulation map fusion method provided by an embodiment of the present invention;

[0061] Figure 2 A schematic diagram of independent triangulation of a single robot provided in an embodiment of the present invention;

[0062] Figure 3 Schematic diagram of multi-robot joint triangulation provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0063] The present invention will be further explained below with reference to the accompanying drawings and specific embodiments:

[0064] like Figure 1 As shown in FIG, a map fusion method for multi-machine monocular vision elastic triangulation includes:

[0065] S101: Robot A and robot B use SuperPoint to extract features from each key frame, and obtain the feature point sets f of robot A and robot B respectively. A 、f B ;

[0066] S102: Robot B transmits the time, feature point number, feature point position and its descriptor, and the pose of robot B in the visual inertial odometry (VIO) coordinate system of the key frame to robot A via network communication;

[0067] S103: Robot A uses a brute force matching method to match the descriptor, and uses the matching algorithm to match the feature point set f A 、f B Segmentation is performed, and the matched feature points are the common view features under the camera coordinate system of robot A and robot B The unmatched feature points are the independent features of the camera systems of robot A and robot B. Complete the management of multiple machine features;

[0068] S104: Perform 3D-3D pose estimation on the common view map points obtained by independent triangulation through multi-machine vision joint initialization, obtain the transformation matrix from the robot B coordinate system to the robot A coordinate system, and establish the global coordinate system g;

[0069] S105: Using elastic ridge estimation triangulation method, and The feature points in the image are processed to complete the global map construction.

[0070] Furthermore, the elastic ridge estimation triangulation method includes:

[0071] The camera poses of robot A and robot B are used to align the The feature points in the image are independently triangulated to obtain the independent map points of robot A and robot B, and the obtained independent map points are unified into the global coordinate system g through multi-machine vision joint initialization;

[0072] Through multi-machine vision joint initialization, the coordinate systems of robots A and B are unified into the global coordinate system g. The common view features in the global coordinate system are jointly triangulated using the camera poses of robots A and B in motion to obtain the common view map points of robots A and B.

[0073] Furthermore, when the multi-machine vision is jointly initialized, the VIO coordinate system of robot A is used as the global coordinate system g, and 3D-3D pose solution is performed through the visual common view map points between robot A and robot B to obtain the relative pose between the two robots, thereby obtaining the transformation matrix between robot A and robot B, and the robot pose and map points in the VIO coordinate system of robot B are converted to the VIO coordinate system of robot A through the transformation matrix.

[0074] As an implementable embodiment, the method specifically includes:

[0075] 1. Acquisition and management of multi-machine features

[0076] In the traditional SLAM field, the process of robots perceiving the environment using visual sensors is achieved by capturing environmental images and extracting feature points. When multiple robots move in the same working environment, the environmental images obtained have more or less overlapping areas. The extracted feature points are matched to establish associated features, which are multi-machine common vision features. In the present invention, the common vision feature mainly participates in the following two functions: first, in the initialization phase, its correlation is applied to perform 3D-3D posture solution to obtain the transformation matrix between the VIO coordinate systems of multiple robots; second, when the relative relationship between multiple robots is known, the epipolar geometric triangle formed by the inter-frame posture on the time span within the sliding window of a single robot and the epipolar geometric triangle formed by the postures of two robots on the spatial span formed by the common vision of multiple robots are jointly solved to obtain the three-dimensional position of the common vision feature, complete the joint triangulation of the common vision feature, provide redundant observations for triangulation solution, realize the feature depth estimation of the monocular system in the case of pure rotational motion, and improve the triangulation accuracy of long-distance features.

[0077] In order to achieve feature association among multiple robots, it is necessary to manage the common view features and non-common view independent features. First, a single robot extracts features from each key frame. Due to the large differences in the view angles of image feature matching between multiple robots, the descriptors obtained by traditional feature extraction operators have a high mismatch rate in the matching process. Therefore, SuperPoint feature extraction is used to obtain the feature point sets f of robots A and B. A 、f B Then, robot B transmits the time, feature point number, feature point position and its descriptor as well as the pose of the key frame in the VIO system of robot B to robot A through network communication. Among them, the time and feature point number are used for feature point management; the descriptor is used for feature matching; the pose in the VIO system of robot B is converted to the VIO system of robot A through the transformation matrix after initialization as the input of joint triangulation, which will be explained later. In order to ensure real-time performance, robot A uses a brute force matching method to match the descriptor, and uses the matching algorithm to match the feature point set f A 、f B Segmentation is performed, and the matched feature points are the common view features of the A and B camera systems The unmatched feature points are the independent features of the A and B camera systems. Complete the management of multiple machine features.

[0078] 2 Elastic Ridge Estimation Triangulation Method

[0079] In single-robot SLAM, triangulation is achieved by measuring and observing the same map point at different locations through the movement of the camera and tracking the feature points, thereby estimating the spatial position of the map point, such as Figure 2During the camera translation process, the IMU measurement value provides the real scale for the inter-frame pose of the monocular camera, thereby ensuring that the estimated map point depth has scale information.

[0080] Therefore, the input required for triangulation is the camera poses with scale information from different perspectives observing the same map point. These camera poses can come from a series of poses during the camera motion of a single robot itself, or from the poses of multiple robots observing the same map point, such as Figure 3 If the feature is a non-common-view independent feature, only the camera pose of a single robot in motion is used to independently triangulate the tracked feature points. When the feature is successfully matched to a common-view feature, the camera poses of multiple robots jointly participate in triangulation. Based on the linear equation constructed by the visual observations of a single robot, the observation information of other robots on the common-view feature is augmented, achieving flexible access to multi-machine visual observations and extending independent triangulation to joint triangulation. The specific implementation method is as follows.

[0081] Independent triangulation uses the OpenVINS triangulation algorithm as the framework. Taking robot A as an example, let its VIO coordinate system be A system, and the sliding window [ m,n ] The camera coordinate system of the key frame that can track the feature points is C i System, i∈[m,n], then the two-dimensional feature and 3D map point p A The relationship is as follows:

[0082]

[0083] in, is the camera frame C i The posture under the A series, is the camera frame C i The position under the A system, z f is the map point p A In the camera coordinate system C i The depth below Characterized by The coordinates in the normalized plane and passed through the attitude matrix Converted to A system, it is expressed as The feature normalization method is as follows:

[0084]

[0085] in, It is a feature Homogeneous form of .

[0086] In order to eliminate the unknown depth z in formula (1) f , multiply both sides of the equation by The antisymmetric matrix

[0087]

[0088] After eliminating the depth term of 0, the linear equation Ax = b is formed:

[0089]

[0090] The linear equation of Equation (4) is augmented by the s camera poses and camera frame observations tracking this feature point in the sliding window to obtain:

[0091]

[0092] set up This constitutes a three-dimensional map point p A The linear equation Ap is the unknown variable A =b. The solution of the equation obtained by least squares is as follows:

[0093]

[0094] The above is the principle of independent triangulation. Considering the scalability of the triangulated linear equation, for the common view map points of robot A and robot B The observation of robot A can establish the triangulation equation (5), and the camera frame C of the feature can be successfully matched during the movement of robot B. i′ and the camera frames in the sliding window of robot B that successfully tracked the feature [C m′ … C n ] A total of s′ camera poses and visual measurement values ​​can be augmented into Equation (5) to form a joint triangulation equation:

[0095]

[0096] Constructing and solving the joint triangulation equations for multi-robot visual elastic access mainly requires solving two problems: one is the unification of multi-robot coordinate systems, and the other is the ill-conditioned treatment of the equations.

[0097] All camera poses of the independent triangulation of robot A are in the VIO system of robot A, that is, the A system, but the camera poses of robot B are in the VIO system of robot B, that is, the B system. Therefore, in order to achieve joint triangulation, it is necessary to initialize the coordinate systems of multiple robots to the global system g through multi-machine vision joint initialization, which will be elaborated in Section 3.

[0098] For the solution of the joint triangulation equation (7), since the matrix size increases with augmentation, the camera frames with small perspective differences have similar observation features for map points, which easily causes the A in the least squares equation to be TThe column vectors of the A matrix are approximately linearly correlated. At the same time, due to the large difference in the random motion trajectories of the two robots, the image coordinates of the common view features in their respective camera coordinate systems are also very different, which may lead to the A T The scales of the elements in matrix A are very different. The above situations are likely to cause A T The condition number of matrix A is large. When the condition number is too large, the matrix may be ill-conditioned.

[0099] OpenVINS sets a threshold on the condition number and discards solutions with high condition numbers. However, to improve the utilization of measurement information, regularization can be used to improve the solution of equations with high condition numbers but still non-ill-conditioned. Therefore, the present invention proposes triangulation based on ridge estimation.

[0100] Ridge estimation is an adjustment estimation method that sacrifices the unbiasedness of the least squares method to reduce the correlation between the column vectors of the normal equation coefficient matrix, thereby improving the tolerance to approximately ill-conditioned data.

[0101] Simplify Equation (7) for ease of expression, let Expressing equation (7) as the standard linear equation Ax=b, its normal equation is:

[0102] A T Ax=A T b (8)

[0103] First, calculate the condition number of the normal equation. The condition number is an indicator to measure the degree of pathological condition of linear equations. The larger the condition number, the more pathological the system of equations. A small error in the input observation data will lead to a significant change in the solution. T A, whose condition number is defined as:

[0104]

[0105] Among them, ||·||2 is the matrix norm, σ max (·),σ min (·) are the maximum and minimum singular values ​​of the matrix.

[0106] Set the lower threshold n and upper threshold N of the condition number. When the condition number is less than the lower threshold n, the solution is considered non-ill-conditioned and the least squares solution is directly calculated; when the condition number is greater than the upper threshold N, the solution is considered completely ill-conditioned and discarded as an invalid solution; when the condition number is between the upper and lower thresholds, the solution is considered approximately ill-conditioned and is solved through ridge estimation as follows.

[0107] In the normal equation coefficient matrix A T Add a small positive number α to the main diagonal elements of A, which is called the ridge estimation parameter, as shown below:

[0108] (A TA+αI)x=A T b (10) Then the ridge estimate solution of the linear equation is:

[0109]

[0110] Where I is the identity matrix.

[0111] The selection of the ridge estimation parameter α is determined by the ridge trace method, the L-curve method or the U-curve method, etc., which is not the focus of the present invention.

[0112] The flexible ridge estimation triangulation method proposed in the present invention innovatively realizes the collaborative depth estimation of monocular vision under high-degree-of-freedom motion of multiple robots. When the robot does not detect the common view map point, the system maintains the triangulation process of a single machine independently running; once the cross-robot feature matching is established, the independent triangulation is expanded to joint triangulation, and the features that fail to match between robots still maintain the independent triangulation channel of the single machine, realizing the flexible access and output of multi-machine visual measurement. At the same time, the ridge estimation method reduces the elimination of effective data and improves the utilization rate of multi-machine joint visual measurement data. The flexible triangulation method not only reduces the constraints of the multi-machine monocular vision system in trajectory planning, giving multiple robots sufficient degrees of freedom to increase the efficiency of map exploration, but also the joint triangulation strengthens the system's depth estimation ability for distant map points, improves the system's robustness for map point estimation in pure rotational motion mode, and further enhances the freedom of motion of multi-robot SLAM.

[0113] 3 Multi-machine vision joint initialization

[0114] The previous section mentioned that elastic triangulation needs to solve the problem of multi-robot coordinate system. The present invention solves this problem through multi-level visual joint initialization. When the single-machine VIO completes the initialization, the VIO coordinate system will be established. Since the establishment of the VIO system is related to the movement of the single machine, its origin and axis are established based on the robot posture at a certain moment in the movement process. Therefore, the VIO systems between multiple machines are independent of each other, namely the A system and B system of the present invention. Multi-machine visual joint initialization is to perform 3D-3D posture solution through the common visual map points between multiple machines to obtain the relative posture between the two robots, thereby converting the robot posture and map points of the B system to the A system, and constructing the global coordinate frame g system.

[0115] First, the two robots respectively use their own VIO coordinate frames to identify the common visual features at the same initial time t0. Perform independent triangulation to obtain the coordinates of the same 3D map point in system A and system B respectively Assuming that there are n common view map points at t0, the three-dimensional map point set under system A and system B is formed The optimal rotation matrix R and translation vector t between the two robots at time t0 are solved by the iterative closest point method of 3D-3D pose solution. The specific algorithm flow is as follows.

[0116] Point set under B system is the source point set, and the point set under system A is the target point set Then the optimal Euclidean transformation from system B to system A satisfies:

[0117]

[0118] In order to solve the optimization problem, we first define the centroids of the two sets of points:

[0119]

[0120] Substituting formula (13) into formula (12) and simplifying it, we can obtain:

[0121]

[0122] Among them, the cross terms Therefore, the objective function is simplified to:

[0123]

[0124] Note that the first term of the objective function is only related to the rotation matrix R, and the second term is related to both R and t, but is only related to the centroid of the point set. Therefore, the rotation matrix and translation vector can be solved separately.

[0125] Let the decentralized point set of the two point sets be

[0126]

[0127] Substituting Equation (16) into the first term of Equation (15), we can expand the error term about R:

[0128]

[0129] Since only the third term is related to R, the actual optimization objective function is

[0130]

[0131] Equation (18) can be decomposed by SVD to obtain the optimal solution of R. Substituting R into the second term of Equation (15), and when the second term is equal to 0, the solution of the translation vector is obtained:

[0132] t=μ A -Rμ B (19)

[0133] Through the above multi-machine vision joint initialization process, the transformation matrix T = [R|t] between the independent VIO coordinate systems A and B is obtained. Then, the A system is used as the global coordinate system g. Since the A system and the g system coincide, the relationship between the characteristics of robot A and its three-dimensional position in the g system in equation (1) is as follows:

[0134]

[0135] For robot B, the camera posture of the B system and location Convert to g-series:

[0136]

[0137] From this, we can construct the relationship between the characteristics of robot B and its three-dimensional position in the g system:

[0138]

[0139] Thus, the observation quantity of robot B in the joint triangulation equation (7) is obtained The observation information of robot B is flexibly integrated into the joint triangulation solution process.

[0140] At the same time, the non-common view features obtained by robot B are triangulated independently to obtain independent map points under system B. Also convert to the global system:

[0141]

[0142] Therefore, the independent map point of robot A Robot B's independent map point And the common view map points of robots A and B Together they form a fusion map under the global system.

[0143] Therefore, through the joint initialization of multi-machine vision, without introducing new sensors, without maintaining the relative observation motion, and only requiring the initial state camera orientation, a global coordinate system of multiple robots was established, and the multi-robot camera pose and map point coordinate system were unified, so that robots can flexibly use the visual observation quantities of other robots, thereby constructing a flexible independent-joint triangulation equation, and finally establishing a global fusion map constructed by the independent map points of each robot and the common view map points of multiple robots.

[0144] In summary, this paper proposes a flexible ridge estimation triangulation method that innovatively enables collaborative monocular depth estimation with multiple robots in high-degree-of-freedom motion. When a robot fails to detect a common view map point, the system maintains a single-machine independent triangulation process. Once cross-robot feature matching is established, independent triangulation is expanded to joint triangulation, while features that fail to match between robots remain in the independent triangulation channel of a single machine. This enables flexible access to and from multi-machine visual measurement. Furthermore, the ridge estimation method reduces the elimination of valid data and improves the utilization of multi-machine joint visual measurement data.

[0145] This paper also proposes a multi-robot vision joint initialization method for common view map points. This method establishes a global coordinate system for multiple robots without introducing new sensors or maintaining relative observation motion, requiring only the initial camera orientation. At the initial moment, when the multiple robots depart in the same direction, the matching feature points of the two robots are independently triangulated in their respective VIO systems. 3D-3D pose calculations are performed on the obtained common view map points in their respective VIO systems to determine the transformation relationship between the two independent VIO systems. This establishes the global coordinate system for the multiple robots, providing a unified coordinate system for joint triangulation and constructing a global map.

[0146] The above is only 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 also be regarded as the scope of protection of the present invention.

Claims

1. A multi-machine monocular vision elastic triangulation map fusion method, characterized by: include: Step 1: Robot A and robot B use SuperPoint to extract features from each key frame and obtain the feature point sets f of robot A and robot B respectively. A 、f B ; Step 2: Robot B transmits the time, feature point number, feature point position and its descriptor, as well as the pose of robot B in the visual inertial odometry (VIO) coordinate system of the key frame to robot A through network communication; Step 3: Robot A uses brute force matching to match the descriptor, and uses the matching algorithm to match the feature point set f A 、f B Segmentation is performed, and the matched feature points are the common view features under the camera coordinate system of robot A and robot B The unmatched feature points are the independent features of the camera systems of robot A and robot B. Complete the management of multiple machine features; Step 4: Perform 3D-3D pose estimation on the common view map points obtained through independent triangulation through multi-machine vision joint initialization, obtain the transformation matrix from the robot B coordinate system to the robot A coordinate system, and establish the global coordinate system g; Step 5: Use the elastic ridge estimation triangulation method to and The feature points in the image are processed to complete the global map construction.

2. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 1 is characterized in that: The elastic ridge estimation triangulation method includes: The camera poses of robot A and robot B are used to align the The feature points in the image are independently triangulated to obtain the independent map points of robot A and robot B, and the obtained independent map points are unified into the global coordinate system g through multi-machine vision joint initialization; Through multi-machine vision joint initialization, the coordinate systems of robots A and B are unified into the global coordinate system g. The common view features in the global coordinate system are jointly triangulated using the camera poses of robots A and B in motion to obtain the common view map points of robots A and B.

3. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 2 is characterized in that: For robot A, the independent triangulation process requires the construction of the corresponding independent triangulation equation: in Two-dimensional features The coordinates of the normalized plane pass through the camera frame C i The posture matrix in the VIO coordinate system of robot A is converted to the representation in the VIO coordinate system of robot A, i∈[m,n], [m,n] is a sliding window, p A represents the 3D map points visible only to robot A, is the camera frame C i The position in the VIO coordinate system of robot A, s represents the number of camera poses of robot A; Then solve the independent triangulated equations constructed for robot A and obtain: Where T represents transpose.

4. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 1 is characterized in that: During the joint triangulation process, it is necessary to construct a joint triangulation equation: in represents the common view map points of robot A and robot B in the g system, Two-dimensional features The coordinates of the normalized plane pass through the camera frame C i The posture matrix of robot A in the VIO coordinate system is converted to the representation in the g system. Two-dimensional features The coordinates of the normalized plane pass through the camera frame C i′ The posture matrix of robot B in the VIO coordinate system is converted to the representation in the g system. Represents the camera frame C i Convert the position of robot A's VIO coordinate system to the position of g system. Represents the camera frame C i′ The position in the VIO coordinate system of robot B is converted to the position in the g system, and s′ represents the number of camera poses of robot B.

5. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 4 is characterized in that: The joint triangulated equation is solved as follows: make Express the joint triangulated equation as a standard linear equation Ax = b, and construct the normal equation of the standard linear equation: A T Ax=A T b For the normal equation coefficient matrix A T A, whose condition number is defined as: Among them, ||·||2 is the matrix norm, σ max (·),σ min (·) are the maximum and minimum singular values ​​of the matrix; Set the lower threshold n and upper threshold N of the condition number. When the condition number is less than the lower threshold n, the least squares solution is calculated directly; when the condition number is greater than the upper threshold N, it is discarded as an invalid solution; when the condition number is between the lower threshold and the upper threshold, the solution is obtained through ridge estimation.

6. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 5 is characterized in that: The solution by ridge estimation includes: In the normal equation coefficient matrix A T Add a ridge estimation parameter α to the main diagonal elements of A, as follows: (A T A+αI)x=A T b Then the ridge estimate solution of the linear equation is: Where I is the identity matrix, That is 7. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 1 is characterized in that: During the joint initialization of the multi-machine vision, the VIO coordinate system of robot A is used as the global coordinate system g, and 3D-3D pose solution is performed through the visual common view map points between robot A and robot B to obtain the relative pose between the two robots, thereby obtaining the transformation matrix between robot A and robot B. The robot pose and map points in the VIO coordinate system of robot B are converted to the VIO coordinate system of robot A through the transformation matrix.

8. The map fusion method of multi-machine monocular vision elastic triangulation according to claim 7 is characterized in that: The transformation matrix is ​​derived as follows: Robot A and robot B establish their own VIO coordinate systems with the camera position at the initial time t0 as the origin and the posture as the axis, and the three-dimensional common view map point set in the VIO coordinate system of robot B is the source point set, and the three-dimensional common view map point set in the VIO coordinate system of robot A is the target point set Since the point set P A 、P B The correspondence between them has been established through 2D feature matching, so the optimal Euclidean transformation from the VIO coordinate system of robot B to the VIO coordinate system of robot A satisfies: Where R and t represent the optimal rotation matrix and translation vector between robot A and robot B at time t0 respectively; Define the centroid of two sets of common view map points: μ A and μ B Substituting into the optimal Euclidean transformation equation, we get: Among them, the cross terms The objective function is simplified to: Solve for R and t separately: Let the decentralized point set of the two point sets be Will Substitute the simplified first term of the objective function and expand it to obtain the error term about R: Since only the third term is related to R, the actual optimization objective function is: The optimal solution of R is obtained by SVD decomposition. R is substituted into the second term of the simplified objective function, and when the second term is equal to 0, the solution of the translation vector is obtained: t=μ A -Rμ B Finally, the transformation matrix T = [R|t] between robot A and robot B is obtained.

Images