CN118031976B - 一种探索未知环境的人机协同系统 - Google Patents

Abstract

本申请提供了一种探索未知环境的人机协同系统，包括：无人设备端、服务器端和增强现实设备端；其中，无人设备端，用于获取环境的点云帧，生成三维语义点云地图和定位轨迹，获得相机和Apriltag码之间的转换矩阵，发布语义点云地图、定位轨迹和转换矩阵；服务器端，用于接受无人设备端发布的消息，将语义点云地图和定位轨迹根据转换矩阵转换到Apriltag码坐标系，并发布转换后的地图和轨迹；增强现实设备端，用于识别Apriltag码进行空间变换，获取点云点的坐标位置，根据点云中点的位置和语义标签信息，绘制点云并渲染。本申请的优势在于：提高了探索感知未知环境的效率；能够辅助增强设备准确的理解和显示实际环境。

Description

一种探索未知环境的人机协同系统

技术领域

本申请属于未知环境感知领域，具体涉及一种探索未知环境的人机协同系统。

背景技术

随着无人系统的不断发展，机器人应用正逐渐扩展至各行各业，逐步替代人类完成一些工作或任务。机器人具备灵活机动、小巧受限、强大环境适应性等优势，因此在安防、救灾救援和商业物流等领域得到广泛应用。它们不仅能够在特定应用场景下实现信息化和智能化，还能提高环境感知速度并拓展感知范围。然而，在面对不同复杂环境下的救援和探测任务时，仍然需要人类的辅助与协作。

同步建图和定位(Simultaneous Localization and Mapping,SLAM)是在机器人移动过程中，通过其搭载的传感器采集感知周围位置环境数据，实现地图的构建并获得机器人的位姿信息。SLAM算法的经典框架主要包括前端里程计、闭环检测和后端优化部分。前端根据传感器获得的数据，通过帧间配准，得到连续帧之间的位姿变换。闭环检测通过识别机器人是否到达过当前场景，发现潜在回环，消除累积漂移误差。后端通过优化前端得到的机器人位姿和观测约束，生成环境地图和机器人位姿。SLAM直接构建的点云地图能够提供场景中物体的位置信息，但只能体现场景的结构，而无法实现对环境更高层次的理解，例如对场景中各物体的种类信息。

发明内容

本申请的目的在于克服现有技术只能体现场景的结构，而无法实现对环境更高层次的理解的缺陷。

为了实现上述目的，本申请提出了一种探索未知环境的人机协同系统，所述系统包括：无人设备端、服务器端和增强现实设备端；其中，

无人设备端，用于利用环境信息采集设备获取环境的点云帧，生成三维语义点云地图和定位轨迹，识别Apriltag码，获得相机和Apriltag码之间的转换矩阵，发布语义点云地图、定位轨迹和转换矩阵；

服务器端，用于接受无人设备端发布的消息，将语义点云地图和定位轨迹根据转换矩阵转换到Apriltag码坐标系下，传输给增强现实设备端；

增强现实设备端，用于识别Apriltag码进行空间变换，获取服务器端传输的点云点的坐标位置，根据点云中点的位置和语义标签信息，绘制点云并渲染。

作为上述系统的一种改进，所述无人设备端包括环境信息采集设备、相机、SLAM建图模块和IMU惯性模块；

所述无人设备端的工作过程包括：

步骤S1：SLAM建图模块将环境信息采集设备获取的点云帧作为输入，实时生成三维语义点云地图以及无人设备端的位姿，对点云分割优化和特征提取，通过对根据曲率值提取的点云帧的平面点和边缘点，对两帧之间进行特征匹配和位姿估计，构建距离残差方程，采用LM法对关联特征迭代优化，获得并优化相对位姿；

步骤S2：计算语义点云地图转换到Apriltag码坐标系下的矩阵；通过相机识别Apriltag码，并根据单应性变换计算得到世界坐标系到相机坐标系的转换矩阵。

作为上述系统的一种改进，所述SLAM建图模块包括点云去畸变子模块、点云语义获取子模块、点云分割子模块、特征点提取子模块、激光里程计子模块和语义地图构建子模块；

所述步骤S1包括：

步骤S1-1：点云去畸变子模块对点云中每个点进行运动畸变校正，通过IMU惯性模块积分估计扫描期间的无人设备端位姿，获得每个点相对于起始点的旋转角度，将每个点的位置补偿到起始时刻；补偿关系如下：

其中：分别表示每个点起始时刻和时当前刻对应的位姿，表示转换矩阵；

