CN104236548B - 一种微型无人机室内自主导航方法 - Google Patents

Abstract

本发明涉及一种微型无人机室内自主导航方法,属于微型无人机自主导航技术领域，该方法包括：基于RGB‑D相机和MEMS惯性传感器的微型无人机运动状态估计；基于RGB‑D相机和MEMS惯性传感器融合的三维环境实时建模；实时可信路径规划与路径跟踪控制，通过所述三个步骤实施位置控制和姿态控制，引导无人机跟踪预定路径飞行。本发明提高了无人机定位以及运动状态估计的精度；提高了微型无人机在室内的自主环境感知能力；生成的路径可在满足路径可行性的基础上有效保证定位精度；有效提高微型无人机的自主能力。

Description

一种微型无人机室内自主导航方法

技术领域

本发明属于微型无人机自主导航技术领域，特别涉及一种基于RGB-D相机和MEMS传感器的微型无人机室内导航方法及其系统。

背景技术

微型无人机具有体积小、机动性强、操作灵活、成本低廉等特点，可在危险及复杂环境下执行任务，因此近年来在侦察、救灾、环境探测等军用和民用领域得到了非常广泛的应用。如何在复杂室内环境(如高层建筑火灾现场、震后建筑物、塌方矿井等)下实现微型无人机的自主飞行是该领域的重要研究问题，与之相对应的自主导航、规划与控制是实现微型无人机自主飞行的关键技术所在。

室内环境是一类典型的复杂任务场景，与大尺度室外空间相比，结构复杂且非结构化，具有封闭三维空间、环境不确定性、无GPS定位信号辅助等特点。因此，在该类环境下实现自主飞行，要求微小无人机在无外部辅助导航设施、自身机载载荷和计算资源有限的情况下，仅采用机载传感器自主确定自身的位置、速度、姿态等运动状态，并利用传感器信息建立环境的三维模型，在此基础上自主规划最优飞行轨迹并控制无人机按照既定轨迹飞行，完成飞行任务。

这些关键技术在当前均有一定的局限性，难以完全满足自主室内飞行的要求。

由于室内环境下无法接收到GPS信号，或者信号的可靠性较低，因此微型无人机的状态估计与环境建模所依赖的机载传感器主要包括惯性测量器件和视觉传感器。其中典型的惯性测量器件主要包括陀螺和加速度计，可感测无人机运动过程中的角速度和加速度，基于惯性测量器件的测量数据可以确定无人机的姿态与速度。微机电器件(MEMS)具有重量轻、成本低、功耗低、集成度高等优点，可满足微型无人机的重量、体积和功耗限制，因此微型无人机系统一般采用MEMS惯性测量器件。但MEMS惯性测量器件的测量值往往存在较大的误差，且随时间漂移，因此必须采用视觉传感器的测量值与惯性测量值通过滤波技术进行信息融合，以修正误差，获得更高精度的状态估计值。目前的微型无人机采用的视觉辅助运动估计技术主要基于激光测距、单目视觉和双目视觉。

激光测距技术依赖激光测距仪所提供的无人机与环境的相对距离测量值，通过对前后两帧激光扫描值进行对比和匹配，解算出前后两帧数据之间的旋转、平移转换关系，从而得到无人机的运动状态参数。但激光测距技术存在很大的局限性。激光测距仪的测量范围有限，且本质上只能提供在测量平面内的二维相对距离信息，因此激光测距技术仅适用于具有特殊垂直结构的环境，当环境仅存在简单结构时(长廊、环形建筑等)，激光测距算法将会失效。

视觉运动估计技术基于摄像头器件获取的环境图像序列，提取连续图像序列中的特征点，对相同的特征点进行匹配，进而计算特征点之间的平移和旋转关系，获得运动体的运动参数。根据所采用的摄像头器件数量，视觉运动估计技术主要包括单目视觉和双目视觉技术。与激光测距技术相比，视觉运动估计技术可以充分利用环境的三维信息。单目视觉技术利用单个摄像头器件提供的环境图像信息计算运动体的方向参数，但无法提供运动体与环境的相对距离信息(即环境深度信息)。与之相比，双目视觉技术可对两个摄像头器件所获取的图像中的相同环境特征点进行立体匹配，从而计算深度参数，并可通过不同帧图像的特征提取、特征匹配和运动估计过程获得运动体的六自由度运动参数。但双目视觉算法一般较为复杂，运算量大，且视觉运动估计技术通常要求环境中包括足够丰富的纹理特征，因此该类技术在微型无人机运动状态估计中的应用也具有较大的局限性。

RGB-D相机是近年来在机器人和计算机视觉领域逐渐得到应用的新型视觉设备，该类型的设备可同时提供外部环境的彩色图像和三维深度数据(即环境中的点相对于RGB-D相机的距离)。其中三维深度的测量基于结构光技术实现：RGB-D相机主动向环境空间发射红外光线，并对环境空间的被测方向进行编码并产生光斑，进而通过检测环境中的光斑测量空间的三维深度。与传统的摄像头器件相比，RGB-D相机成本较低，体积较小，重量较轻；可直接提供三维深度数据，并能够在光照不良或纹理较少的环境中有效检测三维深度；因此更适用于室内环境中的微型无人机的运动估计和环境建模。

