WO2023104207A1 - 一种协同三维建图方法及系统 - Google Patents

一种协同三维建图方法及系统 Download PDF

Info

Publication number
WO2023104207A1
WO2023104207A1 PCT/CN2022/138183 CN2022138183W WO2023104207A1 WO 2023104207 A1 WO2023104207 A1 WO 2023104207A1 CN 2022138183 W CN2022138183 W CN 2022138183W WO 2023104207 A1 WO2023104207 A1 WO 2023104207A1
Authority
WO
WIPO (PCT)
Prior art keywords
coordinate system
visual positioning
camera
positioning mark
visual
Prior art date
Application number
PCT/CN2022/138183
Other languages
English (en)
French (fr)
Inventor
徐坤
冯时羽
李慧云
党少博
潘仲鸣
Original Assignee
深圳先进技术研究院
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 深圳先进技术研究院 filed Critical 深圳先进技术研究院
Publication of WO2023104207A1 publication Critical patent/WO2023104207A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects

Definitions

  • the invention relates to the field of collaborative three-dimensional mapping, in particular to a method and system for collaborative three-dimensional mapping.
  • the present invention provides a method and system for collaborative three-dimensional mapping, and the specific technical solutions are as follows:
  • a collaborative three-dimensional mapping method comprising:
  • the local map construction thread and the closed-loop detection thread of the ORB-SLAM framework are completed through the cloud.
  • it also includes:
  • the multi-agent body comprises a described unmanned aerial vehicle and an described unmanned vehicle, and the described unmanned aerial vehicle and the described unmanned vehicle form a centralized architecture, and the first monocular camera is equipped at the position in front of the unmanned aerial vehicle And the lens of the first monocular camera faces downward, the front position of the unmanned vehicle is equipped with a second monocular camera and the lens of the second monocular camera faces forward;
  • it also includes:
  • the environment 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 through the PnP algorithm, and the point cloud information is obtained;
  • the key frame is extracted and uploaded to the cloud.
  • the "establishing the relationship between image feature points and local point cloud maps” specifically includes:
  • the relocation is performed on the cloud platform through the information of the current frame.
  • the "detection of visual positioning marks through the cloud” specifically includes:
  • the outline edges of the quadrilaterals are decoded to identify the visual location markers.
  • the "optimizing the pose estimation of the unmanned aerial vehicle visual odometry by visual positioning marks” specifically includes:
  • the unmanned aerial vehicle loading camera coordinate system P C the unmanned aerial vehicle coordinate system PA , the visual positioning mark coordinate system P B and the world coordinate system P W , the world coordinate system P W is defined as the unmanned machine first frame;
  • the YOZ plane of the camera coordinate system PC of the drone is parallel to the YOZ plane of the drone coordinate system PA , and the origin of the drone coordinate system PA is set at the center of the drone;
  • the trajectory error is obtained, and the trajectory error is equally divided on each key frame of the drone, so that the closed-loop key frame The actual error is reduced.
  • the "calculating the relationship between the camera coordinate system P C mounted on the drone and the world coordinate system P W " specifically includes:
  • the UAV coordinate system P A is parallel to the UAV loaded camera coordinate system P C , both:
  • P A represents the coordinates of the UAV coordinate system
  • P C represents the coordinates of the UAV loaded camera coordinate system
  • is the translation vector between the UAV coordinate system PA and the UAV camera coordinate system P C representing the distance between the camera and the center of the UAV;
  • P W is the coordinates of the world coordinate system
  • P B is the coordinates of the visual positioning mark coordinate system
  • Angles ⁇ , ⁇ and ⁇ are Euler angles respectively, assuming that the rotation matrix of the world coordinate system P W to the UAV coordinate system PA is The rotation matrix of the coordinate system P B of the visual positioning mark to the coordinate system P C of the camera mounted on the drone is but:
  • the rotation relationship between the coordinate system P B of the visual positioning mark and the coordinate system P C of the camera mounted on the drone includes:
  • M represents the internal reference matrix of the camera
  • [u, v, 1] represents the coordinates of the projection of the visual positioning mark to the normalized plane
  • [XB, YB, ZB] represents the visual positioning mark in the coordinate system P B of the visual positioning mark coordinates in Represents the translation vector of the visual positioning marker coordinate system P B to the UAV-mounted camera coordinate system P C
  • Z C represents the visual positioning mark in the camera coordinate system
  • the Z-axis coordinates below are calculated using the direct linear transformation algorithm and
  • the "optimizing the pose estimation of the unmanned vehicle visual odometry by visual positioning marks” specifically includes:
  • the coordinate system define the unmanned vehicle loading camera coordinate system P C , the visual positioning mark 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 wireless The relationship between the camera coordinate system P C mounted on the man-vehicle and the coordinate system PA of the unmanned vehicle is determined;
  • SO 3 represents a three-dimensional special orthogonal group
  • t cw represents the translation error from the camera coordinate system P C of the unmanned vehicle to the world coordinate system P W
  • t bc represents the translation error from the visual positioning mark coordinate system P B to the translation error of the unmanned vehicle-mounted camera coordinate system P C
  • R 3 represents a set of bases with a dimension of 3
  • R cw represents the translation error from the unmanned vehicle-mounted camera coordinate system P C to the world coordinate system
  • the translation error of P W , R bc represents the rotation error from the visual positioning mark coordinate system P B to the camera coordinate system P C of the unmanned vehicle;
  • S cw represents the similar transformation of the visual positioning marker point from the world coordinate system P W to the camera coordinate system P C of the unmanned vehicle
  • S bc represents the transformation of the visual positioning marker point from the visual positioning marker coordinate system
  • the similar transformation of P B to the camera coordinate system P C loaded on the unmanned vehicle, s represents the unknown scale factor
  • 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
  • t bw represents the rotation matrix of the visual positioning mark point from the world coordinate system P W
  • s represents the unknown to the scale factor
  • a collaborative three-dimensional mapping system for implementing the above-mentioned collaborative three-dimensional mapping method, comprising:
  • the environment preparation module is used to collect environmental information
  • the information processing module is used to extract key frames from the obtained environmental information by using the Tracking thread design idea in the ORB-SLAM algorithm framework;
  • a detection module for detecting visual positioning marks through the cloud
  • the first optimization module is used to optimize the pose estimation of the unmanned aerial vehicle visual odometer by the visual positioning mark;
  • the second optimization module is used to optimize the pose estimation of the unmanned vehicle visual odometer through the visual positioning mark; the execution module is used to complete the local map construction thread and the closed-loop detection thread of the ORB-SLAM framework through the cloud.
  • the present invention has the following beneficial effects:
  • a collaborative 3D mapping method and system 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 a collaborative 3D map with good robustness, high precision and strong real-time performance Mapping system.
  • Fig. 1 is the schematic diagram of the imaging model of camera in the embodiment
  • Fig. 2 is a flow chart of the collaborative three-dimensional mapping method in the embodiment
  • Fig. 3 is a block diagram of the collaborative 3D mapping system in the embodiment.
  • this embodiment provides a collaborative 3D mapping method, including:
  • the local map construction thread and closed-loop detection thread of the ORB-SLAM framework are completed through the cloud.
  • the cloud executes the local map construction thread (Local Mapping thread) and loop closure detection thread (Loop Closing thread) in ORB-SLAM.
  • Cooperative SLAM Cooperative simultaneous localization and mapping, CSLAM
  • CSLAM has advantages over single robots in terms of 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 calculation and storage in the CSLAM system is large, and most individual robots cannot meet the real-time requirements.
  • CSLAM systems usually perform tasks in a large-scale environment, and the system errors (pose estimation errors, etc.) accumulated by a large number of calculations cannot be completely eliminated to a certain extent.
  • 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 landmarks are arranged in the environment so that the robot can optimize its own pose according to the landmarks, which is of great significance to improve the accuracy of mapping. Compared with the two-dimensional map, the three-dimensional map has more abundant information and can better reflect the objective existence form of the real world.
  • visual positioning and marking technology that is, road sign technology can assist camera lidar sensors to achieve more accurate positioning and mapping.
  • Cloud architecture technology can transfer complex calculations in multi-robot SLAM technology to the cloud for implementation, solving the problem of multi-robot computing and storage resources.
  • the three-dimensional plane map environment information is richer, which is more conducive to the drone's navigation and obstacle avoidance functions.
  • this embodiment selects a relatively spacious place in a large-scale unknown environment to mark road signs (AprilTag codes), unmanned aerial vehicles and unmanned vehicles are loaded with a monocular camera, and the monocular camera is used to collect the environment in real time during the multi-agent process.
  • Information use the ORB-SLAM framework for collaborative 3D mapping, and use the AprilTag code to optimize the ORB-SLAM pose estimation, and use Docker+Kubernetes+BRPC+Beego technology to build a cloud platform, and tasks with large amounts of calculation and high storage requirements Deployed on the cloud, the multi-agent side is used for tracking and relocation.
  • this embodiment combines 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.
  • "collecting environmental information” specifically includes:
  • the multi-agent includes a drone and an unmanned vehicle, and the drone and the unmanned vehicle form a centralized architecture;
  • unmanned aerial vehicles and unmanned vehicles form a centralized architecture
  • centralized architecture specifically includes:
  • the front position of the UAV is equipped with a first monocular camera with the lens of the first monocular camera facing down, and the front position of the unmanned vehicle is equipped with a second monocular camera with the lens of the second monocular camera facing forward.
  • information processing specifically includes:
  • Environmental information includes image information, and ORB-SLAM algorithm is used to extract feature points and descriptors for image information;
  • the depth is obtained through the PnP algorithm, and the point cloud information is obtained;
  • the key frame is extracted and uploaded to the cloud.
  • "establishing the relationship between image feature points and local point cloud maps” specifically includes:
  • Relocation is performed on the cloud platform through the information of the current frame.
  • detecting visual positioning marks through the cloud specifically includes:
  • the contour edges of the quadrilaterals are decoded to identify visual positioning markers, ie to identify road signs (AprilTag).
  • optically correcting the pose estimation of the UAV's visual odometer through visual positioning marks specifically includes:
  • the coordinate system define the UAV loading camera coordinate system P C , the UAV coordinate system P A , the visual positioning mark 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 ;
  • the YOZ plane of the UAV-loaded camera coordinate system P C 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;
  • the trajectory error is obtained, and the trajectory error is equally divided on each key frame of the drone, so that the closed-loop key frame The actual error is reduced.
  • calculating the relationship between the UAV-mounted camera coordinate system P C and the world coordinate system P W specifically includes:
  • the coordinate system P A of the UAV is parallel to the coordinate system P C of the camera mounted on the UAV, both of which are:
  • PA represents the coordinates of the coordinate system of the UAV
  • P C represents the coordinates of the camera coordinate system of the UAV
  • P W is the coordinates of the world coordinate system
  • P B is the coordinates of the visual positioning marker coordinate system
  • angles ⁇ , ⁇ and ⁇ are Euler angles respectively, and the rotation matrix from the world coordinate system P W to the UAV coordinate system PA is
  • the rotation matrix of visual positioning marker coordinate system P B to UAV loading camera coordinate system P C is but:
  • the visual positioning mark coordinate system P B and the UAV mounted camera coordinate system P C rotation relationship include:
  • the relationship between the UAV loading camera coordinate system P C and the world coordinate system P W includes:
  • M represents the internal reference matrix of the camera
  • [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
  • Z C represents the Z-axis coordinate of the visual positioning mark in the camera coordinate system
  • the unmanned vehicle loading camera coordinate system P C defines the unmanned vehicle loading camera coordinate system P C , the visual positioning mark 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 loading camera coordinate system
  • P C defines the unmanned vehicle loading camera coordinate system P C , the visual positioning mark 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 loading camera coordinate system
  • the relationship between P C and unmanned vehicle coordinate system PA is determined;
  • SO 3 represents the three-dimensional special orthogonal group
  • t cw represents the translation error from the unmanned vehicle loading camera coordinate system P C to the world coordinate system P W
  • t bc represents the visual positioning marker coordinate system P B to the unmanned vehicle loading
  • R 3 represents a set of bases with a dimension of 3
  • R cw represents the translation error from the unmanned vehicle loaded camera coordinate system P C to the world coordinate system P W
  • R bc represents the visual positioning
  • the Sim3 transformation algorithm is to use 3 pairs of matching points to solve the similarity transformation, and then solve 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 The similarity transformation of the unmanned vehicle loaded camera coordinate system P C
  • S bc represents the similar transformation of the visual positioning mark point from the visual positioning mark coordinate system P B to the unmanned vehicle loaded camera coordinate system P C
  • s represents the unknown scale factor;
  • 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
  • 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
  • a collaborative 3D mapping system is used to implement the above collaborative 3D mapping method, including:
  • the environment preparation module is used to collect environmental information
  • the information processing module is used to extract key frames from the obtained environmental information by 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 unmanned aerial vehicle visual odometry through the visual positioning mark;
  • the second optimization module is used to optimize the pose estimation of the unmanned vehicle visual odometry through visual positioning marks; 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.
  • the collaborative 3D mapping method and system provided in this embodiment combined with road sign AprilTag + cloud architecture + multi-robot + SLAM 3D mapping technology, realize unmanned collaborative 3D mapping, which can solve the problem of collaborative SLAM system Real-time performance is difficult to meet and the problem of inaccurate positioning of the collaborative SLAM system can be solved, and a collaborative 3D mapping system with good robustness, high precision and strong real-time performance can be realized.
  • modules in the devices in the implementation scenario can be distributed among the devices in the implementation scenario according to the description of the implementation scenario, or can be located in one or more devices different from the implementation scenario according to corresponding changes.
  • the modules of the above implementation scenarios can be combined into one module, or can be further split into multiple sub-modules.

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种协同三维建图方法及系统，包括：通过云端检测视觉定位标记；通过所述视觉定位标记优化无人机视觉里程计的位姿估计；通过所述视觉定位标记优化无人车视觉里程计的位姿估计；通过所述云端完成ORB‑SLAM框架的局部地图构建线程和闭环检测线程。本方法主要基于ORB‑SLAM框架和云端实现，由无人机与无人车自身实现ORB‑SLAM中的跟踪线程，由云端实现ORB‑SLAM中的局部地图构建线程和闭环检测线程，利用视觉定位标记优化无人机视觉里程计的位姿估计，以及利用视觉定位标记优化无人车视觉里程计的位姿估计，能够解决协同SLAM系统实时性难以满足以及解决协同SLAM系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的协同三维建图系统。

Description

一种协同三维建图方法及系统 技术领域
本发明涉及协同三维建图领域，具体是涉及一种协同三维建图方法及系统。
背景技术
现有技术中存在采用路标与单目相机传感器技术实现多机器人的三维平面建图的技术，但现有技术系统实时性差；
还存在采用路标与云架构实现单机器人的二维平面建图，但这种系统不适合大规模环境应用。
发明内容
为了克服现有技术的不足，本发明提供了一种协同三维建图方法及系统，具体技术方案如下所示：
一种协同三维建图方法，包括：
通过云端检测视觉定位标记；
通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
通过所述视觉定位标记优化无人车视觉里程计的位姿估计；
通过所述云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
在一个具体的实施例中，还包括：
采集环境信息，采用Docker作为云端容器，采用Kubernetes作为容器的调度服务，采用BRPC和Beego作为网络构架搭建云平台，使多智能体端与所述云端通讯；
多智能体包括一台所述无人机和一台所述无人车，所述无人机和所述无人车构成集中式体系结构，所述无人机前方位置装备第一单目相机且所述第一单目相机的镜头朝下，所述无人车前方位置装备第二单目相机且所述第二单目相机的镜头朝前；
选至少2处环境点，打上所述视觉定位标记。
在一个具体的实施例中，还包括：
所述环境信息包括图像信息，对所述图像信息采用ORB-SLAM算法提取特征点和描述子；
通过PnP算法求得深度，得到点云信息；
利用所述云平台进行地图初始化，若所述云平台上有地图，则将所述图像信息与所述云端的所述关键帧进行匹配确定初始位置，若所述云平台上没有地图，则将所述图像信息和所述地图等信息作为云平台系统地图的起始；
通过匹配特征点对或者重定位方法估计相机位姿；
建立图像特征点和局部点云地图间的关系；
根据所述关键帧的判断条件，提取所述关键帧上传给所述云端。
在一个具体的实施例中，所述“建立图像特征点和局部点云地图间的关系”具体包括：
当局部地图由于环境上的遮挡或纹理缺失等原理导致跟踪失败时，系统采取下列方式进行重定位：
在所述无人机或所述无人车上的局部地图里去重新定位和匹配参考帧；
通过当前帧的信息在所述云平台上进行重定位。
在一个具体的实施例中，所述“通过云端检测视觉定位标记”具体包括：
进行图像边缘检测；
筛选出四边形的轮廓边缘；
对所述四边形的轮廓边缘进行解码，识别所述视觉定位标记。
在一个具体的实施例中，所述“通过视觉定位标记优化无人机视觉里程计的位姿估计”具体包括：
定义坐标系，定义无人机装载相机坐标系P C、无人机坐标系P A、视觉定位标记坐标系P B以及世界坐标系P W，所述世界坐标系P W定义为所述无人机第一帧；
所述无人机装载相机坐标系P C的YOZ平面与所述无人机坐标系P A的YOZ平面平行，并设置所述无人机坐标系P A的原点在所述无人机中心；
计算出所述无人机装载相机坐标系P C到所述世界坐标系P W的关系；
计算出所述无人机装载相机坐标系P C与所述视觉定位标记坐标系P B的相对位姿
Figure PCTCN2022138183-appb-000001
和
Figure PCTCN2022138183-appb-000002
通过所述视觉定位标记得到的相对位姿和视觉里程计得到的相对位姿，求出轨迹误差，并将所述轨迹误差平分在所述无人机的每个关键帧上，使得闭环关键帧与实际误差减小。
在一个具体的实施例中，所述“计算出所述无人机装载相机坐标系P C到所述世界 坐标系P W的关系”具体包括：
所述无人机坐标系P A与所述无人机装载相机坐标系P C是平行关系，既有：
Figure PCTCN2022138183-appb-000003
其中，P A表示所述无人机坐标系的坐标，P C表示所述无人机装载相机坐标系的坐标，
Figure PCTCN2022138183-appb-000004
为所述无人机坐标系P A与所述无人机装载相机坐标系P C之间的平移向量，表示所述相机距离所述无人机中心的距离；
所述视觉定位标记坐标系P B与所述世界坐标系P W之间的关系满足：
Figure PCTCN2022138183-appb-000005
其中，P W为所述世界坐标系的坐标，P B为所述视觉定位标记坐标系的坐标，
Figure PCTCN2022138183-appb-000006
为所述世界坐标系P W与所述视觉定位标记坐标系P B之间的平移向量；
角φ、θ和ψ分别是欧拉角，设所述世界坐标系P W到所述无人机坐标系P A的旋转矩阵为
Figure PCTCN2022138183-appb-000007
所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的旋转矩阵为
Figure PCTCN2022138183-appb-000008
则：
Figure PCTCN2022138183-appb-000009
Figure PCTCN2022138183-appb-000010
上述c代表cos，s代表sin，根据上式可得所述视觉定位标记坐标系P B和所述无人机装载相机坐标系P C旋转关系包括：
Figure PCTCN2022138183-appb-000011
而所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的关系表示是：
Figure PCTCN2022138183-appb-000012
其中，
Figure PCTCN2022138183-appb-000013
为所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的旋转矩阵，
Figure PCTCN2022138183-appb-000014
为所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的平移向量；
则得到所述无人机装载相机坐标系P C到所述世界坐标系P W的关系包括：
Figure PCTCN2022138183-appb-000015
其中，
Figure PCTCN2022138183-appb-000016
为所述无人机坐标系P A到所述世界坐标系P W的旋转矩阵，
Figure PCTCN2022138183-appb-000017
为所述无人机坐标系P A到所述世界坐标系P W的平移向量，
Figure PCTCN2022138183-appb-000018
为所述无人机坐标系P A到所述无人机装载相机坐标系P C的平移向量。
在一个具体的实施例中，所述“计算出所述无人机装载相机坐标系P C与所述视觉定位标记坐标系P B的相对位姿
Figure PCTCN2022138183-appb-000019
和
Figure PCTCN2022138183-appb-000020
”具体包括：
使用相机模型将所述视觉定位标记投影到相机的2D像素平面，得到：
Figure PCTCN2022138183-appb-000021
其中M代表相机内参矩阵，[u,v,1]代表所述视觉定位标记投影到归一化平面的坐标，[XB,YB,ZB]代表视觉定位标记在所述视觉定位标记坐标系P B中的坐标，
Figure PCTCN2022138183-appb-000022
代表所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的平移向量，
Figure PCTCN2022138183-appb-000023
代表所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的旋转矩阵，s＝1/Z C代表未知的尺度因子，Z C代表所述视觉定位标记在相机坐标系下的Z轴坐标，采用直接线性变换算法计算得到
Figure PCTCN2022138183-appb-000024
和
Figure PCTCN2022138183-appb-000025
在一个具体的实施例中，所述“通过视觉定位标记优化无人车视觉里程计的位姿估计”具体包括：
定义坐标系，定义无人车装载相机坐标系P C、视觉定位标记坐标系P B以及世界坐标系P W，所述世界坐标系P W定义为所述无人机第一帧，所述无人车装载相机坐标系P C与所述无人车坐标系P A的关系确定；
得到所述无人车装载相机坐标系P C与所述世界坐标系P W相对位姿T cw、所述视觉定位标记坐标系P B与所述无人车装载相机坐标系P C相对位姿T bc、以及所述视觉定位标记坐标系P B与所述世界坐标系P W相对位姿T bw；
优化无人车位姿与点云坐标；
定义所述视觉定位标记坐标系P B与所述无人车装载相机坐标系P C相互间的相对误差是：
Figure PCTCN2022138183-appb-000026
构建优化目标函数：
Figure PCTCN2022138183-appb-000027
其中：
T cw∈{(R cw,t cw)|R cw∈SO 3,t cw∈R 3}T bc∈{(R bc,t bc)|R bc∈SO 3,t bc∈R 3}
其中，SO 3表示三维特殊正交群，t cw表示从所述无人车装载相机坐标系P C到所述世界坐标系P W的平移误差，t bc表示从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的平移误差，R 3表示维数为3的一组基，R cw表示从所述无人车装载相机坐标系P C到所述世界坐标系P W的平移误差，R bc表示从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的旋转误差；
相机运动不止造成旋转误差R cw、R bc以及平移误差t cw、t bc，还伴随尺度上的漂移，故进行针对尺度的变换，并采用Sim3变换算法，因此：
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
其中，S cw代表视觉定位标记点从所述世界坐标系P W到所述无人车装载相机坐标系P C的相似变换，S bc代表所述视觉定位标记点从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的相似变换，s表示未知到尺度因子；
假设优化后的Sim3姿态为
Figure PCTCN2022138183-appb-000028
那么纠正完成的姿态是：
Figure PCTCN2022138183-appb-000029
其中，R bw表示所述视觉定位标记点从所述世界坐标系P W到所述视觉定位标记坐标系P B的旋转矩阵，t bw表示所述视觉定位标记点从所述世界坐标系P W到所述视觉定位标记坐标系P B的平移，s表示未知到尺度因子，
Figure PCTCN2022138183-appb-000030
代表优化后的旋转矩阵、平移向量和尺度因子，
Figure PCTCN2022138183-appb-000031
代表优化后的相似变换；
设定无人车在优化发生前的3D位置为
Figure PCTCN2022138183-appb-000032
则可以得到变换后的坐标：
Figure PCTCN2022138183-appb-000033
其中
Figure PCTCN2022138183-appb-000034
代表所述无人车优化后的位姿。
一种协同三维建图系统，用于实现上述所述的协同三维建图方法，包括：
环境准备模块，用于采集环境信息；
信息处理模块，用于从获取的所述环境信息中，采用ORB-SLAM算法框架中的Tracking线程设计思想，提取关键帧；
检测模块，用于通过云端检测视觉定位标记；
第一优化模块，用于通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
第二优化模块，用于通过所述视觉定位标记优化无人车视觉里程计的位姿估计；执行模块，用于通过所述云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
相对于现有技术，本发明具有以下有益效果：
本发明提供的一种协同三维建图方法及系统，能够解决协同SLAM系统实时性难以满足以及解决协同SLAM系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的协同三维建图系统。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。
图1是实施例中相机的成像模型示意图；
图2是实施例中协同三维建图方法的流程步骤图；
图3是实施例中协同三维建图系统的模块图。
具体实施方式
实施例
如图1-图2所示，本实施例提供了一种协同三维建图方法，包括：
环境准备，采集环境信息；
信息处理，从获取的环境信息中，采用ORB-SLAM算法框架中的Tracking线程设计思想，提取关键帧；
通过云端检测视觉定位标记，视觉定位标记即路标；
通过视觉定位标记优化无人机视觉里程计的位姿估计；
通过视觉定位标记优化无人车视觉里程计的位姿估计；
通过云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
具体地，云端执行ORB-SLAM中的局部地图构建线程(Local Mapping线程)和闭环检测线程(Loop Closing线程)。协同SLAM(Cooperative simultaneous localization and mapping，CSLAM)在容错性、鲁棒性和执行效率上比单机器人更有优势，在未知环境下的灾难救援、资源探测和空间探测等任务中具有重要影响力。CSLAM系统中的数据计算存储量大，大多数机器人个体无法满足实时性要求。CSLAM系统通常在大规模环境中执行任务，大量的计算所累计的系统误差(位姿估计误差等)在一定程度上不能完全消除。况且当环境中存在大量重复性地貌时，特征点匹配或重叠区域匹配算法可能出现一定程度的误匹配。累计的系统误差和误匹配都会影响CSLAM系统建图精度，因此在环境中布置少量路标，使机器人能够根据路标优化自身位姿，对建图精度提高具有重要意义。三维地图与二维地图相比，信息量更丰富，更能反映真实世界的客观存在形式。
具体地，视觉定位标记技术，即路标技术能够辅助相机激光雷达传感器实现更精准的定位与建图，云架构技术能够将多机器人SLAM技术中的复杂运算转移到云端实现，解决多机器人计算存储资源有限的问题，三维平面的地图环境信息更丰富，更有利于无人机实现导航避障等功能。
优选地，本实施例在大规模未知环境中选择较为宽敞的地方打上路标(AprilTag码)，无人机和无人车装载上单目相机，多智能体行进过程中利用单目相机实时采集环境信息，利用ORB-SLAM框架进行协同三维建图，并利用AprilTag码对ORB-SLAM位姿估计进行优化，利用Docker+Kubernetes+BRPC+Beego技术搭建云平台，将计算量大、存储要求高的任务部署在云端，多智能体端用于跟踪和重定位。
优选地，本实施例结合路标AprilTag+云架构+多机器人+SLAM三维建图技术，实现无人协同三维建图，能够解决协同SLAM系统实时性难以满足以及解决协同SLAM系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的无人协同三维建图系统。
本实施例中，“采集环境信息”具体包括：
采用Docker+Kubernetes+BRPC+Beego技术搭建云平台，使多智能体端与云端通讯，具体地，采用Docker作为云端容器，采用Kubernetes作为容器的调度服务， 采用BRPC和Beego作为网络构架搭建云平台，使多智能体端与云端通讯；
多智能体包括一台无人机和一台无人车，无人机和无人车构成集中式体系结构；
选至少2处环境点，打上视觉定位标记，即打上AprilTag码。
本实施例中，“无人机和无人车构成集中式体系结构”具体包括：
无人机前方位置装备第一单目相机且第一单目相机的镜头朝下，无人车前方位置装备第二单目相机且第二单目相机的镜头朝前。
本实施例中，“信息处理”具体包括：
环境信息包括图像信息，对图像信息采用ORB-SLAM算法提取特征点和描述子；
通过PnP算法求得深度，得到点云信息；
利用云平台进行地图初始化，若云平台上有地图，则将图像信息与云端的关键帧进行匹配确定初始位置，若云平台上没有地图，则将图像信息和地图等信息作为云平台系统地图的起始；
通过匹配特征点对或者重定位方法估计相机位姿；
建立图像特征点和局部点云地图间的关系；
根据关键帧的判断条件，提取关键帧上传给云端。
本实施例中，“建立图像特征点和局部点云地图间的关系”具体包括：
当局部地图由于环境上的遮挡或纹理缺失等原理导致跟踪失败时，系统采取下列方式进行重定位：
在无人机或无人车上的局部地图里去重新定位和匹配参考帧；
通过当前帧的信息在云平台上进行重定位。
本实施例中，“通过云端检测视觉定位标记”具体包括：
进行图像边缘检测；
筛选出四边形的轮廓边缘；
对四边形的轮廓边缘进行解码，识别视觉定位标记，即识别路标(AprilTag)。
本实施例中，“通过视觉定位标记优化无人机视觉里程计的位姿估计”具体包括：
定义坐标系，定义无人机装载相机坐标系P C、无人机坐标系P A、视觉定位标记坐标系P B以及世界坐标系P W，世界坐标系P W定义为无人机第一帧；
无人机装载相机坐标系P C的YOZ平面与无人机坐标系P A的YOZ平面平行，并设置无人机坐标系P A的原点在无人机中心；
计算出无人机装载相机坐标系P C到世界坐标系P W的关系；
计算出无人机装载相机坐标系P C与视觉定位标记坐标系P B的相对位姿
Figure PCTCN2022138183-appb-000035
和
Figure PCTCN2022138183-appb-000036
通过视觉定位标记即通过路标(AprilTag)得到的相对位姿和视觉里程计得到的相对位姿，求出轨迹误差，并将轨迹误差平分在无人机的每个关键帧上，使得闭环关键帧与实际误差减小。
本实施例中，“计算出无人机装载相机坐标系P C到世界坐标系P W的关系”具体包括：
无人机坐标系P A与无人机装载相机坐标系P C是平行关系，既有：
Figure PCTCN2022138183-appb-000037
其中，P A表示无人机坐标系的坐标，P C表示无人机装载相机坐标系的坐标，
Figure PCTCN2022138183-appb-000038
为无人机坐标系P A与无人机装载相机坐标系P C之间的平移向量，表示相机距离无人机中心的距离；
视觉定位标记坐标系P B与世界坐标系P W之间的关系满足：
Figure PCTCN2022138183-appb-000039
其中，P W为世界坐标系的坐标，P B为视觉定位标记坐标系的坐标，
Figure PCTCN2022138183-appb-000040
为世界坐标系P W与视觉定位标记坐标系P B之间的平移向量；
角φ、θ和ψ分别是欧拉角，设世界坐标系P W到无人机坐标系P A的旋转矩阵为
Figure PCTCN2022138183-appb-000041
视觉定位标记坐标系P B到无人机装载相机坐标系P C的旋转矩阵为
Figure PCTCN2022138183-appb-000042
则：
Figure PCTCN2022138183-appb-000043
Figure PCTCN2022138183-appb-000044
上述c代表cos，s代表sin，根据上式可得视觉定位标记坐标系P B和无人机装载相机坐标系P C旋转关系包括：
Figure PCTCN2022138183-appb-000045
而无人机装载相机坐标系P C到视觉定位标记坐标系P B的关系表示是：
Figure PCTCN2022138183-appb-000046
其中，
Figure PCTCN2022138183-appb-000047
为无人机装载相机坐标系P C到视觉定位标记坐标系P B的旋转矩阵，
Figure PCTCN2022138183-appb-000048
为无人机装载相机坐标系P C到视觉定位标记坐标系P B的平移向量；
则得到无人机装载相机坐标系P C到世界坐标系P W的关系包括：
Figure PCTCN2022138183-appb-000049
其中，
Figure PCTCN2022138183-appb-000050
为无人机坐标系P A到世界坐标系P W的旋转矩阵，
Figure PCTCN2022138183-appb-000051
为无人机坐标系P A到世界坐标系P W的平移向量，
Figure PCTCN2022138183-appb-000052
为无人机坐标系P A到无人机装载相机坐标系P C的平移向量。其中
Figure PCTCN2022138183-appb-000053
和
Figure PCTCN2022138183-appb-000054
未知。
本实施例中，“计算出无人机装载相机坐标系P C与视觉定位标记坐标系P B的相对位姿
Figure PCTCN2022138183-appb-000055
和
Figure PCTCN2022138183-appb-000056
”具体包括：
使用相机模型将视觉定位标记投影到相机的2D像素平面，得到：
Figure PCTCN2022138183-appb-000057
其中M代表相机内参矩阵，[u,v,1]代表视觉定位标记投影到归一化平面的坐标，[XB,YB,ZB]代表视觉定位标记在视觉定位标记坐标系P B中的坐标，
Figure PCTCN2022138183-appb-000058
代表视觉定位标记坐标系P B到无人机装载相机坐标系P C的平移向量，
Figure PCTCN2022138183-appb-000059
代表视觉定位标记坐标系P B到无人机装载相机坐标系P C的旋转矩阵，s＝1/Z C代表未知的尺度因子，Z C代表视觉定位标记在相机坐标系下的Z轴坐标，采用DLT(Direct Linear Transform,直接线性变换)算法计算得到
Figure PCTCN2022138183-appb-000060
和
Figure PCTCN2022138183-appb-000061
本实施例中，“通过视觉定位标记优化无人车视觉里程计的位姿估计”具体包括：
定义坐标系，定义无人车装载相机坐标系P C、视觉定位标记坐标系P B以及世界坐标系P W，世界坐标系P W定义为无人机第一帧，无人车装载相机坐标系P C与无人车坐标系P A的关系确定；
得到无人车装载相机坐标系P C与世界坐标系P W相对位姿T cw、视觉定位标记坐标系P B与无人车装载相机坐标系P C相对位姿T bc、以及视觉定位标记坐标系P B与世界坐 标系P W相对位姿T bw；
优化无人车位姿与点云坐标；
定义视觉定位标记坐标系P B与无人车装载相机坐标系P C相互间的相对误差是：
Figure PCTCN2022138183-appb-000062
构建优化目标函数：
Figure PCTCN2022138183-appb-000063
其中：
T cw∈{(R cw,t cw)|R cw∈SO 3,t cw∈R 3}T bc∈{(R bc,t bc)|R bc∈SO 3,t bc∈R 3}
其中，SO 3表示三维特殊正交群，t cw表示从无人车装载相机坐标系P C到世界坐标系P W的平移误差，t bc表示从视觉定位标记坐标系P B到无人车装载相机坐标系P C的平移误差，R 3表示维数为3的一组基，R cw表示从无人车装载相机坐标系P C到世界坐标系P W的平移误差，R bc表示从视觉定位标记坐标系P B到无人车装载相机坐标系P C的旋转误差；
相机运动不止造成旋转误差R cw、R bc以及平移误差t cw、t bc，还伴随尺度上的漂移，故进行针对尺度的变换，并采用Sim3变换算法，因此：
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
其中，Sim3变换算法就是使用3对匹配点来进行相似变换的求解，进而解出两个坐标系之间的旋转矩阵、平移向量和尺度；S cw代表视觉定位标记点从世界坐标系P W到无人车装载相机坐标系P C的相似变换，S bc代表视觉定位标记点从视觉定位标记坐标系P B到无人车装载相机坐标系P C的相似变换，s表示未知到尺度因子；
假设优化后的Sim3姿态为
Figure PCTCN2022138183-appb-000064
那么纠正完成的姿态是：
Figure PCTCN2022138183-appb-000065
其中，R bw表示视觉定位标记点从世界坐标系P W到视觉定位标记坐标系P B的旋转矩阵，t bw表示视觉定位标记点从世界坐标系P W到视觉定位标记坐标系P B的平移，s表示未知到尺度因子，
Figure PCTCN2022138183-appb-000066
代表优化后的旋转矩阵、平移向量和尺度因子，
Figure PCTCN2022138183-appb-000067
代表优化后 的相似变换；
设定无人车在优化发生前的3D位置为
Figure PCTCN2022138183-appb-000068
则可以得到变换后的坐标：
Figure PCTCN2022138183-appb-000069
其中
Figure PCTCN2022138183-appb-000070
代表无人车优化后的位姿。
如图3所示，一种协同三维建图系统，用于实现上述的协同三维建图方法，包括：
环境准备模块，用于采集环境信息；
信息处理模块，用于从获取的环境信息中，采用ORB-SLAM算法框架中的Tracking线程设计思想，提取关键帧；
检测模块，用于通过云端检测视觉定位标记，即检测路标(AprilTag)；
第一优化模块，用于通过视觉定位标记优化无人机视觉里程计的位姿估计；
第二优化模块，用于通过视觉定位标记优化无人车视觉里程计的位姿估计；执行模块，用于通过云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
与现有技术相比，本实施例提供的一种协同三维建图方法及系统，结合路标AprilTag+云架构+多机器人+SLAM三维建图技术，实现无人协同三维建图，能够解决协同SLAM系统实时性难以满足以及解决协同SLAM系统定位不准的问题，能够实现鲁棒性好、精度高和实时性强的协同三维建图系统。
本领域技术人员可以理解附图只是一个优选实施场景的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
本领域技术人员可以理解实施场景中的装置中的模块可以按照实施场景描述进行分布于实施场景的装置中，也可以进行相应变化位于不同于本实施场景的一个或多个装置中。上述实施场景的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
上述本发明序号仅仅为了描述，不代表实施场景的优劣。
以上公开的仅为本发明的几个具体实施场景，但是，本发明并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (10)

  1. 一种协同三维建图方法，其特征在于，包括：
    通过云端检测视觉定位标记；
    通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
    通过所述视觉定位标记优化无人车视觉里程计的位姿估计；
    通过所述云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
  2. 根据权利要求1所述的协同三维建图方法，其特征在于，还包括：
    采集环境信息，采用Docker作为云端容器，采用Kubernetes作为容器的调度服务，采用BRPC和Beego作为网络构架搭建云平台，使多智能体端与所述云端通讯；
    多智能体包括一台所述无人机和一台所述无人车，所述无人机和所述无人车构成集中式体系结构，所述无人机前方位置装备第一单目相机且所述第一单目相机的镜头朝下，所述无人车前方位置装备第二单目相机且所述第二单目相机的镜头朝前；
    选至少2处环境点，打上所述视觉定位标记。
  3. 根据权利要求2所述的协同三维建图方法，其特征在于，还包括：
    所述环境信息包括图像信息，对所述图像信息采用ORB-SLAM算法提取特征点和描述子；
    通过PnP算法求得深度，得到点云信息；
    利用所述云平台进行地图初始化，若所述云平台上有地图，则将所述图像信息与所述云端的所述关键帧进行匹配确定初始位置，若所述云平台上没有地图，则将所述图像信息和所述地图等信息作为云平台系统地图的起始；
    通过匹配特征点对或者重定位方法估计相机位姿；
    建立图像特征点和局部点云地图间的关系；
    根据所述关键帧的判断条件，提取所述关键帧上传给所述云端。
  4. 根据权利要求3所述的协同三维建图方法，其特征在于，所述“建立图像特征点和局部点云地图间的关系”具体包括：
    当局部地图由于环境上的遮挡或纹理缺失等原理导致跟踪失败时，系统采取下列方式进行重定位：
    在所述无人机或所述无人车上的局部地图里去重新定位和匹配参考帧；
    通过当前帧的信息在所述云平台上进行重定位。
  5. 根据权利要求1所述的协同三维建图方法，其特征在于，所述“通过云端检测视觉定位标记”具体包括：
    进行图像边缘检测；
    筛选出四边形的轮廓边缘；
    对所述四边形的轮廓边缘进行解码，识别所述视觉定位标记。
  6. 根据权利要求1所述的协同三维建图方法，其特征在于，所述“通过视觉定位标记优化无人机视觉里程计的位姿估计”具体包括：
    定义坐标系，定义无人机装载相机坐标系P C、无人机坐标系P A、视觉定位标记坐标系P B以及世界坐标系P W，所述世界坐标系P W定义为所述无人机第一帧；
    所述无人机装载相机坐标系P C的YOZ平面与所述无人机坐标系P A的YOZ平面平行，并设置所述无人机坐标系P A的原点在所述无人机中心；
    计算出所述无人机装载相机坐标系P C到所述世界坐标系P W的关系；
    计算出所述无人机装载相机坐标系P C与所述视觉定位标记坐标系P B的相对位姿
    Figure PCTCN2022138183-appb-100001
    和
    Figure PCTCN2022138183-appb-100002
    通过所述视觉定位标记得到的相对位姿和视觉里程计得到的相对位姿，求出轨迹误差，并将所述轨迹误差平分在所述无人机的每个关键帧上，使得闭环关键帧与实际误差减小。
  7. 根据权利要求6所述的协同三维建图方法，其特征在于，所述“计算出所述无人机装载相机坐标系P C到所述世界坐标系P W的关系”具体包括：
    所述无人机坐标系P A与所述无人机装载相机坐标系P C是平行关系，既有：
    Figure PCTCN2022138183-appb-100003
    其中，P A表示所述无人机坐标系的坐标，P C表示所述无人机装载相机坐标系的坐标，
    Figure PCTCN2022138183-appb-100004
    为所述无人机坐标系P A与所述无人机装载相机坐标系P C之间的平移向量，表示所述相机距离所述无人机中心的距离；
    所述视觉定位标记坐标系P B与所述世界坐标系P W之间的关系满足：
    Figure PCTCN2022138183-appb-100005
    其中，P W为所述世界坐标系的坐标，P B为所述视觉定位标记坐标系的坐标，
    Figure PCTCN2022138183-appb-100006
    为所述世界坐标系P W与所述视觉定位标记坐标系P B之间的平移向量；
    角φ、θ和ψ分别是欧拉角，设所述世界坐标系P W到所述无人机坐标系P A的旋转矩阵为
    Figure PCTCN2022138183-appb-100007
    所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的旋转矩阵为
    Figure PCTCN2022138183-appb-100008
    则：
    Figure PCTCN2022138183-appb-100009
    Figure PCTCN2022138183-appb-100010
    上述c代表cos，s代表sin，根据上式可得所述视觉定位标记坐标系P B和所述无人机装载相机坐标系P C旋转关系包括：
    Figure PCTCN2022138183-appb-100011
    而所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的关系表示是：
    Figure PCTCN2022138183-appb-100012
    其中，
    Figure PCTCN2022138183-appb-100013
    为所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的旋转矩阵，
    Figure PCTCN2022138183-appb-100014
    为所述无人机装载相机坐标系P C到所述视觉定位标记坐标系P B的平移向量；
    则得到所述无人机装载相机坐标系P C到所述世界坐标系P W的关系包括：
    Figure PCTCN2022138183-appb-100015
    其中，
    Figure PCTCN2022138183-appb-100016
    为所述无人机坐标系P A到所述世界坐标系P W的旋转矩阵，
    Figure PCTCN2022138183-appb-100017
    为所述无人机坐标系P A到所述世界坐标系P W的平移向量，
    Figure PCTCN2022138183-appb-100018
    为所述无人机坐标系P A到所述无人机装载相机坐标系P C的平移向量。
  8. 根据权利要求6所述的协同三维建图方法，其特征在于，所述“计算出所述无人机装载相机坐标系P C与所述视觉定位标记坐标系P B的相对位姿
    Figure PCTCN2022138183-appb-100019
    和
    Figure PCTCN2022138183-appb-100020
    ”具体包括：
    使用相机模型将所述视觉定位标记投影到相机的2D像素平面，得到：
    Figure PCTCN2022138183-appb-100021
    其中M代表相机内参矩阵，[u,v,1]代表所述视觉定位标记投影到归一化平面的坐标，[XB,YB,ZB]代表视觉定位标记在所述视觉定位标记坐标系P B中的坐标，
    Figure PCTCN2022138183-appb-100022
    代表所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的平移向量，
    Figure PCTCN2022138183-appb-100023
    代表所述视觉定位标记坐标系P B到所述无人机装载相机坐标系P C的旋转矩阵，s＝1/Z C代表未知的尺度因子，Z C代表所述视觉定位标记在相机坐标系下的Z轴坐标，采用直接线性变换算法计算得到
    Figure PCTCN2022138183-appb-100024
    和
    Figure PCTCN2022138183-appb-100025
  9. 根据权利要求1所述的协同三维建图方法，其特征在于，所述“通过视觉定位标记优化无人车视觉里程计的位姿估计”具体包括：
    定义坐标系，定义无人车装载相机坐标系P C、视觉定位标记坐标系P B以及世界坐标系P W，所述世界坐标系P W定义为所述无人机第一帧，所述无人车装载相机坐标系P C与所述无人车坐标系P A的关系确定；
    得到所述无人车装载相机坐标系P C与所述世界坐标系P W相对位姿T cw、所述视觉定位标记坐标系P B与所述无人车装载相机坐标系P C相对位姿T bc、以及所述视觉定位标记坐标系P B与所述世界坐标系P W相对位姿T bw；
    优化无人车位姿与点云坐标；
    定义所述视觉定位标记坐标系P B与所述无人车装载相机坐标系P C相互间的相对误差是：
    Figure PCTCN2022138183-appb-100026
    构建优化目标函数：
    Figure PCTCN2022138183-appb-100027
    其中：
    T cw∈{(R cw,t cw)|R cw∈SO 3,t cw∈R 3}  T bc∈{(R bc,t bc)|R bc∈SO 3,t bc∈R 3}
    其中，SO 3表示三维特殊正交群，t cw表示从所述无人车装载相机坐标系P C到所述世界坐标系P W的平移误差，t bc表示从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的平移误差，R 3表示维数为3的一组基，R cw表示从所述无人车装载相机坐标系P C到所述世界坐标系P W的平移误差，R bc表示从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的旋转误差；
    相机运动不止造成旋转误差R cw、R bc以及平移误差t cw、t bc，还伴随尺度上的漂移，故进行针对尺度的变换，并采用Sim3变换算法，因此：
    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
    其中，S cw代表视觉定位标记点从所述世界坐标系P W到所述无人车装载相机坐标系P C的相似变换，S bc代表所述视觉定位标记点从所述视觉定位标记坐标系P B到所述无人车装载相机坐标系P C的相似变换，s表示未知到尺度因子；
    假设优化后的Sim3姿态为
    Figure PCTCN2022138183-appb-100028
    那么纠正完成的姿态是：
    Figure PCTCN2022138183-appb-100029
    其中，R bw表示所述视觉定位标记点从所述世界坐标系P W到所述视觉定位标记坐标系P B的旋转矩阵，t bw表示所述视觉定位标记点从所述世界坐标系P W到所述视觉定位标记坐标系P B的平移，s表示未知到尺度因子，
    Figure PCTCN2022138183-appb-100030
    代表优化后的旋转矩阵、平移向量和尺度因子，
    Figure PCTCN2022138183-appb-100031
    代表优化后的相似变换；
    设定无人车在优化发生前的3D位置为
    Figure PCTCN2022138183-appb-100032
    则可以得到变换后的坐标：
    Figure PCTCN2022138183-appb-100033
    其中
    Figure PCTCN2022138183-appb-100034
    代表所述无人车优化后的位姿。
  10. 一种协同三维建图系统，用于实现上述权利要求1-9任一项所述的协同三维建图方法，其特征在于，包括：
    环境准备模块，用于采集环境信息；
    信息处理模块，用于从获取的所述环境信息中，采用ORB-SLAM算法框架中的Tracking线程设计思想，提取关键帧；
    检测模块，用于通过云端检测视觉定位标记；
    第一优化模块，用于通过所述视觉定位标记优化无人机视觉里程计的位姿估计；
    第二优化模块，用于通过所述视觉定位标记优化无人车视觉里程计的位姿估计；
    执行模块，用于通过所述云端完成ORB-SLAM框架的局部地图构建线程和闭环检测线程。
PCT/CN2022/138183 2021-12-10 2022-12-09 一种协同三维建图方法及系统 WO2023104207A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111510369.3 2021-12-10
CN202111510369.3A CN114332360A (zh) 2021-12-10 2021-12-10 一种协同三维建图方法及系统

Publications (1)

Publication Number Publication Date
WO2023104207A1 true WO2023104207A1 (zh) 2023-06-15

Family

ID=81051491

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/138183 WO2023104207A1 (zh) 2021-12-10 2022-12-09 一种协同三维建图方法及系统

Country Status (2)

Country Link
CN (1) CN114332360A (zh)
WO (1) WO2023104207A1 (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116934829A (zh) * 2023-09-15 2023-10-24 天津云圣智能科技有限责任公司 无人机目标深度估计的方法、装置、存储介质及电子设备
CN117058209A (zh) * 2023-10-11 2023-11-14 山东欧龙电子科技有限公司 一种基于三维地图的飞行汽车视觉图像深度信息计算方法
CN117893693A (zh) * 2024-03-15 2024-04-16 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117906595A (zh) * 2024-03-20 2024-04-19 常熟理工学院 基于特征点法视觉slam的场景理解导航方法及系统
CN118010008A (zh) * 2024-04-08 2024-05-10 西北工业大学 基于双目slam和机间回环优化双无人机协同定位方法
CN118031976A (zh) * 2024-04-15 2024-05-14 中国科学院国家空间科学中心 一种探索未知环境的人机协同系统

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114332360A (zh) * 2021-12-10 2022-04-12 深圳先进技术研究院 一种协同三维建图方法及系统
CN115965673B (zh) * 2022-11-23 2023-09-12 中国建筑一局(集团)有限公司 基于双目视觉的集中式多机器人定位方法
CN115965758B (zh) * 2022-12-28 2023-07-28 无锡东如科技有限公司 一种图协同单目实例三维重建方法
CN116228870B (zh) * 2023-05-05 2023-07-28 山东省国土测绘院 一种基于二维码slam精度控制的建图方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN110221623A (zh) * 2019-06-17 2019-09-10 酷黑科技（北京）有限公司 一种空地协同作业系统及其定位方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN112115874A (zh) * 2020-09-21 2020-12-22 武汉大学 一种融合云端的视觉slam系统及方法
CN114332360A (zh) * 2021-12-10 2022-04-12 深圳先进技术研究院 一种协同三维建图方法及系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN110221623A (zh) * 2019-06-17 2019-09-10 酷黑科技（北京）有限公司 一种空地协同作业系统及其定位方法
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN112115874A (zh) * 2020-09-21 2020-12-22 武汉大学 一种融合云端的视觉slam系统及方法
CN114332360A (zh) * 2021-12-10 2022-04-12 深圳先进技术研究院 一种协同三维建图方法及系统

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116934829A (zh) * 2023-09-15 2023-10-24 天津云圣智能科技有限责任公司 无人机目标深度估计的方法、装置、存储介质及电子设备
CN116934829B (zh) * 2023-09-15 2023-12-12 天津云圣智能科技有限责任公司 无人机目标深度估计的方法、装置、存储介质及电子设备
CN117058209A (zh) * 2023-10-11 2023-11-14 山东欧龙电子科技有限公司 一种基于三维地图的飞行汽车视觉图像深度信息计算方法
CN117058209B (zh) * 2023-10-11 2024-01-23 山东欧龙电子科技有限公司 一种基于三维地图的飞行汽车视觉图像深度信息计算方法
CN117893693A (zh) * 2024-03-15 2024-04-16 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117893693B (zh) * 2024-03-15 2024-05-28 南昌航空大学 一种密集slam三维场景重建方法及装置
CN117906595A (zh) * 2024-03-20 2024-04-19 常熟理工学院 基于特征点法视觉slam的场景理解导航方法及系统
CN118010008A (zh) * 2024-04-08 2024-05-10 西北工业大学 基于双目slam和机间回环优化双无人机协同定位方法
CN118010008B (zh) * 2024-04-08 2024-06-07 西北工业大学 基于双目slam和机间回环优化双无人机协同定位方法
CN118031976A (zh) * 2024-04-15 2024-05-14 中国科学院国家空间科学中心 一种探索未知环境的人机协同系统

Also Published As

Publication number Publication date
CN114332360A (zh) 2022-04-12

Similar Documents

Publication Publication Date Title
WO2023104207A1 (zh) 一种协同三维建图方法及系统
Yang et al. Cubeslam: Monocular 3-d object slam
CN109100730B (zh) 一种多车协同快速建图方法
Seok et al. Rovo: Robust omnidirectional visual odometry for wide-baseline wide-fov camera systems
Ranganathan et al. Light-weight localization for vehicles using road markings
CN112734852A (zh) 一种机器人建图方法、装置及计算设备
CN109615698A (zh) 基于互回环检测的多无人机slam地图融合算法
Chen et al. Vision-based autonomous vehicle guidance for indoor security patrolling by a SIFT-based vehicle-localization technique
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN106556395A (zh) 一种基于四元数的单目视觉系统的导航方法
Zhao et al. RTSfM: Real-time structure from motion for mosaicing and DSM mapping of sequential aerial images with low overlap
CN111812978B (zh) 一种多无人机协作slam方法与系统
CN109871024A (zh) 一种基于轻量级视觉里程计的无人机位姿估计方法
CN116989772B (zh) 一种空地多模态多智能体协同定位与建图方法
Lin et al. PVO: Panoramic visual odometry
Ye et al. Robust and efficient vehicles motion estimation with low-cost multi-camera and odometer-gyroscope
Roozing et al. Low-cost vision-based 6-DOF MAV localization using IR beacons
Xue et al. Visual-marker based localization for flat-variation scene
Wang et al. Monocular visual-inertial localization in a point cloud map using feature-to-distribution registration
Fang et al. Marker-based mapping and localization for autonomous valet parking
CN113791423A (zh) 一种基于多传感器融合的机器人重定位方法
Ye et al. Crossview mapping with graph-based geolocalization on city-scale street maps
Han et al. A robust stereo camera localization method with prior LiDAR map constrains
Zhi et al. Key technology of mine underground mobile positioning based on LiDAR and coded sequence pattern

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22903624

Country of ref document: EP

Kind code of ref document: A1