步骤S1-2：点云语义获取子模块接收环境信息采集设备获取的实时点云帧，通过相机图像帧语义分割得到语义图像，将语义信息映射到点云帧，以获取点云中各物体的种类信息，并对点云点赋予语义标签信息，同时，对点云帧中点的位置信息、强度信息以及语义类别信息进行存储，实现语义点云帧的构建；

步骤S1-3：点云分割子模块，将语义点云帧投影为距离图像，根据点的环线，计算图像中点和相邻点的夹角，如果夹角小于第二设定阈值则判断为地面，将地面点云分离出来；将分割掉地面后的点云通过深度优先遍历递归查找点，比较相邻四个点的相对角度，如果角度大于第三设定阈值则认为是同一点云簇，剔除点数量少于第四设定阈值的点云簇，实现对点云去噪，并对同一点云簇进行标记；

步骤S1-4：特征点提取子模块将点云帧的360°方向均分成多个区域，计算每个区域中点的曲率，如果曲率大于第五设定阈值则为线特征，小于第五设定阈值则为面特征；每个区域内选择多个面特征和多个线特征；

所述点的曲率通过从当前点的左右各取多个点与当前点的深度差值计算，公式如下：

其中，表示点的集合，表示当前点的深度，表示相邻点的深度，表示当前点的曲率；

在特征点选取过程中，每个方向上的特征点数量和类别是固定的，对已经选择为特征点的周围点不再进行选择；

步骤S1-5：激光里程计子模块，通过特征点提取子模块提取的线特征和面特征进行帧间匹配，得到两帧之间的变换矩阵；

步骤S1-6：语义地图构建子模块，通过特征集合与周围点云地图匹配得到帧到地图的匹配位姿，然后对节点之间的约束利用LM方法得到变换矩阵；具体包括：

将当前位置要求范围内的带有语义标签信息的特征点集合进行融合拼接得到语义点云地图，选取距离最近的一组特征集作为新节点和点云地图中的节点计算空间距离约束，通过LM优化得到位姿变换；

根据IMU惯性模块数据时间戳与环境信息采集设备扫描时间戳对应，通过线性插 值计算当前时刻IMU惯性模块的横滚角和俯仰角，根据LM残差平方根设置权重，将IMU惯性 模块数据的角度与环境信息采集设备扫描匹配的角度融合，更新得到最终的变换矩阵：

其中，表示融合系数，表示IMU惯性模块的变换矩阵，表示环境信息采集 设备扫描匹配的变换矩阵。

作为上述系统的一种改进，所述步骤S1-2包括：

使用实时语义分割网络对相机获取的图像帧进行处理，以获取每一帧的语义图像，对去畸变后的点云帧和分割后的语义图像帧进行时间戳对齐；

根据转换矩阵，将点云帧投影到相应的图像帧上，过滤掉不在相机视野内的点；根据点的坐标计算点在距离图像中的行列索引，保留距离更近的点，将这些点根据相机模型将3D坐标点转为2D坐标点，并根据2D坐标得到相对应的语义点标签；

对每个映射到图像帧中的点云点计算邻域范围内是否有与当前点语义标签不同的点，若存在则标记为边缘点，即该点为两种语义之间的边界；遍历图像中所有像素的点，判断相邻范围内是否存在边缘点，若存在，则判断两点之间的强度信息是否在第一设定阈值范围内，如果满足要求，则将该点的语义标签赋值给边缘点。

作为上述系统的一种改进，所述S1-2还包括：

设定映射点云的距离阈值，防止点云帧误映射到图像帧中天空区域。

作为上述系统的一种改进，所述步骤S1-5包括：

根据步骤S1-3中对点云聚类的结果，将标签信息作为帧间匹配的约束条件，只有 标签相同的点能进行匹配；采用LM方法得到帧间位姿，通过面特征匹配，优化相邻两帧点到 面的距离，得到：，其中，表示两帧在z轴方向上的移动距离，表示两帧横 滚的角度，表示两帧俯仰的角度；通过线特征匹配，优化相邻两帧点到线的距离，得到，其中，表示两帧在x轴方向上的移动距离，表示两帧在y轴方向上的移动距 离，表示两帧航向的角度；将和融合得到两帧之间的变换矩 阵；

面特征点的距离的计算公式如下：

其中：表示第+1帧中的第i个特征点的坐标；表示第帧中离第i个 特征点最近的点的坐标，该点称为j；表示第帧中在点所在的扫描束上离最近的 点的坐标；表示第k帧中在点所在的扫描束的相邻束上离最近的点的坐标；

线特征点的距离的计算公式如下：

。

作为上述系统的一种改进，所述步骤S2包括：

步骤S2-1：对相机进行标定，得到相机的内参矩阵和畸变参数作为先验；