另一方面，自主环境感知和三维建模也是微型无人机实现室内自主飞行的关键技术，其目的是为了建立微型无人机所处环境的三维模型，为实现路径规划和控制提供基础。微型无人机机载传感器(激光测距仪、双目视觉系统)通过测量其与环境中各点的三维相对距离获得环境的深度信息，通常以三维点云(一系列的三维点)形式表达。通过对连续不同帧的三维点云数据进行配准和融合，即可得到完整的三维环境模型。当前的三维环境建模技术一般单纯利用视觉传感器获取的三维点云数据进行匹配，以计算不同三维点云之间的旋转和平移关系，本质上与运动估计为同一过程。在计算过程中，每一步中两帧数据之间匹配的误差会随着计算过程累积，易导致最终的建模误差。目前的系统一般利用SLAM(即时定位与建模)的闭环检测进行优化，但该类方法计算量较大，难以满足实时性要求。

路径规划指的是依据环境感知信息，在无人机状态空间内规划出一条从初始点到目标点的、满足各方面约束的最优连续路径的过程。目前常用的路径规划算法包括概率路径图(PRM)、快速随机树(RRT)、A*算法、混合整数线性规划(MILP)等。而在室内复杂环境下的路径规划具有其特殊性，微型无人机的定位和运动状态估计、以及环境建模的精度高度依赖传感器能够检测到的环境中有效特征信息，因此要求算法能够结合机载传感器的特性以及环境特征，自主规划出能够确保定位和运动状态估计性能的路径。在实际应用中，微型无人机一般采用滤波(如Kalman滤波等)的方法融合机载各传感器的测量信息，从而实现定位和运动状态估计。滤波方法的基本原理是认为无人机位置和各运动状态服从某种类型的概率分布模型(如高斯分布等)，进而根据机载各传感器的测量信息估计无人机的位置和各运动状态的后验概率分布(由概率论可知，概率分布以期望和协方差表示)。因此衡量定位和运动状态估计性能的主要指标为定位和运动状态估计的可信度(由概率论可知，通过滤波方法估计得到的后验概率分布的协方差的大小表征了定位和运动状态估计的可信度：若协方差较大，则定位和运动状态估计的可信度较低；反之则可信度较高)。而目前常用的方法在规划过程中均未考虑定位和状态估计可信度因素，因此难以完全适应室内自主飞行的要求。

因此，当前微型无人机系统的导航、环境建模以及路径规划方法均存在一定的局限性，难以满足室内环境下自主飞行的特殊需求。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种微型无人机室内自主导航方法及其导航系统，以实现在无外部定位和计算辅助的条件下，仅依赖机载传感器和设备的微型无人机自主定位、环境感知建模以及路径规划，提高微型无人机在复杂室内环境下的自主飞行能力。

本发明提出的一种微型无人机室内自主导航方法,其特征在于，该方法包括以下步骤：

S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计：利用RGB-D相机获取环境的二维彩色图像和三维深度数据，通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值，利用MEMS传感器获取惯性测量值，将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合，用以抑制惯性测量值误差积累，得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值；

S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模：利用步骤S1中RGB-D相机获取的三维深度数据，以及融合后的运动状态估计值进行三维点云的精确配准与融合，构建三维环境模型，并触发式地对该模型进行全局优化，用以提高模型精度；

S3:实时可信路径规划与路径跟踪控制：设定规划周期,在每个规划周期中，以步骤S2得到的三维环境模型为基础，同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径，在每个规划周期结束后，选择当前最优路径执行；根据所述当前最优路径，与步骤S1得到运动状态估计值进行比较，得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令，根据位置控制指令解算为无人机的姿态控制指令，实施位置控制和姿态控制，引导无人机跟踪预定路径飞行。

本发明提出的微型无人机室内自主导航方法及其导航系统，其优点是：

1、本发明提供的方法中，利用机载RGB-D相机和MEMS惯性传感器进行集成，实现了基于多传感器融合的状态估计，有效修正低成本MEMS惯性测量器件的误差积累，提高了无人机定位以及运动状态估计的精度，可在室内无GPS及其它外部定位辅助的特殊情况下，完全依靠机载传感器自主为微型无人机提供高精度、可靠的运动状态估计；

2、本发明提供的方法实现了基于RGB-D相机和MEMS惯性传感器融合的实时三维环境建模，基于融合后的高精度运动参数实现三维点云的精确配准和融合，有效提高了三维环境建模的精度；采用并行的闭环检测及优化过程对三维环境模型进行全局优化，可在避免影响建模实时性的前提下有效提高整体建模精度，提高了微型无人机在室内的自主环境感知能力；

3、本发明基于RGB-D相机的工作特性，实现了一种可信路径实时规划和路径跟踪控制方法，可有效适应不确定和动态环境，以及无人机的复杂动力学和运动学约束，生成的路径可在满足路径可行性的基础上有效保证定位精度；

4、本发明实现的微型无人机自主飞行系统可在复杂室内环境以及无外部定位和计算辅助条件下，完全依赖机载传感器和处理单元实现微型无人机的自主导航、环境感知与建模以及路径规划，有效提高微型无人机的自主能力。

附图说明

图1为本发明提出的微型无人机室内自主导航方法的总体流程框图；

图2为根据本发明实施例步骤1基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程图；

图3为根据本发明实施例步骤2基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程图；

图4为根据本发明实施例步骤3实时可信路径规划与路径跟踪控制流程图；

图5为根据本发明实施例步骤3可信路径规划框架示意图；

图6为根据本发明实施例的微型无人机室内自主导航系统结构示意图。

具体实施方式

下面结合附图和实施例对本发明的具体实施方式进行详细描述。

为达到所述目标，本发明的第一方面的目的在于提供一种微型无人机室内自主导航方法流程，如图1所示,该方法包括以下步骤：

