CN103926933A - 一种无人飞行器室内同时定位与环境建模方法 - Google Patents

Abstract

本发明公开了一种无人飞行器室内同时定位与环境建模方法，可以对无人飞行器进行飞行轨迹与定位，并快速绘制室内三维环境模型。构建无人飞行器的操作平台和操作方法，利用其上搭载的RGB-D传感器对数据进行收集，并采用视觉测程法对无人飞行器在室内飞行的飞行轨迹及位置进行估计，采用扩展的卡尔曼滤波算法得到更为准确的飞行器飞行轨迹及位置。进一步，利用无人飞行器与地面通信装置的数据传输功能使其将机上传感器收集信息实时反馈给地面操作人员，并利用计算机三维数据显示方法对收集数据进行计算、处理与室内三维环境建模与显示。

Description

一种无人飞行器室内同时定位与环境建模方法

技术领域

本发明属于人工智能以及计算机视觉领域，特别涉及机器人室内定位以及三维环境建模方法。

背景技术

随着智能机器人的研究不断发展，利用机器人代替人对环境进行探索和建模已经逐渐成为可能。需要解决的关键问题是机器人的室内定位和环境建模方法。

目前研究人员所研究和实验的机器人平台，大多数是地面机器人，例如智能车，人形机器人以及其他一些功能性的机器人，这些研究取得了一定的成果，覆盖了人工智能以及图像识别等很多领域。然而对于一些特定的环境，仅依靠地面机器人难以完成对环境探索的任务。机器人所处的环境一般分为室外环境和室内环境。相对于室外开阔的环境，室内环境中的智能体探索更具挑战性，首先机器人所处的环境更为杂乱和狭小，这为机器人避障增加了难度；其次在室内环境下一般没有GPS等外部导航***的支持，所以无法直接获取自身的位置信息，通常这种问题采用同时定位与构图（SLAM）算法来解决。

SLAM(simultaneous localization and mapping)，也称为CML(Concurrent Mapping andLocalization)，即时定位与地图构建。SLAM最早于1988年提出。由于其重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。同时定位与构图问题可以描述为：移动机器人在自身位置不确定和未知环境中运动时逐步构建周围环境的地图，同时运用此地图对机器人位置和姿态进行估计，进行自主定位和导航。无人飞行器在室内飞行，首先利用外部传感器来估计相对位置变化。由于传感器自身是有误差的，特别是大规模环境中，这种测量错误会逐渐累加。一旦飞行器两次或多次通过同一个地点，就可能会算出来不一致的坐标。为了解决这一问题，首先会选用某种滤波器来融合各种可用的数据，减弱噪声的影响，获取更准确的相对位置变化估计值。另外，SLAM算法还会定期地用当前环境特征与历史环境特征进行匹配，来检测飞行轨迹是否存在一个环。如果存在，就根据环的信息来矫正之前的飞行轨迹。经过这样的定期优化过程，飞行器就能够在大规模环境中绘制出更准确且全局一致的地图。

对于外部RGB-D（即颜色、深度信息）传感器的选择，使用PrimeSense或与其相似的三维感知与扫描传感器设备。它包含有普通摄像头与红外感知设备。其距离感知原理是一种光编码技术，用光源照明给需要测量的空间编上码，这种光源叫做激光散斑，是当激光照射到粗糙物体或穿透毛玻璃后形成的随机衍射斑点。这些散斑具有高度的随机性，而且会随着距离的不同变换图案。也就是说空间中任意两处的散斑图案都是不同的。只要在空间中打上这样的结构光，整个空间就都被做了标记，把一个物体放进这个空间，只要看到物体上面的散斑图案，即可知道这个物体所在位置。