步骤S2-2：根据Apriltag码上提取的四个角点与世界坐标系中的四个对应假设点形成四个点对，通过直接线性法求解得到单应性矩阵，通过单应性矩阵和相机内参得到世界坐标系到相机坐标系的转换矩阵。

作为上述系统的一种改进，所述服务器端包括消息获取模块和点云处理模块；

所述服务器端的工作过程包括：

步骤S3：通过无人设备端和服务器端设置ROS主从机的方式，服务器端的消息获取模块从无人设备端获取消息；根据ROS话题，设定获取频率，在消息中读取并保存转换矩阵以及无人设备端构建的语义点云地图和实时定位轨迹信息；

步骤S4：点云处理模块将获取的ROS消息数据转换为旋转矩阵，结合平移向量生成转换矩阵，与点云的点坐标位置进行矩阵乘法，得到点云在Apriltag码坐标系下的坐标：

坐标系变换公式表示如下：

其中，表示转换矩阵，表示旋转矩阵，表示平移向量，表示点云 中点在无人设备坐标系下的坐标，表示点云中点在Apriltag坐标系下的坐 标。

作为上述系统的一种改进，所述增强现实设备端包括坐标变换模块和显示模块；

所述增强现实设备端的工作过程包括：

步骤S5：坐标变换模块与无人设备端识别同一位置的Apriltag码，计算转换矩阵，将增强现实设备端置于Apriltag码坐标系下；

步骤S6：显示模块获得服务器端发送的点云中点的坐标位置以及当前无人设备端的位置，在增强现实设备上对点进行绘制，实时显示无人设备端构建的地图。

作为上述系统的一种改进，所述显示模块包括模型绘制子模块和虚实融合子模块；

所述步骤S6包括：

步骤S6-1：模型绘制子模块通过Unity引擎将点通过方块的形式表示，并根据不同的语义呈现不同的颜色；

步骤S6-2：虚实融合子模块对建模的点云进行渲染，根据模型矩阵、视图矩阵和投影矩阵之间的坐标变换，实现虚拟场景和真实场景的定位配准，将虚拟信息准确呈现到真实环境中。

作为上述系统的一种改进，所述显示模块还包括人机交互子模块，用于实现与用户的交互功能，通过识别用户操作，生成操控命令实现对场景的控制。

与现有技术相比，本申请的优势在于：

1、提出了一种人机协同探索感知未知环境的系统，通过设计人和无人设备协作智能交互框架，基于多设备位姿解算以及语义信息融合，实现无人设备以及多个增强现实设备的协同，提高了探索感知未知环境的效率；

2、提出边缘点云语义优化方法，基于所设计的多传感器融合的定位与语义建图框架，使无人设备能够提供更精确的复杂环境信息，从而能够辅助增强设备准确的理解和显示实际环境；

3、多个增强现实设备能够同时对同一个场景进行展示，并且能够根据不同增强现实设备的位置显示场景的不同方向。

附图说明

图1所示为探索未知环境的人机协同系统架构图；

图2所示为探索未知环境的人机协同系统模块框架图；

图3所示为图像帧语义分割效果图；

图4所示为映射语义点云帧效果图；

图5所示为边缘优化区域效果图；

图6（a）所示为无人设备所构建的环境点云效果图一；

图6（b）所示为无人设备所构建的环境点云效果图二；

图7所示为增强现实设备所展示的虚实融合效果图。

具体实施方式

下面结合附图对本申请的技术方案进行详细的说明。

增强现实(Augmented Reality,AR)是将现实世界和虚拟进行融合，将虚拟信息通过位置匹配呈现到真实物理世界中，对真实世界进行增强，在实现可视化的同时能够实时交互。目前，增强现实的代表性产品有2012年谷歌推出的Google Glass以及2015年微软推出的HoloLens。增强现实技术是对虚拟场景进行渲染，生成虚拟物体场景。同时，对摄像机等采集的真实场景通过跟踪定位对场景进行量测，最终实现虚拟场景在真实世界上的叠加形成增强现实的场景呈现给用户，而用户能够通过手势等信息产生一系列的操控指令。相比于传统的在PC端图形化的呈现方式，不仅提高了对环境的感知程度，也提高了便携性。

人机协同是指在一定的协作区域内，人与无人设备交互协同实现目标。通过人与无人设备的合作，将人与机器的优势互补，从而在复杂多变的环境下更高效的完成感知、控制和决策等任务。人机协同既利用了机器自动化的优势也避免了机器无法应对复杂环境的问题，同时提高人的决策能力，从而做出更符合当前状态的决策。