S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计：利用RGB-D相机获取环境的二维彩色图像和三维深度数据，通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值，利用MEMS传感器获取惯性测量值，将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合，用以抑制惯性测量值误差积累，得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值；

S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模：利用步骤S1中RGB-D相机获取的三维深度数据，以及融合后的运动状态估计值进行三维点云的精确配准与融合，构建三维环境模型，并触发式地对该模型进行全局优化，用以提高环境模型精度；

S3:实时可信路径规划与路径跟踪控制：设定规划周期,在每个规划周期中，以步骤S2得到的三维环境模型为基础，同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径，在每个规划周期结束后，选择当前最优路径执行；根据所述当前最优路径，与步骤S1得到运动状态估计值进行比较，得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令，根据位置控制指令解算为无人机的姿态控制指令，实施位置控制和姿态控制，引导无人机跟踪预定路径飞行。

进一步地，步骤S1中所述的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程，如图2所示，具体包括以下步骤：

S11：通过RGB-D相机获取当前环境的二维彩色图像与三维深度数据，并将该二维彩色图像进行预处理，转换为以灰度值表示的二维灰度图像；

S12：从步骤S11预处理后的每帧二维灰度图像中提取出一组特征点，并计算每一特征点的特征描述向量，并通过步骤S11中获取的三维深度数据对所述特征点进行筛选，剔除与三维深度数据无对应的特征点；

S13：对相邻两帧图像中的特征点进行特征匹配，通过对比特征点的对应描述向量获得两组匹配的特征点,并再剔除匹配有误的特征点；

S14：根据特征点对应的三维深度数据，将步骤S13匹配后的特征点映射到三维空间，从相邻两帧三维深度数据中获得具有匹配关系的相邻两组三维点云；

S15：求解步骤S14中获得的该两组匹配的三维点云之间的旋转和平移参数，利用所述旋转和平移参数反算出无人机的姿态和位置参数估计值；

S16：根据无人机的动力学特性建立状态空间模型(其中选择无人机的位置、速度、姿态等作为状态空间模型的状态量)以及RGB-D相机、MEMS惯性传感器的观测模型，将S15中得到的无人机的位置参数估计值作为系统的状态空间模型的观测量，与MEMS惯性传感器提供的由角速度、加速度、磁观测量信息组成的惯性测量值通过滤波方法进行融合，进一步修正惯性测量值误差，以有效抑制惯性测量值误差积累，获得包括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。

进一步地，步骤S2中基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程，如图3所示，具体包括以下步骤：

S21：利用如步骤S16获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值，以及步骤S14获得的两组匹配的三维点云，求解所述两组匹配的三维点云之间的初始旋转和平移关系；

S22：利用步骤S21中获得的两组三维点云及其初始旋转和平移关系，定义目标函数为所述两组三维点云中的每对三维点云之间旋转和平移误差之和，通过最小二乘优化迭代方法优化所述目标函数，得到更为精确的三维点云之间的旋转和平移关系；

S23：定义与环境固连的全局坐标系，根据步骤S22得到的不同帧三维点云之间的相对旋转、平移关系，以及各帧三维点云与初始帧三维点云之间的旋转和平移关系，将连续帧的三维点云组合至统一的全局坐标系中，得到三维环境模型；

S24：采用通用的SLAM(同时定位与地图构建)方法进行闭环监测，当监测到闭环时，触发全局优化过程，进一步校正不同帧三维点云之间的旋转和平移关系，对步骤S23已经建立的三维环境模型进行优化，得到更为精确的的三维环境模型。

进一步地，所述步骤3中所述的实时可信路径规划与路径跟踪控制流程，如图4所示，具体包括以下步骤：

S31：对步骤S2获得的三维环境模型进行后处理，将三维环境模型转换为体元形式(栅格等)存储，以便于利用三维环境模型进行路径规划；

S32：设定无人机的起始点和目标点，根据步骤S16建立的状态空间模型设计控制律，建立无人机的闭环系统数学模型(在自动控制理论中，通过反馈可获得被控系统的实际输出，控制律用于根据反馈计算控制量，从而改变被控系统的实际输出，使其趋近期望值。包含所述反馈、控制律和状态空间模型的数学模型称为闭环系统数学模型)，将所述起始点、目标点和闭环系统数学模型作为无人机路径规划的输入条件，对路径进行初始化；

S33：设定单步路径规划周期为Δt(该周期根据无人机系统的实际计算能力、飞行速度等确定，一般Δt为秒的量级)，在Δt周期内，通过步骤S1获得无人机包括位置、速度、姿态在内的运动状态估计值，记为x(t)，并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt)；

S34：在同一个规划周期Δt的时间内，在S31获得的体元形式的三维环境模型中连续进行随机采样，获得一系列采样点，利用路径的代价以及定位和运动状态估计可信度作为启发式信息，根据该启发式信息在上一规划周期中生成的、以步骤S33预测得到的无人机状态x(t+Δt)为起点的路径集合中选择与每个采样点对应的最近点，利用步骤32中建立的无人机的闭环系统数学模型预测无人机的状态，生成由一系列可行路径组成的路径集合，并计算每条可行路径的代价以及定位和运动状态估计可信度，从而对路径连续进行增量式扩展；

所述路径代价代表无人机沿路径飞行所对应的代价，如该路径的总距离、无人机沿该路径飞行消耗的时间、燃料或电能等；

