加速度与光流传感器数据融合无人机水平速度控制方法

摘要

本发明涉及无人机定位方法，为提供一种新的基于方向信息的自适应二次噪声点检测方法，该方法可以有效地降低了非噪声点的误判概率，并且能够更有效地去除图像中的椒盐噪声，对不同强度噪声去噪的鲁棒性较强。为此，本发明采取的技术方案是，加速度与光流传感器数据融合无人机水平速度控制方法，利用安装在四旋翼无人机底部的光流传感器获取无人机的水平速度信息，并利用飞行控制器PCB板上的加速度传感器获取无人机的加速度信息，采用互补滤波器将上述数据进行融合，获取较为精准的无人机与地面的相对速度。本发明主要应用于无人机定位。

说明书

技术领域

本发明涉及无人机定位方法，特别是涉及一种基于加速度传感器与光流传感器数据融合的无人机自主定位方法。

技术背景

无人机的定位问题主要是指利用自身传感器确定无人机在飞行环境中相对于惯性坐标系的位置和姿态信息。准确的位姿估计是实现四旋翼无人机安全飞行、轨迹规划以及目标跟踪等复杂飞行任务的前提和基础。

目前广泛使用的无人机导航系统主要是基于GPS定位的方法，但是其定位精度较低，且在室内几乎没有信号，故而在室内实现无人机的定位飞行并不能利用GPS传感器。

日本千叶大学将实时光流视觉系统用于四旋翼无人机的定位与控制。它采用了一个垂直向下的摄像头完成光流计算，并将其与惯导数据融合，采用了三层嵌套的卡尔曼滤波技术，通过非线性控制器，实现了四旋翼无人机的室内外自主起飞、定点悬停、轨迹跟踪、自主降落等复杂任务，且控制效果较为良好。(会议：IEEEInternationalConferenceonRoboticsandAutomation；著者：F.Kendoula，I.Fantoni，K.Nonami；出版年月：2007年；文章题目：ThreeNestedKalmanFilters-basedAlgorithmforReal-timeEstimationofOpticalFlow,UAVMotionandObstaclesDetection；页码：4746-4751)(期刊：JournalofFieldRobotics；著者：F.Kendoula,I.Fantoni,K.Nonami，出版年月：2010年；文章题目：GuidanceandNonlinearControlSystemforAutonomousFlightofMinirotorcraftUnmannedAerialVehicles，页码：311–334)。

瑞士苏黎世联邦理工学院的研究人员采用PX4FLOW光流传感器作为位置测量单元并用于室内外无人机的定位控制系统中，该研究机构虽实现了基于光流法的室内轨迹跟踪实验，但从图中可以看出，光流传感器获取的无人机速度信息经长距离积分作用后，在边长约为3米的连续两圈矩形轨迹跟踪过程中，其最大定位误差大于0.5米，在随时间累加过程中其位置信息存在较大的漂移(会议：IEEEInternationalConferenceonRoboticsandAutomation；著者：HoneggerD,MeierL,TanskanenP；出版年月：2013年；文章题目：AnOpenSourceandOpenHardwareEmbeddedMetricOpticalFlowCMOSCameraforIndoorandOutdoorApplications；页码：1736-1741)。

此外，瑞士苏黎世联邦工学院通过光流法实现了四旋翼无人机在走廊中的避障飞行的高难度任务，他们利用超前的190°鱼眼摄像头获取光流信息，采用金字塔Lucas-Kanada法完成光流计算，采用Shi-Tomasi角点检测方法进行特征点提取，并将其与惯导数据融合，消除了光流旋转分量的影响。(会议：IEEEInternationalConferenceonRoboticsandAutomation；著者：S.Zingg,D.Scaramuzza,S.Weiss,etal；出版年月：2010年；文章题目：MAVnavigationthroughindoorcorridorsusingopticalflow；页码：3361-3368)。

发明内容

为克服现有技术的不足，提供一种新的基于方向信息的自适应二次噪声点检测方法，该方法可以有效地降低了非噪声点的误判概率，并且能够更有效地去除图像中的椒盐噪声，对不同强度噪声去噪的鲁棒性较强。为此，本发明采取的技术方案是，加速度与光流传感器数据融合无人机水平速度控制方法，利用安装在四旋翼无人机底部的光流传感器获取无人机的水平速度信息，并利用飞行控制器PCB板上的加速度传感器获取无人机的加速度信息，采用互补滤波器将上述数据进行融合，获取较为精准的无人机与地面的相对速度。

当携带有光流传感器的四旋翼无人机飞行时，外界影像相对于光流传感器运动，光流传感器的感光平面上便形成了像素运动，其速度表示为v_optical，它的值正比于无人机的相对速度v_q，反比于无人机与地面的相对距离h，其关系可以表示为：

$v_{optical}\propto \frac{v_q}{h}$

