一种微型无人机室内定位方法

一种微型无人机室内定位方法
【专利摘要】本发明涉及一种微型无人机室内定位方法，属于微型无人机导航定位【技术领域】；该方法包括：实时读取无人机的当前的三轴角速度、三轴加速度和三轴磁分量，解算采样时刻的姿态角；实时读取当前帧m的RGB图像和深度图像，取此时刻无人机姿态角；计算当前帧无人机和上一帧无人机的姿态差对应的姿态矩阵；提取分布均匀的特征点，并计算特征向量；计算特征点集中每个特征点的距离；选出该特征点与最近的两个特征点中的一个特征点，组成一个匹配对；将计算当前帧RGB图像的匹配对集中所有特征点对应的三维位置；估计出上一帧到当前帧期间无人机的位移；计算出的当前帧无人机的姿态矩阵和位移。本发明减少了计算量，也一定程度上提高算法的稳定性。
【专利说明】一种微型无人机室内定位方法

【技术领域】
[0001] 本发明属于微型无人机导航定位【技术领域】，特别涉及一种室内无 GPS情况下MEMS 惯性传感器辅助的RGB-D传感器的微型无人机定位方法及系统。

【背景技术】
[0002] 近年来，微型无人机领域快速发展，尤其无人机室内自主飞行领域快速前进，出现 了许多突破性的进展，将使微小型自主无人机在工业生产、环境探测、紧急救援甚至日常生 活中产生重要的作用。虽然室内自主微小无人机具有巨大的潜力，但其发展尚面临着巨大 挑战，其中一个就是在未知室内环境的准确和高精度定位技术。由于受到自身尺寸和载荷 的严重限制，无人机使用的惯性传感器只能是低性能低精度的MEMS(微机电系统）类型的 导航传感器。该类惯性传感器仅能提供一个精度较低的姿态信息，而不能提供一个可用的 位置和速度信息。而当无人机在室内作业时通常是没有GPS(全球定位系统）信号的，故此 时必须通过其他传感器来获取一个有效的高精度的定位信息来现实无人机的室内自主飞 行并执行任务。
[0003] 目前，未知室内环境定位方法通常采用视觉传感器来实现，常见的视觉设备包 括激光传感器、单目相机、双目相机和RGB-D (彩色和深度）相机等。激光传感器使用 ICP(Iterative Closest Point,最近邻点迭代算法）对扫描点阵进行配准和定位，该算法 具有计算实时性好和输出定位信息稳定，有许多国内外的实验室已经利用该方法实现了某 些特定室内环境中的定位和无人机自主飞行。该方法的主要缺点是只能得到的二维的扫描 信息，适用于多垂直面的环境，而在复杂的三维环境中显得感知能力不够。对于单目相机来 说，一般使用SFM (Structure From Motion,运动结构估计）方法来计算基本矩阵，从而得 到相机的运动方向，但该方法无法恢复出运动距离，使得该方法不能用于未知的复杂室内 环境。与单目相机相比，双目视觉系统能够恢复出图中许多点的深度，将图片中的像素点映 射到三维空间中，从而获得三维的深度信息，再利用这些三维深度信息的关联关系解算出 相机系统的运动方向和距离。该方法的一个缺点是对于相机的标定很严格，价格昂贵。而 RGB-D相机得到环境和双目相机类似，能直接获取空间点到相机的三维位置信息和普通的 二维图像信息，通过和双目相机类似的方法，同样可获取相机的运动方向和距离这6个自 由度的信息。相比双目相机，RGB-D相机优势是价格低廉，但是其数据质量比较差，噪声大， 同时存在固有的数据延迟等问题。
[0004] 基于RGB-D相机的室内定位算法通常是提取RGB图像的特征点，并将相邻两帧 的RGB图像的特征点进行匹配，再利用匹配的特征点所对应的景深信息来估计这两帧之 间相机的6维运动情况，即姿态变化和平移运动。这个过程被称为视觉位置估计（Visual Odometry)或简称V0。这个方法一般存在如下三个问题：一是图像特征提取算法耗时巨大， 导致该算法不能实时运行；二是图像特征提取由于环境而集中在某个局部，导致定位算法 性能下降，甚至估计错误；三是利用关联的三维点对直接估计出六维的运动，容易受到噪声 和错误数据的影响，而导致姿态变化和平移运动相互影响，进而产生更大的估计误差。


【发明内容】

[0005] 本发明的目的是针对已有技术存在的问题，提供一种微型无人机室内定位方法， 以实现微小无人机的实时、鲁棒、高精度的室内位置估计。
[0006] 为实现上述目的，本发明提供了一种微型无人机室内定位方法，其特征在于，该无 人机载有MEMS惯性器件及RGB-D相机，其中MEMS惯性器件以采样间隔Ts输出无人机的当 前的三轴角速度

【权利要求】
1· 一种微型无人机室内定位方法，其特征在于，该无人机载有MEMS惯性器件及RGB-D 相机，其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度
三 轴加速度
和无人机所在位置的地球磁场的三轴磁分量
上角标 b表示该数据是测量值在机体坐标系b中的表示结果，RGB-D相机以30Hz输出RGB图像和 深度图像，该方法包括以下步骤： 1) 实时读取MEMS惯性器件的无人机的当前的三轴角速度
v三轴加速度
和无人机所在位置的地球磁场的三轴磁分量
，解算无人机的当 前米样时刻η的姿态角
，其中
分别表示俯仰角、横滚角和偏航角； 2) 实时读取RGB-D相机的当前帧m的RGB图像和深度图像，取此时刻步骤1)中无人机 姿态角
作为当前帧RGB图像的无人机姿态角（
；设当前帧m和上一 帧m-Ι的无人机姿态角对应的姿态矩阵分别为心和R^，则当前帧mRGB图像的无人机姿态 和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δ Rm ; 3) 从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征 向量； 4) 计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个 特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB 图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的 一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集C m ; 5) 计算当前帧RGB图像的匹配对集Cm中所有特征点对应的三维位置； 6) 结合姿态矩阵δ &和当前帧三维点匹配对集合之，使用RANSAC方法估计出上一帧 到当前帧之间无人机的位移L; 7) 根据上一帧到当前帧之间无人机的位移^计算出的当前帧无人机的姿态矩阵为Rm、 位移Χπ;即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功 能。
2.如权利要求1所述方法，其特征在于，所述步骤1)具体包括以下步骤： 11) 实时读取MEMS惯性器件的三轴角速度数据，设上一采样时刻η-1通过三轴角速度 计算的相机姿态的四元数为qn-i，用四元数法计算当前采样时刻η的通过三轴角速度计算 的相机姿态四元数q n如下式：
然后将qn转换（公知方法）成对应的姿态角（
12) 设三轴加速度判断式如下：
其中gQ是当地重力加速度值； 实时读取MEMS惯性器件的三轴加速度和三轴磁分量，若读取的三轴加速度不满足判 断式，则当前采样时刻η无人机的姿态角（θη，Υη，ψη)如下式：
， 并转到步骤2); 若读取的三轴加速度满足判断式，则转到步骤13); 13)求解式下式可得当前采样时刻η三轴加速度和三轴磁分量计算的另一种姿态角
，其对应的姿态矩阵为瓦；
其中和是当地地磁的y和ζ方向的磁分量，上标g表示该数据是测量值在当地 地理坐标系g中的表示结果； 则当前采样时刻η无人机的姿态角
如下式：
3.如权利要求2所述方法，其特征在于，所述步骤3)具体包括以下步骤： 31) 将当前帧RGB图像划分成若干尺寸一样的子图像； 32) 使用ORB算法对每个子图像进行特征点提取，使得在每个子图像上提取的的特征 点数量一致；将所有子图像中提取的特征点组成一个当前帧RGB图像的特征点集合； 33) 使用ORB算法计算当前帧RGB图像的特征点集中每个特征点对应的特征向量。
【文档编号】G01C21/00GK104154910SQ201410350665
【公开日】2014年11月19日 申请日期:2014年7月22日 优先权日:2014年7月22日
【发明者】程农, 李清, 唐良文, 吴沁凡 申请人:清华大学