所述路径对应的定位和运动状态估计可信度的具体含义如下：由步骤1可知，无人机从某路径的起始点飞行到该路径终点的过程中，利用机载RGB-D相机可获取环境的图像和深度数据，在此基础上通过滤波方法可获得无人机的包括位置、速度、姿态在内的运动状态估计值，所述运动状态估计值的协方差阵的大小用于衡量无人机运动状态估计的可信度，因此路径终点对应的无人机运动状态估计值本质上为无人机沿该路径飞行过程中通过滤波方法获得的后验运动状态估计值，无人机运动状态估计值的协方差阵的大小即为该路径对应的定位和运动状态估计可信度。

S35：以路径的代价以及定位和运动状态估计可信度为选择指标，从步骤S34中生成的当前可行路径集合中选择最优路径，作为无人机的执行指令，引导无人机按照该最优路径飞行；循环执行步骤S33和S34，直至无人机飞行到目标点。

上述基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计流程，基于机载RGB-D相机环境的二维彩色图像和三维深度数据，通过特征点检测、特征点匹配、运动状态估计等计算过程，可获得稳定、可靠的微型无人机的姿态和位置参数估计值；利用机载RGB-D相机的运动状态估计值和MEMS惯性传感器提供的惯性测量值，实现了基于多传感器融合的无人机定位和运动状态估计，可有效修正低成本MEMS器件的误差和漂移，提高无人机定位以及运动状态估计的精度，可在室内无GPS及外部定位辅助的特殊情况下，完全依靠机载传感器自主为微型无人机提供高精度、可靠的无人机运动状态估计参数；

上述基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模流程，一方面通过RGB-D运动状态估计和MEMS惯性测量值的融合获得更高精度的无人机位置和姿态估计参数，并用于确定两组三维点云之间的相对关系，实现三维点云的精确配准和融合，相比于单纯采用图像或三维深度数据进行三维环境建模的方法，本发明提出的三维环境实时建模流程可获得更高精度的三维环境模型；另一方面，采用闭环检测方法触发式地对三维环境模型进行优化，以抑制误差的积累，同时触发式的优化方式也减少了整体的运算量，避免了对三维环境建模实时性的影响。

上述实时可信路径规划与路径跟踪控制流程的优点如下：采用基于随机采样的路径规划架构，可以有效适应无人机的复杂动力学和运动学约束，在大规模环境空间下降低计算复杂度，提高计算效率，同时生成的路径可保证路径代价的最优性；在路径规划过程中引入包含控制；率的无人机闭环系统数学模型进行状态预测，可保证生成的路径更接近无人机实际物理特性，减少路径跟踪控制误差；在路径执行的同时连续扩展路径，保证路径规划的实时性，可有效应对不确定和动态环境；将传感器特性和定位和运动状态估计可信度因素结合到路径规划中，使生成的路径在满足路径代价约束的同时保证无人机的定位性能。

本发明的方法可利用无人机的机载计算处理单元，通过常规的编程技术实现实时执行上述方法中的各项运算和处理过程。上述方法中通过RGB-D相机，用于获取环境的RBG图像信息和三维深度参数；MEMS惯性测量单元，测量并输出无人机当前的角速度、加速度等惯性测量参数。

实施例：

S1基于RGB-D和MEMS传感器的微型无人机运动状态估计，如图2所示，具体包括以下步骤：

S11:通过RGB-D相机获取环境的图像和深度数据，包括一系列二维色彩域(RGB)像素点构成的二维彩色图像以及对应的三维深度域数据。

对于三维环境中的某点p_i，RGB-D相机所获得的信息形式如下：p_i＝(x_i,y_i,z_i,r_i,g_i,b_i)，其中x_i,y_i,z_i为该点的三维深度数据，代表改点相对于RGB-D相机中心的三维位置，r_i,g_i,b_i为该点对应的色彩域(RGB)信息。因此，某时刻RGB-D相机视场中的所有环境点的所述如p_i形式的信息构成了RGB-D相机在该时刻产生的一帧二维彩色图像和三维深度数据。为进一步实现特征点提取，将所述二维彩色图像转换为二维灰度图像，转换公式如下：

G_i＝λ₁r_i+λ₂g_i+λ₃b_i

其中λ₁,λ₂,λ₃为转换系数，G_i为像素点转换后对应的灰度值。

S12:基于SURF(快速鲁棒图像特征法，已公开的通用方法)方法从步骤S11产生的二维灰度图像中通过灰度梯度极值识别并提取一组特征点，计算每个特征点邻域像素的灰度梯度方向，基于灰度梯度方向构建特征点的SURF特征描述向量，本实施例中采用128维的SURF特征描述向量。为减少错误特征点，利用步骤S11中RGB-D相机获取的三维深度数据对提取的特征点进行筛选，将无对应深度数据的特征点剔除。

S13:对比步骤S12提取的相邻两帧图像中特征点的特征向量，进行特征匹配。在本实施例中，对其中一帧图像中的特征点，以步骤S12中计算的两特征点的SURF特征向量的欧式距离来作为特征相似的测度，通过k-近邻搜索方法(通用方法)在相邻帧图像中寻找与某特征点最相近的k个对应特征点(本实施例中采用k＝2)。令p_i为所述一帧图像中的某个特征点，d_i1d_i2分别为特征点p_i与其最优和次优匹配点q_i1,q_i2的欧式距离，为提高匹配的准确性，设定ε₁,ε₂为阈值，当d_i1d_i2满足约束d_i1＜ε₁,d_i1/d_i2＜ε₂时，确认q_i1为p_i对应的匹配特征点(本实施例中ε₁＝0.5,ε₂＝0.8)。最后，通过RANSAC(随机一致性采样，已公开的通用方法)方法再剔除匹配有误的特征点。

