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

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

【技术实现步骤摘要】
一种微型无人机室内定位方法
本专利技术属于微型无人机导航定位
，特别涉及一种室内无GPS情况下MEMS惯性传感器辅助的RGB-D传感器的微型无人机定位方法及系统。
技术介绍
近年来，微型无人机领域快速发展，尤其无人机室内自主飞行领域快速前进，出现了许多突破性的进展，将使微小型自主无人机在工业生产、环境探测、紧急救援甚至日常生活中产生重要的作用。虽然室内自主微小无人机具有巨大的潜力，但其发展尚面临着巨大挑战，其中一个就是在未知室内环境的准确和高精度定位技术。由于受到自身尺寸和载荷的严重限制，无人机使用的惯性传感器只能是低性能低精度的MEMS(微机电系统)类型的导航传感器。该类惯性传感器仅能提供一个精度较低的姿态信息，而不能提供一个可用的位置和速度信息。而当无人机在室内作业时通常是没有GPS(全球定位系统)信号的，故此时必须通过其他传感器来获取一个有效的高精度的定位信息来现实无人机的室内自主飞行并执行任务。目前，未知室内环境定位方法通常采用视觉传感器来实现，常见的视觉设备包括激光传感器、单目相机、双目相机和RGB-D(彩色和深度)相机等。激光传感器使用ICP(IterativeClosestPoint，最近邻点迭代算法)对扫描点阵进行配准和定位，该算法具有计算实时性好和输出定位信息稳定，有许多国内外的实验室已经利用该方法实现了某些特定室内环境中的定位和无人机自主飞行。该方法的主要缺点是只能得到的二维的扫描信息，适用于多垂直面的环境，而在复杂的三维环境中显得感知能力不够。对于单目相机来说，一般使用SFM(StructureFromMotion，运动结构估计)方法来计算基本矩阵，从而得到相机的运动方向，但该方法无法恢复出运动距离，使得该方法不能用于未知的复杂室内环境。与单目相机相比，双目视觉系统能够恢复出图中许多点的深度，将图片中的像素点映射到三维空间中，从而获得三维的深度信息，再利用这些三维深度信息的关联关系解算出相机系统的运动方向和距离。该方法的一个缺点是对于相机的标定很严格，价格昂贵。而RGB-D相机得到环境和双目相机类似，能直接获取空间点到相机的三维位置信息和普通的二维图像信息，通过和双目相机类似的方法，同样可获取相机的运动方向和距离这6个自由度的信息。相比双目相机，RGB-D相机优势是价格低廉，但是其数据质量比较差，噪声大，同时存在固有的数据延迟等问题。基于RGB-D相机的室内定位算法通常是提取RGB图像的特征点，并将相邻两帧的RGB图像的特征点进行匹配，再利用匹配的特征点所对应的景深信息来估计这两帧之间相机的6维运动情况，即姿态变化和平移运动。这个过程被称为视觉位置估计(VisualOdometry)或简称VO。这个方法一般存在如下三个问题：一是图像特征提取算法耗时巨大，导致该算法不能实时运行；二是图像特征提取由于环境而集中在某个局部，导致定位算法性能下降，甚至估计错误；三是利用关联的三维点对直接估计出六维的运动，容易受到噪声和错误数据的影响，而导致姿态变化和平移运动相互影响，进而产生更大的估计误差。
技术实现思路
本专利技术的目的是针对已有技术存在的问题，提供一种微型无人机室内定位方法，以实现微小无人机的实时、鲁棒、高精度的室内位置估计。为实现上述目的，本专利技术提供了一种微型无人机室内定位方法，其特征在于，该无人机载有MEMS惯性器件及RGB-D相机，其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量上角标b表示该数据是测量值在机体坐标系b中的表示结果，RGB-D相机以30Hz输出RGB图像和深度图像，该方法包括以下步骤：1)实时读取MEMS惯性器件的无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量解算无人机的当前采样时刻n的姿态角(θn,γn,ψn)，其中θ、γ、ψ分别表示俯仰角、横滚角和偏航角；2)实时读取RGB-D相机的当前帧m的RGB图像和深度图像，取此时刻步骤1)中无人机姿态角(θn,γn,ψn)作为当前帧RGB图像的无人机姿态角设当前帧m和上一帧m-1的无人机姿态角对应的姿态矩阵分别为Rm和Rm-1，则当前帧mRGB图像的无人机姿态和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δRm；3)从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征向量；4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集Cm；5)计算当前帧RGB图像的匹配对集Cm中所有特征点对应的三维位置；6)结合姿态矩阵δRm和当前帧三维点匹配对集合使用RANSAC方法估计出上一帧到当前帧之间无人机的位移Tm；7)根据上一帧到当前帧之间无人机的位移Tm计算出的当前帧无人机的姿态矩阵为Rm、位移Xm；即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能。本专利技术的特点及有益效果：本专利技术使用快速的特征提取算法，使得定位算法耗时大大减少，从而实现了该方法的实时性。本专利技术还采用了将图片分块再分别提取特征点的方法，使得特征点的分布更为均匀，提高了算法的鲁邦性和精度。同时，本专利技术使用了MEMS传感器的姿态信息，使得视觉算法只需要估计三维的平移运动，减少了计算量，也一定程度上提高算法的稳定性。附图说明图1为本专利技术室内定位算法的实施例流程框图；具体实现方式下面结合附图及实施例对本专利技术进行详细说明。本专利技术提出的一种微型无人机室内定位方法实施例，该无人机载有MEMS惯性器件及RGB-D相机(其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量上角标b表示该数据是测量值在机体坐标系b中的表示结果，RGB-D相机以30Hz输出RGB图像和深度图像)，该方法如图1所示，包括以下步骤：1)实时读取MEMS惯性器件的无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量解算无人机的当前采样时刻n的姿态角(θn,γn,ψn)，其中θ、γ、ψ分别表示俯仰角、横滚角和偏航角；具体包括以下步骤：11)实时读取MEMS惯性器件的三轴角速度数据，设上一采样时刻n-1通过三轴角速度计算的相机姿态的四元数为qn-1，用四元数法计算当前采样时刻n的通过三轴角速度计算的相机姿态四元数qn如式(1)：然后将qn转换(公知方法)成对应的姿态角12)设三轴加速度判断式(2)：其中g0是当地重力加本文档来自技高网...

