扑翼飞行器室内视觉定位方法及系统

技术领域

本发明涉及计算机视觉领域和自动化控制领域，尤其涉及一种基于视觉的扑翼飞行器室内定位算法及系统。

背景技术

扑翼飞行器是一种仿生类飞行机器人，具有质量轻、功耗低、飞行效率高、抗干扰性强等特点，同时还具有良好的灵活性和飞行机动性，在军民用领域和其他科技领域都具有广阔的发展前景。

在自然界中有能力进行飞行的动物都采用扑翼方式进行飞行，例如鸟类、昆虫和龙。在扑动过程中通过翅膀的上下摆动，扰乱周围气流，为飞行同时提供升力和推力，有效利用能源进行长距离飞行。

传统的定位方法主要依靠卫星定位系统，在室内等信号较弱的区域定位精度较低；基于视觉与惯性单元的定位方法是一种开环的定位方法，容易产生定位漂移的现象；采用词袋模型做闭环矫正的方法需要预先建立词袋库，所占用的储存空间较大，消耗计算资源较多。

发明内容

为解决上述技术问题，本发明提供一种扑翼飞行器室内视觉定位方法及系统，该方法不需要依赖卫星定位系统或词袋库，仅需要惯性单元与单目相机即可进行工作，对飞行器载重性能与机载设备性能要求低，室内定位精度较高。

本发明所采用的解决方案为：

一种扑翼飞行器室内视觉定位方法，该方法基于扑翼飞行器的传感器并采用基于滤波的传感器融合方法实现，所述传感器包括惯性传感器与摄像头，所述方法包括内环的基于 MSCKF(多状态约束下的Kalman滤波器)的飞行器定位以及外环的基于室内已知目标点的位姿修正两部分，具体包括：首先获得扑翼飞行器初始状态的位置信息以及姿态信息，在扑翼飞行器飞行的过程中，获得飞行器的累积位姿信息，利用惯性传感器获得的测量值进行扑翼飞行器位姿的预测，并利用摄像头得到的周围环境的点云信息进行内环的位姿修正；利用摄像头检测的周围环境中的目标点信息进行外环的位姿修正。

具体包括如下：

步骤S1，获得扑翼飞行器的初始状态的位置信息X

步骤S2，通过扑翼飞行器初始状态的位置信息X

步骤S3，在扑翼飞行器飞行过程中，通过惯性传感器获得扑翼飞行器机体坐标系下三轴的加速度信息与角速度信息，通过加速度与角速度测量值对时间积分，得到飞行器的累积位置与姿态信息；

步骤S4，通过机载单目摄像头获得周围环境的点云信息，利用点云信息得到飞行器当前位姿的观测量，进而在滤波器框架下对步骤S3中得到的飞行器累积位姿进行修正；

步骤S5，若单目摄像头检测到环境中位置已知的物品，则通过目标检测的方法得到飞行器相对于物品的位置，进而得到飞行器在世界坐标系下的位置，对飞行器内环得到的位姿信息进行外环修正。

上述方案中，由于同一特征点会在多个相机位姿下被观测到，因此多个相机位姿之间通过特征点进行互相约束。通过所述约束即可计算出特征点的真实值与估计值之间的残差，作为滤波器的观测量。具体为：使用FAST角点检测法采集图像中的特征点，使用LK光流法对所述特征点进行跟踪，得到每个特征点在不同相机观测位姿下的重投影误差，作为滤波器的内环观测量；利用最小二乘法得到的特征点在世界坐标系下的坐标估计值，并转化到相机坐标系下，得到特征点在相机坐标系下的估计值为：

其中

则特征点的观测残差为：

其中

对周围环境中位置已知的物体，采用图像颜色域分割，再采用角点检测，得到物体的角点的大致位置后，采用非极大值抑制的方法，得到角点的精确位置。进一步的，对所述的采集得到的物体角点的精确位置，采用EPnP方法，得到飞行器自身位姿，作为飞行器的外环观测量，进行飞行器位姿修正。

本发明的有益效果在于：

本发明的基于视觉的扑翼飞行器室内定位系统仅需要惯性单元与单目相机即可进行工作，并且不需要显卡等设备进行计算加速，对飞行器载荷能力与设备性能要求较低。

本发明的基于视觉的扑翼飞行器室内定位系统不依赖于卫星定位系统与词袋模型，仅需要部分物体已知的位置信息即可对扑翼飞行器位姿进行修正，计算效率与定位精度均较高。

附图说明

下面结合附图和实施例对本发明做进一步说明。附图是示意性的，不应理解为对本发明进行任何限制，对于本领域的技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：

图1是本发明的扑翼飞行器室内视觉定位方法的步骤图；

图2是本发明的扑翼飞行器室内视觉定位方法的流程逻辑图；

图3是本发明的扑翼飞行器室内视觉定位方法的滤波器结构示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