S14:根据步骤S11中获取的特征点的三维深度数据，将S13产生的匹配后的特征点映射到三维空间，获得具有匹配关系的相邻两组三维点云。如步骤S11中所述，RGB-D中获取的单个点的参数如下：p_i＝(x_i,y_i,z_i,r_i,g_i,b_i)。针对步骤S13中获得的二维图像匹配点，从步骤S11获得的三维深度数据中提取出相邻两组二维图像匹配点对应的三维深度数据(x_i,y_i,z_i)，并将提取出的三维深度数据组成两组三维点云P_i＝{p_i},Q_i＝{q_i}，即可将两组具有匹配关系的二维图像特征点映射到三维空间，获得具有匹配关系的相邻两组三维点云：P_i＝{p_i},Q_i＝{q_i}，i＝1...n，p_i,q_i∈□³

S15:根据S14步骤获得的相邻两组具有匹配关系的三维点云之间的旋转和平移关系，求解无人机的姿态和位置参数估计值。具体过程如下：对于步骤S14获得的两组具有匹配关系的三维点云P_i＝{p_i},Q_i＝{q_i}，i＝1...n，其中p_i＝(x_i1,x_i2,x_i3)^T,q_i＝(y_i1,y_i1,y_i1)^T为三维点云中具有匹配关系的某一对特征点，在特征点严格匹配且不存在传感器误差的理想情况下，二者之间的关系可表示为：

Q_i＝RP_i+t

其中R∈□^3×3代表三维点云在三维空间的旋转，t代表三维点云在三维空间的平移；以上关系从物理意义上即对应相机(及其无人机)在采集两帧三维点云数据时刻之间的运动。因此通过计算连续两帧三维点云之间的旋转和平移关系即可估计无人机的相对运动。

在本实施例中，为提高计算精度和对噪声的鲁棒性，通过基于RANSAC架构的迭代方法求解最小二乘意义上的最优R,t，使得以下误差函数取最小值：

无人机在采集两帧三维点云数据时刻之间的相对运动可通过下式确定：

ΔR＝R^T,Δt＝-R^Tt

其中ΔR为无人机在采集两帧三维点云数据时刻之间的相对旋转，Δt为无人机在采集两帧三维点云数据时刻之间的相对平移。

因此，定义全局坐标系为固定在环境中三维坐标系，设无人机在全局坐标系的相对运动初始值为T₀，其中：

无人机在时刻t相对于全局坐标系原点的刚体运动T_c关系可通过以下的连续变换确定：

T_c＝T₀·ΔT_t-n+1·...·ΔT_t-1·ΔT_t

其中通过T_c即可反算出t时刻无人机在全局坐标系下的位置和姿态参数。

S16:根据无人机的动力学特性建立状态空间模型以及RGB-D相机、MEMS惯性传感器的观测模型，通过卡尔曼滤波(通用方法)将步骤S15中得到的RGB-D运动参数估计值与MEMS惯性传感器提供的测量值进行融合，修正惯性测量值误差，获得括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。

在本实施例中，采用状态空间方式表示无人机的系统模型，建立无人机系统的状态空间模型如下：

F(X)＝F(t)X+Gw

其中X为待估计的状态向量：

其中δs为位置误差，δv为速度误差，为姿态平台误差角，b_ω,b_f分别为MEMS惯性传感器中陀螺和加速度计的漂移。系统模型的输入量包括陀螺提供的角速度测量值和加速度计提供的加速度测量值。其中状态转移矩阵为：

建立无人机系统RGB-D相机的观测模型：

其中为步骤S15提供的无人机位置估计值，观测矩阵为H_s(t)＝[I_3×3 0_3×12]

建立无人机系统MEMS惯性传感器的观测模型：

其中观测矩阵为且

其中为通过MEMS惯性传感器中的加速度计和磁传感器的测量值计算得到的姿态矩阵，为通过MEMS惯性传感器中的陀螺的测量值积分得到的姿态矩阵。

在本实施例中，通过卡尔曼滤波方法的时间更新和观测更新过程实现状态估计。

卡尔曼滤波方法的时间更新过程如下：

X_k,k-1＝Φ_k,k-1X_k-1

其中P为卡尔曼滤波器的协方差阵，Q为系统噪声的协方差阵。

卡尔曼滤波方法的观测更新过程如下：

X_k＝X_k,k-1+K_k[Z_k-HX_k,k-1]

P_k＝[I-K_kH_k]P_k,k-1

其中K为卡尔曼增益矩阵，H为无人机系统观测模型相对于状态向量的Jacobian矩阵，R为观测噪声的协方差阵。

完成卡尔曼滤波的状态更新后，对状态向量进行修正，得到无人机的的位置、速度和姿态估计值：

1.位置：s_k＝s_k-δs_k

2.速度：v_k＝v_k-δv_k

3.姿态：

S2基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模，如图3所示，具体包括以下步骤：

S21：设当前时刻为i,利用如S16步骤获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值，以及S14步骤获得的当前时刻两组匹配的三维点云，求解所述两组匹配的三维点云之间的初始旋转和平移关系；

其中一帧三维点云P为某时刻RGB-D相机获取的一组三维深度数据的集合，即P＝{p_i},i＝1...n，P中的单个数据p_i∈P代表环境中对应点相对于相机中心的三维距离，即p_i＝(x_i,y_i,z_i)，RGB-D相机在不同时刻获得的三维点云表示为P₀...P_n；通过步骤S14获得的具有匹配关系的三维点云可表示为P_i＝{p_i},Q_i＝{q_i}。

利用步骤S16 RGB-D运动参数估计值和MEMS测量值融合后获得的运动状态估计值(主要利用融合后获得的无人机位置和姿态估计值)，进一步转化为当前时刻两组匹配的三维点云之间的旋转R_i0和平移关系t_i0，并整体表示为：

