CN110243381B - 一种陆空机器人协同感知监测方法 - Google Patents

Abstract

本发明公开了一种陆空机器人协同感知监测方法，空中机器人和地面机器人通过处理原始图像和点云数据构建三维地图，提取高维特征用于构建地图增广描述子，先利用陆空机器人各自提取的描述子进行初步定位，再将初步定位结果作为陆空机器人的初始姿态，再利用三维概率地图进行密集匹配，实现精确定位，最终实现全局地图精确融合；由于本发明的陆空机器人之间只发送三维栅格概率地图，因此不要求高带宽网络，提高了环境感知的效率和鲁棒性。

Description

一种陆空机器人协同感知监测方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种陆空机器人协同感知监测方法。

背景技术

公共安全领域涉及自然灾害和安全事故等领域，急需对场地进行全方位的态势感知和实时监测，全面迅速探明情况。当前的公共安全感知预警主要依赖于固定摄像机，对突发事件、摄像头无法覆盖等情况缺乏实时感知能力。

应用智能机器人可提供移动的实时感知监测能力，在近两年受到社会各界的广泛关注。现有的智能机器人平台均为单一载体的解决方案，比如无人车或无人机。单一机器人平台在面临快速响应、机动部署和复杂环境等综合任务时往往面临着巨大的挑战。无人车具备了载荷充足，执行时间长的优势，但机动性能不足且视野有限。空中无人机在具有广域监控，灵活机动，快速部署等优点，但受到续航不足和载荷有限的限制。并且，在有限带宽通讯的情况下，难以实现对原始数据的实时传输，必须提取高维特征地图来表征环境。当前主流陆空协同监测方法均假设通讯带宽没有限制，通过共享原始数据的方式获取全局信息。因此，迫切需要一个系统去结合无人机和无人车的各自优势，使公共安全感知进入空地协同一体化的时代。

发明内容

有鉴于此，本发明的目的是提供一种陆空机器人协同感知监测方法，可实现全局地图精确融合。

一种陆空机器人协同感知监测方法，包括如下步骤：

步骤1、空中机器人通过相机获取监测区域的图像信息，地面机器人通过三维激光获取监测区域的激光点云信息；

步骤2、空中机器人利用图像信息生成空中机器人三维栅格概率地图m_uav，地面机器人利用激光点云信息生成三维栅格概率地图m_ugv；

步骤3、空中机器人与地面机器人之间将各自的三维栅格概率地图发送给对方；

步骤4、空中机器人与地面机器人对自身和从对方机器人接收的三维栅格概率地图中每个三维栅格的几何特征进行提取，其中第i个栅格的几何特征表示为feature(i)＝[x_i，y_i，z_i，α_i，β_i，γ_i，p_i]；[x_i，y_i，z_i]代表第i个栅格的几何坐标，[α_i，β_i，γ_i]表示第i个栅格的三维栅格边界轮廓，p_i为第i个栅格的栅格概率；将空中机器人三维栅格概率地图提取的所有几何特征构建成空中机器人的增广描述子集合g_uav；将地面机器人三维栅格概率地图提取的所有几何特征构建成地面机器人的增广描述子集合g_ugv；

步骤5、将步骤4提取的空中机器人增广描述子g_uav与地面机器人增广描述子g_ugv进行匹配，则地图匹配概率为：

其中T表示空中机器人与地面机器人之间的姿态转换矩阵；求得满足式(1)所示条件的姿态转换矩阵，记为T_initial；

T_initial＝min(-log p(g_ugv|T，g_uav)) (1)

步骤6，将步骤5获得的姿态转换矩阵T_initial作为陆空机器人之间的初始姿态转换矩阵，再对空中机器人三维栅格概率地图m_uav和地面机器人三维栅格概率地图m_ugv进行匹配，则栅格匹配概率为：

然后求得满足式(2)所示条件的姿态转换矩阵，记为T_final；