通过上述关系，便可以推算出无人机相对于地面的相对速度v_q。

由于采用基于加速度传感器和光流法的算法只能直接或间接获取无人机的水平的速度信息，所以只考虑水平方向无人机的水平速度信息融合方法，无人机的机体坐标系的x方向上：

在理想状态下，无人机x方向的水平速度信息v_x与其对应加速度信息a具有如下关系：

${\stackrel{·}{v}}_x=a_x$

a_x为加速度信息a的x方向的加速度，上式符号上方的点表示一阶导数，在实际测量过程中，由于传感器自身精度及外界干扰的存在，测量结果往往包含有大量的噪声信号和干扰信息，这里将由光流法获取的速度信息简化为如下形式：

v_{optical_x}＝v_x+μ_x

这里的v_{optical_x}为光流传感器读取的速度值，v_x为无人机传感器相对于地面的相对速度真值；μ_x为测量噪声，为恒定值；

飞行控制器获取的无人机的飞行状态信息是离散的信息，每一时间点的融合后的水平位置速度信息可以表示为v_x(n)，其中n为无人机信息的采样次数。

在融合算法中，令第一次的融合速度值与光流传感器的裸数据值相等，即：

v_x(1)＝v_{optical_x}

在第一个时间点后，在时域上的互补滤波器的输出值v_x(k)，即融合后的水平速度写成如下形式：

v_x(k)＝∫a_x-K₁(v_{optical_x}-v_x(k-1))

其中v_x(k)为本时间互补滤波器输出的x方向的水平速度信息，v_x(k-1)为上一时间互补滤波器输出的x方向的水平速度信息，a_x为加速度信息a的x方向的加速度，K₁为x方向的速度偏差的比例系数；该滤波器利用光流传感器的裸数据v_{optical_x}与上一时间的输出v_x(k-1)做差作为并与比例系数K₁构造比例反馈，以加速度传感器的数据a_x的积分作为前向通道，利用反馈通道补偿积分得到的速度信息，通过测量值与上一时间点做差，消去光流传感器测量的速度值的测量噪声μ_x，从而得到一个较为准确的x方向的速度信息。

在实际控制系统中，通过调节K₁的大小，改变无人机互补滤波器对加速度传感器和光流传感器的置信系数；当K₁较大时，对于光流传感器的置信系数较大，反之，对加速度传感器的置信系数较大。

同理对于无人机的机体坐标系y方向，采用与x方向相同的速度融合算法，表述如下：

光流法获得的y方向的速度信息可以表示为：

v_{optical_y}＝v_y+μ_y

这里的v_{optical_y}为光流传感器读取的速度值，v_y为无人机传感器相对于地面的相对速度真值；μ_y为测量噪声，为恒定值；

同样，在融合算法中，令第一次的融合速度值与光流传感器的裸数据值相等，即：

v_y(1)＝v_{optical_y}

在第一个时间点后，在时域上的互补滤波器的输出值v_y(k)，即融合后的水平速度写成如下形式：

v_y(k)＝∫a_y-K₂(v_{optical_y}-v_y(k-1))

其中v_y(k)为本时间互补滤波器输出的y方向的水平速度信息，v_y(k-1)为上一时间互补滤波器输出的y方向水平速度信息，a_y为加速度传感器获得的y方向的加速度，K₂为y方向的速度偏差的比例系数。与机体坐标系的x方向类似，通过改变通过调节K₂的大小，可以调节滤波器对于光流传感器与加速度传感器的置信系数。

本发明的技术特点及效果：

本发明采用互补滤波算法将采用光流法获取的无人机速度信息与加速度传感器获取的加速度信息进行融合，实现在较长时间范围内获取无人机的高精度准确速度信息，满足无人机室内稳定自主悬停的需要。

附图说明：

图1是本发明采用的互补滤波器结构框图；图中A_accel为加速度传感器的测量值，v_optical为光流传感器的测量值，v为互补滤波器输出的输的速度值。

图2是采用无人机水平速度控制器结构图；图中a_d为遥控器给定的水平加速度，v_t为对a_d积分后的给定速度，v为经过互补滤波器融合后的无人机实际速度，a_t为速度控制器输出的给定加速度。

图3是手持无人机水平x、y方向光流传感器的裸数据和融合后的数据，其中图3(a)为x方向的融合效果，图3(b)位y方向的融合效果。

图4是悬停过程中目标姿态角和实际姿态角的数据，其中图4(a)为给定的俯仰角及测量俯仰角，图4(b)为给定的滚转角及测量滚转角。

具体实施方式

本发明所要解决的技术问题是，提供一种基于光流传感器与加速度传感器数据融合的无人机自主定位方法，实现室内环境下无人机的定点悬停。

本发明采用的技术方案是：采用光流传感器与加速度传感器数据融合的方法用于无人机的定位系统中，包括如下步骤：