对于一些室内环境，例如地震后的倒塌的房屋，坍塌的地下矿井，甚至对于一个高层大楼，对于没有自主爬楼梯能力的地面机器人，要完成室内搜索和建模都是不可能的，面对这样的需求近年来科研人员提出将无人飞行器作为在室内复杂环境中执行探测任务的平台。因此实现无人飞行器的自主室内定位和三维环境建模，成为了一个亟待解决的问题。然而，相比于地面机器人，在无人飞行器上实现一个完整的环境探测与建模***存在着很多挑战，主要有以下几个：

1）有限的载重：作为一种飞行器，需要不间断地提供升力才能维持自身的平衡，因此载重是有限的，难以像地面机器人那样，可以携带各种各样的重型传感器。对于外界环境信息的获取势必存在一定的局限性。

2）有限的计算能力：有限的载重决定了无人飞行器智能携带一个轻量级的嵌入式计算机***，而基于无人飞行器的SLAM算法以及联机搜索策略时间复杂度较高，同时还要保证实时性，无人飞行器的计算能力是有限的，所以必须对相关算法进行特殊的优化或重新设计。

3）动作变化快：无人飞行器运动速度很快，任何延迟都可能会被这种特性放大，导致更大的错误，因此***的各个模块都要保证一定的实时性。

4）频繁的震动：无人飞行器不能像地面机器人一样停止不动，在空中会轻微震动，这给规划工作带来了很大的困难。

5）三维空间：无人飞行器运行在三维空间中，以前的一些针对地面机器人的联机搜索策略往往面对二维空间，三维空间会使智能体的状态空间复杂度提升。另外，改变高度时，可能会有障碍物突然出现或消失。这要求它具备良好的鲁棒性和实时性。

6）需要对速度进行估计：四旋翼飞行器属于欠驱动***，为了更好的控制，需要准确的估计速度。

7）相对位置估计：为了实现无人飞行器的室内搜索策略，需要准确的进行室内定位，基于地面机器人的测程法可以直接获取相对位置估计进行定位，但无人飞行器上的惯性测量单元是轻量级的，动作计算误差较大，没法直接使用。因此，必须借助外部传感器来间接地进行位置估计。

从以上可以看出，目前无人机上的室内探索和建模仍存在着很大的挑战和研究前景，具有很大的科学意义。

发明内容

为了克服上述缺陷，本发明提出一种无人飞行器室内同时定位与环境建模方法，可以利用本发明中所涉及的方法实现无人飞行器的室内飞行快速定位并绘制室内三维模型。

为实现上述发明目的，提出一种无人飞行器室内同时定位与环境建模方法，其特征在于：

步骤1）：构建无人飞行器硬件结构，机上搭载嵌入式主控CPU、飞行控制板、RGB-D传感器，以及供电设备和无线传输设备；

步骤2）：控制无人飞行器在室内飞行，并利用RGB-D传感器收集数据；

步骤3）：假设飞行器在室内空间飞行的某时刻为t，采用视觉测程法估计t时刻飞行器动作与t-1时刻相比的变化情况。该方法具体包括平移与旋转两种变化，实现如下：首先基于满足实时要求的特征检测技术与特征匹配技术来得到前后两帧的对应特征对，并利用一些限制，如长度和角度来去掉一些异常的特征匹配，然后基于最小误差原则计算飞行位置的改变量；

更优选的，我们可以使用仅基于RGB-D传感器来自其自身集成红外传感器获取的深度信息来完成视觉测程算法，实现在黑暗无光照等环境中的定位与建模；

更优选的，在上述方法中采用计算机多线程处理机制，用来保证各模块的独立运行和模型的实时计算、生成与展示效果；

步骤4）：由于无人飞行器的运动模型是非线性***，所以在数据融合阶段，我们需要平滑在视觉测程法中估计的飞行位置和真实位置之间的差距。显然，在非线性***中，被普遍采用的处理方法卡尔曼滤波器有一定缺陷，因此采用扩展卡尔曼滤波器算法，来完成对飞行轨迹曲线的降噪和平滑；

步骤5）：在机上完成视觉测程法与滤波算法后，即可得到初步的位置估计。地面站必须与飞行器进行数据通信来获取室内环境的RGB-D数据与估计的位置，供路径规划和室内环境建模使用；