本发明设计了一种扑翼飞行器室内视觉定位方法，所述方法包括内环的基于MSCKF (多状态约束下的Kalman滤波器)的飞行器定位以及外环的基于室内已知目标点的位姿修正两部分，具体包括：首先获得扑翼飞行器初始状态的位置信息以及姿态信息，在扑翼飞行器飞行的过程中，获得飞行器的累积位姿信息，利用惯性传感器获得的测量值进行扑翼飞行器位姿的预测，并利用摄像头得到的周围环境的点云信息进行内环的位姿修正；利用摄像头检测的周围环境中的目标点信息进行外环的位姿修正。其具体步骤如图1所示，基本过程为：

步骤S1，获得扑翼飞行器的初始状态的位置信息X

步骤S2，通过初始状态的位置信息以及惯性传感器三轴加速度信息，进行多状态限制卡尔曼滤波器的参数初始化，得到初始状态扑翼飞行器的姿态信息，以及惯性单元传感器的传感器零偏与噪声等特征信息。

步骤S3，在扑翼飞行器飞行过程中，通过惯性传感器获得扑翼飞行器机体坐标系下三轴的加速度信息与角速度信息，通过加速度与角速度测量值对时间积分，得到飞行器的累积位置与姿态信息。

步骤S4，通过机载单目摄像头获得周围环境的点云信息，对飞行器的累积位置与姿态进行内环修正。

步骤S5，若单目摄像头检测到环境中位置已知的物品，则通过目标检测的方法得到飞行器相对于物品的位置，进而得到飞行器在世界坐标系下的位置，对飞行器内环得到的位姿信息进行外环修正。

本发明中扑翼飞行器只需用到自身携带的传感器包括惯性测量单元传感器与单目摄像头即可实现，惯性传感器获得扑翼飞行器的累积位姿信息，单目摄像头获得飞行器周围环境信息。所述滤波器包含的状态量由惯性单元有关量与单目相机有关量构成。所述滤波器中与惯性单元有关的状态量为

由于惯性单元的状态量波动较大，数据分布特性不稳定，因此采用真实值与估计值之间的残差量作为滤波器状态量

所述滤波器中与单目相机有关的状态量为每个采样时刻相机的位姿信息。惯性单元与单目相机的信息构成完整的滤波器状态量

同样变为状态量的残差形式

本发明中使用FAST角点检测法采集特征点，使用LK光流法对特征点进行跟踪，得到每个特征点在不同相机观测位姿下的重投影误差，作为滤波器的内环观测量。利用最小二乘法得到的特征点在世界坐标系下的坐标估计值，并转化到相机坐标系下，得到特征点在相机坐标系下的估计值为：

其中

对周围环境中位置已知的物体，比如门窗，采用图像颜色域分割，得到物体的大致位置；对所述的分割物体，采用角点检测，得到物体的角点的大致位置；对所述的采集得到的角点，采用非极大值抑制的方法，得到角点的精确位置。

以门为例，具体方法为：获得门的四个角点在世界坐标系下的坐标，同时获得门的颜色的像素值，存入扑翼飞行器的存储器中。在运动过程中，若在图像中检测到存在一个区域，其颜色像素值与门的像素值相近，则通过施加掩膜的方法将图像二值化，提取出该区域。然后利用角点检测的方法提取出该区域的四个角点。由于在提取角点的时候会存在特征点位置集中的现象，即在门的一个角点出检测出多个特征点，因此对于每个角点附近的特征点采用极大值抑制的方法，选取出其中像素值变化最明显的特征点，作为门的角点所在的位置。然后利用EPnP的方法得到飞行器自身位置的观测值。若观测值与状态量预测值相差在允许范围内，则认为得到了合理的观测值，进行外环校正。对所述的采集得到的物体角点的精确位置，采用EPnP方法，得到飞行器自身位姿，作为飞行器的外环观测量，进行飞行器位姿修正。

本发明中滤波器的运行流程可以如图2所示，主要分为

(1)处理器接收到惯性传感器测量得到的数据，利用加速度与角速度信息进行状态量的累积，得到飞行器位姿的预测量。

(2)由于惯性传感器的采样频率为200Hz左右，远大于图像的采样频率(20Hz左右)，因此如果处理器未接收到最新的图像数据，则不断重复利用惯性传感器数据累积，得到位姿预测量；若处理器接收到最新的图像数据，则计算观测量误差，进行内环更新；

若相机未采集到位置已知的物体，则重复(1)(2)；若相机采集到了含有位置已知的物体的图像，则在进行(1)(2)的同时，进行状态量的外环校正，减小状态量的漂移。

所述的滤波器结构如图3所示。本发明使用的是基于多状态限制卡尔曼滤波器的滤波算法。状态量预测公式为：

协方差矩阵由四部分构成，分别是惯性单元协方差矩阵

观测模型为

增益矩阵为

状态量更新

ΔX＝Kr

协方差矩阵更新

P