CN114332360A - A collaborative three-dimensional mapping method and system - Google Patents

Abstract

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

Description

A collaborative three-dimensional mapping method and system

technical field

The invention relates to the field of collaborative three-dimensional mapping, in particular to a collaborative three-dimensional mapping method and system.

Background technique

In the prior art, there is a technology that uses road sign and monocular camera sensor technology to realize the three-dimensional plane mapping of multi-robots, but the real-time performance of the prior art system is poor;

There are also road signs and cloud architectures to realize two-dimensional plane mapping of a single robot, but this system is not suitable for large-scale environmental applications.

SUMMARY OF THE INVENTION

In order to overcome the deficiencies of the prior art, the present invention provides a collaborative three-dimensional mapping method and system, and the specific technical solutions are as follows:

A collaborative three-dimensional mapping method, comprising:

Detect visual positioning markers through the cloud;

Optimize the pose estimation of the UAV visual odometer by using the visual positioning marker;

Optimize the pose estimation of the unmanned vehicle visual odometer by using the visual positioning marker;

The local map construction thread and closed-loop detection thread of the ORB-SLAM framework are completed through the cloud.

In a specific embodiment, it also includes:

Collect environmental information, use Docker as the cloud container, use Kubernetes as the scheduling service of the container, and use BRPC and Beego as the network architecture to build a cloud platform, so that the multi-agent terminal communicates with the cloud;

The multi-agent includes one UAV and one UAV, the UAV and the UAV form a centralized architecture, and the position in front of the UAV is equipped with a first monocular camera and the lens of the first monocular camera is facing downward, the front position of the unmanned vehicle is equipped with a second monocular camera and the lens of the second monocular camera is facing forward;

Select at least 2 environmental points and mark them with the visual positioning marks.

In a specific embodiment, it also includes:

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

The depth is obtained by the PnP algorithm, and the point cloud information is obtained;

Use the cloud platform to initialize the map, if there is a map on the cloud platform, then match the image information with the key frame in the cloud to determine the initial position, if there is no map on the cloud platform, then The image information and the information such as the map are used as the start of the cloud platform system map;

Estimate camera pose by matching feature point pairs or relocation methods;

Establish the relationship between image feature points and local point cloud maps;

According to the judgment condition of the key frame, the key frame is extracted and uploaded to the cloud.

In a specific embodiment, 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 occlusion or lack of texture in the environment, the system performs relocation in the following ways:

Reposition and match reference frames in the local map on the UAV or the UAV;

Relocation is performed on the cloud platform through the information of the current frame.

In a specific embodiment, the "detecting visual positioning marks through the cloud" specifically includes:

Perform image edge detection;

Filter out the contour edges of the quadrilateral;

The outline edges of the quadrilateral are decoded to identify the visual location markers.

In a specific embodiment, the "optimizing the pose estimation of the UAV's visual odometry by visual positioning markers" specifically includes:

Define the coordinate system, define the UAV mounted camera coordinate system PC, the UAV coordinate system PA, the visual positioning marker coordinate system _PB and the world coordinate system _PW , and the world coordinate system _PW is defined as the _unmanned aerial vehicle _. machine first frame;

The YOZ plane of the UAV-mounted camera coordinate system PC is parallel to the YOZ plane of the UAV coordinate system P _A , and the origin of the UAV coordinate system _{P A is} set at the center of the UAV;

Calculate the relationship between the UAV-mounted camera coordinate system P _C and the world coordinate system P _W ;

和

Calculate the relative pose of the UAV-mounted camera coordinate system P _C and the visual positioning marker coordinate system P _B

and

Through the relative pose obtained by the visual positioning mark and the relative pose obtained by the visual odometry, the trajectory error is obtained, and the trajectory error is equally divided into each key frame of the UAV, so that the closed-loop key frame The actual error is reduced.

In a specific embodiment, the "calculating the relationship between the UAV-mounted camera coordinate system P _C and the world coordinate system P _W " specifically includes:

The UAV coordinate system P _A and the UAV mounted camera coordinate system P _C are in a parallel relationship, including:

为所述无人机坐标系P_A与所述无人机装载相机坐标系P_C之间的平移向量，表示所述相机距离所述无人机中心的距离；Wherein, _PA represents the coordinates of the UAV coordinate system, PC represents the coordinates of the _UAV mounted camera coordinate system,

is the translation vector between the UAV coordinate system _{P A} and the UAV mounted camera coordinate system PC, representing the distance between the camera and the center of the UAV;

The relationship between the visual positioning marker coordinate system P _B and the world coordinate system P _W satisfies:

为所述世界坐标系P_W与所述视觉定位标记坐标系P_B之间的平移向量；Wherein, P _W is the coordinates of the world coordinate system, P _B is the coordinates 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 ;

