A collaborative three-dimensional mapping method and system

Abstract

The present invention provides a collaborative three-dimensional mapping method and system, including: detecting visual positioning markers through the cloud; optimizing the pose estimation of the UAV visual odometer through the visual positioning markers; optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning markers; and completing the local map construction thread and loop closure detection thread of the ORB-SLAM framework through the cloud. Compared with the prior art, the present invention is mainly implemented based on the ORB-SLAM framework and the cloud. The tracking thread in ORB-SLAM is implemented by the UAV and the unmanned vehicle itself, the local map construction thread and loop closure detection thread in ORB-SLAM are implemented by the cloud, the pose estimation of the UAV visual odometer is optimized using visual positioning markers, and the pose estimation of the unmanned vehicle visual odometer is optimized using visual positioning markers. It can solve the problems that the real-time performance of the collaborative SLAM system is difficult to meet and the positioning of the collaborative SLAM system is inaccurate, and can realize a collaborative three-dimensional mapping system with good robustness, high precision and strong real-time performance.

Description

Technical Field

[0001] The present invention relates to the field of collaborative 3D mapping, and specifically to a collaborative 3D mapping method and system. Background Art

[0002] In the prior art, there is a technology that uses road signs and monocular camera sensors to achieve 3D plane mapping of multiple robots, but the existing technology system has poor real-time performance;

[0003] There is also a technology that uses road signs and cloud architecture to achieve 2D plane mapping of a single robot, but this system is not suitable for large-scale environment applications. Summary of the Invention

[0004] In order to overcome the deficiencies of the prior art, the present invention provides a collaborative 3D mapping method and system, and the specific technical solutions are as follows:

[0005] A collaborative 3D mapping method includes:

[0006] Detecting visual positioning markers through the cloud;

[0007] Optimizing the pose estimation of the UAV visual odometer through the visual positioning marker;

[0008] Optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning marker;

[0009] Completing the local map construction thread and loop closure detection thread of the ORB-SLAM framework through the cloud.

[0010] In a specific embodiment, it further includes:

[0011] Collecting environmental information, using Docker as the cloud container, using Kubernetes as the container scheduling service, and using BRPC and Beego as the network architecture to build a cloud platform to enable communication between multiple agent ends and the cloud;

[0012] The multiple agents include one UAV and one unmanned vehicle. The UAV and the unmanned vehicle form a centralized architecture. A first monocular camera is equipped in front of the UAV and the lens of the first monocular camera faces downward. A second monocular camera is equipped in front of the unmanned vehicle and the lens of the second monocular camera faces forward;

[0013] Selecting at least 2 environmental points and marking the visual positioning markers.

[0014] In a specific embodiment, it further includes:

[0015] The environmental information includes image information, and feature points and descriptors are extracted from the image information using the ORB-SLAM algorithm;

[0016] The depth is obtained through the PnP algorithm to obtain point cloud information;

[0017] The cloud platform is used for map initialization. If there is a map on the cloud platform, the image information is matched with the key frames on the cloud to determine the initial position. If there is no map on the cloud platform, the image information and other information such as the map are used as the starting point of the cloud platform system map;

[0018] Estimate the camera pose by matching feature point pairs or using the relocalization method;

[0019] Establish the relationship between image feature points and the local point cloud map;

[0020] Extract the key frames according to the judgment conditions of the key frames and upload them to the cloud.

[0021] In a specific embodiment, the "establishing the relationship between image feature points and the local point cloud map" specifically includes:

[0022] When the local map fails to be tracked due to principles such as environmental occlusion or texture loss, the system relocates in the following ways:

[0023] Relocate and match the reference frame in the local map on the drone or the unmanned vehicle;

[0024] Relocate on the cloud platform through the information of the current frame.

[0025] In a specific embodiment, the "detecting visual positioning markers through the cloud" specifically includes:

[0026] Perform image edge detection;

[0027] Filter out the contour edges of the quadrilateral;

[0028] Decode the contour edges of the quadrilateral to identify the visual positioning markers.

[0029] In a specific embodiment, the "optimizing the pose estimation of the drone visual odometer through visual positioning markers" specifically includes:

[0030] Define the coordinate system, define the camera coordinate system P carried by the drone C 、the drone coordinate system P A 、the visual positioning marker coordinate system P B and the world coordinate system P W ,the world coordinate system P W is defined as the first frame of the drone;

[0031] the camera coordinate system P carried by the droneC The YOZ plane of A is parallel to the YOZ plane of the UAV coordinate system P, and the origin of the UAV coordinate system P A is set at the center of the UAV;

[0032] Calculate the relationship between the camera coordinate system P C loaded on the UAV and the world coordinate system P W ;