步骤6）：通常，飞行器上算法得到的位置估计的误差会随着时间而累积。当环境很大时，这种误差的累积现象会导致整个环境地图的紊乱。为了解决这一问题，采用闭环检测技术优化飞行轨迹。该技术具体指：当飞行器在室内空间飞行时，每当飞行轨迹与之前某时刻的飞行轨迹重合时，即构成了一个封闭的环路，此时***将进行再次迭代算法对形成环路的飞行轨迹进行修正，从而减小位置估计误差；

步骤7）：经过步骤3、4、5、6的优化后，得到更为准确的飞行轨迹与RGB-D数据。为了完成实时显示效果，采用OpenGL计算机图形算法接口对RGB-D数据进行处理从而实现三维地图的实时显示。

本发明的优点在于，我们可以通过该技术利用无人飞行器进行位置室内环境建模与路径可视化呈现。无人飞行器具有灵活性强、控制性好、不受地面环境影响等多种优点，而搭载RBG-D传感器等相关设备以及利用本发明涉及到的相关方法的无人飞行器更可以在室内环境监测与测绘中起到更为重要的作用。因此作为一个崭新的发明技术，它可以应用的领域是十分广泛的。

首先，基于RGB-D传感器的具有三维视觉与建模功能的无人飞行器可以在位置环境下的地下矿井探测、火情侦察、地震等自然灾难后的应急处理中起到举足轻重的作用。可以在相对较短的时间内对位置环境进行整体的三维模型计算、建立、显示与分析使用，大大提高了井下工作人员、火情及应急救援人员获得现场环境最新情况的能力，为人们进一步采取措施节约了更多的时间。

基于RGB-D传感器的具有三维视觉与建模功能的无人飞行器最基础的功能就是完成和位置环境相关的室内复杂环境扫描与测绘功能。利用这一特点，可以更加方便人们对室内环境进行捕捉与建模，从而更好地利用生成的模型进行下一步的室内定位与导航相关的工作，为“智慧城市”的设想提供可能性。

该技术可以用于无人飞行器的室内自主飞行与避障功能的实现上。由于基于RGB-D传感器的三维视觉建模技术可以短时间高效的生成更为精准的室内环境模型，所以可以在更短的时间内将模型信息加以利用，从而使无人飞行器可以感知到前方障碍物的方向与距离，综合自身所搭载的惯性单元以及获得的飞行数据信息可以更为方便的是无人飞行器躲避在空间飞行中遇到的障碍物，为真正实现无人飞行器的室内自主飞行提供可能。

该技术也可以在大量的公共场所得以应用。比如说，我们可以在博物馆、运动场等公共场所通过三维视觉建模对初次来到公共场所的人们进行陌生路况导航。进一步，也为盲人等残疾人士的位置寻路与导航提供可能。我们通过模型的生成与飞行、行走路线的显示告诉使用者自己身处地图中的具体方位，从而选择接下来要前往的地点。

附图说明

图1***硬件结构图；

图2实现飞行器室内定位及建模的方法处理流程；

图3视觉测程法流程图。

具体实施方式

下面结合附图，对本发明的技术方案作进一步的详细说明。

首先，我们通过图1对本发明中所涉及的***硬件进行整体搭建。在平台搭建的过程中应遵循载重量最小原则以保证无人飞行器更够在室内空间获得相对更为平稳、灵活的飞行姿态，以便我们进行接下来的数据采集与计算工作。

接下来利用图2所示的***软件框架搭建无人飞行器软件***，利用其上搭载的RGB-D传感器对外部数据进行收集，并采用视觉测程法对无人飞行器在室内飞行的路线轨迹及位移进行计算与滤波，从而得到飞行器的相对更为精确地飞行数据。进一步，我们利用无人飞行器与地面接收装置的数据传输功能使其将机上传感器收集信息实时反馈给地面操作人员，并利用计算机三维数据显示方法对收集数据进行计算、处理与模型显示。

具体实施步骤如下：

步骤1）：从重量和稳定性两个方面来搭建无人飞行器，并考虑进行室内飞行需要满足的灵活性与操控性。由于提供飞行动力的电池电压输出大小有波动，为了提高稳定性，为机上计算***单独配备足够运行飞行时间的稳压电源。

步骤2）：在无人飞行器上安装RGB-D传感器，并调整无人飞行器平衡。通过飞行器与地面之间的数据通信与传输模块，控制飞行器在室内的飞行。同时，飞行器上搭载的计算***控制RGB-D传感器以30fps的速度采集320×240的RGB-D数据，并生成数据存储文件按照RGB信息、三维空间坐标信息、深度信息格式存储，为接下来的飞行轨迹计算、定位与室内环境建模提供原始数据。

步骤3）：对前后两帧所存储的文件中的RGB信息做单独提取与特征分析。首先利用灰度图生成浮点算法对RGB图像进行灰度处理，即将RGB数据中的每个数值进行如下公式运算：R*0.3+G*0.59+B*0.11并将结果作为灰度值，即可生成RGB图对应的灰度图。然后对灰度图进行维纳滤波去除模糊处理，该方法是一种相对较为基本的图像去除模糊算法，设置灰度图像的期望输出与实际输出之间的差值为误差，对该误差求均方，即为均方误差，当均方误差越小，噪声滤除效果就越好，从而解决飞行器在室内移动导致图像模糊而无法检测到有效特征的问题。接着，在图像中某一随机位置寻找一个特征候选点，选取候选点周围的一些像素进行比较，来确定此点是否是特征点。如果周围有足够多的点都与候选点的像素差别很大，则认为该候选点是特征点，这种选取特征点的方法也称为FAST特征提取。然后基于最小误差原则计算飞行位置的改变量。

最后需要排除由于RGB-D传感器硬件自身原因采集到的无深度信息的特征点，确定成功匹配的特征点。

更优选的，可以选取其他去除模糊算法优化上述的维纳滤波处理，从而达到更好地去除模糊效果。

更优选的，可以在RGB-G传感器获取的深度信息数据中里进行FAST特征提取，并利用上述过程计算出相对动作变化，适应黑暗室内环境。

更优选的，在进行FAST特征提取后，可以使用SAD图像匹配算法进行特征粗匹配。其基本流程如下：

1.在图像内构造一个小窗口，类似于卷积；

2.用窗口覆盖左边的图像，选择出窗口覆盖区域内的所有像素点；

3.同样用窗口覆盖右边的图像并选择出覆盖区域的像素点；

4.左边覆盖区域减去右边覆盖区域，并求出所有像素点差的绝对值的和，也称为SAD值；

5.移动右边图像的窗口，重复3，4的动作，指定搜索范围，当搜索结果不在指定范围内时将结束本动作，进行下一动作；

6.找到这个范围内SAD值最小的窗口，即找到左边图像的最佳匹配的像素块。

更优选的，可以对最终选定的特征点进行正常值检测，为了排除可能存在的异常的特征点，从而提高动作估计的准确性。正常值，即利用选取特征点所在区域的长度、角度或其他限制条件，如灰度变化等进行比较，设定合理的数值作为判定特征点是否有效的标准进而筛选出匹配正常的特征点。

步骤4）：由于无人飞行器的运动模型是非线性***，所以在数据融合阶段，我们需要平滑在视觉测程法中估计的飞行位置和真实位置之间的差距。显然，在非线性***中，被普遍采用的处理方法卡尔曼滤波器有一定缺陷，因此采用扩展卡尔曼滤波器算法，来完成对飞行轨迹曲线的降噪和平滑。具体算法如下：EKF的基本思想是将非线性***线性化，然后进行卡尔曼滤波，从而得到所需的较为精确的飞行轨迹。

EKF算法通过对观测量Y_k的更新，获得对状态向量X_k的估计，他们满足如下关系：

X(k+1)=f(X_k)+W_k

Y(k)=h(X_k)+V_k

其中，X（k+1）为由前一时刻Xk的值估计出来的，f（X_k），h（X_k）为非线性的过程模型和观测模型；Y_k是可以获得的观测向量；W_k和V_k为过程和测量误差，假设两者相互独立，且均为白高斯噪音，即具有如下概率分布：p(W)～N(0，Q)，p(V)～N(0，R)。

X_k|k-1=F*X_k-1|k-1

P_k|k-1=F*P_k-1|k-1F^T+wQ_kw^T

K_k=P_k|k-1*H_k ^T[R_k+H_k*P_k|k-1*H_k ^T]^-1

X_k|k=X_k|k-1+K_k*[Y_k-h_k*(X_k|k-1)]

P_k|k-1=[I-K_k*H_k]*P_k|k-1

第一个公式F为状态转换矩阵，X_k-1|k-1为k-1时刻的状态向量，得到k时刻估计的X_k|k-1。

第二个公式为协方差更新，Q_k为过程噪声，P_k-1|k-1是k-1时刻的协方差矩阵，P_k|k-1为k时刻的协方差矩阵。

第三个公式为卡尔曼增益，控制收敛速度，K_k为卡尔曼增益，R_k为测量噪声，H_k是k时刻h函数对X的偏导的雅克比矩阵。

第四个是要得到的在k时刻的最优估计值X。

第五个对h（X_k）线性化方法，其中I为单位矩阵。

经过EKF算法可以得到视觉测程法的更为精确的位置移动，将精确位移传送回地面进行显示。

步骤5）：通过上一步我们在无人飞行器的机上完成视觉测程法与滤波算法后，得到了飞行器自身飞行的初步位置估计。之后利用无人飞行器与地面站的通信原理使两者进行通信获取RGBD数据与估计的位置，供路径规划或其他功能使用。由于考虑到无线网络传输稳定性较差，且带宽有限。因此，在网络传输方案中，选取UDP网络传输协议来传输RGB-D数据，可以允许丢失个别帧进而达到很快的速度。

步骤6）：采样历史RGB帧，新得到的帧与之进行匹配。若发现当前帧与历史某帧重合，则判断为该无人飞行器在之前某一时刻飞过该点，并形成了一个封闭的环。当室内环境较大时，步骤3，4中所述的无人飞行器的位置估计将会出现一定误差。这种误差的累积现象会导致整个环境地图的紊乱。为了解决这一问题，采用闭环检测技术优化三维地图。该技术具体指：当飞行器在室内空间飞行时，每当飞行轨迹与之前某时刻的飞行轨迹重合时，即构成了一个封闭的环路，此时***将进行再次迭代算法对形成环路的飞行轨迹进行修正，从而减小位置估计误差。

步骤7）：经过步骤3、4、5、6的优化后，得到更为准确的飞行轨迹与RGB-D数据。为了完成实时显示效果，采用OpenGL计算机图形算法接口对RGB-D数据进行处理从而实现三维地图的实时显示。

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

Claims (7)

1.一种无人飞行器室内同时定位与环境建模方法，其特征在于，

步骤1）：构建无人飞行器硬件结构，机上搭载嵌入式主控CPU、飞行控制板、RGB-D传感器，以及供电设备和无线传输设备；

步骤2）：控制无人飞行器在室内飞行，并利用RGB-D传感器收集数据；

步骤3）：假设飞行器在室内空间飞行的某时刻为t，采用视觉测程法估计t时刻飞行器动作与t-1时刻相比的变化情况；该变化情况具体包括平移与旋转两种变化，具体实现如下：

首先基于特征检测技术与特征匹配技术来得到前后两帧的对应特征点，并利用长度、角度限制条件去掉一些异常的特征匹配，然后基于最小误差原则计算飞行位置的改变量；