通过增强现实设备能直观的感知机器人在未知环境的利用SLAM技术的建图效果，从而识别环境信息，同时更加便携，能够实时观察机器人地图重建的变化和机器人运动轨迹等。此外，可以通过手势控制等方式，与三维地图进行交互。最终，实现人机交互协作完成场景理解。

因此，本申请提供了一种探索未知环境的人机协同系统，通过主从的人机协作模式，操控无人车探索未知的感兴趣区域，实现实时的定位和建图，并通过增强现实技术在增强现实设备中呈现探索区域的语义地图以及无人车的实时位置，能够在救灾救援以及远程环境考察等场景下实现人机交互协作。

本发明中提出了一种探索未知环境的人机协同系统，能够获得并在增强现实设备中呈现无人设备探索位置区域构建的语义地图，实现人机环境之间的交互融合，从而能够在后续进行目标识别和态势分析等任务。

从逻辑角度，探索未知环境的人机协同系统包括感知层、决策层和执行层，系统架构图如图1所示。其中，感知层利用无人设备上的传感器获取环境信息并实现语义地图的构建，为用户提供环境变化的实时反馈，决策层根据感知层的信息进行处理和反馈，执行层根据决策层发出的命令进行执行。

从物理角度，探索未知环境的人机协同系统包括无人设备端、服务器端和增强现实设备端。其中，在无人设备端，通过SLAM建图模块生成三维语义点云地图和定位轨迹，并识别Apriltag码（一种视觉基准系统，广泛应用于增强现实、机器人导航、相机校准等多种任务中，它通过特定的标志来快速检测标签，并计算出标签相对于相机的精确3D位置、方向和标识）计算获得相机和Apriltag码之间的转换矩阵，将语义点云地图、定位轨迹和转换矩阵通过ROS消息（Robot Operating System，是一种在ROS系统中用于节点间通信的数据格式）进行发布；在服务器端，通过ROS消息获取无人设备端发布的消息，并将地图和定位轨迹根据转换矩阵转换到Apriltag码坐标系下。对地图保存后，通过TCP传输给增强现实设备端；在增强现实设备端首先识别Apriltag码进行空间变换，然后获取服务器端传输的点云点的坐标位置，根据点云中点的位置和语义标签信息，绘制点云并渲染。增强现实设备端可以有一个或多个，多个增强现实设备能够同时对同一个场景进行展示，并且能够根据不同增强现实设备的位置显示场景的不同方向。无人设备、服务端以及增强现实设备通过WIFI实现无线连接。系统模块组成如图2所示。

无人设备端上设有环境信息采集设备、相机、SLAM建图模块以及IMU惯性模块，用于扫描场景的环境以及获取位置信息。

环境信息采集设备可以是激光雷达、摄像头等。

无人设备端包括以下模块，具体功能步骤包括：

步骤A：SLAM建图模块将环境信息采集设备获取的点云帧作为输入，实现包括点云去畸变、点云语义获取、点云分割、特征提取、激光里程计、激光建图等子模块，实时生成三维语义点云地图以及机器人的位姿。对点云分割优化特征提取，通过对根据曲率值提取的点云帧的平面点和边缘点，对两帧之间进行特征匹配和位姿估计。构建距离残差方程，采用Levenberg-Marquardt(LM)法对关联特征迭代优化，获得并优化相对位姿，提高收敛速度。此外，加入回环检测模块，修正误差累积引起的漂移问题。具体过程包括：

步骤A1：点云去畸变子模块。根据IMU数据对点云中每个点进行运动畸变校正。通过IMU积分估计扫描期间的机器人位姿，获得每个点相对于起始点的旋转，将每个点的位置都补偿到起始时刻。转换关系如公式（1）所示：

（1）

其中：分别表示每个起始点时刻和当前时刻对应的位姿，表示 转换矩阵。

步骤A2：点云语义获取子模块。根据无人设备端上安装的3D激光雷达实时获取点云帧，通过相机图像帧语义分割得到语义图像，将语义信息映射到点云帧，以获取点云中各物体的种类信息，并对点云点赋予语义标签信息，同时，对点云帧的中点的位置信息、强度信息以及语义类别信息进行存储，从而实现语义点云帧的构建。如图3所示为图像帧语义分割效果图；如图4所示为映射语义点云帧效果图。

首先，使用实时语义分割网络对相机获取的图像帧进行处理，以获取每一帧的语义图像。然而，由于对激光雷达点云帧的去畸变处理和图像帧的语义分割处理，会影响图像帧和点云帧的时间戳同步，因此需要对去畸变后的点云帧和分割后的语义图像帧进行时间戳对齐的操作。