利用安装在四旋翼无人机底部的光流传感器获取无人机的速度信息及速度信息处理，利用飞行控制器PCB板上的加速度传感器获取无人机的加速度位置信息，采用互补滤波器将上述数据进行融合，获取较为精准的无人机与地面的相对速度。

所述的利用光流传感器获取无人机的速度信息及速度信息处理是：

光流是外界影像由于相对运动在视网膜上形成的视在运动，一般可以通过光流算法对视频流进行处理得到。当携带有光流传感器的四旋翼无人机飞行时，外界影像相对于光流传感器运动，传感器的感光平面上便形成了像素运动，其速度表示为v_optical，它的值正比于无人机的相对速度v_q，反比于无人机与地面的相对距离h，其关系可以表示为：

$v_{optical}\propto \frac{v_q}{h}$

通过上述关系，便可以推算出无人机相对于地面的相对速度v_q。

光流法直接获取的是无人机的水平速度信息，然而由于光流法会受到室内光线强度，飞行器飞行高度等因素影响，光流传感器的读数有较强的噪声，单独使用光流传感器会影响到控制的精度。

利用加速度传感器可以将无人机的加速度信息积分后获取速度信息。但是，对加速度信息进行积分，会将加速度传感器的噪声也进行积分，从而影响所获取的速度信息的精度。

所述的采用互补滤波器进行数据融合是：

采用互补滤波算法将采用加速度传感器和光流法获取的无人机位置信息进行融合，可以获取的无人机相对于地面的速度信息，且较为准确，通过构造单闭环比例积分控制器，实现了无人机在室内的定点悬停。以下将主要对基于互补滤波器的数据融合算法进行介绍：

由于采用基于加速度传感器和光流法的定位算法只能直接或间接获取无人机的水平位置的速度信息，所以只考虑水平方向无人机的水平速度信息融合方法。此外，由于水平方向上x方向和y方向数据处理方法类似，以下以x方向为例进行说明。

在理想状态下，无人机x方向的水平速度信息v_x与其对应速度信息a_x具有如下关系：

${\stackrel{·}{v}}_x=a_x$

在实际测量过程中，由于传感器自身精度及外界干扰的存在，测量结果往往包含有大量的噪声信号和干扰信息，这里将由光流法获取的速度信息简化为如下形式：

v_{optical_x}＝v_x+μ_x

这里的v_{optical_x}为光流传感器读取的速度值，v_x为无人机传感器相对于地面的相对速度真值；μ_x为测量噪声，为恒定值。

采用互补滤波算法的结构框图如图1所示，根据系统的结构框图，可以将系统的输出v_x(k)写成如下形式：

v_x(k)＝∫a-K₁(v_{optical_x}-v_x(k-1))

其中v_x(k)为本状态互补滤波器输出的水平速度信息，v_x(k-1)为上一时间点互补滤波器输出的水平速度信息。

在实际控制系统中，通过调节K₁的大小，可以改变无人机互补滤波器对加速度传感器和光流传感器的置信系数。当K₁较大时，对于光流传感器的置信系数较大，反之，对加速度传感器的置信系数较大。

对于无人机集体坐标系y方向，与x方的速度融合算法相同，可以表述为：

光流法获得的y方向的速度信息为：

v_{optical_y}＝v_y+μ_y

上述v_{optical_y}为光流传感器读取的速度值，v_y为无人机传感器相对于地面的相对速度真值；μ_y为速度测量的噪声值，为恒定值；

在融合算法中，令第一次的融合速度值与光流传感器的裸数据值相等，即：

v_y(1)＝v_{optical_y}

第一个时间点之后，在时域上的互补滤波器的输出值v_y(k)，即融合后的水平速度写成如下形式：

v_y(k)＝∫a_y-K₂(v_{optical_y}-v_y(k-1))

其中v_y(k)为本时间点互补滤波器输出的y方向的水平速度信息，v_y(k-1)为上一时间点互补滤波器输出的y方向水平速度信息，a_y为加速度传感器获得的y方向的加速度，K₂为机体坐标系y方向的速度偏差的比例系数，调整K₂可以得到一个较为准确的y方向的速度信息。与机体坐标系的x方向类似，调整K₂也是在调节滤波器对于光流传感器与加速度传感器的置信系数。

下面结合实例和附图对本发明基于光流传感器和加速度传感器数据融合的无人机自主定位方法做出详细说明。

本发明综合考虑基于光流法的定位方法的优点，充分利用光流法短时间内定位精度较高能够在室内工作和加速度传感器频率较快的优势，通过互补滤波方法将两种定位算法进行融合，并在室内环境中通过手持四旋翼无人机进行了相应的实验验证，实现室内基于多传感器数据融合的四旋翼无人机精准定位功能。

本发明基于光流传感器和加速度传感器数据融合的无人机自主定位方法，包括以下步骤：