所述视觉定位标记坐标系P_B到所述无人机装载相机坐标系P_C的旋转矩阵为

则：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 of the coordinate system P _B of the visual positioning marker to the coordinate system P _C of the UAV mounted camera is:

but:

The above-mentioned c represents cos, and s represents sin. According to the above formula, the rotational relationship between the coordinate system P _B of the visual positioning mark and the coordinate system P _C of the UAV mounted camera can be obtained, including:

And the relationship between the coordinate system P _C of the camera mounted on the drone and the coordinate system P _B of the visual positioning mark is expressed as:

为所述无人机装载相机坐标系P_C到所述视觉定位标记坐标系P_B的旋转矩阵，

为所述无人机装载相机坐标系P_C到所述视觉定位标记坐标系P_B的平移向量；in,

Load the rotation matrix of the camera coordinate system P _C to the visual positioning marker coordinate system P _B for the UAV,

Load the translation vector of the camera coordinate system P _C to the visual positioning marker coordinate system P _B for the UAV;

Then, the relationship between the UAV-mounted camera coordinate system P _C and the world coordinate system P _W includes:

为所述无人机坐标系P_A到所述世界坐标系P_W的旋转矩阵，

为所述无人机坐标系P_A到所述世界坐标系P_W的平移向量，

为所述无人机坐标系P_A到所述无人机装载相机坐标系P_C的平移向量。in,

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 ,

_A translation vector from the UAV coordinate system PA to the UAV loading camera coordinate system PC _.

和

”具体包括：In a specific embodiment, the "calculating the relative pose of the UAV _- mounted camera coordinate system PC and the visual positioning marker coordinate system _PB "

and

"Includes:

Using the camera model to project the visual localization markers onto the camera's 2D pixel plane yields:

代表所述视觉定位标记坐标系P_B到所述无人机装载相机坐标系P_C的平移向量，

代表所述视觉定位标记坐标系P_B到所述无人机装载相机坐标系P_C的旋转矩阵，s＝1/Z_C代表未知的尺度因子，Z_C代表所述视觉定位标记在相机坐标系下的Z轴坐标，采用直接线性变换算法计算得到

和

where M represents the camera internal parameter matrix, [u, v, 1] represents the coordinates of the visual positioning mark projected to the normalized plane, [XB, YB, ZB] represents the visual positioning mark in the visual positioning mark coordinate system P _B coordinates in ,

represents the translation vector of the coordinate system P _B of the visual positioning marker to the coordinate system P _C of the UAV mounted camera,

Represents the rotation matrix from the visual positioning marker coordinate system _{P B} to the UAV-mounted camera coordinate system PC, s=1/Z _C represents the unknown scale factor, and Z _C represents the visual positioning marker in the camera coordinate system The Z-axis coordinates below are calculated by the direct linear transformation algorithm.

and

In a specific embodiment, the "optimizing the pose estimation of the visual odometry of the unmanned vehicle through visual positioning markers" specifically includes:

Define the coordinate system, define the unmanned vehicle loading 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 no _The relationship between the coordinate system PC of the camera for loading the vehicle and the coordinate system _PA of the unmanned vehicle is determined;

Obtain the relative pose T cw of the unmanned vehicle-mounted camera coordinate system P _C and the world coordinate system P _W , and the relative pose T _cw of the visual positioning marker coordinate system _{P B} and the unmanned vehicle-mounted camera coordinate system PC T _bc , and the relative pose T _bw of the visual positioning marker coordinate system P _B and the world coordinate system P _W ;

Optimize the pose and point cloud coordinates of the unmanned vehicle;

The relative error between the coordinate system P _B of the visual positioning mark and the coordinate system P _C of the camera mounted on the unmanned vehicle is defined as:

Build the optimization objective function:

in:

T _cw ∈{(R _cw ,t _cw )|R _cw ∈ SO ₃ ,t _cw ∈ R ³ }T _bc ∈{(R _bc ,t _bc )|R _bc ∈ SO ₃ ,t _bc ∈R ³ }

Among them, SO ₃ represents a three-dimensional special orthogonal group, t _cw represents the translation error from the camera coordinate system PC _{mounted on the unmanned vehicle to the world coordinate system P W ,} and t _bc represents the coordinate system P from the visual positioning marker The translation error from _B to the camera coordinate system PC mounted on the unmanned vehicle, _R ³ represents a set of bases with a dimension of 3, and _{R cw} represents from the camera coordinate system PC mounted on the unmanned vehicle to the world coordinate system The translation error of P _W , _{R bc} represents the rotation error from the visual positioning marker coordinate system P _B to the unmanned vehicle loading camera coordinate system PC;

Camera motion not only causes rotation errors R _cw , R _bc and translation errors t _cw , t _bc , but also accompanies scale drift, so scale transformation is carried out, and Sim3 transformation algorithm is used, so:

S _cw =(R _cw ,t _cw ,s=1),(R _cw ,t _cw )=T _cw

S _bc =(R _bc ,t _bc ,s=1),(R _bc ,t _bc )=T _bc

Wherein, _{S cw} represents the similar transformation of the visual positioning mark point from the world coordinate system P _W to the unmanned vehicle mounted camera coordinate system PC, and S _bc represents the visual positioning mark point from the visual positioning mark coordinate system. Similar transformation from _{P B} to the coordinate system PC of the camera mounted on the unmanned vehicle, s represents the unknown scale factor;

那么纠正完成的姿态是：Suppose the optimized Sim3 pose is

Then the corrected pose is:

代表优化后的旋转矩阵、平移向量和尺度因子，

代表优化后的相似变换；Wherein, 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 , and t _bw represents the visual positioning marker point from the world coordinate system P _W to the translation of the visual positioning marker coordinate system P _B , s represents the unknown to scale factor,

represents the optimized rotation matrix, translation vector and scale factor,

represents the optimized similarity transformation;

则可以得到变换后的坐标：Set the 3D position of the unmanned vehicle before the optimization takes place as

Then you can get the transformed coordinates:

代表所述无人车优化后的位姿。in

represents the optimized pose of the unmanned vehicle.

A collaborative 3D mapping system for implementing the above-mentioned collaborative 3D mapping method, comprising:

Environment preparation module for collecting environment information;

The information processing module is used for extracting key frames from the obtained environmental information using the Tracking thread design idea in the ORB-SLAM algorithm framework;

The detection module is used to detect the visual positioning mark through the cloud;

a first optimization module, used for optimizing the pose estimation of the UAV visual odometry through the visual positioning mark;

The second optimization module is used for optimizing the pose estimation of the visual odometry of the unmanned vehicle through the visual positioning mark; the execution module is used for completing the local map construction thread and closed-loop detection thread of the ORB-SLAM framework through the cloud.

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

The method and system for collaborative 3D mapping provided by the present invention can solve the problem that the real-time performance of the collaborative SLAM system is difficult to meet and solve the problem of inaccurate positioning of the collaborative SLAM system, and can realize the collaborative 3D mapping with good robustness, high precision and strong real-time performance. Mapping system.

In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, preferred embodiments are given below, and are described in detail as follows in conjunction with the accompanying drawings.

Description of drawings

In order to illustrate the technical solutions of the embodiments of the present invention more clearly, the following briefly introduces the accompanying drawings used in the embodiments. It should be understood that the following drawings only show some embodiments of the present invention, and therefore do not It should be regarded as a limitation of the scope, and for those of ordinary skill in the art, other related drawings can also be obtained according to these drawings without any creative effort.

1 is a schematic diagram of an imaging model of a camera in an embodiment;

Fig. 2 is a flow chart of a collaborative three-dimensional mapping method in an embodiment;

FIG. 3 is a block diagram of a collaborative three-dimensional mapping system in an embodiment.

Detailed ways

Example

As shown in FIGS. 1-2 , this embodiment provides a collaborative 3D mapping method, including:

Environmental preparation, collecting environmental information;

Information processing, from the obtained environmental information, using the Tracking thread design idea in the ORB-SLAM algorithm framework to extract key frames;

Detect visual positioning marks through the cloud, and visual positioning marks are road signs;

Optimize the pose estimation of UAV visual odometry through visual localization markers;

Optimize the pose estimation of the unmanned vehicle visual odometry through visual localization markers;

The local map construction thread and closed-loop detection thread of the ORB-SLAM framework are completed through the cloud.

Specifically, the cloud executes the local map construction thread (Local Mapping thread) and the loop closure detection thread (Loop Closing thread) in ORB-SLAM. Cooperative SLAM (Cooperative simultaneous localization and mapping, CSLAM) has more advantages than a single robot in fault tolerance, robustness and execution efficiency, and has important influence in tasks such as disaster rescue, resource detection and space detection in unknown environments. The amount of data computing and storage in the CSLAM system is large, and most individual robots cannot meet the real-time requirements. CSLAM systems usually perform tasks in large-scale environments, and the systematic errors (pose estimation errors, etc.) accumulated by a large number 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 mismatching. Accumulated system errors and mismatches will affect the mapping accuracy of the CSLAM system. Therefore, a small number of road signs are arranged in the environment, so that the robot can optimize its own pose according to the road signs, which is of great significance to improve the mapping accuracy. Compared with two-dimensional maps, three-dimensional maps are richer in information and can better reflect the objective existence of the real world.

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