在时间戳对齐后，根据实时获取的相机与雷达的转换关系矩阵，将点云帧投影到相应的图像帧上。由于相机和雷达可视范围存在差异，因此在投影之前需要对点云帧进行处理。将激光雷达坐标系下的点云帧转换到相机FLU坐标系下表示，然后过滤掉不在相机视野内的点。根据点的坐标计算点在距离图像中的行列索引，并保留距离更近的点。随后，将这些点根据相机模型将3D坐标点转为2D坐标点，并根据2D坐标得到相对应的语义点标签。

由于映射对图像帧和点云帧时间戳的敏感性，设计边缘点云语义优化方法。该方法对每个映射到图像帧中的点云点计算邻域范围内是否有与当前点语义标签不同的点，若存在则标记为边缘点，即该点为两种语义之间的边界。考虑相近点的强度大小基本一致的特性，因此，遍历图像中所有像素的点，判断相邻范围内是否存在边缘点，若存在，则判断两点之间的强度信息是否在阈值范围内，如果满足要求，则将该点的语义标签赋值给边缘点。

此外，对于可能存在的激光点云帧误映射到图像帧中天空区域的问题，规定了映射点云的距离阈值。

步骤A3：点云分割子模块。首先将语义点云帧投影为距离图像，根据点的环线，计算图像中点和相邻点的夹角，如果夹角小于阈值则判断为地面，从而将地面点云分离出来。将分割掉地面后的点云通过深度优先遍历递归查找点，比较相邻四个点的相对角度，如果角度大于阈值则认为是同一点云簇，将数量较少的点云簇进行剔除，实现对点云去噪，并对同一点云簇进行标记。

步骤A4：特征点提取子模块。将点云帧的360°方向均分成6个区域，计算每个区域中点的曲率，如果曲率大于阈值则为线特征，小于阈值则为面特征。每个区域内选择4个面特征和2个线特征。曲率是通过取当前点的左右各5个点与当前点的深度差值计算，公式如式2所示。

（2）

式（2）中：表示点的集合，表示当前点的深度，表示相邻点的深度，表示当 前点的曲率。

在特征点选取过程中，每个方向上的特征点数量和类别是固定的。同时，对已经选择为特征点的周围点不再进行选择。

步骤A5：激光里程计子模块。通过特征点提取模块提取的线特征和面特征进行帧间匹配，得到两帧之间的变换矩阵。

首先根据步骤A1中对点云聚类的结果，将标签信息作为帧间匹配的约束条件，只 有标签相同的点能进行匹配。然后，采用LM方法得到帧间位姿。通过面特征匹配，优化相邻 两帧点到面的距离，从而得到（表示在z轴方向上的移动距离，表示横滚 的角度，表示俯仰的角度），通过线特征匹配，优化相邻两帧点到线的距离，得到（表示在x轴方向上的移动距离，表示在y轴方向上的移动距离，表示航 向的角度）。最后，将和融合得到变换矩阵。

面特征点的距离计算公式如式（3）所示：

（3）

其中：表示坐标，表示第帧，表示在第帧中的一个点，表示在第帧 中离最近的点，表示在点所在的扫描束上离最近的点，表示在点所在的扫描束的 相邻束上离最近的点，L表示雷达上的特征点，表示要匹配的点的坐标，表示被匹配 的点的坐标。

线特征点的距离计算公式如式（4）所示：

（4）

其中：表示坐标，表示第帧，表示在第帧中的一个点，表示在第帧 中离最近的点，表示在第帧中在点所在扫描束上离最近的点。

步骤A6：语义地图构建子模块。通过特征集合与周围点云地图匹配得到帧到地图的匹配位姿，然后对节点之间的约束利用LM方法得到变换矩阵。

将当前位置要求范围内的带有语义标签信息的特征点集合进行融合拼接得到语义点云地图，选取距离最近的一组特征集作为新节点和点云地图中的节点计算空间距离约束，通过LM优化得到位姿变换。

同时，根据IMU数据时间戳与激光扫描时间戳对应，通过线性插值计算当前时刻IMU的横滚角和俯仰角，根据LM残差平方根设置权重，将IMU数据的角度与激光扫描匹配的角度融合，更新得到最终的变换。

（5）

其中：表示融合系数，表示IMU的变换，表示激光扫描匹配的变换。

此外，利用回环检测模块使用ICP算法匹配当前帧和历史点云帧，如果匹配成功则使用iSAM（增量平滑与映射，Incremental Smoothing And Mapping）对位姿优化。历史点云帧即用于提取特征点的点云帧。

SLAM的建图效果如图5所示。

步骤B：计算语义点云地图转换到Apriltag码坐标系下的矩阵。通过无人设备端的相机识别Apriltag码，并根据单应性变换计算得到相机外参。