[0033] Calculate the relative pose of the camera coordinate system P C loaded on the UAV and the visual positioning marker coordinate system P B ; and

[0034] Obtain the trajectory error from the relative pose obtained through the visual positioning marker and the relative pose obtained through visual odometry, and evenly distribute the trajectory error to each key frame of the UAV, so as to reduce the closed-loop key frame and the actual error.

[0035] In a specific embodiment, the "calculate the relationship between the camera coordinate system P C loaded on the UAV and the world coordinate system P W " specifically includes:

[0036] The UAV coordinate system P A is parallel to the camera coordinate system P C loaded on the UAV, that is:

[0037]

[0038] where P A represents the coordinates of the UAV coordinate system, and P C represents the coordinates of the camera coordinate system loaded on the UAV, is the translation vector between the UAV coordinate system P A and the camera coordinate system P C loaded on the UAV, representing the distance between the camera and the center of the UAV;

[0039] The relationship between the visual positioning marker coordinate system P B and the world coordinate system P W satisfies:

[0040]

[0041] where P W is the coordinate of the world coordinate system, and P B is the coordinate of the visual positioning marker coordinate system, For the world coordinate system P W The translation vector between the visual positioning marker coordinate system P B ;

[0042] The angles φ, θ, and ψ are Euler angles respectively. Let the rotation matrix from the world coordinate system P W to the UAV coordinate system P A be The rotation matrix from the visual positioning marker coordinate system P B to the UAV-mounted camera coordinate system P C be Then:

[0043]

[0044]

[0045] In the above, c represents cos and s represents sin. According to the above formula, the rotation relationship between the visual positioning marker coordinate system P B and the UAV-mounted camera coordinate system P C includes:

[0046]

[0047] The relationship from the UAV-mounted camera coordinate system P C to the visual positioning marker coordinate system P B is expressed as:

[0048]

[0049] Among them, is the rotation matrix from the UAV-mounted camera coordinate system P C to the visual positioning marker coordinate system P B , is the translation vector from the UAV-mounted camera coordinate system P C to the visual positioning marker coordinate system P B ;

[0050] Then the relationship from the UAV-mounted camera coordinate system P C to the world coordinate system P W includes:

[0051]

[0052] Among them, is the rotation matrix from the UAV coordinate system P A to the world coordinate system P W , is the UAV coordinate system PA the translation vector to the world coordinate system P W ; is the translation vector from the UAV coordinate system P A to the UAV-mounted camera coordinate system P C ;

[0053] In a specific embodiment, the "calculating the relative pose of the UAV-mounted camera coordinate system P C and the visual positioning marker coordinate system P B " specifically includes: and " specifically includes:

[0054] Using the camera model to project the visual positioning marker onto the 2D pixel plane of the camera, obtaining:

[0055]

[0056] where M represents the camera internal parameter matrix, [u, v, 1] represents the coordinates of the visual positioning marker projected onto the normalized plane, [XB, YB, ZB] represents the coordinates of the visual positioning marker in the visual positioning marker coordinate system P B ; represents the translation vector from the visual positioning marker coordinate system P B to the UAV-mounted camera coordinate system P C ; represents the rotation matrix from the visual positioning marker coordinate system P B to the UAV-mounted camera coordinate system P C , s = 1 / Z C represents the unknown scale factor, Z C represents the Z-axis coordinate of the visual positioning marker in the camera coordinate system, calculated using the direct linear transformation algorithm and

[0057] In a specific embodiment, the "optimizing the pose estimation of the self-driving vehicle visual odometer through visual positioning markers" specifically includes:

[0058] Defining coordinate systems, defining the self-driving vehicle-mounted camera coordinate system P C , the visual positioning marker coordinate system P B and the world coordinate system P W , the world coordinate system P W is defined as the first frame of the UAV, and the relationship between the self-driving vehicle-mounted camera coordinate system P C and the self-driving vehicle coordinate system P A is determined;

[0059] Obtain the camera coordinate system \(P\) loaded on the unmanned vehicle C and the world coordinate system \(P\) W relative pose \(T\) cw 、the visual positioning marker coordinate system \(P\) B and the camera coordinate system \(P\) loaded on the unmanned vehicle C relative pose \(T\) bc 、and the visual positioning marker coordinate system \(P\) B and the world coordinate system \(P\) W relative pose \(T\) bw ;

[0060] Optimize the pose of the unmanned vehicle and the point cloud coordinates;

[0061] Define the relative error between the visual positioning marker coordinate system \(P\) B and the camera coordinate system \(P\) loaded on the unmanned vehicle C is:

[0062]

[0063] Construct an optimization objective function:

[0064]

[0065] where:

[0066] \(T\) cw \(\in\{(R\) cw ,t\) cw )|R cw \(\in SO3,t\) cw \(\in R\) 3 \}\(T\) bc \(\in\{(R\) bc ,t\) bc )|R bc \(\in SO3,t\) bc \(\in R\) 3 \}\)

[0067] where \(SO3\) represents the three-dimensional special orthogonal group, \(t\) cw represents the translation error from the camera coordinate system \(P\) loaded on the unmanned vehicle C to the world coordinate system \(P\) W , \(t\) bc represents the translation error from the visual positioning marker coordinate system \(P\) B to the camera coordinate system \(P\) loaded on the unmanned vehicle C , \(R\) 3 represents a set of bases of dimension 3, \(R\) cw represents the translation error from the camera coordinate system \(P\) loaded on the unmanned vehicle C to the world coordinate system \(P\) W , \(R\) bcRepresents the rotational error from the visual positioning marker coordinate system P B to the unmanned vehicle loading camera coordinate system P C .

[0068] The movement of the camera not only causes rotational errors R cw , R bc and translational errors t cw , t bc , but also accompanied by scale drift. Therefore, a transformation for scale is performed and the Sim3 transformation algorithm is adopted. Thus:

[0069] S cw =(R cw , t cw , s = 1), (R cw , t cw ) = T cw

[0070] S bc =(R bc , t bc , s = 1), (R bc , t bc ) = T bc

[0071] where S cw represents the similarity transformation of the visual positioning marker point from the world coordinate system P W to the unmanned vehicle loading camera coordinate system P C , S bc represents the similarity transformation of the visual positioning marker point from the visual positioning marker coordinate system P B to the unmanned vehicle loading camera coordinate system P C , and s represents the unknown scale factor;

[0072] Assume that the optimized Sim3 pose is Then the corrected pose is:

[0073]

[0074] where R bw represents the rotation matrix of the visual positioning marker point from the world coordinate system P W to the visual positioning marker coordinate system P B , t bw represents the translation of the visual positioning marker point from the world coordinate system P W to the visual positioning marker coordinate system P B , s represents the unknown scale factor, represents the optimized rotation matrix, translation vector and scale factor, Represents the optimized similarity transformation;

[0075] Set the 3D position of the unmanned vehicle before the optimization to Then the transformed coordinates can be obtained:

[0076]

[0077] Where Represents the pose of the unmanned vehicle after optimization.

[0078] A collaborative three-dimensional mapping system for implementing the collaborative three-dimensional mapping method described above, including:

[0079] An environment preparation module for collecting environment information;

[0080] An information processing module for extracting key frames from the obtained environment information by using the design idea of the Tracking thread in the ORB-SLAM algorithm framework;

[0081] A detection module for detecting visual positioning markers through the cloud;

[0082] A first optimization module for optimizing the pose estimation of the UAV visual odometer through the visual positioning marker;

[0083] A second optimization module for optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning marker; An execution module for completing the local map construction thread and the loop closure detection thread of the ORB-SLAM framework through the cloud.

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

[0085] A collaborative three-dimensional mapping method and system provided by the present invention can solve the problems that the real-time performance of the collaborative SLAM system is difficult to meet and the positioning of the collaborative SLAM system is inaccurate, and can realize a collaborative three-dimensional mapping system with good robustness, high precision and strong real-time performance.

[0086] To make the above objects, features and advantages of the present invention more obvious and understandable, the following specific embodiments are given, and in conjunction with the accompanying drawings, the detailed description is as follows. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following will briefly introduce the drawings required in the embodiments. It should be understood that the following drawings only show some embodiments of the present invention, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, without creative efforts, other related drawings can also be obtained based on these drawings.

[0088] Figure 1 It is a schematic diagram of the imaging model of the camera in the embodiment;

[0089] Figure 2 It is a flowchart of the process steps of the collaborative three-dimensional mapping method in the embodiment;

[0090] Figure 3 It is a module diagram of the collaborative three-dimensional mapping system in the embodiment. Detailed implementation manners

[0091] Embodiment

[0092] As Figure 1 - Figure 2 shown, this embodiment provides a collaborative three-dimensional mapping method, including:

[0093] Environment preparation, collecting environmental information;

[0094] Information processing, extracting key frames from the obtained environmental information by adopting the design idea of the Tracking thread in the ORB-SLAM algorithm framework;

[0095] Detecting visual positioning markers through the cloud, where the visual positioning markers are road signs;

[0096] Optimizing the pose estimation of the UAV visual odometer through the visual positioning markers;

[0097] Optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning markers;

[0098] Completing the local map construction thread and the loop closure detection thread of the ORB-SLAM framework through the cloud.