1)利用光流传感器获取无人机的速度信息及速度信息处理：

光流法直接获取的是无人机的水平速度信息，当携带有光流传感器的四旋翼无人机飞行时，由于外界影像相对于光流传感器运动，传感器的感光平面上便形成了像素运动，其速度表示为v_flow，它的值正比于无人机的相对速度v_q，反比于无人机与地面的相对距离h，可以表示为：

$v_{flow}\propto \frac{v_q}{h}$

由此便可以得到四旋翼无人机的水平速度信息。然而，由于受到光照条件，地面纹理等因素的影响，通常由光流传感器直接获取的无人机的相对速度存在高频率的高斯白噪声，若不进行融合，会影响控制效果。

2)利用加速度传感器获取无人机的加速度信息：

加速度仪传感器对光流传感器的噪声进行补偿，通过SPI总线，实现了微控制器与加速度传感器之间的通信，在1kHz频率下利用可编程中断读取加速度传感器的读数，并对其进行积分得到无人机的速度信息。

3)采用互补滤波器进行数据融合：

采用互补滤波算法将光流传感器和加速度传感器获取的无人机水平速度信息进行融合，实现在室内环境下获取无人机的准确速度信息的目的，满足无人机室内自主悬停飞行控制的需要。以下将主要对基于互补滤波器的数据融合算法进行介绍。

由于采用基于光流传感器或加速度传感器的定位算法目的是直接或间接获取无人机的水平速度信息，所以本章中只考虑水平方向无人机的速度信息融合方法。

在理想状态下，无人机x方向的速度信息v_x，y方向的速度信息v_y，与其对应加速度信息a_x，a_y具有如下关系：

${\stackrel{·}{v}}_x=a_x$

${\stackrel{·}{v}}_y=a_y$

但在实际测量过程中，由于传感器自身精度及外界干扰的存在，测量结果往往包含有大量的噪声信号和干扰信息，这里将采用光流传感器获取的速度信息简化为如下形式：

v_{optical_x}＝v_x+μ_x

v_{optical_y}＝v_y+μ_y

这里的v_{optical_x}、v_{optical_y}、v_x、v_y分别为采用光流传感器获得的x和y方向的测量值和无人机水平速度x和y方向的真值，μ_y和μ_x分别为测量噪声，均为恒定值。

采用互补滤波算法的结构框图如图1所示，根据系统的结构框图，可以将系统的输出v_x(k)写成如下形式：

v_x(k)＝∫a-K₁(v_{optical_x}-v_x(k-1))

v_y(k)＝∫a_y-K₂(v_{optical_y}-v_y(k-1))

其中其中v_x(k)为本时间互补滤波器输出的x方向的水平速度信息，v_x(k-1)为上一时间互补滤波器输出的x方向的水平速度信息，a_x为加速度传感器获得的x方向的加速度，K₁为x方向的速度偏差的比例系数；v_y(k)为本时间互补滤波器输出的y方向的水平速度信息，v_y(k-1)为上一时间互补滤波器输出的y方向水平速度信息，a_y为加速度传感器获得的y方向的加速度，K₂为y方向的速度偏差的比例系数；在实际控制系统中，将K₁或K₂调整大时，滤波器对光流传感器的置信系数较大，反之，则对加速度传感器的置信系数较大。

下面给出具体的实例：

一、系统硬件连接及配置

本发明的基于视觉的四旋翼无人机自主飞行控制方法采用基于嵌入式架构的飞行控制结构，所搭建的实验平台包括四旋翼无人机本体、地面站、遥控器等。其中四旋翼无人机搭载了机载PX4FLOW光流传感器、飞行控制器(内核芯片为FreescaleK60芯片并集成了惯性导航单元和气压计模块等)。地面站包括一台装有Linux操作系统的笔记本计算机，用于机载程序的启动及远程监控。该平台可通过遥控器进行手动起飞和降落，并在发生意外时紧急切换为手动模式，以确保实验安全。

二、飞行实验结果

本实施例对上述实验平台进行了多组飞行控制实验，飞行实验环境为室内环境中。控制目标是实现四旋翼无人机的室内悬停定位功能。

室内手持实验过程中无人机的数据曲线如图3所示。其中，手持无人机保持不动，其速度与融合速度如图3所示，可以认为无人机的实际速度几乎为0，此时x轴光流传感器的裸数据的平均值为19.64cm/s，融合之后的数据为6.185cm/s，y轴的数据则分别为27.07cm/s和8.34cm/s，可见融合之后的速度信息能够消除光流传感器的误差等，融合算法是有效的。此外，由图4所示，无人机在悬停过程中目标俯仰角pitch与实际俯仰角偏差的绝对值均值为2.077°，滚转角roll的这一数据为0.986°，控制器有着较为良好的控制效果。