将该初始旋转和平移关系T_i0用于后续步骤的计算。

S22：基于步骤S21中获得的两组匹配的三维点云之间的初始旋转和平移关系，通过最小二乘迭代计算进一步求解更为精确的两组匹配的三维点云之间的旋转和平移变换关系。在本实施例中，采用基于RANSAC的方法迭代求解两组匹配的三维点云之间最小二乘意义上的旋转和平移关系，利用步骤S22得到的初始旋转和平移关系T_i0作为RANSAC方法的输入，通过重复的RANSAC迭代计算，获得两组匹配的三维点云之间的最优更为精确的旋转关系R_i和平移关系t_i。

S23:定义与环境固连的全局坐标系，根据步骤S22计算得到的匹配的三维点云之间的相对旋转和平移关系，计算每一帧三维点云P_i相对于初始帧三维点云P₀之间的旋转和平移关系T_i。根据以上旋转和平移关系，将每一帧三维点云进行组合：

通过以上过程将每一帧三维点云组合到全局坐标系下，最终得到三维环境模型P。

S24:为修正三维环境实时建模过程中的累积误差，通过SLAM闭环检测触发运行三维环境模型的全局优化过程，通过全局优化过程对步骤S23已经建立的进行三维环境模型进行优化，得到更为精确的三维环境模型。

在本实施例中，闭环指的是当RGB-D相机运动(即无人机的运动)到环境中之前曾经经过的位置时，RGB-D相机获取的环境特征点与以往某一帧图像中的特征点中有较大的公共部分，可根据所述公共部分的特征点对三维环境模型进行全局优化。

所述闭环检测的原理如下：在步骤S14的计算过程后，本实施例中以图(graph)的数据结构形式存储RGB-D相机获取的二维彩色图像、特征点和三维点云，并在步骤S15获得无人机的姿态和位置参数估计值后，进一步将姿态和位置参数估计值存储到已经建立的图中，其中图的节点C_i包括无人机相机当前的位置、姿态估计值以及在该点获取的二维彩色图像、特征点和三维点云，通过步骤S22得到不同帧三维点云之间的相对旋转和平移关系(R_ij,t_ij)后，将(R_ij,t_ij)作为图中相邻两节点C_i、C_j之间的边，同时也作为全局优化过程中的约束。在本发明的实例中，采用快速图像特征匹配方法(已公开的通用方法)检测闭环，全局优化过程采用触发式并行计算的机制。当检测到闭环时，触发全局优化过程，通过SBA(Spare Bundle Adjustment，通用方法)方法对已经建立的图进行全局优化求解。本实施例中首先在图中存储的连续帧二维彩色图像中以n帧为间隔，选择其中第k帧作为关键帧图像，并从图中获取当前帧二维彩色图像与关键帧图像之间的约束关系，当检测到闭环时，基于此触发Levenberg-Marquardt非线性优化计算过程(通用方法)，对所述的二维彩色图像与关键帧图像之间的约束关系和三维点云之间的旋转和平移关系进行优化，从而生成更精确的三维环境模型。

S3实时可信路径规划与路径跟踪控制，如图4所示，具体包括以下步骤：

S31:对如步骤S2三维环境实时建模过程建立的三维点云形式的三维环境模型进行后处理，将三维点云形式的三维环境模型转化为体元(voxel)形式存储；

S32:执行初始化过程，根据任务要求，设定无人机的起始点x_init和目标点x_goal，根据步骤S16中建立的无人机状态空间模型设计控制律(本实施例中采用PID方法设计无人机的控制律，其中PID方法为控制律设计的通用方法)，结合无人机状态空间模型和控制律建立无人机的闭环系统数学模型，并将起始点x_init、目标点x_goal、无人机的闭环系统数学模型与步骤S31生成的体元形式的三维环境模型共同作为路径规划的输入；(在本实施例中，以树(Tree)的数据结构存储和表达路径，其中无人机状态空间中的点代表树的节点，两节点之间的边代表无人机的路径，路径规划的目的在于寻找连接所述起始点x_init和x_goal的连续路径。)

S33:设定单步周期为Δt(本实施例中选择Δt为5秒)，在Δt周期内，通过步骤1获得无人机包括位置、速度、姿态在内的当前运动状态估计值x(t)，并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt)；

S34:在同一个规划周期Δt的时间内，对步骤S32所述的无人机路径树进行增量式扩展，具体过程如下：

S34-1:在步骤S32中建立的无人机闭环系统数学模型的参考空间X_r中进行随机采样，获得采样点x_sample；(在本实施例中，设定参考空间X_r为4维，对于参考点x_r∈X_r：x_r＝(x,y,z,ψ)^T，其中x,y,z为无人机在全局坐标系下的三维位置，ψ代表无人机的航向)

S34-2:根据启发式目标函数在已有路径树中选择与所述采样点x_sample最接近的点N_near，在本实施例中，采用如下的混合式启发目标函数：

其中代表路径基本代价，为路径树中候选节点N_i到采样点x_sample的代价(本实施例中采用平均时间代价，即两点间的欧式距离除以无人机的平均速度)，τ(N_i|N_root)为从当前路径树根节点N_root到候选节点N_i的代价，ω为权值。

为路径的定位和运动状态估计可信度函数。其中Σ为无人机运动状态估计值的协方差阵，本实施例中以tr(Σ)代表定位和运动状态估计可信度。Σ_samp为无人机在采样x_sample的状态估计后验协方差阵。因此，通过可衡量从候选节点N_i到x_sample的路径所对应的定位和运动状态估计可信度，根据在当前路径树中选择使得定位和运动状态估计可信度较高的候选节点。

