CN110580740B - 多智能体协同三维建模方法及装置 - Google Patents

多智能体协同三维建模方法及装置 Download PDF

Info

Publication number
CN110580740B
CN110580740B CN201910798522.3A CN201910798522A CN110580740B CN 110580740 B CN110580740 B CN 110580740B CN 201910798522 A CN201910798522 A CN 201910798522A CN 110580740 B CN110580740 B CN 110580740B
Authority
CN
China
Prior art keywords
local
constraint
dimensional
global
dimensional model
Prior art date
Legal status (The legal status 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 status listed.)
Active
Application number
CN201910798522.3A
Other languages
English (en)
Other versions
CN110580740A (zh
Inventor
李一鹏
张翔
林文镔
戴琼海
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN201910798522.3A priority Critical patent/CN110580740B/zh
Publication of CN110580740A publication Critical patent/CN110580740A/zh
Application granted granted Critical
Publication of CN110580740B publication Critical patent/CN110580740B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2200/00—Indexing scheme for image data processing or generation, in general
    • G06T2200/08—Indexing scheme for image data processing or generation, in general involving all processing steps from image acquisition to 3D model generation

Abstract

本发明公开了一种多智能体协同三维建模方法及装置，其中，该方法包括：通过SLAM算法生成多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息；通过RPC协议将多个局部三维模型和多个轨迹节点信息发送至云端；根据多个局部三维模型和多个轨迹节点信息在云端构建全局约束；根据全局约束对多个局部三维模型进行融合生成全局三维模型。该方法利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。

Description

多智能体协同三维建模方法及装置
技术领域
本发明涉及智能体自主定位与建图技术领域，特别涉及一种多智能体协同三维建模方法及装置。
背景技术
随着计算机视觉、人工智能以及控制技术的快速发展，以无人机、无人车、自动驾驶汽车等为代表的智能体逐渐在国民经济建设和国家安全保障方面发挥巨大作用。
异质异构的智能体在执行任务的过程中，经常会受到障碍物、复杂电磁环境以及恶劣天气的影响，导致其丧失GPS定位信息和远程控制信号，影响其作业安全，急需复杂自然场景下全自主的定位、建图和导航技术；此外，大范围应用场景下，需要多个智能体同时工作，这就对执行任务的多个智能体集群提出了协同定位与建图的技术需求。
如何利用现有的自主定位与建图技术，构建普适的多智能体协同定位与建图的系统与方法，是学术界和工业界非常关注的科学问题和工程难题。核心关键技术的突破将会极大拓展异质异构智能体在复杂大范围自然场景下的应用模式，发挥各自的工作优势，提高任务完成的效率。
发明内容
本发明旨在至少在一定程度上解决相关技术中的技术问题之一。
为此，本发明的一个目的在于提出一种多智能体协同三维建模方法，该方法利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。
本发明的另一个目的在于提出一种多智能体协同三维建模装置。
为达到上述目的，本发明一方面实施例提出了一种多智能体协同三维建模方法，包括：
通过SLAM算法生成所述多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息；
通过RPC协议将所述多个局部三维模型和所述多个轨迹节点信息发送至云端；
根据所述多个局部三维模型和所述多个轨迹节点信息在云端构建全局约束；
根据所述全局约束对所述多个局部三维模型进行融合生成全局三维模型。
本发明实施例的多智能体协同三维建模方法，通过智能体端使用SLAM算法实现局部环境的三维模型重建；智能体端通过RPC协议将局部三维模型及轨迹节点信息上传至云端；云端接收局部三维模型信息，构建全局约束；云端基于全局约束融合多个局部模型，重建大范围场景的三维模型。由此，利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。
另外，根据本发明上述实施例的多智能体协同三维建模方法还可以具有以下附加的技术特征：
进一步地，在本发明的一个实施例中，所述多个智能体包括自动驾驶汽车、自动导引运输车、自主式水下航行器和无人机中的一种或多种。
进一步地，在本发明的一个实施例中，所述轨迹节点信息包括：
所述多个智能体的在所述多个局部三维模型中的位置、姿态信息、环境特征信息、激光点云信息和激光点云分布直方图。
进一步地，在本发明的一个实施例中，在构建所述全局约束中，通过所述激光点云分布直方图进行航偏角匹配，并使用分支定界的方式进行三维方向的匹配。
进一步地，在本发明的一个实施例中，所述多个局部三维模型包括所述多个智能体在局部范围执行所述SLAM算法生成的地图信息。
进一步地，在本发明的一个实施例中，对所述多个局部三维模型和所述多个轨迹节点信息进行粗匹配和精匹配。
进一步地，在本发明的一个实施例中，根据每个局部三维模型中的轨迹节点个数阈值对每个局部三维模型进行构建。
进一步地，在本发明的一个实施例中，通过构建滑动窗口生成所述多个局部三维模型。
为达到上述目的，本发明另一方面实施例提出了一种多智能体协同三维建模装置，包括：
第一生成模块，用于通过SLAM算法生成所述多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息；
发送模块，用于通过RPC协议将所述多个局部三维模型和所述多个轨迹节点信息发送至云端；
构建模块，用于根据所述多个局部三维模型和所述多个轨迹节点信息在云端构建全局约束；
第二生成模块，用于根据所述全局约束对所述多个局部三维模型进行融合生成全局三维模型。
本发明实施例的多智能体协同三维建模装置，通过智能体端使用SLAM算法实现局部环境的三维模型重建；智能体端通过RPC协议将局部三维模型及轨迹节点信息上传至云端；云端接收局部三维模型信息，构建全局约束；云端基于全局约束融合多个局部模型，重建大范围场景的三维模型。由此，利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。
另外，根据本发明上述实施例的多智能体协同三维建模装置还可以具有以下附加的技术特征：
进一步地，在本发明的一个实施例中，所述轨迹节点信息包括：
所述多个智能体的在所述多个局部三维模型中的位置、姿态信息、环境特征信息、激光点云信息和激光点云分布直方图。
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为根据本发明一个实施例的多智能体协同三维建模方法流程图；
图2为根据本发明一个实施例的多智能体协同三维建模方法流程框图；
图3为根据本发明一个实施例的智能体端点云分布直方图构建示意图；
图4为根据本发明一个实施例的云端点云分布直方图匹配示意图；
图5为根据本发明一个实施例的多线激光与IMU数据的同步触发系统示意图；
图6为根据本发明一个实施例的大范围室内场景三维模型重建结果示意图；
图7为根据本发明一个实施例的多智能体协同三维建模装置结构示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
下面参照附图描述根据本发明实施例提出的多智能体协同三维建模方法及装置。
首先将参照附图描述根据本发明实施例提出的多智能体协同三维建模方法。
图1为根据本发明一个实施例的多智能体协同三维建模方法流程图。
如图1和图2所示，该多智能体协同三维建模方法包括以下步骤：
在步骤S101中，通过SLAM算法生成多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息。
进一步地，多个智能体包括但不限于自动驾驶汽车、自动导引运输车(AutomatedGuided Vehicle，简称AGV)、自主式水下航行器(Autonomous Underwater Vehicle，简称AUV)和无人机(Unmanned Aerial Vehicle，简称UAV)等移动平台。
各个智能体端独立运行选定的同步定位与建图(Simultaneous LocalizationAnd Mapping，简称SLAM)算法，获得其所在场景下的局部环境三模模型和轨迹节点信息。
进一步地，局部环境三模模型是各个智能体在局部范围内执行SLAM算法所获得的地图信息。智能体利用传感器获得地图信息，具体指利用多线激光传感器获取的稠密点云数据构建的三维栅格地图，根据所使用的传感器如单线/多线激光传感器、单目/多目可见光传感器和RGBD传感器等以及所选用的SLAM算法不同，局部地图可以是包含概率信息的栅格地图、稀疏/半稠密/稠密的点云地图。
进一步地，轨迹节点包含了该智能体在局部三维模型中的位置和姿态信息，以及用于匹配计算其位姿的环境特征信息，以及用于匹配计算其位姿的激光点云信息和激光点云分布直方图。
在本发明的一个实例中，智能体端使用多线激光获取距离信息，构建点云，并利用经体素滤波器(voxel filter)处理后的点云得到局部三维模型。
该局部三维模型M是离散化的三维栅格地图，其每一个正方体单元的棱长即栅格地图的分辨率，且每一个单元都有对应的概率值，这一概率值表征该单元上存在障碍物的概率。局部SLAM需要维护两个不同分辨率的三维栅格地图。
通过多线激光的距离数据可判断是否遇到障碍物，若遇到障碍物则标记为hit，否则标记为miss，每一个点云都能找到其对应的栅格单元，该栅格单元的概率值将被更新得到概率phit和pmiss。对栅格单元x，概率更新计算的公式为：
Figure BDA0002181645280000041
Mnew(x)＝clamp(odds-1(odds(Mold(x))·odds(phit)))
通过连续的激光点云数据可以对栅格地图进行更新，但是在运动中，激光雷达本身的位姿将出现一定的改变，需要将点云数据和栅格地图进行配准，得到点云相对于栅格地图的位姿。
进一步地，在本发明的一个实施例中，还包括：对多个三维模型和多个轨迹节点信息进行粗匹配和精匹配。
轨迹节点与局部三维模型的匹配分为粗匹配和精匹配两步：粗匹配时，搜索范围可以描述为3D空间中6自由度的(Δx，Δy，Δz，Δθ，
Figure BDA0002181645280000056
Δγ)向量，在此范围内，按照一定的分辨率进行穷举，即可得到在指定分辨率下的最佳估计；精匹配时，以当前估计为初始值构建非线性优化问题，使用非线性优化库Ceres进行求解。
配准的过程分为两步，首先通过搜索的方式进行粗匹配，随后构建非线性优化问题找到更精确的匹配。由于在较小的时间范围内，激光雷达的位姿变换量不大，从而可以在一个较小的范围内进行搜索。局部搜索仅使用高分辨率点云与高分辨率三维栅格地图进行匹配，对于6自由度的三维姿态，搜索范围可描述为(Δx，Δy，Δz，Δθ，
Figure BDA0002181645280000057
Δγ)向量，在此范围内，按照一定的分辨率进行穷举，即可得到在指定分辨率下的最佳估计。
估计的评分计算方式为：
Figure BDA0002181645280000051
其中n为激光点云中点的数量，probabilityi为其中第i个点在三维栅格中对应的概率值，normtransform为姿态平移变化量的范数，angle为姿态的旋转角变化量，wt，wr分别为平移量和旋转量的权重。总体上，指数项为对过大的平移量与旋转量的惩罚。
基于此预估计，可构建一个非线性优化问题，以当前估计为初始值，对当前分辨率下的最佳估计进行更精确的优化。非线性优化使用同时使用高低两种分辨率的点云以及对应分辨率的栅格，配合低分辨率的点云和栅格地图可有效防止匹配过程中出现过拟合。
非线性优化问题使用开源库Ceres进行，构建如下最小二乘问题：
Figure BDA0002181645280000052
其中，ξ表示位姿，Tξ为从激光点云到栅格地图的变换矩阵，hk为激光点云中第k点，Msmooth为插值后的栅格单元概率值。
为防止匹配结果相对初始位置出现较大的偏移，可对最小二乘中的平移以及旋转变化量添加惩罚，从而得到以下残差：
Figure BDA0002181645280000053
其中ζ为惩罚比例系数，Δx，Δy，Δz，Δθ，
Figure BDA0002181645280000054
Δγ分别为x，y，z，θ，
Figure BDA0002181645280000055
γ的变化量。
通过将点云的位置坐标在栅格地图上进行双三次插值，可计算点云和栅格地图之间的偏差。对于点云中的第i个点有：
Figure BDA0002181645280000061
其中ζ为比例系数，
Figure BDA0002181645280000062
为点云中的第i个点在栅格地图上对应位置的插值。
采用以上配准方式，通过连续的激光点云数据构建局部的栅格地图。在这里将局部的栅格地图定义为局部三维模型i，以每帧激光数据作为一个轨迹节点j，当i积累了超过th数量的轨迹节点时即视为该局部三维模型构建完成。
进一步地，在本发明的一个实施例中，根据每个局部三维模型中的轨迹节点个数阈值对每个局部三维模型进行构建。当第i个局部三维模型获得新的轨迹节点时，先将其与i进行匹配并加入到i中，当i积累了超过th数量的轨迹节点，即视为该局部三维模型构建完成。
具体地，通过构建滑动窗口实现对同一智能体所构建的多个局部三维模型的组织，具体方法为：构建大小为2的滑动窗口，设某时刻该智能体已经构建了n个局部三维模型，新加入的轨迹节点先与局部三维模型n进行匹配，然后加入到局部三维模型n+1中，直到其构建完成，后续的轨迹节点将参与局部三维模型n+2的构建。即该滑动窗口内的前一个元素为最新完成的局部三维模型，后一个元素为正在构建的未完成的局部三维模型。当后者构建完成后，滑动窗口后移一个单位，保证新的轨迹节点只与最新完成的局部三维模型相匹配。
此外，若局部三维模型n与局部三维模型n-1匹配后位姿变化较小，即(Δx，Δy，Δz，Δθ，
Figure BDA0002181645280000064
Δγ)未达到一定阈值，则直接用n覆盖n-1且滑动窗口不后移，在不损失关键信息的前提下减少局部三维模型的数量。
进一步地，智能体端在局部三维模型构建结束后，需要计算与点云分布相关的直方图，该直方图反映各个角度范围内的点云分布情况，用于后续的全局匹配。
具体地，在局部三维模型构建完成后，将根据激光点云，计算与点云分布有关的直方图，如图3所示。该直方图的x轴为范围在0～π之间的角度值，对0～π这一范围进行n等分，即可得到n个类别，各个类别对应一个数值，该数值的计算方式如下：
首先，通过求和取平均计算点云的中心点，并对点云进行去中心处理。其次，可计算得到点云中任意一点与中心点在x，y方向上的坐标之差Δx，Δy，将
Figure BDA0002181645280000063
作为该点对应的角度，该角度范围在0～π之间，于是可以得到点云中所有点对应的角度。随后，根据该角度值的大小，对点云进行排序，于是相邻的点云相对中心点的角度是几乎相等的。接下来，按序取出排序后的点云中的点，计算该点与其前一点的x，y方向上的坐标之差Δx′，Δy′，若两点距离差别在一定范围内，则计算向量(Δx，Δy)与向量(Δx′，Δy′)的夹角的余弦值，即1-|cos(<(Δx，Δy)，(Δx′，Δy′)>)|。根据该点角度值，可找到其在直方图中对应的类别，在该类别中累加1-|cos(<(Δx，Δy)，(Δx′，Δy′)>)|即可得到该类别对应的数值。此直方图将在后续的全局匹配中使用。
在步骤S102中，通过RPC协议将多个局部三维模型和多个轨迹节点信息发送至云端。
进一步地，各个智能体端将其构建的局部三维模型、与之相对应的轨迹节点信息以及点云分布直方图通过RPC协议发送至云端，即将局部三维模型i和构建其过程中使用的迹节点j，j＝1，2，...，th一起上传。
在本发明的一个实施例中，各个智能体端将其构建的局部地图以及与之相对应的轨迹节点信息通过RPC协议发送至云端。这里的局部地图即不同分辨率的两个局部三维栅格地图，包含每个单元的概率值以及该地图在全局坐标内的位姿
Figure BDA0002181645280000071
轨迹节点包含不同分辨率的激光点云数据以及该节点相对于所在局部地图的位姿
Figure BDA0002181645280000072
若轨迹节点j用于构建局部地图i，则定义为节点j所在局部地图为i，同时轨迹节点的数据中还包括计算得到的点云分布直方图。
在步骤S103中，根据多个局部三维模型和多个轨迹节点信息在云端构建全局约束。
在本发明的一个实施例中，云端接收各个智能体端发送的局部三维模型及轨迹节点信息，构建全局三维模型约束：记
Figure BDA0002181645280000073
表示第k个智能体端上传的局部三维模型i，
Figure BDA0002181645280000074
表示第k个智能体端的第j个轨迹节点，
Figure BDA0002181645280000075
表示该轨迹节点所在的局部三维模型，约束操作
Figure BDA0002181645280000076
表示
Figure BDA0002181645280000077
与
Figure BDA0002181645280000078
之间的约束或约束评分，该评分与二者的匹配度正相关，且最低评分记作βmin。约束种类分为内约束和外约束。若
Figure BDA0002181645280000079
则
Figure BDA00021816452800000710
为内约束，否则为外约束。云端存储的局部三维模型集合为
Figure BDA00021816452800000711
轨迹节点集合为
Figure BDA00021816452800000712
约束集为
Figure BDA00021816452800000713
具体的，设某时刻云端收到
Figure BDA00021816452800000714
Figure BDA00021816452800000715
若
Figure BDA00021816452800000716
则约束成功，
Figure BDA00021816452800000717
否则约束失败，
Figure BDA00021816452800000718
保持不变；相应的，若云端收到
Figure BDA00021816452800000719
Figure BDA00021816452800000720
也有同样的
Figure BDA00021816452800000721
操作。根据
Figure BDA00021816452800000722
和
Figure BDA00021816452800000723
的定义，此约束操作对所有轨迹的局部三维模型以及轨迹节点均适用。
在本发明的一个实施例中，建立约束的过程需要进行匹配和评分，主要算法利用与步骤S101中的构建局部三维模型的配准过程类似的思路，同样是先进行基于搜索的粗匹配，后构建非线性优化问题进行优化。但是，由于数据来自不同智能体，此处待处理的路径节点和局部地图之间可能存在较大的位姿偏差，若采用一定分辨率下的穷举搜索复杂度较高，因此对旋转的配准利用先前得到的点云分布直方图进行，而x，y，z三个方向上的平移采用分支定界的方式进行配准。
进一步地，构建全局约束的过程中使用点云分布直方图进行偏航角匹配，随后使用分支定界的方式进行x，y，z方向上的匹配，提高搜索速度。
点云的三维栅格地图间的旋转匹配以偏航角为重点。每一个位姿节点对应的激光点云的直方图，首先将所有节点的直方图进行求和，得到一个全局直方图，以此直方图作为标准。对待构建约束的位姿节点，将其直方图进行循环移位，生成一系列直方图，将生成的直方图与标准直方图进行匹配评分，评分方式为：
score＝histogrami·histogramj
即取两直方图向量的点积。
如图4，处于同一场景下的智能体，其激光点云分布具有类似的分布趋势，仅存在偏航角上的偏移。若对其中一个直方图进行循环移位，则当角度移动量恰好约为偏航角偏移量时，两直方图向量点积达到极大，以此作为偏航角对齐的判定依据。
当旋转匹配的评分达到一定门限时，使用分支定界算法进一步进行平移配准。
分支定界的总搜索区间表示为[xs1，xt1]×[ys1，yt1]×[zs1，zt1]，递归层数为i(i≥1)时，搜索的分辨率为
Figure BDA0002181645280000081
在第i层递归处，搜索区间为[xsi，xti]×[ysi，yti]×[zsi，zti]，将x，y，z区间分别二分，则搜索区间的分割有8种情况，算出每个分割的匹配度，设匹配度最高的为
Figure BDA0002181645280000082
Figure BDA0002181645280000083
其中m，n，k均为0或1，令[xs(i+1)，xt(i+1)]×[ys(i+1)，yt(i+1)]×[zs(i+1)，zt(i+1)]＝χ，然后进行下一层的递归，直至递归层数达到预设值，返回最佳匹配区间以及匹配度评分。其中匹配度的算法同局部SLAM中激光数据与栅格地图匹配度的算法。对于来自同一轨迹的时间戳相差不大的局部三维模型和路径节点，或已经建立约束的局部三维模型与路径节点，可认为局部三维模型与该路径节点位姿差异不大，故可以在较小的范围内进行分支定界匹配。在以上情况之外，则认为二者之间可能有较大的位姿偏差，需要进行大范围搜索。搜索完成后，进行非线性优化计算。
在本发明的一个实施例中，由于构建的各个局部三维模型可能存在较多的重叠部分，而过多的局部三维模型将增加约束的建立以及后续的优化的时间代价，需要对局部三维模型进行删减。具体来说，云端进行全局建图时，维护一个记录每个局部三维模型所包含环境特征信息的表格，建立方法为：对于每个环境特征信息，统计覆盖了该特征的局部三维模型并保留最新的n个，将这n个地图覆盖的特征个数增加1。遍历完所有环境特征后，该表格建立完成；然后遍历该表格，若某局部三维模型
Figure BDA0002181645280000084
覆盖的特征个数小于预设的最低值，则标记该局部三维模型为删除，同时操作约束集
Figure BDA0002181645280000085
以保证约束集的稀疏性。
在步骤S104中，根据全局约束对多个局部三维模型进行融合生成全局三维模型。
具体而言，在步骤S103中建立了大量的局部三维模型与路径节点的约束，在局部三维模型与位姿节点不断增长的同时，误差将不断累积，综合各个约束进行全局的优化，减小累积误差，对于大范围自然场景的三维重建是十分必要的。
事实上，由于每个局部三维模型仅与其附近的路径节点建立了约束，且步骤S103中将冗余的局部三维模型进行了删减操作，最后需要优化的位姿图具有一定的稀疏性。
在本发明的一个实施例中，位姿图的求解使用的是Konolige，Kurt et al.“Efficient Sparse Pose Adjustment for 2D mapping.”2010 IEEE/RSJ InternationalConference on Intelligent Robots and Systems(2010)：22-29.中介绍的SPA(SparsePose Adjustment)方法。SPA方法基于稀疏的位姿约束，构建如下一个非线性优化问题：
Figure BDA0002181645280000091
其中，
Figure BDA0002181645280000092
与
Figure BDA0002181645280000093
分别表示第i个局部三维模型和第j个路径节点的全局三维模型中的位姿，ξij表示第i个局部三维模型与第j个路径节点在建立约束时得到的相对位姿，∑ij为与ξij对应的协方差矩阵。上式中残差E的计算方式如下：
Figure BDA0002181645280000094
Figure BDA0002181645280000095
而ρ为核函数，对于内约束，ρ(x)＝x，即不使用核函数，对于外约束，选用ρ(x)为Huberloss，即
Figure BDA0002181645280000096
对外约束使用Huberloss可以有效减小外点的影响。
除此之外，非线性优化中还使用的IMU数据的积分对轨迹节点添加额外的约束，进行优化。
对于一系列轨迹节点，在先前的匹配过程中，已计算得到两个轨迹节点的相对位姿，即两个轨迹节点的平移量Δx，Δy，Δz和旋转量Δθ，
Figure BDA00021816452800000910
Δγ。
同时，利用图5所示的系统，在上传局部三维模型和轨迹节点信息的同时，云端可获取一系列带有时间戳的IMU数据。IMU数据中包含x，y，z三个方向上线加速度ax，ay，az，以及三个角速度ωθ，
Figure BDA0002181645280000097
ωγ，对此数据进行积分，可得到x，y，z三个方向上线速度变化量Δvximu，Δvyimu，Δvzimu，以及角度变化量Δθimu，
Figure BDA0002181645280000098
Δγimu。
对于平移量的约束，需要取连续的3个轨迹节点，记此三个轨迹节点为Node1，Node2，Node3，对应的时间戳为t1，t2，t3，并取
Figure BDA0002181645280000099
通过对IMU数据的积分，计算tcenter1到tcenter2过程中的线速度变化量Δvimu＝(Δvximu，Δvyimu，Δvzimu)。同时，由位姿匹配的结果计算：
Figure BDA0002181645280000101
Δv＝v2-v1
其中ΔT1＝(Δx1，Δy1，Δz1)和ΔT2＝(Δx2，Δy2，Δz2)，分别为从Node1到Node2和从Node2到Node3之间的位姿平移量。
对于旋转量的约束，可直接由相邻2个轨迹节点在匹配结果得到
Figure BDA0002181645280000102
同时通过IMU积分得到的
Figure BDA0002181645280000103
添加的残差为：
Figure BDA0002181645280000104
其中ζ为惩罚比例系数。
在全局姿态优化结束后，可输出三维模型。三维模型在存储时以三维栅格地图的形式存储，设该地图的分辨率为r。
如图6所示，需要输出三维模型时，通过遍历三维栅格地图进行点云重建。每一个栅格有一对应的概率值pi，为[0，216-1]内的整数。栅格的坐标(xi，yi，zi)，其中xi，yi，zi均为整数，通过如下的转换可以将栅格还原为三维空间内的点：
Figure BDA0002181645280000105
其中R，t为该三维栅格地图在世界坐标系下的姿态和位置。
令
Figure BDA0002181645280000106
即可得到该点的Intensity值。
根据本发明实施例提出的多智能体协同三维建模方法，通过智能体端使用SLAM算法实现局部环境的三维模型重建；智能体端通过RPC协议将局部三维模型及轨迹节点信息上传至云端；云端接收局部三维模型信息，构建全局约束；云端基于全局约束融合多个局部模型，重建大范围场景的三维模型。由此，利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。
其次参照附图描述根据本发明实施例提出的多智能体协同三维建模装置。
图7为根据本发明一个实施例的多智能体协同三维建模装置结构示意图。
如图7所示，该多智能体协同三维建模装置包括：第一生成模块100、发送模块200、构建模块300、第二生成模块400。
第一生成模块100，用于通过SLAM算法生成多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息。
发送模块200，用于通过RPC协议将多个局部三维模型和多个轨迹节点信息发送至云端。
构建模块300，用于根据多个局部三维模型和多个轨迹节点信息在云端构建全局约束。
第二生成模块400，用于根据全局约束对多个局部三维模型进行融合生成全局三维模型。
进一步地，在本发明的一个实施例中，多个智能体包括自动驾驶汽车、自动导引运输车、自主式水下航行器和无人机中的一种或多种。
进一步地，在本发明的一个实施例中，轨迹节点信息包括：
多个智能体的在多个局部三维模型中的位置、姿态信息、环境特征信息、激光点云信息和激光点云分布直方图。
进一步地，在本发明的一个实施例中，在构建全局约束中，通过激光点云分布直方图进行航偏角匹配，并使用分支定界的方式进行三维方向的匹配。
进一步地，在本发明的一个实施例中，多个局部三维模型包括多个智能体在局部范围执行SLAM算法生成的地图信息。
进一步地，在本发明的一个实施例中，对多个局部三维模型和多个轨迹节点信息进行粗匹配和精匹配。
进一步地，在本发明的一个实施例中，根据每个局部三维模型中的轨迹节点个数阈值对每个局部三维模型进行构建。
进一步地，在本发明的一个实施例中，通过构建滑动窗口生成多个局部三维模型。
需要说明的是，前述对方法实施例的解释说明也适用于该实施例的装置，此处不再赘述。
根据本发明实施例提出的多智能体协同三维建模装置，通过智能体端使用SLAM算法实现局部环境的三维模型重建；智能体端通过RPC协议将局部三维模型及轨迹节点信息上传至云端；云端接收局部三维模型信息，构建全局约束；云端基于全局约束融合多个局部模型，重建大范围场景的三维模型。由此，利用多个异质异构智能体的自主定位与建图能力，构建全局约束融合多个局部三维模型，从而实现大范围场景的实时、分布式三维重建，将会极大拓展全自主智能体的应用场景，为多智能体在复杂大范围场景下的协同工作提供理论和技术基础。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种多智能体协同三维建模方法，其特征在于，包括以下步骤：
通过SLAM算法生成多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息；
通过RPC协议将所述多个局部三维模型和所述多个轨迹节点信息发送至云端；
根据所述多个局部三维模型和所述多个轨迹节点信息在云端构建全局约束，所述构建全局约束，包括：云端接收各个智能体端发送的局部三维模型及轨迹节点信息，构建全局三维模型约束：其中，
Figure FDA0002977255000000011
表示第k个智能体端上传的局部三维模型i，
Figure FDA0002977255000000012
表示第k个智能体端的第j个轨迹节点，
Figure FDA0002977255000000013
表示该轨迹节点所在的局部三维模型，约束操作
Figure FDA0002977255000000014
表示
Figure FDA0002977255000000015
与
Figure FDA0002977255000000016
之间的约束或约束评分，该评分与二者的匹配度正相关，且最低评分记作βmin；约束种类分为内约束和外约束；若
Figure FDA0002977255000000017
则
Figure FDA0002977255000000018
为内约束，否则为外约束；云端存储的局部三维模型集合为
Figure FDA0002977255000000019
轨迹节点集合为
Figure FDA00029772550000000110
约束集为
Figure FDA00029772550000000111
任一时刻所述云端收到
Figure FDA00029772550000000112
Figure FDA00029772550000000113
若
Figure FDA00029772550000000114
则约束成功，
Figure FDA00029772550000000115
否则约束失败，
Figure FDA00029772550000000116
保持不变；相应的，若云端收到
Figure FDA00029772550000000117
Figure FDA00029772550000000118
进行同样的
Figure FDA00029772550000000119
操作；
根据所述全局约束对所述多个局部三维模型进行融合生成全局三维模型。
2.根据权利要求1所述的方法，其特征在于，
所述多个智能体包括自动驾驶汽车、自动导引运输车、自主式水下航行器和无人机中的一种或多种。
3.根据权利要求1所述的方法，其特征在于，所述轨迹节点信息包括：
所述多个智能体的在所述多个局部三维模型中的位置、姿态信息、环境特征信息、激光点云信息和激光点云分布直方图。
4.根据权利要求3所述的方法，其特征在于，
在构建所述全局约束中，通过所述激光点云分布直方图进行航偏角匹配，并使用分支定界的方式进行三维方向的匹配。
5.根据权利要求1所述的方法，其特征在于，
所述多个局部三维模型包括所述多个智能体在局部范围执行所述SLAM算法生成的地图信息。
6.根据权利要求1所述的方法，其特征在于，
对所述多个局部三维模型和所述多个轨迹节点信息进行粗匹配和精匹配。
7.根据权利要求1所述的方法，其特征在于，
根据每个局部三维模型中的轨迹节点个数阈值对每个局部三维模型进行构建。
8.根据权利要求1所述的方法，其特征在于，
通过构建滑动窗口生成所述多个局部三维模型。
9.一种多智能体协同三维建模装置，其特征在于，包括：
第一生成模块，用于通过SLAM算法生成多个智能体所在的局部环境的多个局部三维模型和多个轨迹节点信息；
发送模块，用于通过RPC协议将所述多个局部三维模型和所述多个轨迹节点信息发送至云端；
构建模块，用于根据所述多个局部三维模型和所述多个轨迹节点信息在云端构建全局约束，所述构建全局约束，包括：云端接收各个智能体端发送的局部三维模型及轨迹节点信息，构建全局三维模型约束：其中，
Figure FDA0002977255000000021
表示第k个智能体端上传的局部三维模型i，
Figure FDA0002977255000000022
表示第k个智能体端的第j个轨迹节点，
Figure FDA0002977255000000023
表示该轨迹节点所在的局部三维模型，约束操作
Figure FDA0002977255000000024
表示
Figure FDA0002977255000000025
与
Figure FDA0002977255000000026
之间的约束或约束评分，该评分与二者的匹配度正相关，且最低评分记作βmin；约束种类分为内约束和外约束；若
Figure FDA0002977255000000027
则
Figure FDA0002977255000000028
为内约束，否则为外约束；云端存储的局部三维模型集合为
Figure FDA0002977255000000029
轨迹节点集合为
Figure FDA00029772550000000210
约束集为
Figure FDA00029772550000000211
任一时刻所述云端收到
Figure FDA00029772550000000212
Figure FDA00029772550000000213
若
Figure FDA00029772550000000214
则约束成功，
Figure FDA00029772550000000215
否则约束失败，
Figure FDA00029772550000000216
保持不变；相应的，若云端收到
Figure FDA00029772550000000217
Figure FDA00029772550000000218
进行同样的
Figure FDA00029772550000000219
操作；
第二生成模块，用于根据所述全局约束对所述多个局部三维模型进行融合生成全局三维模型。
10.根据权利要求9所述的装置，其特征在于，所述轨迹节点信息包括：
所述多个智能体的在所述多个局部三维模型中的位置、姿态信息、环境特征信息、激光点云信息和激光点云分布直方图。
CN201910798522.3A 2019-08-27 2019-08-27 多智能体协同三维建模方法及装置 Active CN110580740B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910798522.3A CN110580740B (zh) 2019-08-27 2019-08-27 多智能体协同三维建模方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910798522.3A CN110580740B (zh) 2019-08-27 2019-08-27 多智能体协同三维建模方法及装置

Publications (2)

Publication Number Publication Date
CN110580740A CN110580740A (zh) 2019-12-17
CN110580740B true CN110580740B (zh) 2021-08-20

Family

ID=68812128

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910798522.3A Active CN110580740B (zh) 2019-08-27 2019-08-27 多智能体协同三维建模方法及装置

Country Status (1)

Country Link
CN (1) CN110580740B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111474953B (zh) * 2020-03-30 2021-09-17 清华大学 多动态视角协同的空中目标识别方法及系统
CN111812730B (zh) * 2020-06-16 2021-07-06 山东大学 一种用于滑坡探测的电阻率数据融合三维成像方法及系统
CN112828311B (zh) * 2021-01-03 2022-05-24 苏州星祥益精密制造有限公司 一种基于实时三维检测的金属增材制造在线轨迹调整方法
CN113031620A (zh) * 2021-03-19 2021-06-25 成都河狸智能科技有限责任公司 一种机器人复杂环境定位方法
CN113568410B (zh) * 2021-07-29 2023-05-12 西安交通大学 一种异构智能体轨迹预测方法、系统、设备及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2761323A1 (en) * 2011-09-30 2014-08-06 Chancellors, Masters & Scholars of the University of Oxford Localising transportable apparatus
CN104240297A (zh) * 2014-09-02 2014-12-24 东南大学 一种救援机器人三维环境地图实时构建方法
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
WO2018140748A1 (en) * 2017-01-26 2018-08-02 The Regents Of The University Of Michigan Localization using 2d maps which capture vertical structures in 3d point data
CN109079736A (zh) * 2018-08-02 2018-12-25 广东工业大学 一种基于ros的移动机器人平台的控制方法及系统
CN109615698A (zh) * 2018-12-03 2019-04-12 哈尔滨工业大学（深圳） 基于互回环检测的多无人机slam地图融合算法
CN109682357A (zh) * 2019-01-28 2019-04-26 安徽智训机器人技术有限公司 三维地图测绘系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102289814B (zh) * 2011-08-30 2012-12-26 北京理工大学 一种心脏核磁共振图像分割方法
US8798840B2 (en) * 2011-09-30 2014-08-05 Irobot Corporation Adaptive mapping with spatial summaries of sensor data
CN104532883B (zh) * 2014-11-28 2017-08-01 国网上海市电力公司 一种桩基混凝土浇筑的监测方法
CN106683182B (zh) * 2017-01-12 2019-09-20 南京大学 一种权衡立体匹配和视觉外形的三维重建方法
CN108401461B (zh) * 2017-12-29 2021-06-04 达闼机器人有限公司 三维建图方法、装置、系统、云端平台、电子设备和计算机程序产品

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2761323A1 (en) * 2011-09-30 2014-08-06 Chancellors, Masters & Scholars of the University of Oxford Localising transportable apparatus
CN104240297A (zh) * 2014-09-02 2014-12-24 东南大学 一种救援机器人三维环境地图实时构建方法
CN106127739A (zh) * 2016-06-16 2016-11-16 华东交通大学 一种结合单目视觉的rgb‑d slam方法
WO2018140748A1 (en) * 2017-01-26 2018-08-02 The Regents Of The University Of Michigan Localization using 2d maps which capture vertical structures in 3d point data
CN109079736A (zh) * 2018-08-02 2018-12-25 广东工业大学 一种基于ros的移动机器人平台的控制方法及系统
CN109615698A (zh) * 2018-12-03 2019-04-12 哈尔滨工业大学（深圳） 基于互回环检测的多无人机slam地图融合算法
CN109682357A (zh) * 2019-01-28 2019-04-26 安徽智训机器人技术有限公司 三维地图测绘系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
cartographer 3D scan matching 理解;p是马甲;《https://blog.csdn.net/weixin_30243533/article/details/96991338》;20190518;第1-20页 *
微型无人机视觉定位与环境建模研究;吕科 等;《电子科技大学学报》;20170531;第543-548页 *

Also Published As

Publication number Publication date
CN110580740A (zh) 2019-12-17

Similar Documents

Publication Publication Date Title
CN110580740B (zh) 多智能体协同三维建模方法及装置
Marin-Plaza et al. Global and local path planning study in a ROS-based research platform for autonomous vehicles
CN110490809B (zh) 多智能体协同定位与建图方法及装置
Gao et al. Gradient-based online safe trajectory generation for quadrotor flight in complex environments
CN110068335B (zh) 一种gps拒止环境下无人机集群实时定位方法及系统
Furgale et al. Toward automated driving in cities using close-to-market sensors: An overview of the v-charge project
Lindhé et al. Flocking with obstacle avoidance: A new distributed coordination algorithm based on voronoi partitions
CN105022401B (zh) 基于视觉的多四旋翼无人机协同slam方法
CN111880573B (zh) 一种基于视觉惯导融合的四旋翼自主导航方法
CN105865449A (zh) 基于激光和视觉的移动机器人的混合定位方法
CN107036594A (zh) 智能电站巡检智能体的定位与多粒度环境感知技术
CN111651705B (zh) 集群编队跟踪控制方法
Yang et al. Target-enclosing affine formation control of two-layer networked spacecraft with collision avoidance
CN112762957B (zh) 一种基于多传感器融合的环境建模及路径规划方法
CN113495566B (zh) 一种基于曲率约束融合势场法的D*Lite无人车局部路径规划方法
Jing et al. Coverage path planning using path primitive sampling and primitive coverage graph for visual inspection
CN114610066A (zh) 复杂未知环境下分布式集群无人机编队飞行轨迹生成方法
CN114911265A (zh) 一种四旋翼无人机编队协同机动控制方法
CN116774247A (zh) 基于ekf的多源信息融合的slam前端策略
CN114815899A (zh) 基于3d激光雷达传感器的无人机三维空间路径规划方法
Bender et al. Map-based drone homing using shortcuts
Ligocki et al. Atlas fusion-modern framework for autonomous agent sensor data fusion
CN112747752B (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
CN112445238B (zh) 一种基于神经网络的无人机循迹系统
Piperigkos et al. Cooperative Five Degrees Of Freedom Motion Estimation For A Swarm Of Autonomous Vehicles

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant