CN104154910A - 一种微型无人机室内定位方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

近年来，微型无人机领域快速发展，尤其无人机室内自主飞行领域快速前进，出现了许多突破性的进展，将使微小型自主无人机在工业生产、环境探测、紧急救援甚至日常生活中产生重要的作用。虽然室内自主微小无人机具有巨大的潜力，但其发展尚面临着巨大挑战，其中一个就是在未知室内环境的准确和高精度定位技术。由于受到自身尺寸和载荷的严重限制，无人机使用的惯性传感器只能是低性能低精度的MEMS(微机电系统)类型的导航传感器。该类惯性传感器仅能提供一个精度较低的姿态信息，而不能提供一个可用的位置和速度信息。而当无人机在室内作业时通常是没有GPS(全球定位系统)信号的，故此时必须通过其他传感器来获取一个有效的高精度的定位信息来现实无人机的室内自主飞行并执行任务。

目前，未知室内环境定位方法通常采用视觉传感器来实现，常见的视觉设备包括激光传感器、单目相机、双目相机和RGB-D(彩色和深度)相机等。激光传感器使用ICP(IterativeClosest Point，最近邻点迭代算法)对扫描点阵进行配准和定位，该算法具有计算实时性好和输出定位信息稳定，有许多国内外的实验室已经利用该方法实现了某些特定室内环境中的定位和无人机自主飞行。该方法的主要缺点是只能得到的二维的扫描信息，适用于多垂直面的环境，而在复杂的三维环境中显得感知能力不够。对于单目相机来说，一般使用SFM(Structure From Motion，运动结构估计)方法来计算基本矩阵，从而得到相机的运动方向，但该方法无法恢复出运动距离，使得该方法不能用于未知的复杂室内环境。与单目相机相比，双目视觉系统能够恢复出图中许多点的深度，将图片中的像素点映射到三维空间中，从而获得三维的深度信息，再利用这些三维深度信息的关联关系解算出相机系统的运动方向和距离。该方法的一个缺点是对于相机的标定很严格，价格昂贵。而RGB-D相机得到环境和双目相机类似，能直接获取空间点到相机的三维位置信息和普通的二维图像信息，通过和双目相机类似的方法，同样可获取相机的运动方向和距离这6个自由度的信息。相比双目相机，RGB-D相机优势是价格低廉，但是其数据质量比较差，噪声大，同时存在固有的数据延迟等问题。

基于RGB-D相机的室内定位算法通常是提取RGB图像的特征点，并将相邻两帧的RGB图像的特征点进行匹配，再利用匹配的特征点所对应的景深信息来估计这两帧之间相机的6维运动情况，即姿态变化和平移运动。这个过程被称为视觉位置估计(Visual Odometry)或简称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的无人机姿态角对应的姿态矩阵分别为R_m和R_m-1，则当前帧mRGB图像的无人机姿态和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δR_m；

3)从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征向量；

4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集C_m；

5)计算当前帧RGB图像的匹配对集C_m中所有特征点对应的三维位置；

6)结合姿态矩阵δR_m和当前帧三维点匹配对集合使用RANSAC方法估计出上一帧到当前帧之间无人机的位移T_m；

7)根据上一帧到当前帧之间无人机的位移T_m计算出的当前帧无人机的姿态矩阵为R_m、位移X_m；即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能。

本发明的特点及有益效果：

本发明使用快速的特征提取算法，使得定位算法耗时大大减少，从而实现了该方法的实时性。本发明还采用了将图片分块再分别提取特征点的方法，使得特征点的分布更为均匀，提高了算法的鲁邦性和精度。同时，本发明使用了MEMS传感器的姿态信息，使得视觉算法只需要估计三维的平移运动，减少了计算量，也一定程度上提高算法的稳定性。

附图说明

图1为本发明室内定位算法的实施例流程框图；

具体实现方式

下面结合附图及实施例对本发明进行详细说明。

本发明提出的一种微型无人机室内定位方法实施例，该无人机载有MEMS惯性器件及RGB-D相机(其中MEMS惯性器件以采样间隔Ts输出无人机的当前的三轴角速度 三轴加速度和无人机所在位置的地球磁场的三轴磁分量上角标b表示该数据是测量值在机体坐标系b中的表示结果，RGB-D相机以30Hz输出RGB图像和深度图像)，该方法如图1所示，包括以下步骤：

1)实时读取MEMS惯性器件的无人机的当前的三轴角速度三轴加速度 和无人机所在位置的地球磁场的三轴磁分量解算无人机的当前采样时刻n的姿态角(θ_n,γ_n,ψ_n)，其中θ、γ、ψ分别表示俯仰角、横滚角和偏航角；

具体包括以下步骤：

11)实时读取MEMS惯性器件的三轴角速度数据，设上一采样时刻n-1通过三轴角速度计算的相机姿态的四元数为q_n-1，用四元数法计算当前采样时刻n的通过三轴角速度计算的相机姿态四元数q_n如式(1)：