S34-3:以所述N_near节点对应的无人机最终状态为起始点，对比和x_sample生成步骤22建立的无人机闭环系统控制律的参考输入，控制律进一步根据计算控制输入，无人机闭环系统数学模型根据控制输入预测下一时刻的无人机状态。循环执行以上过程，直到路径不可行或者状态已经到达x_sample点。通过以上预测过程，生成一系列节点集合{N_j}。

S34-4:对于所述节点集合N_j∈{N_j}，检查N_j是否与环境中的障碍物有碰撞，若N_j与障碍物无碰撞，计算节点对应的路径代价τ(N_j|N_root)以及定位和运动状态估计可信度Σ_j，将节点N_j以及N_j和N_near之间的可行路径添加至路径树中，从而生成由一系列可行路径组成的路径集合。

S35:从步骤S34生成的所有可行路径中根据选择指标选择最优路径，作为无人机的执行指令，控制无人机按照所述最优路径飞行。循环执行步骤S33和S34，直至无人机飞行到目标点。

在本实施例中，采用以下的选择指标选择最优路径：

其中τ(N_i|N_root)为从根节点到N_i的路径代价，为从N_i到目标点x_goal的路径代价，tr(Σ_i)为N_i的定位和运动状态估计可信度。λ₁，λ₂和λ₃为权重值。

本实施例中的实时可信路径规划与路径跟踪控制的基本框架如图5所示。

本发明实施例另一方面提供了一种实现所述基于RGB-D相机和MEMS惯性传感器的微型无人机室内自主导航方法的微型无人机室内自主导航系统装置。具体包括：

1.无人机机载计算处理单元，用于实时执行所述的各项运算和处理过程。本实施例中采用GB-BXi7H-4500 Brix型号的计算机系统作为机载计算处理单元，在此基础上运行Linux操作系统。本实施例中在无人机机载计算处理单元中执行的算法模块包括：

1)RGB-D运动状态估计模块，采用如本发明中所述的基于RGB-D的运动状态估计步骤，利用RGB-D传感器获取的图像数据和三维深度数据，通过特征提取、特征匹配，计算无人机的位置和姿态参数；

2)三维环境建模模块，采用如本发明中所述的基于RGB-D和MEMS惯性传感器融合的三维环境建模步骤，基于RGB-D传感器获取的三维深度信息以及二维图像特征匹配结果，利用RGB-D和MEMS惯性传感器融合得到的运动参数估计值，将三维点云进行配准和融合，构建精确的三维环境模型；

3)实时路径规划模块，采用如本发明中所述的实时可信路径规划方法，根据无人机飞行任务和三维环境模型，并结合RGB-D相机特性，生成从初始点到目标点的最优路径；

本实施例中所述算法模块均采用C++语言开发。

2.无人机机载飞行控制单元，用于执行RGB-D/MEMS传感器融合和飞行控制算法模块，本实施例中采用STM32F407型号的单片机作为无人机机载飞行控制单元。本实施例中在无人机机载飞行控制单元中执行的算法模块包括：

1)RGB-D/MEMS惯性传感器融合模块，采用如本发明中所述的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计步骤，通过滤波算法对RBG-D运动状态估计值和MEMS惯性测量结果进行融合，修正惯性测量误差，输出包括无人机位置、速度、姿态在内的运动状态估计值；

2)飞行控制模块，根据实时路径规划模块生成的参考路径和RGB-D/MEMS传感器融合模块提供的无人机运动状态估计值计算飞行控制指令，实施姿态控制和路径跟踪控制，引导无人机按照参考路径飞行。

本实施例中RGB-D/MEMS惯性传感器融合和飞行控制算法模块均采用C语言开发。

3.无人机机载传感器系统，包括：

1)RGB-D相机，获取并提供无人机周围环境的RGB图像和三维深度数据。本实施例中采用的RGB-D相机具体型号为PrimeSense Carmine 1.08，该型号的RGB-D相机可以30Hz的频率提供分辨率为640×480的二维彩色图像和三维深度数据；

2)MEMS惯性测量单元，包括三轴角速率陀螺，三轴加速度计和磁传感器，测量并输出无人机当前的角速度、加速度、姿态角等惯性测量数据和当地的磁测量值。本实施例中采用ADIS16405型号的集成式惯性测量模块，该模块集成了所述各惯性测量单元；

3)高度传感器，测量并输出无人机当前相对于地面的高度参数。本实施例中采用超声波测距模块作高度传感器，该模块的高度测量范围为2cm至450cm，分辨率为2mm，数据输出频率为25Hz；

以上仅为本发明的较佳实施例，仅用于解释本发明，而并非对本发明的限制。且本发明的保护范围并不局限于此，任何熟悉本领域的技术人员，在不脱离本发明的精神、原理和范围的情况下，还可以对这些实施例进行多种变化、变型、修改和替换，因此所有等同的技术方法都应属于本发明的范畴，本发明的保护范围应由所附权利要求及其等同限定。

Claims (4)

1.一种微型无人机室内自主导航方法,其特征在于，该方法包括以下步骤：

S1:基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计：利用RGB-D相机获取环境的二维彩色图像和三维深度数据，通过特征点检测、特征点匹配、运动状态估计获得微型无人机的姿态和位置参数估计值，利用MEMS传感器获取惯性测量值，将所述姿态和位置参数估计值与惯性测量值通过滤波方法进行信息融合，用以抑制惯性测量值误差积累，得到更为精确的无人机包括位置、速度、姿态在内的运动状态估计值；