[0099] Specifically, the cloud executes the Local Mapping thread and the Loop Closing thread in ORB-SLAM. Cooperative Simultaneous Localization and Mapping (CSLAM) has more advantages than single-robot in terms of fault tolerance, robustness, and execution efficiency, and has an important influence in tasks such as disaster rescue, resource exploration, and space exploration in unknown environments. The data calculation and storage in the CSLAM system are large, and most individual robots cannot meet the real-time requirements. The CSLAM system usually executes tasks in a large-scale environment, and the system errors (such as pose estimation errors) accumulated by a large amount of calculations cannot be completely eliminated to a certain extent. Moreover, when there are a large number of repetitive landforms in the environment, the feature point matching or overlapping area matching algorithm may have a certain degree of mis-matching. The accumulated system errors and mis-matches will affect the mapping accuracy of the CSLAM system. Therefore, arranging a small number of road signs in the environment so that the robot can optimize its own pose according to the road signs is of great significance for improving the mapping accuracy. Compared with the two-dimensional map, the three-dimensional map has richer information and can better reflect the objective existence form of the real world.

[0100] Specifically, the visual positioning marking technology, that is, the road sign technology, can assist the camera lidar sensor to achieve more accurate positioning and mapping. The cloud architecture technology can transfer the complex operations in the multi-robot SLAM technology to the cloud to achieve, solving the problem of limited computing and storage resources of multi-robots. The map environment information of the three-dimensional plane is richer and more conducive to the UAV to realize functions such as navigation and obstacle avoidance.

[0101] Preferably, in this embodiment, road signs (AprilTag codes) are placed in relatively spacious places in a large-scale unknown environment. The UAV and the unmanned vehicle are loaded with monocular cameras. During the movement of multiple agents, the monocular cameras are used to collect environmental information in real time, the ORB-SLAM framework is used for cooperative three-dimensional mapping, and the AprilTag code is used to optimize the ORB-SLAM pose estimation. The cloud platform is built using Docker + Kubernetes + BRPC + Beego technology, and tasks with large computational requirements and high storage requirements are deployed on the cloud. The multi-agent side is used for tracking and repositioning.

[0102] Preferably, this embodiment combines the road sign AprilTag + cloud architecture + multi-robot + SLAM three-dimensional mapping technology to realize unmanned cooperative three-dimensional mapping, which can solve the problem that the real-time performance of the cooperative SLAM system is difficult to meet and the problem that the positioning of the cooperative SLAM system is inaccurate, and can realize an unmanned cooperative three-dimensional mapping system with good robustness, high precision, and strong real-time performance.

[0103] In this embodiment, "collecting environmental information" specifically includes:

[0104] Build a cloud platform using Docker + Kubernetes + BRPC + Beego technology to enable communication between multiple agent terminals and the cloud. Specifically, use Docker as the cloud container, Kubernetes as the container scheduling service, and BRPC and Beego as the network architecture to build the cloud platform for communication between multiple agent terminals and the cloud;

[0105] The multiple agents include a drone and an unmanned vehicle, and the drone and the unmanned vehicle form a centralized architecture;

[0106] Select at least 2 environmental points and mark them with visual positioning markers, that is, mark them with AprilTag codes.

[0107] In this embodiment, "the drone and the unmanned vehicle form a centralized architecture" specifically includes:

[0108] A first monocular camera is equipped at the front position of the drone and the lens of the first monocular camera faces downward, and a second monocular camera is equipped at the front position of the unmanned vehicle and the lens of the second monocular camera faces forward.

[0109] In this embodiment, "information processing" specifically includes:

[0110] The environmental information includes image information, and the ORB-SLAM algorithm is used to extract feature points and descriptors from the image information;

[0111] The depth is obtained through the PnP algorithm to obtain point cloud information;

[0112] Use the cloud platform for map initialization. If there is a map on the cloud platform, match the image information with the key frames on the cloud to determine the initial position. If there is no map on the cloud platform, use the image information and map and other information as the starting point of the cloud platform system map;

[0113] Estimate the camera pose by matching feature point pairs or the relocalization method;

[0114] Establish the relationship between the image feature points and the local point cloud map;

[0115] Extract key frames according to the judgment conditions of the key frames and upload them to the cloud.

[0116] In this embodiment, "establish the relationship between the image feature points and the local point cloud map" specifically includes:

[0117] When the local map fails to track due to principles such as environmental occlusion or texture loss, the system relocates in the following ways:

[0118] Relocate and match the reference frame in the local map on the drone or the unmanned vehicle;

[0119] Relocate on the cloud platform through the information of the current frame.

[0120] In this embodiment, "detecting visual positioning markers through the cloud" specifically includes:

[0121] Perform image edge detection;

[0122] Filter out the contour edges of quadrilaterals;