步骤4）：在数据融合阶段，平滑在视觉测程法中估计的飞行位置和真实位置之间的差距；采用扩展卡尔曼滤波器算法，来完成对飞行轨迹曲线的降噪和平滑；

步骤5）：在机上完成视觉测程法与扩展的卡尔曼滤波算法后，即得到初步的位置估计；地面站必须与飞行器进行数据通信来获取室内环境的RGB-D数据与估计的位置，供路径规划和室内环境建模使用；

步骤6）：采用闭环检测技术优化飞行轨迹，具体包括：当飞行器在室内空间飞行时，每当飞行轨迹与之前某时刻的飞行轨迹重合时，即构成了一个封闭的环路，此时将进行再次迭代算法对形成环路的飞行轨迹进行修正，从而减小位置估计误差；

步骤7）：经过步骤3、4、5、6后，得到更为准确的飞行轨迹与RGB-D数据；为了完成实时显示效果，采用OpenGL计算机图形算法接口对RGB-D数据进行处理从而实现三维地图的实时显示。

2.根据权利要求1所述的方法，其特征在于，搭载在无人飞行器上的RGB-D传感器是能够收集到室内准确图像信息和深度信息的传感器设备。

3.根据权利要求1所述的方法，其特征在于，还包括根据无人飞行器上RGB-D传感器采集到的室内环境信息通过地面计算***的OpenGL计算机图形算法接口实时显示颜色、深度信息，并利用数据进行三维室内环境建模。

4.根据权利要求1所述的方法，其特征在于，所述步骤2中使用仅基于深度图像来完成视觉测程算法，实现在黑暗无光照等环境中的定位与建模。

5.根据权利要求1所述的方法，其特征在于，所述步骤4中应用扩展的卡尔曼滤波算法(EKF)具体如下：

通过对观测量Y_k的更新，获得对状态向量X_k估计，其满足如下关系：

X(k+1)=f(X_k)+W_k

Y(k)=h(X_k)+V_k

其中，X（k+1）为由前一时刻X_k的值估计出来的，f（X_k），h（X_k）为非线性的过程模型和观测模型；Y_k是通过计算获得的观测向量；W_k和V_k为过程和测量误差，假设两者相互独立，且均为白色高斯噪音，即具有如下概率分布：p(W)～N(0，Q)，p(V)～N(0，R)；

X_k|k-1=F*X_k-1|k-1

P_k|k-1=F*P_k-1|k-1F^T+wQ_kw^T

K_k=P_k|k-1*H_k ^T[R_k+H_k*P_k|k-1*H_k ^T]^-1

X_k|k=X_k|k-1+K_k*[Y_k-h_k*(X_k|k-1)]

P_k|k-1=[I-K_k*H_k]*P_k|k-1

第一个公式F为状态转换矩阵，X_k-1|k-1为k-1时刻的状态向量，得到k时刻估计的X_k|k-1；

第二个公式为协方差更新，Q_k为过程噪声，P_k-1|k-1是k-1时刻的协方差矩阵，P_k|k-1为k时刻的协方差矩阵；

第三个公式为卡尔曼增益，控制收敛速度，K_k为卡尔曼增益，R_k为测量噪声，H_k是k时刻h函数对X的偏导的雅克比矩阵；

第四个公式是要得到的在k时刻的最优估计值X；

第五个公式是对h（X_k）线性化方法，其中I为单位矩阵；

经过EKF算法得到视觉测程法的更为精确的位置移动，将精确位移传送回地面进行显示。

6.根据权利要求1所述的方法，其特征在于，所述步骤5在网络传输中选取UDP来传输RGBD数据，容忍丢帧并能快速传输数据；同时，选取TCP协议来完成对准确性要求很高，但容量小的数据。

7.根据权利要求1或2所述的方法，其特征在于，采用计算机多线程处理机制，用来保证各模块的独立运行和模型的实时计算、生成与展示效果。