$q_n=(\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]+\frac{T_s}{2}\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x& -{\mathrm{\ω}}_y& -{\mathrm{\ω}}_z\\ {\mathrm{\ω}}_x& 0& {\mathrm{\ω}}_z& -{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_y& -{\mathrm{\ω}}_z& 0& {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_z& {\mathrm{\ω}}_y& -{\mathrm{\ω}}_x& 0\end{array}\right])q_{n-1}---\left(1\right)$

然后将q_n转换(公知方法)成对应的姿态角

12)设三轴加速度判断式(2)：

$0.95<\frac{\sqrt{{\left(f_x^b\right)}^2+{\left(f_y^b\right)}^2+{\left(f_z^b\right)}^2}}{g_0}<1.05---\left(2\right)$

其中g₀是当地重力加速度值；

实时读取MEMS惯性器件的三轴加速度和三轴磁分量，若读取的三轴加速度不满足判断式(2)，则当前采样时刻n无人机的姿态角(θ_n,γ_n,ψ_n)如式(3)：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_n={\widehat{\mathrm{\&theta;}}}_n\\ {\mathrm{\&gamma;}}_n={\widehat{\mathrm{\&gamma;}}}_n\\ {\mathrm{\&psi;}}_n={\widehat{\mathrm{\&psi;}}}_n\end{array}\right.---\left(3\right)$

并转到步骤2)；

若读取的三轴加速度满足判断式(2)，则转到步骤13)；

13)求解式(4)可得当前采样时刻n三轴加速度和三轴磁分量计算的另一种姿态角其对应的姿态矩阵为

$\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]={\tilde{R}}_n\left[\begin{array}{c}0\\ 0\\ g_0\end{array}\right],\left[\begin{array}{c}{m}_x^b\\ m_y^b\\ m_z^b\end{array}\right]={\tilde{R}}_n\left[\begin{array}{c}0\\ m_y^g\\ m_z^g\end{array}\right]---\left(4\right)$

其中和是当地地磁的y和z方向的磁分量，上标g表示该数据是测量值在当地地理坐标系g中的表示结果；

则当前采样时刻n无人机的姿态角(θ_n,γ_n,ψ_n)如式(5)：

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_n=0.9{\widehat{\mathrm{\&theta;}}}_n+0.1{\widetilde{\mathrm{\&theta;}}}_n\\ {\mathrm{\&gamma;}}_n=0.9{\widehat{\mathrm{\&gamma;}}}_n+0.1{\widetilde{\mathrm{\&gamma;}}}_n\\ {\mathrm{\&psi;}}_n=0.9{\widehat{\mathrm{\&psi;}}}_n+0.1{\widetilde{\mathrm{\&psi;}}}_n\end{array}\right.---\left(5\right)$

2)实时读取RGB-D相机的当前帧m的RGB图像和深度图像，取此时刻步骤1)中无人机姿态角(θ_n,γ_n,ψ_n)作为当前帧RGB图像的无人机姿态角设当前帧m和上一帧m-1的无人机姿态角对应的姿态矩阵分别为R_m和R_m-1，那么当前帧mRGB图像的无人机姿态和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δR_m如式(6)：

δR_m＝R_m(R_m-1)^-1   (6)；

3)从当前帧RGB图像中提取一定数量(特征点的数量是根据实际运算能力及期望达到的精度确定,本实施例取1000个点)且分布均匀的特征点，并计算特征点对应的特征向量(该步骤的计算结果中特征向量是步骤4)中特征点距离计算的基础)；步骤3)具体包括以下步骤：

31)将当前帧RGB图像划分成若干尺寸一样的子图像(本实施例划分12个，子图像的数量应取一个合适的值，如果太少则对于特征点的均匀性没有太大作用，如果太多会使得特征提取算法不能充分发挥作用补充取值范围)；

32)使用ORB算法(公知算法)对每个子图像进行特征点提取，使得在每个子图像上提取的的特征点数量基本一致；将所有子图像中提取的特征点组成一个当前帧RGB图像的特征点集合；

33)使用ORB算法计算当前帧RGB图像的特征点集中每个特征点对应的特征向量；

4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；针对当前帧RGB图像的特征点集中的每个特征点，在前一帧RGB图像的特征点集中选出与该特征点最近的两个特征点(此处特征点之间的距离是指特征点对应的特征向量之间的Hamming距离，具体算法采用公知的2最近邻算法)，分别称为第一近邻点和第二近邻点；该特征点到第一近邻点和第二近邻点的距离分别为d₁和d₂，其中d₁＜d₂，则当式(6)得到满足时，取第一近邻点为该特征点的匹配点，该特征点和其第一近邻点组成当前帧RGB图像的一个匹配对；