[0123] Decode the contour edges of quadrilaterals to identify visual positioning markers, that is, identify road signs (AprilTag).

[0124] In this embodiment, "optimizing the pose estimation of the UAV visual odometer through visual positioning markers" specifically includes:

[0125] Define a coordinate system, define the camera coordinate system P carried by the UAV C , the UAV coordinate system P A , the visual positioning marker coordinate system P B and the world coordinate system P W , the world coordinate system P W is defined as the first frame of the UAV;

[0126] The YOZ plane of the camera coordinate system P carried by the UAV C is parallel to the YOZ plane of the UAV coordinate system P A , and set the origin of the UAV coordinate system P A at the center of the UAV;

[0127] Calculate the relationship between the camera coordinate system P carried by the UAV C and the world coordinate system P W ;

[0128] Calculate the relative pose between the camera coordinate system P carried by the UAV C and the visual positioning marker coordinate system P B ; and

[0129] Obtain the trajectory error through the relative pose obtained by the visual positioning marker, that is, the road sign (AprilTag), and the relative pose obtained by the visual odometer, and evenly distribute the trajectory error on each key frame of the UAV, so that the closed-loop key frame and the actual error are reduced.

[0130] In this embodiment, "calculating the relationship between the camera coordinate system P carried by the UAV C and the world coordinate system P W " specifically includes:

[0131] The UAV coordinate system P A and the camera coordinate system P carried by the UAVC is a parallel relationship, that is:

[0132]

[0133] where P A represents the coordinates of the UAV coordinate system, and P C represents the coordinates of the camera coordinate system loaded on the UAV. is the translation vector between the UAV coordinate system P A and the camera coordinate system P C loaded on the UAV, indicating the distance between the camera and the center of the UAV;

[0134] The relationship between the visual positioning marker coordinate system P B and the world coordinate system P W satisfies:

[0135]

[0136] where P W is the coordinate of the world coordinate system, and P B is the coordinate of the visual positioning marker coordinate system. is the translation vector between the world coordinate system P W and the visual positioning marker coordinate system P B ;

[0137] The angles φ, θ, and ψ are the Euler angles respectively. Let the rotation matrix from the world coordinate system P W to the UAV coordinate system P A be The rotation matrix from the visual positioning marker coordinate system P B to the camera coordinate system P C loaded on the UAV is Then:

[0138]

[0139]

[0140] The above c represents cos, and s represents sin. According to the above formula, the rotation relationship between the visual positioning marker coordinate system P B and the camera coordinate system P C loaded on the UAV includes:

[0141]

[0142] And the relationship from the camera coordinate system P C loaded on the UAV to the visual positioning marker coordinate system P B is expressed as:

[0143]

[0144] Among them, is the rotation matrix from the camera coordinate system P mounted on the UAV C to the visual positioning marker coordinate system P B , is the translation vector from the camera coordinate system P mounted on the UAV C to the visual positioning marker coordinate system P B ;

[0145] Then the relationship from the camera coordinate system P mounted on the UAV C to the world coordinate system P W includes:

[0146]

[0147] Among them, is the rotation matrix from the UAV coordinate system P A to the world coordinate system P W , is the translation vector from the UAV coordinate system P A to the world coordinate system P W , is the translation vector from the UAV coordinate system P A to the camera coordinate system P mounted on the UAV C . Among them and are unknown.

[0148] In this embodiment, "calculating the relative pose C between the camera coordinate system P mounted on the UAV B and the visual positioning marker coordinate system P and " specifically includes:

[0149] Using the camera model to project the visual positioning marker onto the 2D pixel plane of the camera, obtaining:

[0150]

[0151] where M represents the camera internal parameter matrix, [u, v, 1] represents the coordinates of the visual positioning marker projected onto the normalized plane, [XB, YB, ZB] represents the coordinates of the visual positioning marker in the visual positioning marker coordinate system P B , represents the translation vector from the visual positioning marker coordinate system P B to the camera coordinate system P mounted on the UAV C , represents the rotation matrix from the visual positioning marker coordinate system P B to the camera coordinate system P mounted on the UAV C , s = 1 / ZC represents an unknown scale factor, Z C represents the Z-axis coordinate of the visual positioning marker in the camera coordinate system, which is calculated using the DLT (Direct Linear Transform) algorithm and

[0152] In this embodiment, "optimizing the pose estimation of the unmanned vehicle visual odometer through visual positioning markers" specifically includes:

[0153] Define a coordinate system, define the camera coordinate system P of the unmanned vehicle C , the visual positioning marker coordinate system P B and the world coordinate system P W , the world coordinate system P W is defined as the first frame of the unmanned aerial vehicle, and the relationship between the camera coordinate system P of the unmanned vehicle C and the unmanned vehicle coordinate system P A is determined;