步骤B1：对相机进行标定，得到相机的内参矩阵和畸变参数作为先验。

步骤B2：根据Apriltag码上提取的四个角点与世界坐标系中的四个对应假设点形成四个点对，通过直接线性法求解得到单应性矩阵。通过单应性矩阵和相机内参得到世界坐标系到相机坐标系的转换矩阵。

服务器端包括以下模块，具体功能步骤包括：

在服务器端获取无人设备端发布的语义点云地图以及转换矩阵消息，并根据转换矩阵将点云从雷达坐标系转换到Apriltag坐标系下，保存到服务器端。然后通过TCP将点云坐标位置信息发送给增强现实设备端。

步骤C：消息获取模块。ROS支持分布式多机通信，通过无人设备端和服务器端设置ROS主从机的方式，在服务器端从无人设备端运行消息获取的节点，根据特定的ROS话题，设定获取频率，在消息中读取并保存转换矩阵以及无人设备端构建的点云地图和实时定位轨迹等信息。

步骤D：点云处理模块。将根据ROS消息获取的四元数转换为旋转矩阵，结合平移向量生成转换矩阵，与点云的点坐标位置进行矩阵乘法，得到点云在Apriltag码坐标系下的坐标。

坐标系变换公式表示如式6、式7所示：

(6)

(7)

其中，为转换矩阵，为旋转矩阵，为平移向量。为点云中点在 雷达坐标系下的坐标，为点云中点在Apriltag坐标系下的坐标。

增强现实设备端包括以下模块，具体功能步骤包括：

步骤E：坐标变换模块。增强现实设备与无人设备端识别同一位置的Apriltag码，计算转换矩阵，将增强现实设备端置于Apriltag码坐标系下，从而与无人设备端保持在同一坐标系下。

步骤F：显示模块。通过TCP获得服务器端发送的点云中点的坐标位置以及当前无人设备端的位置。在增强现实设备上对点进行绘制，实时显示无人设备端构建的地图。无人设备所构建的环境点云效果如图6（a）和图6（b）所示，显示结果如图7所示。

步骤F1：模型绘制子模块。通过Unity引擎将点通过方块的形式表示，并根据不同的语义呈现不同的颜色。

步骤F2：虚实融合子模块。对建模的点云进行渲染，根据模型矩阵、视图矩阵和投影矩阵之间的坐标变换，实现虚拟场景和真实场景的定位配准，将虚拟信息准确呈现到真实环境中。

步骤F3：人机交互子模块。实现用户的交互功能，通过识别用户手势等操作，生成操控命令实现对场景的控制。

步骤G：通过在增强现实设备接收到的感知信息，对无人设备的下一步行动进行决策，最后无人设备接收命令并执行。

无人设备端上设置有移动控制模块，用于控制无人设备行进方向和速度，通过无线遥控器能够实现无人设备在任意方向上的移动。根据无人设备实时采集并构建的场景环境，通过增强现实设备显示单元将信息转化为虚拟场景，用户能够实时查看无人设备位置以及识别环境的结构与语义信息，从而与无人设备协作，实现大范围的探索，提高远程操作的效率和准确性。

本发明提出的一种探索未知环境的人机协同系统，关注在未知环境下利用无人车等无人设备以及增强现实设备进行人机协同探索真实环境的问题。该系统利用无人设备对环境进行探测，通过服务器端对信息处理，最终在增强现实设备中实现虚实配准显示，同时呈现了物体的语义信息。通过无人设备的探索建图以及增强设备的实时显示，决策和操控无人设备的行进方向，从而实现对感兴趣区域的探索并及时躲避障碍物。同时，该系统能够在人不进入未知环境的情况下，达到保护人安全，避免潜在危险，提高探索效率，并准确配准人、无人设备以及环境之间的位置关系的目的，从而全面知悉未知环境中在相应的物理位置上不可见的空间结构和物体信息。本发明有效提高了人机之间的交互协同性，同时增强了对环境的感知水平，提高了安全性，有助于人更好地进行决策。

最后所应说明的是，以上实施例仅用以说明本申请的技术方案而非限制。尽管参照实施例对本申请进行了详细说明，本领域的普通技术人员应当理解，对本申请的技术方案进行修改或者等同替换，都不脱离本申请技术方案的精神和范围，其均应涵盖在本申请的权利要求范围当中。

Claims (9)

1.一种探索未知环境的人机协同系统，其特征在于，所述系统包括：无人设备端、服务器端和增强现实设备端；其中，