Preferably, in this embodiment, a relatively spacious place is selected to be marked with road signs (AprilTag codes) in a large-scale unknown environment, and a monocular camera is mounted on the drone and the unmanned vehicle, and the monocular camera is used to collect the environment in real time during the traveling of the multi-agent. information, use the ORB-SLAM framework for collaborative 3D mapping, and use the AprilTag code to optimize the ORB-SLAM pose estimation, and use the Docker+Kubernetes+BRPC+Beego technology to build a cloud platform to perform tasks with large amounts of computation and high storage requirements. Deployed in the cloud, the multi-agent side is used for tracking and relocation.

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

In this embodiment, "collecting environmental information" specifically includes:

Docker+Kubernetes+BRPC+Beego technology is used to build a cloud platform, so that multi-agent terminals communicate with the cloud. Specifically, Docker is used as the cloud container, Kubernetes is used as the scheduling service of the container, and BRPC and Beego are used as the network architecture to build the cloud platform. Enable the multi-agent terminal to communicate with the cloud;

The multi-agent includes a drone and an unmanned vehicle, and the drone and the unmanned vehicle form a centralized architecture;

Select at least 2 environmental points and mark them with visual positioning marks, that is, mark them with the AprilTag code.

In this embodiment, "UAVs and unmanned vehicles form a centralized architecture" specifically include:

The position in front of the drone is equipped with a first monocular camera with the lens of the first monocular camera facing downward, and the position in front of the unmanned vehicle is equipped with a second monocular camera with the lens of the second monocular camera facing forward.

In this embodiment, "information processing" specifically includes:

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

The depth is obtained by the PnP algorithm, and the point cloud information is obtained;

Use the cloud platform to initialize the map. If there is a map on the cloud platform, match the image information with the key frames in the cloud to determine the initial position. If there is no map on the cloud platform, use the image information and map information as the information of the cloud platform system map. start;

Estimate camera pose by matching feature point pairs or relocation methods;

Establish the relationship between image feature points and local point cloud maps;

According to the judgment conditions of the key frame, the key frame is extracted and uploaded to the cloud.

In this embodiment, "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 occlusion or lack of texture in the environment, the system performs relocation in the following ways:

Relocate and match reference frames in local maps on drones or unmanned vehicles;

Relocation is performed on the cloud platform through the information of the current frame.

In this embodiment, "detecting visual positioning marks through the cloud" specifically includes:

Perform image edge detection;

Filter out the contour edges of the quadrilateral;

The outline edge of the quadrilateral is decoded, and the visual positioning mark is identified, that is, the road sign (AprilTag) is identified.

In this embodiment, "optimizing the pose estimation of the UAV visual odometry by visual positioning markers" specifically includes:

Define the coordinate system, define the UAV mounted camera coordinate system P _C , UAV coordinate system P _A , the visual positioning marker coordinate system P _B and the world coordinate system P _W , and the world coordinate system P _W is defined as the first frame of the UAV ;

The YOZ plane of the UAV-mounted camera coordinate system PC is parallel to the YOZ plane of the UAV coordinate system P _A , and the origin of the UAV coordinate system _{P A is} set at the center of the UAV;

Calculate the relationship between the UAV mounted camera coordinate system P _C and the world coordinate system P _W ;

和

Calculate the relative pose of the UAV-mounted camera coordinate system P _C and the visual positioning marker coordinate system P _B

and

Through the visual positioning mark, that is, the relative pose obtained by the road sign (AprilTag) and the relative pose obtained by the visual odometry, the trajectory error is obtained, and the trajectory error is equally divided into each key frame of the UAV, so that the closed-loop key frame The actual error is reduced.

In this embodiment, "calculating the relationship between the UAV-mounted camera coordinate system P _C and the world coordinate system P _W " specifically includes:

The UAV coordinate system _{P A} is in a parallel relationship with the UAV mounted camera coordinate system PC, including:

为无人机坐标系P_A与无人机装载相机坐标系P_C之间的平移向量，表示相机距离无人机中心的距离；Among them, _{P A} represents the coordinates of the UAV coordinate system, PC represents the coordinates of the UAV mounted camera coordinate system,

is the translation vector between the UAV coordinate system _{P A} and the UAV-mounted camera coordinate system PC, indicating the distance between the camera and the center of the UAV;

The relationship between the visual positioning marker coordinate system P _B and the world coordinate system P _W satisfies:

为世界坐标系P_W与视觉定位标记坐标系P_B之间的平移向量；Among them, P _W is the coordinate of the world coordinate system, 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 ;

视觉定位标记坐标系P_B到无人机装载相机坐标系P_C的旋转矩阵为

则：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 PC is:

but:

The above c represents cos, and s represents sin. According to the above formula, the rotation relationship between the coordinate system P _B of the visual positioning mark and the coordinate system P _C of the drone mounted camera can be obtained including:

The relationship between the UAV-mounted camera coordinate system P _C and the visual positioning marker coordinate system P _B is expressed as:

为无人机装载相机坐标系P_C到视觉定位标记坐标系P_B的旋转矩阵，

为无人机装载相机坐标系P_C到视觉定位标记坐标系P_B的平移向量；in,

Load the rotation matrix of the camera coordinate system P _C to the visual positioning marker coordinate system P _B for the UAV,

Load the translation vector of the camera coordinate system P _C to the visual positioning marker coordinate system P _B for the UAV;

Then the relationship between the UAV-mounted camera coordinate system P _C and the world coordinate system P _W includes:

为无人机坐标系P_A到世界坐标系P_W的旋转矩阵，

为无人机坐标系P_A到世界坐标系P_W的平移向量，

为无人机坐标系P_A到无人机装载相机坐标系P_C的平移向量。其中

和

未知。in,

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 ,

Translation vector for the UAV coordinate system _{P A} to the UAV loading camera coordinate system PC. in

and

unknown.

和

”具体包括：In this embodiment, "calculate the relative pose of the UAV _- mounted camera coordinate system PC and the visual positioning marker coordinate system _PB

and

"Includes:

Using the camera model to project the visual localization markers onto the camera's 2D pixel plane yields:

代表视觉定位标记坐标系P_B到无人机装载相机坐标系P_C的平移向量，

代表视觉定位标记坐标系P_B到无人机装载相机坐标系P_C的旋转矩阵，s＝1/Z_C代表未知的尺度因子，Z_C代表视觉定位标记在相机坐标系下的Z轴坐标，采用DLT(Direct Linear Transform,直接线性变换)算法计算得到

和

where M represents the camera internal parameter matrix, [u, v, 1] represents the coordinates of the visual positioning mark projected to the normalized plane, [XB, YB, ZB] represents the coordinates of the visual positioning mark in the visual positioning mark coordinate system P _B ,

represents the translation vector from the coordinate system P _B of the visual positioning marker to the coordinate system P _C of the UAV mounted camera,

Represents the rotation matrix from the visual positioning marker coordinate system P _B to the UAV mounted camera coordinate system PC, 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 by DLT (Direct Linear Transform, direct linear transform) algorithm

and

In this embodiment, "optimizing the pose estimation of the visual odometry of the unmanned vehicle through visual positioning marks" specifically includes:

Define the coordinate system, define the unmanned 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 drone, and the unmanned vehicle mounted camera coordinate system _The relationship between _PC and unmanned vehicle coordinate system PA is determined;

Obtain the relative pose _{T cw} of the unmanned vehicle mounted camera coordinate system PC and the world coordinate system P _W , the relative pose T _bc of the visual positioning marker coordinate system P _B and the unmanned vehicle mounted camera coordinate system _PC , and the visual positioning marker coordinates The relative pose T _bw of the system P _B and the world coordinate system P _W ;

Optimize the pose and point cloud coordinates of the unmanned vehicle;

Define the relative error between the coordinate system P _B of the visual positioning mark and the coordinate system P _C of the camera mounted on the unmanned vehicle:

Build the optimization objective function:

in:

T _cw ∈{(R _cw ,t _cw )|R _cw ∈ SO ₃ ,t _cw ∈ R ³ }T _bc ∈{(R _bc ,t _bc )|R _bc ∈ SO ₃ ,t _bc ∈R ³ }

Among them, SO ₃ represents the three-dimensional special orthogonal group, t _cw represents the translation error from the camera coordinate system P _C of the unmanned vehicle loading to the world coordinate system P _W , and t _bc represents the visual positioning marker coordinate system P _B to the unmanned vehicle loading. The translation error of the camera coordinate system PC, R ³ represents a set of bases with a dimension of 3, R _cw represents the translation error from the camera coordinate system PC loaded by the unmanned vehicle to the world coordinate system _{P W} , and R _bc represents the visual positioning _. The rotation error from the marker coordinate system _{P B} to the coordinate system PC of the camera mounted on the unmanned vehicle;

Camera motion not only causes rotation errors R _cw , R _bc and translation errors t _cw , t _bc , but also accompanies scale drift, so scale transformation is carried out, and Sim3 transformation algorithm is used, so:

S _cw =(R _cw ,t _cw ,s=1),(R _cw ,t _cw )=T _cw

S _bc =(R _bc ,t _bc ,s=1),(R _bc ,t _bc )=T _bc

Among them, the Sim3 transformation algorithm uses 3 pairs of matching points to solve the similarity transformation, and then solves the rotation matrix, translation vector and scale between the two coordinate systems; S _cw represents the visual positioning marker point from the world coordinate system P _W to Similar transformation of unmanned vehicle mounted camera coordinate system _{PC, S bc represents the} similarity transformation of visual positioning marker point from visual positioning marker coordinate system P _B to unmanned vehicle mounted camera coordinate system PC, s represents unknown to scale factor;