[0154] Obtain the relative pose T C between the camera coordinate system P of the unmanned vehicle W and the world coordinate system P cw , the relative pose T B between the visual positioning marker coordinate system P C and the camera coordinate system P of the unmanned vehicle bc , and the relative pose T B between the visual positioning marker coordinate system P W and the world coordinate system P bw ;

[0155] Optimize the unmanned vehicle pose and point cloud coordinates;

[0156] Define the relative error between the visual positioning marker coordinate system P B and the camera coordinate system P of the unmanned vehicle C as:

[0157]

[0158] Construct an optimization objective function:

[0159]

[0160] Where:

[0161] T cw ∈{(R cw ,t cw )|R cw ∈SO3,t cw ∈R 3}T bc∈{(R bc , t bc ) | R bc ∈SO3, t bc ∈R 3}

[0162] where SO3 represents the three - dimensional special orthogonal group, and t cw represents the translation error from the camera coordinate system P C mounted on the unmanned vehicle to the world coordinate system P W , and t bc represents the translation error from the visual positioning marker coordinate system P B to the camera coordinate system P C mounted on the unmanned vehicle. R 3 represents a set of bases of dimension 3, and R cw represents the rotation error from the camera coordinate system P C mounted on the unmanned vehicle to the world coordinate system P W , and R bc represents the rotation error from the visual positioning marker coordinate system P B to the camera coordinate system P C mounted on the unmanned vehicle;

[0163] The movement of the camera not only causes rotation errors R cw , R bc and translation errors t cw , t bc , but also is accompanied by scale drift. Therefore, a transformation for scale is performed, and the Sim3 transformation algorithm is adopted. Thus:

[0164] S cw =(R cw , t cw , s = 1), (R cw , t cw ) = T cw

[0165] S bc =(R bc , t bc , s = 1), (R bc , t bc ) = T bc

[0166] where the Sim3 transformation algorithm is to use three pairs of matching points to solve the similarity transformation, and then solve the rotation matrix, translation vector, and scale between two coordinate systems; S cw represents the similarity transformation of the visual positioning marker points from the world coordinate system P W to the camera coordinate system P C mounted on the unmanned vehicle, and S bc represents the visual positioning marker points from the visual positioning marker coordinate system PB to the camera coordinate system P of the driverless vehicle C Similarity transformation, where s represents the unknown scale factor;

[0167] Assume the optimized Sim3 pose is Then the corrected pose is:

[0168]

[0169] where R bw represents the rotation matrix of the visual positioning marker points from the world coordinate system P W to the visual positioning marker coordinate system P B and t bw represents the translation of the visual positioning marker points from the world coordinate system P W to the visual positioning marker coordinate system P B and s represents the unknown scale factor, represents the optimized rotation matrix, translation vector and scale factor, represents the optimized similarity transformation;

[0170] Set the 3D position of the driverless vehicle before the optimization occurs as Then the transformed coordinates can be obtained:

[0171]

[0172] where represents the pose of the driverless vehicle after optimization.

[0173] As Figure 3 shown, a collaborative three-dimensional mapping system for implementing the above collaborative three-dimensional mapping method includes:

[0174] An environment preparation module for collecting environmental information;

[0175] An information processing module for extracting key frames from the acquired environmental information by adopting the design idea of the Tracking thread in the ORB-SLAM algorithm framework;

[0176] A detection module for detecting visual positioning markers through the cloud, i.e., detecting road signs (AprilTag);

[0177] A first optimization module for optimizing the pose estimation of the UAV visual odometer through visual positioning markers;

[0178] A second optimization module for optimizing the pose estimation of the driverless vehicle visual odometer through visual positioning markers;

[0179] An execution module for completing the local map construction thread and loop closure detection thread of the ORB-SLAM framework through the cloud.

[0180] Compared with the prior art, a collaborative three-dimensional mapping method and system provided in this embodiment combines the road sign AprilTag + cloud architecture + multi-robot + SLAM three-dimensional mapping technology to achieve unmanned collaborative three-dimensional mapping, and can solve the problems that the real-time performance of the collaborative SLAM system is difficult to meet and the positioning of the collaborative SLAM system is inaccurate, and can realize a collaborative three-dimensional mapping system with good robustness, high precision and strong real-time performance.

[0181] Those skilled in the art can understand that the drawings are only schematic diagrams of a preferred implementation scenario, and the modules or processes in the drawings are not necessarily essential for implementing the present invention.

[0182] Those skilled in the art can understand that the modules in the devices in the implementation scenario can be distributed in the devices in the implementation scenario according to the description of the implementation scenario, or can be correspondingly changed and located in one or more devices different from the present implementation scenario. The modules in the above implementation scenario can be combined into one module, or further split into multiple sub-modules.

[0183] The above serial numbers of the present invention are only for description and do not represent the advantages and disadvantages of the implementation scenarios.

[0184] The above-disclosed are only several specific implementation scenarios of the present invention. However, the present invention is not limited thereto, and any changes that can be thought of by those skilled in the art should fall within the protection scope of the present invention.

Claims

1. A collaborative three-dimensional mapping method, characterized in that, Including: Environmental preparation, collecting environmental information; Select at least two environmental points and mark them with visual positioning markers; Detect the visual positioning markers through the cloud; Optimize the pose estimation of the UAV visual odometer through the visual positioning markers; Optimize the pose estimation of the unmanned vehicle visual odometer through the visual positioning markers; Complete the local map construction thread and loop detection thread of the ORB-SLAM framework through the cloud; Collect environmental information, use Docker as the cloud container, use Kubernetes as the container scheduling service, and use BRPC and Beego as the network architecture to build a cloud platform to enable communication between multiple intelligent agent terminals and the cloud; The environmental information includes image information, and feature points and descriptors are extracted from the image information using the ORB-SLAM algorithm; Calculate the depth through the PnP algorithm to obtain point cloud information; The "optimizing the pose estimation of the UAV visual odometer through the visual positioning markers" specifically includes: Define the coordinate system, define the coordinate system of the camera loaded on the UAV, the coordinate system of the UAV, the coordinate system of the visual positioning marker, and the world coordinate system, and the world coordinate system is determined by the first frame of the UAV; The YOZ plane of the coordinate system of the camera loaded on the UAV is parallel to the YOZ plane of the coordinate system of the UAV, and the origin of the coordinate system of the UAV is set at the center of the UAV; Calculate the relationship between the coordinate system of the camera loaded on the UAV and the world coordinate system; Calculate the relative pose between the coordinate system of the camera loaded on the UAV and the coordinate system of the visual positioning marker; Based on the relative pose obtained through the visual positioning marker and the relative pose obtained through the visual odometer, calculate the trajectory error, and evenly distribute the trajectory error on each key frame of the UAV to reduce the actual error of the loop closure key frame; The "optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning markers" specifically includes: Define the coordinate system, and define the coordinate system of the camera loaded on the driverless vehicle , the coordinate system of the visual positioning marker and the world coordinate system , the world coordinate system is determined by the first frame of the drone, and determine the relationship between the coordinate system of the camera loaded on the driverless vehicle and the coordinate system of the driverless vehicle ; Obtain the camera coordinate system loaded on the driverless vehicle and the world coordinate system relative pose , the visual positioning marker coordinate system and the camera coordinate system loaded on the driverless vehicle relative pose , and the visual positioning marker coordinate system and the world coordinate system relative pose ; Optimize the pose of the unmanned vehicle and the point cloud coordinates; Define the coordinate system of the visual positioning marker and the coordinate system of the camera mounted on the driverless vehicle The relative error between them is: Construct an optimization objective function: Where: Among them, represents the three-dimensional special orthogonal group, represents the translation error from the camera coordinate system loaded on the driverless vehicle to the world coordinate system ; represents the translation error from the visual positioning marker coordinate system to the camera coordinate system loaded on the driverless vehicle ; represents a set of bases with a dimension of 3, represents the rotation error from the camera coordinate system loaded on the driverless vehicle to the world coordinate system ; represents the rotation error from the visual positioning marker coordinate system to the camera coordinate system loaded on the driverless vehicle ; Camera motion causes not only rotational errors , but also translational errors , , and is also accompanied by scale drift. Therefore, a scale transformation is performed and the transformation algorithm is used. Thus: Among them, S cw represents the similarity transformation of the visual positioning marker point from the world coordinate system to the camera coordinate system mounted on the driverless vehicle , S bc represents the similarity transformation of the visual positioning marker point from the visual positioning marker coordinate system to the camera coordinate system mounted on the driverless vehicle , and s represents the scale factor; Let the optimized pose be , then the corrected pose is: Wherein, R bw represents the rotation matrix of the visual positioning marker point from the world coordinate system to the visual positioning marker coordinate system , t bw represents the translation of the visual positioning marker point from the world coordinate system to the visual positioning marker coordinate system , s represents the scale factor, represents the optimized rotation matrix, translation vector and scale factor, represents the optimized similarity transformation; Set the 3D position of the driverless vehicle before the optimization occurs as , then the transformed coordinates are obtained: Among them represents the optimized pose of the unmanned vehicle 2. The collaborative three-dimensional mapping method according to claim 1, wherein Also including: The multiple intelligent agents include one UAV and one unmanned vehicle. The UAV and the unmanned vehicle form a centralized architecture. A first monocular camera is equipped at the front position of the UAV and the lens of the first monocular camera faces downward. A second monocular camera is equipped at the front position of the unmanned vehicle and the lens of the second monocular camera faces forward.