无人设备端，用于利用环境信息采集设备获取环境的点云帧，生成三维语义点云地图和定位轨迹，识别Apriltag码，获得相机和Apriltag码之间的转换矩阵，发布语义点云地图、定位轨迹和转换矩阵；

服务器端，用于接受无人设备端发布的消息，将语义点云地图和定位轨迹根据转换矩阵转换到Apriltag码坐标系下，传输给增强现实设备端；

增强现实设备端，用于识别Apriltag码进行空间变换，获取服务器端传输的点云点的坐标位置，根据点云中点的位置和语义标签信息，绘制点云并渲染；

所述无人设备端包括环境信息采集设备、相机、SLAM建图模块和IMU惯性模块；

所述无人设备端的工作过程包括：

步骤S1：SLAM建图模块将环境信息采集设备获取的点云帧作为输入，实时生成三维语义点云地图以及无人设备端的位姿，对点云分割优化和特征提取，通过对根据曲率值提取的点云帧的平面点和边缘点，对两帧之间进行特征匹配和位姿估计，构建距离残差方程，采用LM法对关联特征迭代优化，获得并优化相对位姿；

步骤S2：计算语义点云地图转换到Apriltag码坐标系下的矩阵；通过相机识别Apriltag码，并根据单应性变换计算得到世界坐标系到相机坐标系的转换矩阵；

所述SLAM建图模块包括点云去畸变子模块、点云语义获取子模块、点云分割子模块、特征点提取子模块、激光里程计子模块和语义地图构建子模块；

所述步骤S1包括：

步骤S1-1：点云去畸变子模块对点云中每个点进行运动畸变校正，通过IMU惯性模块积分估计扫描期间的无人设备端位姿，获得每个点相对于起始点的旋转角度，将每个点的位置补偿到起始时刻；补偿关系如下：

；

其中：分别表示每个点起始时刻和当前时刻对应的位姿，表示转换矩阵；

步骤S1-2：点云语义获取子模块接收环境信息采集设备获取的实时点云帧，通过相机图像帧语义分割得到语义图像，将语义信息映射到点云帧，以获取点云中各物体的种类信息，并对点云点赋予语义标签信息，同时，对点云帧中点的位置信息、强度信息以及语义类别信息进行存储，实现语义点云帧的构建；

步骤S1-3：点云分割子模块，将语义点云帧投影为距离图像，根据点的环线，计算图像中点和相邻点的夹角，如果夹角小于第二设定阈值则判断为地面，将地面点云分离出来；将分割掉地面后的点云通过深度优先遍历递归查找点，比较相邻四个点的相对角度，如果角度大于第三设定阈值则认为是同一点云簇，剔除点数量少于第四设定阈值的点云簇，实现对点云去噪，并对同一点云簇进行标记；

步骤S1-4：特征点提取子模块将去噪后的点云帧的360°方向均分成多个区域，计算每个区域中点的曲率，如果曲率大于第五设定阈值则为线特征，小于第五设定阈值则为面特征；每个区域内选择多个面特征和多个线特征；

所述点的曲率通过从当前点的左右各取多个点与当前点的深度差值计算，公式如下：

；

其中，表示点的集合，表示当前点的深度，表示相邻点的深度，表示当前点的曲率；

在特征点选取过程中，每个方向上的特征点数量和类别是固定的，对已经选择为特征点的周围点不再进行选择；

步骤S1-5：激光里程计子模块，通过特征点提取子模块提取的线特征和面特征进行帧间匹配，得到两帧之间的变换矩阵；

步骤S1-6：语义地图构建子模块，通过特征集合与周围点云地图匹配得到帧到地图的匹配位姿，然后对节点之间的约束利用LM方法得到变换矩阵；具体包括：

将当前位置要求范围内的带有语义标签信息的特征点集合进行融合拼接得到语义点云地图，选取距离最近的一组特征集作为新节点和点云地图中的节点计算空间距离约束，通过LM优化得到位姿变换；

根据IMU惯性模块数据时间戳与环境信息采集设备扫描时间戳对应，通过线性插值计算当前时刻IMU惯性模块的横滚角和俯仰角，根据LM残差平方根设置权重，将IMU惯性模块数据的角度与环境信息采集设备扫描匹配的角度融合，更新得到最终的变换矩阵：

；

其中，表示融合系数，表示IMU惯性模块的变换矩阵，表示环境信息采集设备扫描匹配的变换矩阵。

2.根据权利要求1所述的探索未知环境的人机协同系统，其特征在于，所述步骤S1-2包括：

使用实时语义分割网络对相机获取的图像帧进行处理，以获取每一帧的语义图像，对去畸变后的点云帧和分割后的语义图像帧进行时间戳对齐；