T_final＝min(-log p(m_ugv|T，(T_initial×m_uav)) (2)；

步骤7、利用步骤6获得的姿态转换矩阵T_final，将空中机器人三维栅格概率地图转移到地面机器人的坐标系中：T_final×m_uav；然后空中机器人三维栅格概率地图T_final×m_uav和地面机器人三维栅格概率地图m_ugv进行地图融合，得到全局地图。

较佳的，所述步骤5中，采用梯度下降的优化算法，通过对转换矩阵T进行不断更新，获得满足所述公式(1)的姿态转换矩阵T_initial。

较佳的，所述步骤6中，采用梯度下降的优化算法，通过对转换矩阵T进行不断更新，获得满足所述公式(2)的姿态转换矩阵T_final。

进一步的，所述步骤7中，在地图融合之前，利用相对熵值滤波器计算空中机器人和地面机器人三维栅格概率地图中两个对应栅格之间的KL-差异值，如果KL-差异值小于设定值时，则对应栅格进行融合；反之两个对应栅格不进行融合，最后融合地图中仍保留两个对应栅格的值。

较佳的，所述步骤7中，所述设定值取0.1.

较佳的，所述对应栅格进行融合时，将两个对应栅格的概率值求平均，作为最后的融合值，保存在融合后全局地图中。

较佳的，三维栅格概率地图的栅格概率为当前时刻概率和上一时刻概率之和。

本发明具有如下有益效果：

本发明的一种陆空机器人协同感知监测方法，空中机器人和地面机器人通过处理原始图像和点云数据构建三维地图，提取高维特征用于构建地图增广描述子，先利用陆空机器人各自提取的描述子进行初步定位，再将初步定位结果作为陆空机器人的初始姿态，再利用三维概率地图进行密集匹配，实现精确定位，最终实现全局地图精确融合；由于本发明的陆空机器人之间只发送三维栅格概率地图，因此不要求高带宽网络，提高了环境感知的效率和鲁棒性。

具体实施方式

针对当前陆空感知方法均假设通讯带宽充足并直接传输原始数据，导致在低带宽通讯环境下无法实现实时数据传输和感知。为解决该问题，本发明提供一种陆空机器人协同感知监测方法，空中机器人和地面机器人通过处理原始图像和点云数据构建三维地图，提取高维特征用于构建地图增广描述子，应用多层次概率定位框架实现精确定位并使用基于相对熵值滤波器的地图融合技术实现全局地图融合，提供全局感知信息。为使本发明的目的、技术方案和优势更加清晰，以下结合具体实施案例对本发明进一步详细说明。

步骤1，多源传感器融合：从空中机器人、地面机器人平台上的多种机载传感器获取原始数据，具体的讲，空中机器人通过相机获取图像信息，地面机器人通过三维激光获取激光点云信息。

步骤2，单机地图构建：将从步骤1生产的图像和激光点云原始数据处理后构建单机器人三维栅格概率地图。具体的步骤是将机器人平台r生成的三维栅格概率地图表征为m_r，该概率地图由三维栅格组成m_r，i，i＝1，2，...，N，N表示三维栅格的个数；zⁱ _1:t表示该机器人平台从时刻1：t得到的对应i栅格的原始数据观测(图像或激光点云)，三维栅格概率地图通过融合原始观测数据构建，其概率

由过去时刻

和当前时刻

概率更新而成，即

通过此过程可生成地面机器人三维栅格概率地图m_ugv和空中机器人三维栅格概率地图m_uav。与传统基于原始数据的环境表征方式不同，该概率地图具备跟随环境变化实时更新的特点，具有良好的动态环境适应性。此外，相比于占据大量存储空间的原始数据，该三位地图具有数据占空间小的优势适合在多机器人之间通讯共享。

步骤3，分布式数据传输：空中机器人与地面机器人在不依靠地面中转基站的情况下，空中机器人与地面机器人之间进行实时信息共享。该方式的主要特点为仅传递陆空机器人地图m_ugv和m_uav，不传递原始图像和激光数据，极大的减少了通讯冗余量，保证机器人之间实现实时通讯。

步骤4，增广描述子提取：

针对每一个机器人，对自身以及从对方机器人接收的三维栅格概率地图分别进行高维几何特征提取，每一幅三维栅格概率地图对应提取到各个三维栅格的几何特征feature(i)，每个特征feature(i)＝[x_i，y_i，z_i，α_i，β_i，γ_i，p_i]使用七维向量描述，通过遍历地图中每个栅格m_r，i计算对应feature(i)。具体的讲，[x_i，y_i，z_i]代表该栅格的几何坐标系，可直接从三维栅格概率地图中提取；[α_i，β_i，γ_i]代表是三维边界轮廓，可通过

进行计算(j表示i栅格的相邻序号)；p_i为栅格概率，可通过

得到。在得到每个特征feature(i)的基础上，将提取的所有feature(i)构建地图的增广描述子集合g_m＝{feature(i)}，其中，空中机器人的增广描述子集合用g_uav表示，地面机器人利用增广描述子集合用g_ugv表示。该步骤的主要特点在使用g_m用以表征三维地图的高维几何特征，而传统方法仅通过几何坐标系[x_i，y_i，z_i]表征三维地图。

步骤5，陆空协同初步定位：采用步骤4提取的增广描述子集合用于表征地图，利用概率匹配框架使空中机器人与地面机器人利用增广描述子g_ugv和g_uav进行初步定位，求得空中机器人与地面机器人之间的位置转换矩阵T。传统算法通常是基于坐标系绝对误差距离，缺乏概率表征特性。。陆空机器人地图间的匹配概率为

其中，

表示其概率大小反比于匹配误差，即匹配误差越小，概率越大。该匹配算法的创新特点在于使用梯度下降的优化算法，对陆空机器人地图间的匹配概率进行迭代。迭代过程将会执行1:N次，在第n次迭代过程中陆空机器人之间的定位误差将会不断减小，在每一次迭代过程中其姿态转换矩阵T将得到更新，并得到新的匹配概率。随后，第n次迭代过程输出的姿态转换矩阵T，将会作为第n+1次迭代过程转换矩阵T的输入。该匹配算法将会迭代执行，直到匹配概率误差低于规定阈值时停止迭代，并通过计算获得姿态转换矩阵T_initial。

T_initial＝min(-log p(g_ugv|T，g_uav))

步骤6，陆空协同高精定位：根据步骤5获得的陆空机器人此时的姿态转换矩阵T_initial，得到该姿态转换下陆空机器人的初步定位的关系。此时，该算法将姿态转换矩阵T_initial作为输入，继续进行三维地图m_ugv和m_uav间栅格的密集匹配，该匹配算法通过搜索最邻近节点并建立概率关联

使用梯度下降的优化算法使地图之间的匹配误差不断减小，实现精确定位达到毫米级。迭代过程将会执行1:K次，在第k次迭代过程中陆空机器人之间的定位误差将会不断减小，在每一次迭代过程中其相对姿态转换矩阵T将得到更新，并得到新的匹配概率。随后，第k次迭代过程输出的转换矩阵T，将会作为第k+1次迭代过程转换矩阵T的输入。通过提出步骤5-6的多层次概率定位算法，可以在不依赖原始数据的基础上，通过使用增广描述子和栅格提高定位算法的收敛速度，并提高定位精度。

T_final＝min(-log p(m_ugv|T，(T_initial×m_uav))

步骤7，全局地图融合：在获取精确定位信息T_final的基础上，利用转移矩阵关系T_finnal×m_uav将空中机器人地图转移到地面机器人的坐标系m_ugv中并进行地图融合(也可利用转移矩阵关系T_final ^-1×m_ugv将地面机器人地图转移到空中机器人的坐标系m_uav)。在地图融合的过程中，该算法采用相对熵值滤波器计算空中机器人地图栅格m_uav，i和地面机器人地图栅格m_ugv，j(m_ugv，j表示相对于空中机器人地图栅格m_uav，i的匹配对)的差异值

该KL_i，j值用于表征两个概率地图概率栅格m_uav，i，m_ugv，j之间的差异性。该阈值大小可根据对地图融合的容错度来设定，通常情况下可设置为0.1。全局融合地图M＝{M_l}_l＝(1:Z)(Z表示全局地图栅格个数)当差异性KL_i，j小于某阈值时，将两个栅格进行融合并使

若差异值大于某阈值，则拒绝对应栅格，并将两个栅格直接输入全局地图中，使M_l，l+1＝{m_ugv，j，m_uav，i}。最终，M表示最终融合生成的全局三维概率栅格地图。该步骤的创新在于拒绝融合熵值增大的匹配队并接受使融合熵值减小的匹配队，相比传统方法可进一步通过滤波实现的全局地图获得最小平均熵值。

本发明的算法的硬件平台可选为空中机器人和地面机器人。其中，空中机器人平台包括无人机平台，微型处理器，卫星定位系统，飞控系统，照相机，二维激光雷达，电池系统，通讯设备等；地面机器人平台包括无人车平台，中央处理器，图形处理器，卫星定位系统，双目视觉相机，三维激光雷达，电池系统，通讯设备等。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种陆空机器人协同感知监测方法，其特征在于，包括如下步骤：

步骤1、空中机器人通过相机获取监测区域的图像信息，地面机器人通过三维激光获取监测区域的激光点云信息；

步骤2、空中机器人利用图像信息生成空中机器人三维栅格概率地图m_uav，地面机器人利用激光点云信息生成三维栅格概率地图m_ugv；

步骤3、空中机器人与地面机器人之间将各自的三维栅格概率地图发送给对方；

步骤4、空中机器人与地面机器人对自身和从对方机器人接收的三维栅格概率地图中每个三维栅格的几何特征进行提取，其中第i个栅格的几何特征表示为feature(i)＝[x_i，y_i，z_i，α_i，β_i，γ_i，p_i]；[x_i，y_i，z_i]代表第i个栅格的几何坐标，[α_i，β_i，γ_i]表示第i个栅格的三维栅格边界轮廓，p_i为第i个栅格的栅格概率；将空中机器人三维栅格概率地图提取的所有几何特征构建成空中机器人的增广描述子集合g_uav；将地面机器人三维栅格概率地图提取的所有几何特征构建成地面机器人的增广描述子集合g_ugv；

步骤5、将步骤4提取的空中机器人增广描述子g_uav与地面机器人增广描述子g_ugv进行匹配，则地图匹配概率为：

其中T表示空中机器人与地面机器人之间的姿态转换矩阵；求得满足式(1)所示条件的姿态转换矩阵，记为T_initial；

T_initial＝min(-log p(g_ugv|T，g_uav)) (1)

步骤6，将步骤5获得的姿态转换矩阵T_initial作为陆空机器人之间的初始姿态转换矩阵，再对空中机器人三维栅格概率地图m_uav和地面机器人三维栅格概率地图m_ugv进行匹配，则栅格匹配概率为：

然后求得满足式(2)所示条件的姿态转换矩阵，记为T_final；

T_final＝min(-log p(m_ugv|T，(T_initial×m_uav)) (2)；

步骤7、利用步骤6获得的姿态转换矩阵T_final，将空中机器人三维栅格概率地图转移到地面机器人的坐标系中：T_final×m_uav；然后空中机器人三维栅格概率地图T_final×m_uav和地面机器人三维栅格概率地图m_ugv进行地图融合，得到全局地图；

其中，在地图融合之前，利用相对熵值滤波器计算空中机器人和地面机器人三维栅格概率地图中两个对应栅格之间的KL-差异值，如果KL-差异值小于设定值时，则对应栅格进行融合，将两个对应栅格的概率值求平均，作为最后的融合值，保存在融合后全局地图中；反之两个对应栅格不进行融合，最后融合地图中仍保留两个对应栅格的值。

2.如权利要求1所述的陆空机器人协同感知监测方法，其特征在于，所述步骤5中，采用梯度下降的优化算法，通过对转换矩阵T进行不断更新，获得满足所述公式(1)的姿态转换矩阵T_initial。

3.如权利要求1所述的陆空机器人协同感知监测方法，其特征在于，所述步骤6中，采用梯度下降的优化算法，通过对转换矩阵T进行不断更新，获得满足所述公式(2)的姿态转换矩阵T_final。

4.如权利要求1所述的陆空机器人协同感知监测方法，其特征在于，所述步骤7中，所述设定值取0.1。

5.如权利要求1所述的陆空机器人协同感知监测方法，其特征在于，三维栅格概率地图的栅格概率为当前时刻概率和上一时刻概率之和。