3. The collaborative three-dimensional mapping method according to claim 2, wherein Also including: Use the cloud platform for map initialization. If there is a map on the cloud platform, match the image information with the key frames on the cloud to determine the initial position. If there is no map on the cloud platform, use the image information and the map information as the starting point of the cloud platform system map; Estimate the camera pose through feature point matching pairs or relocalization methods; Establish the relationship between the image feature points and the local point cloud map; Extract the key frames according to the judgment conditions of the key frames and upload them to the cloud; 4. The collaborative three-dimensional mapping method according to claim 3, wherein The "establishing the relationship between the image feature points and the local point cloud map" specifically includes: When the local map fails to track due to environmental occlusion or texture loss, the system relocates in the following way: Relocate and match the reference frame in the local map on the UAV or the unmanned vehicle; Relocate on the cloud platform based on the information of the current frame.

5. The collaborative three-dimensional mapping method according to claim 1, characterized in that The "detecting visual positioning markers through the cloud" specifically includes: Performing image edge detection; Filtering out the contour edges of quadrilaterals; Decoding the contour edges of the quadrilaterals to identify the visual positioning markers.

6. The collaborative three-dimensional mapping method according to claim 1, characterized in that "Calculating the relationship between the camera coordinate system mounted on the UAV and the world coordinate system " specifically includes: The UAV coordinate system and the camera coordinate system loaded on the UAV are in a parallel relationship: Among them, represents the coordinates of the UAV coordinate system, represents the coordinates of the coordinate system of the camera loaded on the UAV, is the UAV coordinate system and the coordinate system of the camera loaded on the UAV is the translation vector between them, representing the distance of the camera from the center of the UAV; The visual positioning mark coordinate system and the world coordinate system satisfy the following relationship: Among them, is the world coordinate system, is the visual positioning marker coordinate system, is the world coordinate system and the visual positioning marker coordinate system is the translation vector between them; The angles φ, θ, and ψ represent Euler angles. Let the world coordinate system to the UAV coordinate system have a rotation matrix of , and the rotation matrix from the visual positioning marker coordinate system to the UAV-mounted camera coordinate system is Then: Where c represents cos and s represents sin, the visual positioning marker coordinate system can be obtained according to the above formula and the camera coordinate system loaded on the UAV The rotation relationship includes: The relationship representation from the camera coordinate system loaded by the UAV to the visual positioning marker coordinate system is as follows: Wherein, is the rotation matrix from the camera coordinate system loaded on the UAV to the visual positioning marker coordinate system , is the translation vector from the camera coordinate system loaded on the UAV to the visual positioning marker coordinate system ; Then the relationship between the camera coordinate system loaded on the drone and the world coordinate system is as follows: Among them, is the rotation matrix from the UAV coordinate system to the world coordinate system ; is the translation vector from the UAV coordinate system to the world coordinate system ; is the translation vector from the UAV coordinate system to the camera coordinate system loaded on the UAV .

7. The collaborative three-dimensional mapping method according to claim 1, wherein "Calculating the relative pose of the camera coordinate system mounted on the UAV and the visual positioning marker coordinate system " specifically includes: and " Using the camera model to project the visual positioning markers onto the 2D pixel plane of the camera, obtaining: Where M represents the camera internal parameter matrix, [u, v, 1] represents the coordinates of the visual positioning marker projected onto the normalized plane, , , represents the coordinates of the visual positioning marker in the visual positioning marker coordinate system . represents the visual positioning marker coordinate system to the coordinate system of the camera mounted on the drone . represents the visual positioning marker coordinate system to the coordinate system of the camera mounted on the drone . represents the scale factor, represents the Z-axis coordinate of the visual positioning marker in the camera coordinate system, calculated using the direct linear transformation algorithm and .

8. A collaborative three-dimensional mapping system for implementing the collaborative three-dimensional mapping method according to any one of claims 1-7 above, characterized in that, Including: An environment preparation module for collecting environmental information; An information processing module for extracting key frames from the obtained environmental information using the design concept of the Tracking thread in the ORB-SLAM algorithm framework; A detection module for detecting visual positioning markers through the cloud; A first optimization module for optimizing the pose estimation of the UAV visual odometer through the visual positioning markers; A second optimization module for optimizing the pose estimation of the unmanned vehicle visual odometer through the visual positioning markers; An execution module for completing the local map construction thread and the loop closure detection thread of the ORB-SLAM framework through the cloud.

Images