根据转换矩阵，将点云帧投影到相应的图像帧上，过滤掉不在相机视野内的点；根据点的坐标计算点在距离图像中的行列索引，保留距离更近的点，将这些点根据相机模型将3D坐标点转为2D坐标点，并根据2D坐标得到相对应的语义点标签；

对每个映射到图像帧中的点云点计算邻域范围内是否有与当前点语义标签不同的点，若存在则标记为边缘点，即该点为两种语义之间的边界；遍历图像中所有像素的点，判断相邻范围内是否存在边缘点，若存在，则判断两点之间的强度信息是否在第一设定阈值范围内，如果满足要求，则将该点的语义标签赋值给边缘点。

3.根据权利要求2所述的探索未知环境的人机协同系统，其特征在于，所述S1-2还包括：

设定映射点云的距离阈值，防止点云帧误映射到图像帧中天空区域。

4.根据权利要求1所述的探索未知环境的人机协同系统，其特征在于，所述步骤S1-5包括：

根据步骤S1-3中对点云聚类的结果，将标签信息作为帧间匹配的约束条件，只有标签相同的点能进行匹配；采用LM方法得到帧间位姿，通过面特征匹配，优化相邻两帧点到面的距离，得到：，其中，表示两帧在z轴方向上的移动距离，表示两帧横滚的角度，表示两帧俯仰的角度；通过线特征匹配，优化相邻两帧点到线的距离，得到，其中，表示两帧在x轴方向上的移动距离，表示两帧在y轴方向上的移动距离，表示两帧航向的角度；将和融合得到两帧之间的变换矩阵；

面特征点的距离的计算公式如下：

；

其中：表示第+1帧中的第i个特征点的坐标；表示第帧中离第i个特征点最近的点的坐标，该点称为j；表示第帧中在点所在的扫描束上离最近的点的坐标；表示第k帧中在点所在的扫描束的相邻束上离最近的点的坐标；

线特征点的距离的计算公式如下：

。

5.根据权利要求1所述的探索未知环境的人机协同系统，其特征在于，所述步骤S2包括：

步骤S2-1：对相机进行标定，得到相机的内参矩阵和畸变参数作为先验；

步骤S2-2：根据Apriltag码上提取的四个角点与世界坐标系中的四个对应假设点形成四个点对，通过直接线性法求解得到单应性矩阵，通过单应性矩阵和相机内参得到世界坐标系到相机坐标系的转换矩阵。

6.根据权利要求1所述的探索未知环境的人机协同系统，其特征在于，所述服务器端包括消息获取模块和点云处理模块；

所述服务器端的工作过程包括：

步骤S3：通过无人设备端和服务器端设置ROS主从机的方式，服务器端的消息获取模块从无人设备端获取消息；根据ROS话题，设定获取频率，在消息中读取并保存转换矩阵以及无人设备端构建的语义点云地图和实时定位轨迹信息；

步骤S4：点云处理模块将获取的ROS消息数据转换为旋转矩阵，结合平移向量生成转换矩阵，与点云的点坐标位置进行矩阵乘法，得到点云在Apriltag码坐标系下的坐标：

坐标系变换公式表示如下：

；

；

其中，表示转换矩阵，表示旋转矩阵，表示平移向量，表示点云中点在无人设备坐标系下的坐标， 表示点云中点在Apriltag坐标系下的坐标。

7.根据权利要求1所述的探索未知环境的人机协同系统，其特征在于，所述增强现实设备端包括坐标变换模块和显示模块；

所述增强现实设备端的工作过程包括：

步骤S5：坐标变换模块与无人设备端识别同一位置的Apriltag码，计算转换矩阵，将增强现实设备端置于Apriltag码坐标系下；

步骤S6：显示模块获得服务器端发送的点云中点的坐标位置以及当前无人设备端的位置，在增强现实设备上对点进行绘制，实时显示无人设备端构建的地图。

8.根据权利要求7所述的探索未知环境的人机协同系统，其特征在于，所述显示模块包括模型绘制子模块和虚实融合子模块；

所述步骤S6包括：

步骤S6-1：模型绘制子模块通过Unity引擎将点通过方块的形式表示，并根据不同的语义呈现不同的颜色；

步骤S6-2：虚实融合子模块对建模的点云进行渲染，根据模型矩阵、视图矩阵和投影矩阵之间的坐标变换，实现虚拟场景和真实场景的定位配准，将虚拟信息准确呈现到真实环境中。

9.根据权利要求8所述的探索未知环境的人机协同系统，其特征在于，所述显示模块还包括人机交互子模块，用于实现与用户的交互功能，通过识别用户操作，生成操控命令实现对场景的控制。