那么纠正完成的姿态是：Suppose the optimized Sim3 pose is

Then the corrected pose is:

代表优化后的旋转矩阵、平移向量和尺度因子，

代表优化后的相似变换；Among them, R _bw represents the rotation matrix of the visual positioning mark point from the world coordinate system P _W to the visual positioning mark coordinate system P _B , and t _bw represents the translation of the visual positioning mark point from the world coordinate system P _W to the visual positioning mark 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;

则可以得到变换后的坐标：Set the 3D position of the unmanned vehicle before the optimization takes place as

Then you can get the transformed coordinates:

代表无人车优化后的位姿。in

Represents the optimized pose of the unmanned vehicle.

As shown in Figure 3, a collaborative 3D mapping system is used to implement the above-mentioned collaborative 3D mapping method, including:

Environment preparation module for collecting environment information;

The information processing module is used to extract key frames from the obtained environmental information, using the Tracking thread design idea in the ORB-SLAM algorithm framework;

The detection module is used to detect visual positioning marks through the cloud, that is, to detect road signs (AprilTag);

The first optimization module is used to optimize the pose estimation of the UAV visual odometry through visual positioning markers;

The second optimization module is used to optimize the pose estimation of the visual odometry of the unmanned vehicle through the visual positioning mark;

The execution module is used to complete the local map construction thread and closed-loop detection thread of the ORB-SLAM framework through the cloud.

Compared with the prior art, a collaborative 3D mapping method and system provided by this embodiment, combined with the road sign AprilTag + cloud architecture + multi-robot + SLAM 3D mapping technology, realizes unmanned collaborative 3D mapping, and can solve the collaborative SLAM system. It is difficult to meet the real-time performance and solve the problem of inaccurate positioning of the collaborative SLAM system, which can realize a collaborative 3D mapping system with good robustness, high precision and strong real-time performance.

Those skilled in the art can understand that the accompanying drawing is only a schematic diagram of a preferred implementation scenario, and the modules or processes in the accompanying drawing are not necessarily necessary to implement the present invention.

Those skilled in the art can understand that the modules in the device in the implementation scenario may be distributed in the device in the implementation scenario according to the description of the implementation scenario, or may be located in one or more devices different from the implementation scenario with corresponding changes. The modules of the above implementation scenarios may be combined into one module, or may be further split into multiple sub-modules.

The above serial numbers of the present invention are only for description, and do not represent the pros and cons of the implementation scenarios.

The above disclosures are only a few specific implementation scenarios of the present invention, however, the present invention is not limited thereto, and any changes that can be conceived by those skilled in the art should fall within the protection scope of the present invention.

Claims (10)

1. A collaborative three-dimensional mapping method is characterized by comprising the following steps:

detecting the visual positioning mark through a cloud end;

optimizing pose estimation of the unmanned aerial vehicle visual odometer through the visual positioning mark;

optimizing the pose estimation of the unmanned vehicle vision odometer through the vision positioning mark;

and completing a local map construction thread and a closed loop detection thread of the ORB-SLAM framework through the cloud.

2. The collaborative three-dimensional mapping method according to claim 1, further comprising:

collecting environment information, adopting Docker as a cloud container, Kubernetes as a scheduling service of the container, and BRPC and Beego as network frameworks to build a cloud platform, so that a multi-agent end communicates with the cloud end;

the multi-agent comprises the unmanned aerial vehicle and the unmanned vehicle, the unmanned aerial vehicle and the unmanned vehicle form a centralized system structure, a first monocular camera is arranged in the front of the unmanned aerial vehicle, the lens of the first monocular camera faces downwards, a second monocular camera is arranged in the front of the unmanned vehicle, and the lens of the second monocular camera faces forwards;

and (4) selecting at least 2 environmental points, and printing the visual positioning mark.

3. The collaborative three-dimensional mapping method according to claim 2, further comprising:

the environment information comprises image information, and feature points and descriptors are extracted from the image information by adopting an ORB-SLAM algorithm;

obtaining depth through a PnP algorithm to obtain point cloud information;

map initialization is carried out by utilizing the cloud platform, if a map exists on the cloud platform, the image information is matched with the key frame of the cloud end to determine an initial position, and if no map exists on the cloud platform, the image information, the map and other information are used as the start of a cloud platform system map;

estimating the pose of the camera by matching the feature point pairs or a repositioning method;

establishing a relation between the image feature points and the local point cloud map;

and extracting the key frame and uploading the key frame to the cloud according to the judgment condition of the key frame.

4. The collaborative three-dimensional mapping method according to claim 3, wherein the establishing of the relationship between the image feature points and the local point cloud map specifically comprises:

when the local map fails to track due to the principles of shielding or texture missing on the environment and the like, the system adopts the following modes to reposition:

relocating and matching reference frames in a local map on the drone or the drone vehicle;

and carrying out repositioning on the cloud platform according to the information of the current frame.

5. The collaborative three-dimensional mapping method according to claim 1, wherein the "visual positioning mark through cloud detection" specifically includes:

carrying out image edge detection;

screening out the outline edges of the quadrangle;

and decoding the outline edge of the quadrangle and identifying the visual positioning mark.

6. The collaborative three-dimensional mapping method according to claim 1, wherein the optimizing the pose estimation of the unmanned aerial vehicle visual odometer by the visual positioning markers specifically comprises:

defining coordinate system, defining coordinate system P of unmanned aerial vehicle loading camera_CUnmanned aerial vehicle coordinate system P_AVisual positioning mark coordinate system P_BAnd a world coordinate system P_WSaid world coordinate system P_WDefining as the drone first frame;

unmanned aerial vehicle loads camera coordinate system P_CThe YOZ plane and the unmanned aerial vehicle coordinate system P_AYOZ planes are parallel, and the unmanned aerial vehicle coordinate system P is arranged_AIs at the drone center;

calculating the coordinate system P of the unmanned aerial vehicle loading camera_CTo the world coordinate system P_WThe relationship of (1);

calculating the coordinate system P of the unmanned aerial vehicle loading camera_CAnd the visual positioning mark coordinate system P_BRelative position and attitude of

And

and solving a track error through the relative pose obtained by the visual positioning mark and the relative pose obtained by the visual odometer, and equally dividing the track error on each key frame of the unmanned aerial vehicle, so that the closed loop key frame and the actual error are reduced.

7. The collaborative three-dimensional mapping method according to claim 6, wherein the "calculating the UAV" is performedLoad camera coordinate system P_CTo the world coordinate system P_WThe relationship of (1) specifically includes:

unmanned aerial vehicle coordinate system P_AWith the unmanned aerial vehicle loads camera coordinate system P_CIs a parallel relationship, existing:

wherein, P_ACoordinates, P, representing the coordinate system of the drone_CCoordinates representing the drone loading camera coordinate system,

is the unmanned aerial vehicle coordinate system P_AWith the unmanned aerial vehicle loads camera coordinate system P_CA translation vector therebetween representing a distance of the camera from the center of the drone;

the visual positioning marker coordinate system P_BAnd the world coordinate system P_WThe relationship between them satisfies:

wherein, P_WIs the coordinate of the world coordinate system, P_BFor the coordinates of the visual positioning marker coordinate system,

for the world coordinate system P_WAnd the visual positioning mark coordinate system P_BA translation vector therebetween;

angles phi, theta and psi are euler angles, respectively, given said world coordinate system P_WTo the unmanned aerial vehicle coordinate system P_AIs a rotation matrix of

The visual positioning marker coordinate system P_BTo the unmanned aerial vehicle load camera coordinate system P_CIs a rotation matrix of

Then:

c represents cos, s represents sin, and the visual positioning mark coordinate system P can be obtained according to the formula_BAnd the unmanned aerial vehicle loads the camera coordinate system P_CThe rotational relationship includes:

and the unmanned aerial vehicle loads the camera coordinate system P_CTo the visual positioning marker coordinate system P_BThe relational expression of (A) is:

wherein,

for the unmanned aerial vehicle to be loaded with a camera coordinate system P_CTo the visual positioning marker coordinate system P_BThe rotation matrix of (a) is,

for the unmanned aerial vehicle to be loaded with a camera coordinate system P_CTo the visual positioning marker coordinate system P_BThe translation vector of (a);

obtaining the coordinates of the unmanned aerial vehicle loading cameraIs P_CTo the world coordinate system P_WThe relationship of (1) includes:

wherein,

is the unmanned aerial vehicle coordinate system P_ATo the world coordinate system P_WThe rotation matrix of (a) is,

is the unmanned aerial vehicle coordinate system P_ATo the world coordinate system P_WThe translation vector of (a) is calculated,

is the unmanned aerial vehicle coordinate system P_ATo the unmanned aerial vehicle load camera coordinate system P_CThe translation vector of (2).

8. The collaborative three-dimensional mapping method according to claim 6, wherein said "calculating said drone loading camera coordinate system P_CAnd the visual positioning mark coordinate system P_BRelative position and attitude of

And

the method specifically comprises the following steps:

projecting the visual localization markers to a 2D pixel plane of a camera using a camera model, resulting in:

wherein M represents the camera internal reference momentMatrix, [ u, v,1 ]]Coordinates representing the projection of said visual positioning markers onto a normalized plane, [ XB, YB, ZB]Representing a visual positioning mark in the visual positioning mark coordinate system P_BThe coordinates of (a) are (b),

