[发明专利]一种微型无人机室内定位方法

摘要

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

主权项

一种微型无人机室内定位方法，其特征在于，该无人机载有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，用四元数法计算当前采样时刻n的通过三轴角速度计算的相机姿态四元数qn如下式：qn=(1000010000100001+Ts20-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0)qn-1]]>然后将qn转换成对应的姿态角Ts为采样间隔；12)设三轴加速度判断式如下：0.95<(fxb)2+(fyb)2+(fzb)2g0<1.05]]>其中g0是当地重力加速度值；实时读取MEMS惯性器件的三轴加速度和三轴磁分量，若读取的三轴加速度不满足判断式，则当前采样时刻n无人机的姿态角(θn,γn,ψn)如下式：θn=θ^nγn=γ^nψn=ψ^n,]]>并转到步骤2)；若读取的三轴加速度满足判断式，则转到步骤13)；13)求解式下式可得当前采样时刻n三轴加速度和三轴磁分量计算的另一种姿态角其对应的姿态矩阵为fxbfybfzb=R~n00g0,mxbmybmzb=R~n0mygmzg]]>其中和是当地地磁的y和z方向的磁分量，上标g表示该数据是测量值在当地地理坐标系g中的表示结果；则当前采样时刻n无人机的姿态角(θn,γn,ψn)如下式：θn=0.9θ^n+0.1θ~nγn=0.9γ^n+0.1γ~nψn=0.9ψ^n+0.1ψ~n;]]>述步骤3)具体包括以下步骤：31)将当前帧RGB图像划分成若干尺寸一样的子图像；32)使用ORB算法对每个子图像进行特征点提取，使得在每个子图像上提取的的特征点数量一致；将所有子图像中提取的特征点组成一个当前帧RGB图像的特征点集合；33)使用ORB算法计算当前帧RGB图像的特征点集中每个特征点对应的特征向量。