$\left\{\begin{array}{c}{d}_1<\mathrm{\&alpha;}\\ \frac{d_1}{d_2}<\mathrm{\&beta;}\end{array}\right.---\left(6\right)$

其中α和β是设定的阈值(α和特征向量的性质有关，其大小表明了特征向量的相似性，β代表了两个距离d₁和d₂的差异性，α和β取值越小，匹配结果的正确性越高，本实施例α和β分别取40和0.7)；

将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集C_m；

5)计算当前帧RGB图像的匹配对集C_m中所有特征点对应的三维位置，具体为：设某特征点在该RGB图像上的坐标为(i，j)，该点在当期帧深度图中对应坐标点的值为d_i,j，则该点的在无人机机体坐标系中三维坐标为 $\left[\begin{array}{c}(j-D_I)*d_{i,j}\\ (j-D_J)*d_{i,j}\\ d_{i,j}\end{array}\right],$ 其中D_I、D_J分别表示RGB图像宽度和高度的一半(D_I、D_J大小与RGB-D相机具体参数和设置相关)；则当前帧RGB图像的匹配对集C_m中所有特征点对应的三维位置组成当前帧RGB图像三维点匹配对集合集合中元素为(P_k，Q_k)，其中P_k表示位于当前帧RGB图像上的特征点对应的三维坐标点，Q_k表示P_k在上一帧RGB图像中的匹配特征点对应的三维坐标点；

6)结合姿态矩阵δR_m和当前帧三维点匹配对集合使用RANSAC方法(公知方法)估计出上一帧到当前帧之间无人机的位移T_m；

7)根据上一帧到当前帧之间无人机的位移T_m计算出的当前帧无人机的姿态矩阵为R_m、位移为X_m如式(7)所示：

$\left\{\begin{array}{c}{R}_m={\mathrm{\&delta;R}}_m{R}_{m-1}\\ X_m=X_{m-1}+{\mathrm{\&delta;R}}_m{T}_m\end{array}\right.---\left(7\right)$

式中，R_m-1、X_m-1分别为上一帧无人机的姿态矩阵、位移；

利用计算得到的无人机的姿态矩阵为R_m、位移为X_m可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能。

以上的实施例仅用于说明本发明，而并非对发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有同等的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。

Claims (3)

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的无人机姿态角对应的姿态矩阵分别为R_m和R_m-1，则当前帧mRGB图像的无人机姿态和上一帧m-1RGB图像的无人机姿态的姿态差对应的姿态矩阵δR_m； 

3)从当前帧RGB图像中提取一定数量且分布均匀的特征点，并计算特征点对应的特征向量； 

4)计算当前帧RGB图像的特征点集中每个特征点与前一帧RGB图像的特征点集中每个特征点的距离；对当前帧RGB图像的特征点集中的每个特征点选出该特征点与前一帧RGB图像的特征点集中与该特征点最近的两个特征点中的一个特征点，组成当前帧RGB图像的一个匹配对；将当前帧RGB图像的所有匹配对组成当前帧RGB图像的匹配对集C_m； 

5)计算当前帧RGB图像的匹配对集C_m中所有特征点对应的三维位置； 

6)结合姿态矩阵δR_m和当前帧三维点匹配对集合使用RANSAC方法估计出上一帧到当前帧之间无人机的位移T_m； 

7)根据上一帧到当前帧之间无人机的位移T_m计算出的当前帧无人机的姿态矩阵为R_m、位移X_m；即可确定无人机相对于起飞点的姿态和位置信息，即完成了无人机的室内定位功能。 

2.如权利要求1所述方法，其特征在于，所述步骤1)具体包括以下步骤： 

11)实时读取MEMS惯性器件的三轴角速度数据，设上一采样时刻n-1通过三轴角速度计算的相机姿态的四元数为q_n-1，用四元数法计算当前采样时刻n的通过三轴角速度计算的相机姿态四元数q_n如下式： 

然后将q_n转换(公知方法)成对应的姿态角

12)设三轴加速度判断式如下： 

其中g₀是当地重力加速度值； 

实时读取MEMS惯性器件的三轴加速度和三轴磁分量，若读取的三轴加速度不满足判断式，则当前采样时刻n无人机的姿态角(θ_n,γ_n,ψ_n)如下式： 

并转到步骤2)； 

若读取的三轴加速度满足判断式，则转到步骤13)； 

13)求解式下式可得当前采样时刻n三轴加速度和三轴磁分量计算的另一种姿态角 其对应的姿态矩阵为

其中和是当地地磁的y和z方向的磁分量，上标g表示该数据是测量值在当地地理坐标系g中的表示结果； 

则当前采样时刻n无人机的姿态角(θ_n,γ_n,ψ_n)如下式： 

。

3.如权利要求2所述方法，其特征在于，所述步骤3)具体包括以下步骤： 

31)将当前帧RGB图像划分成若干尺寸一样的子图像； 

32)使用ORB算法对每个子图像进行特征点提取，使得在每个子图像上提取的的特征点数量一致；将所有子图像中提取的特征点组成一个当前帧RGB图像的特征点集合； 

33)使用ORB算法计算当前帧RGB图像的特征点集中每个特征点对应的特征向量。