S2:基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模：利用步骤S1中RGB-D相机获取的三维深度数据，以及融合后的运动状态估计值进行三维点云的精确配准与融合，构建三维环境模型，并触发式地对该模型进行全局优化，用以提高模型精度；

S3:实时可信路径规划与路径跟踪控制：设定规划周期,在每个规划周期中，以步骤S2得到的三维环境模型为基础，同时建立RGB-D相机的测量误差模型和定位可信度的评价函数,从而生成无人机从当前点到目标点的路径，在每个规划周期结束后，选择当前最优路径执行；根据所述当前最优路径，与步骤S1得到运动状态估计值进行比较，得到当前的位置偏差,从而根据当前位置偏差生成无人机的位置控制指令，根据位置控制指令解算为无人机的姿态控制指令，实施位置控制和姿态控制，引导无人机跟踪预定路径飞行。

2.如权利要求1所述方法，其特征在于，所述步骤S1中的基于RGB-D相机和MEMS惯性传感器的微型无人机运动状态估计，具体包括以下步骤：

S11：通过RGB-D相机获取当前环境的二维彩色图像与三维深度数据，并将该二维彩色图像进行预处理，转换为以灰度值表示的二维灰度图像；

S12：从步骤S11预处理后的每帧二维灰度图像中提取出一组特征点，并计算每一特征点的特征描述向量，并通过步骤S11中获取的三维深度数据对所述特征点进行筛选，剔除与三维深度数据无对应的特征点；

S13：对相邻两帧图像中的特征点进行特征匹配，通过对比特征点的对应描述向量获得两组匹配的特征点,并再剔除匹配有误的特征点；

S14：根据特征点对应的三维深度数据，将步骤S13匹配后的特征点映射到三维空间，获得具有匹配关系的相邻两帧图像中的两组三维点云；

S15：求解步骤S14中获得的该两组匹配的三维点云之间的旋转和平移参数，利用所述旋转和平移参数反算出无人机的姿态和位置参数估计值；

S16：根据无人机的动力学特性建立状态空间模型以及RGB-D相机、MEMS惯性传感器的观测模型，将S15中得到的无人机的位置参数估计值作为系统的状态空间模型的观测量，与MEMS惯性传感器提供的由角速度、加速度、磁观测量信息组成的惯性测量值通过滤波方法进行融合，进一步修正惯性测量值误差，以有效抑制惯性测量值误差积累，获得包括无人机的位置、速度、姿态在内的更为精确的最终运动状态估计值。

3.如权利要求2所述方法，其特征在于，所述步骤S2中基于RGB-D相机和MEMS惯性传感器融合的三维环境实时建模，具体包括以下步骤：

S21：利用如步骤S15获得的RGB-D运动参数估计值与MEMS测量值融合后的运动状态估计值，以及步骤S14获得的相邻两帧图像中的两组匹配的三维点云，求解所述两组匹配的三维点云之间的初始旋转和平移关系；

S22：利用步骤S21中获得的两组三维点云及其初始旋转和平移关系，定义目标函数为所述两组三维点云中的每对三维点云之间旋转和平移误差之和，通过最小二乘优化迭代方法优化所述目标函数，得到更为精确的三维点云之间的旋转和平移关系；

S23：定义与环境固连的全局坐标系，根据步骤S22得到的不同帧三维点云之间的相对旋转、平移关系，以及各帧三维点云与初始帧三维点云之间的旋转和平移关系，将连续帧的三维点云组合至统一的全局坐标系中，得到三维环境模型；

S24：采用通用的SLAM方法进行闭环监测，当监测到闭环时，触发全局优化过程，进一步校正不同帧三维点云之间的旋转和平移关系，对步骤S23已经建立的三维环境模型进行优化，得到更为精确的的三维环境模型。

4.如权利要求3所述方法，其特征在于，所述步骤3中的实时可信路径规划与路径跟踪控制，具体包括以下步骤：

S31：对步骤S2获得的三维环境模型进行后处理，将三维环境模型转换为体元形式存储，以便于利用三维环境模型进行路径规划；

S32：设定无人机的起始点和目标点，根据步骤S16建立的状态空间模型设计控制律，建立无人机的闭环系统数学模型，将所述起始点、目标点和闭环系统数学模型作为无人机路径规划的输入条件，对路径进行初始化；

S33：设定单步路径规划周期为Δt，在Δt周期内，通过步骤S1获得无人机包括位置、速度、姿态在内的运动状态估计值，记为x(t)，并根据步骤S16中建立的系统状态空间模型预测Δt时刻后的无人机状态x(t+Δt)；

S34：在同一个规划周期Δt的时间内，在S31获得的体元形式的三维环境模型中连续进行随机采样，获得一系列采样点，利用路径的代价以及定位和运动状态估计可信度作为启发式信息，根据该启发式信息在上一规划周期中生成的、以步骤S33预测得到的无人机状态x(t+Δt)为起点的路径集合中选择与每个采样点对应的最近点，利用步骤32中建立的无人机的闭环系统数学模型预测无人机的状态，生成由一系列可行路径组成的路径集合，并计算每条可行路径的代价以及定位和运动状态估计可信度，从而对路径连续进行增量式扩展；

S35：以路径的代价以及定位和运动状态估计可信度为选择指标，从步骤S34中生成的当前可行路径集合中选择最优路径，作为无人机的执行指令，引导无人机按照该最优路径飞行；循环执行步骤S33和S34，直至无人机飞行到目标点。