representing the visual positioning marker coordinate system P_BTo the unmanned aerial vehicle load camera coordinate system P_CThe translation vector of (a) is calculated,

representing the visual positioning marker coordinate system P_BTo the unmanned aerial vehicle load camera coordinate system P_CS 1/Z_CRepresenting an unknown scale factor, Z_CRepresenting the Z-axis coordinate of the visual positioning mark under a camera coordinate system, and obtaining the Z-axis coordinate by adopting a direct linear transformation algorithm

And

9. the collaborative three-dimensional mapping method according to claim 1, wherein the optimizing pose estimation of the unmanned vehicle visual odometer by the visual positioning markers specifically comprises:

defining a coordinate system, defining a coordinate system P of an unmanned vehicle-mounted camera_CVisual positioning mark coordinate system P_BAnd a world coordinate system P_WSaid world coordinate system P_WDefined as the unmanned aerial vehicle first frame, the unmanned aerial vehicle is loaded with a camera coordinate system P_CAnd the unmanned vehicle coordinate system P_ADetermining the relationship of (1);

obtaining the coordinate system P of the unmanned vehicle loading camera_CAnd the world coordinate system P_WRelative pose T_cwThe visual positioning mark coordinate system P_BAnd the unmanned vehicle-mounted camera coordinate system P_CRelative positionPosture T_bcAnd the visual positioning mark coordinate system P_BAnd the world coordinate system P_WRelative pose T_bw；

Optimizing the pose and point cloud coordinates of the unmanned vehicle;

defining said visual positioning marker coordinate system P_BAnd the unmanned vehicle-mounted camera coordinate system P_CThe relative error between each other is:

constructing an optimization objective function:

wherein:

T_cw∈{(R_cw,t_cw)|R_cw∈SO₃,t_cw∈R³} T_bc∈{(R_bc,t_bc)|R_bc∈SO₃,t_bc∈R³}

wherein, SO₃Representing three-dimensional special orthogonal groups, t_cwRepresenting camera coordinate system P loaded from said unmanned vehicle_CTo the world coordinate system P_WTranslation error of t_bcRepresenting a coordinate system P from said visual positioning marker_BTo the unmanned vehicle-mounted camera coordinate system P_CTranslation error of R³Representing a set of radicals of dimension 3, R_cwRepresenting camera coordinate system P loaded from said unmanned vehicle_CTo the world coordinate system P_WTranslation error of R_bcRepresenting a coordinate system P from said visual positioning marker_BTo the unmanned vehicle-mounted camera coordinate system P_CThe rotational error of (a);

the camera motion not only causes a rotation error R_cw、R_bcAnd translation error t_cw、t_bcSince the scale shift is also accompanied, the scale conversion is performed and Sim is used3 transformation algorithm, therefore:

S_cw＝(R_cw,t_cw,s＝1),(R_cw,t_cw)＝T_cw

S_bc＝(R_bc,t_bc,s＝1),(R_bc,t_bc)＝T_bc

wherein S is_cwRepresenting visual alignment mark points from the world coordinate system P_WTo the unmanned vehicle-mounted camera coordinate system P_CBy similarity transformation of S_bcRepresenting the visual alignment mark point from the visual alignment mark coordinate system P_BTo the unmanned vehicle-mounted camera coordinate system P_CS denotes the unknown to scale factor;

assume an optimized Sim3 pose of

Then the pose for which the correction is complete is:

wherein R is_bwRepresenting the visual alignment marker point from the world coordinate system P_WTo the visual positioning marker coordinate system P_BRotation matrix of t_bwRepresenting the visual alignment marker point from the world coordinate system P_WTo the visual positioning marker coordinate system P_BS denotes the unknown to scale factor,

representing the optimized rotation matrix, translation vector and scale factor,

representing the optimized similarity transformation;

setting the 3D position of the unmanned vehicle before optimization occurs as

The transformed coordinates can be found:

wherein

Representing the optimized pose of the unmanned vehicle.

10. A collaborative three-dimensional mapping system for implementing the collaborative three-dimensional mapping method according to any one of claims 1 to 9, comprising:

the environment preparation module is used for acquiring environment information;

the information processing module is used for extracting the key frame from the acquired environment information by adopting a Tracking thread design idea in an ORB-SLAM algorithm framework;

the detection module is used for detecting the visual positioning mark through the cloud end;

the first optimization module is used for optimizing the pose estimation of the unmanned aerial vehicle visual odometer through the visual positioning mark;

the second optimization module is used for optimizing the pose estimation of the unmanned vehicle vision odometer through the vision positioning mark;

and the execution module is used for completing a local map construction thread and a closed loop detection thread of the ORB-SLAM framework through the cloud.

Images