【技术保护点】
一种微型无人机室内定位方法，其特征在于，该无人机载有MEMS惯性器件及RGB‑D相机，其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量上角标b表示该数据是测量值在机体坐标系b中的表示结果，RGB‑D相机以30Hz输出RGB图像和深度图像，该方法包括以下步骤：1)实时读取MEMS惯性器件的无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量解算无人机的当前采样时刻n的姿态角(θn,γn,ψn)，其中θ、γ、ψ分别表示俯仰角、横滚角和偏航角；2)实时读取RGB‑D相机的当前帧m的RGB图像和深度图像，取此时刻步骤1)中无人机姿态角(θn,γn,ψn)作为当前帧RGB图像的无人机姿态角设当前帧m和上一帧m‑1的无人机姿态角对应的姿态矩阵分别为Rm和Rm‑1，则当前帧mRGB图像的无人机姿态和上一帧m‑1RGB图像的无人机姿态的姿态差对应的姿态矩阵δRm；3)从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征向量； 4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集Cm； 5)计算当前帧RGB图像的匹配对集Cm中所有特征点对应的三维位置； 6)结合姿态矩阵δRm和当前帧三维点匹配对集合使用RANSAC方法估计出上一帧到当前帧之间无人机的位移Tm；7)根据上一帧到当前帧之间无人机的位移Tm计算出的当前帧无人机的姿态矩阵为Rm、位移Xm；即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能。...

【技术特征摘要】
1.一种微型无人机室内定位方法，其特征在于，该无人机载有MEMS惯性器件及RGB-D相机，其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量上角标b表示该数据是测量值在机体坐标系b中的表示结果，RGB-D相机以30Hz输出RGB图像和深度图像，该方法包括以下步骤：1)实时读取MEMS惯性器件的无人机的当前的三轴角速度三轴加速度和无人机所在位置的地球磁场的三轴磁分量解算无人机的当前采样时刻n的姿态角(θn,γn,ψn)，其中θ、γ、ψ分别表示俯仰角、横滚角和偏航角；2)实时读取RGB-D相机的当前帧m的RGB图像和深度图像，取此时刻步骤1)中无人机姿态角(θn,γn,ψn)作为当前帧RGB图像的无人机姿态角设当前帧m和上一帧m-1的无人机姿态角对应的姿态矩阵分别为Rm和Rm-1，则当前帧mRGB图像的无人机姿态和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δRm；3)从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征向量；4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集Cm；5)计算当前帧RGB图像的匹配对集Cm中所有特征点对应的三维位置；6)结合姿态矩阵δRm和当前帧三维点匹配对集合使用RANSAC方法估计出上一帧到当前帧之间无人机的位移Tm；7)根据上一帧到当前帧之间无人机的位移Tm计算出的当前帧无人机的姿态矩阵为Rm、位移Xm；即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能；所述步骤1)具体包括以下步骤：11)实时读取MEMS惯性器件的三轴角速度数据，设上一采样时刻n-1通过三轴角速度计算的相机姿态的四元数为qn-1，用四元数法计算当前采样...

【专利技术属性】
技术研发人员：程农，李清，唐良文，吴沁凡，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京;11