一种基于手机和无人机多传感参数融合的无人机运动跟踪方法与流程

本发明涉及无人飞行器航拍领域，尤其是针对旋翼飞行器的多传感参数融合来实现的一种运动跟踪方法。

背景技术

近年来，随着计算机技术，自动控制理论，嵌入式开发，芯片设计以及传感器技术的迅速发展，让无人飞行器能够在更加小型化的同时，拥有更强的处理能力，无人机上的相关技术也受到越来越多的关注；小型无人机拥有操控灵活，续航能力强等优势，从而能够在狭小环境中处理复杂任务，在军事上能够执行军事打击，恶劣环境下搜索，情报收集，等高风险环境下替代士兵的工作；在民用上，为各行各业从业人员提供航拍，远程设备巡检，环境监测，抢险救灾等等功能；

四旋翼为常见旋翼无人飞行器，通过调节电机转速实现飞行器的俯仰，横滚以及偏航动作；相对于固定翼无人机，旋翼无人机拥有明显的优势：首先，机身结构简单、体积小，单位体积可产生更大升力；其次，动力系统简单，只需调整各旋翼驱动电机转速即可完成空中姿态的控制，可实现垂直起降、空中悬停等多种特有的飞行模式，且系统智能度高，飞行器空中姿态保持能力强；

在无人机上搭载高清摄像头，实时运行机器视觉算法已经成为近年来热点研究领域，无人机拥有灵活的视角，能帮助人们捕获一些地面移动摄像机难以捕捉到的图像，如果将轻量级摄像头嵌入到小型四旋翼无人机上，还能提供丰富并廉价的信息；目标跟踪是指在低空飞行的无人机，通过计算相机获得的视觉信息来得到目标与无人机间的相对位移，进而自动调整无人机的姿态和位置，使被跟踪的地面移动目标保持在相机视野中心附近，实现无人机跟随目标运动完成跟踪任务，但是由于单目摄像机的技术限制，想要得到移动物体的三维坐标信息是非常困难的，因此，想要实现运动目标的跟踪需要有一种融合手机和无人机多传感参数的相对位姿估计方法。

技术实现要素：

为了解决基于单目相机的无人机运动跟踪方法存在因图像退化带来的错检和漏检问题，可以通过app将手机的imu参数发送至无人机，无人机可以跟踪手机的imu参数和自身的imu参数计算两者之间的相对位姿，因为imu会产生误差，可以通过图像信息对imu误差进行校正，基于这样的思路，本发明提出一种基于手机和无人机多传感参数融合的无人机运动跟踪方法。

本发明解决其技术问题所采用的技术方案是：

一种基于手机和无人机多传感参数融合的无人机运动跟踪方法，所述方法包括以下步骤：

1)设计android应用程序，获取手机的加速度计和陀螺仪参数，并将imu参数制成ros信息格式，最后通过wi-fi发送至无人机端；

2)获取手机和无人机的imu参数，建立imu状态模型，误差状态模型，过程如下：首先需要对无人机和运动目标的运动分别进行建模，通过加速度计和陀螺仪的数据建立位置、速度、角度的状态空间模型，然后根据imu的误差特性建立误差状态模型，最后将两者的状态模型联立，获取整个系统的状态方程；

3)根据获取到的图像提取运动目标；

4)使用多速率扩展卡尔曼滤波对相对位姿进行滤波。

进一步，所述步骤2)的步骤如下：

(2.1)建立imu状态模型

imu由三轴陀螺仪和三轴加速度计组成，陀螺仪获取到imu自身的旋转角速度，加速度计获取到自身的线性加速度，由于测量误差的存在，给出imu的测量模型：

ωm＝ⁱω+bg+ng(1)

其中，ωm，am分别代表陀螺仪和加速度计的测量值，ⁱω为imu坐标系下的实际角速度值，^ga为世界坐标系下的线加速度值，na，ng为测量高斯白噪声，ba，bg为测量零偏，定义为随机游走噪声，为世界坐标系到imu坐标系的旋转矩阵，^gg则为当地的重力加速度在世界坐标系下的表示；

已知imu的测量模型，得到imu的状态向量：

其中，^gp，^gv分别代表世界坐标系下的imu的位置和速度，则表示的是从世界坐标系到imu坐标系的单位旋转四元数，使用的四元数符合hamilton定义，根据运动学方程，得到imu的连续时间状态：

其中^ga(t)＝i^gr(t)(am(t)-ba(t)-na(t))+^gg，nba，nbg是均值分别为σba和σbg的高斯白噪声，ω(ⁱω(t))由式(9)所得：

其中，表示反对称矩阵，由式(6)得到：

在无人机运动跟踪过程中，需要时刻估计无人机与运动目标的相对位姿，由式(1)、(2)得到角速度和线加速度的估计，分别由式(7)、(8)给出：

再根据式(7)、(8)进行离散化得到[k,k+1]时刻的状态估计值：

其中，δt＝tk+1-tk代表相邻imu采样时间间隔，值得注意的是，在计算式(9)时，假定在[k,k+1]时刻内角速度和加速度是线性变化的；四元数状态估计公式中代表四元数乘法，同时可以通过将离散化得到；

(2.2)建立imu误差状态模型

在获取到imu的状态估计后，通过imu误差状态转移矩阵fc描述误差在imu状态估计和传播过程中产生的影响，imu误差状态向量由得到，如式(10)示：

代表旋转角度误差，已知四元数误差由一个小角度旋转来表示，如式(11)所示，

由此得将系统误差状态向量降维，得到15×1的误差状态向量，同时得到角度误差求解公式(12)：

在确定系统误差状态向量后，根据imu运动连续时间状态公式(4)和imu状态估计公式(9)得出imu误差状态连续时间转移矩阵：

式(13)中同时式(13)简化为：

将误差转移方程离散化，可以得到fd和gd，用于求imu估计过程中的协方差，设协方差p的初始值为零，p的更新方程如式(15)所示：

pk+1＝fd·pk·fd^t+gd·q·gd^t(19)

式(15)中，q为噪声矩阵，如式(16)所示：

包含无人机和运动目标的imu状态，设无人机和运动目标的imu误差状态分别为且设

则完整的系统误差状态向量为由式(19)给出，

其中δn＝nuav-ntar。

再进一步，所述步骤1)的步骤为：

android传感器数据获取时首先需要获取传感器管理对象(sensormanager)，然后通过sensormanager定义传感器类型，然后注册监听器监听加速度传感器的数据变化，注册好监听器后，每次数据改变都会触发回调函数，可在回调函数中获取传感器数据，然后将数据制作成ros信息格式，并通过wi-fi将消息发布，无人机端可订阅节点消息。

所述步骤3)中，根据获取到的图像提取运动目标的步骤如下：

(3.1)采集图像

基于四旋翼飞行器平台的linux开发环境，使用机器人操作系统ros订阅图像主题的方式获取图像的，相机驱动由ros和opencv实现；

(3.2)图像预处理

采集到的彩色图像首先要进行灰度化，去除无用的图像彩色信息，这里使用的方法是求出每个像素点的r、g、b三个分量的加权平均值即为这个像素点的灰度值，这里不同通道的权值根据运行效率进行优化，这里避免浮点运算计算公式为：

gray＝(r×30+g×59+b×11+50)/100(20)

其中gray为像素点的灰度值，r、g、b分别为红、绿、蓝色通道的数值；

(3.3)orb提取特征点

orb也称为rbrief，提取出局部不变的特征，是对brief算法的改进，brief运算速度快，然而没有旋转不变性，并且对噪声比较敏感，orb解决了brief的这两个缺点；为了让算法能有旋转不变性，orb首先利用harris角点检测方法检测角点，之后利用亮度中心来测量旋转方向；假设一个角点的亮度从其中心偏移而来，则合成周围点的方向强度，计算角点的方向，定义如下强度矩阵：

mpq＝∑x，yx^py^qi(x，y)(21)

其中x，y为图像块的中心坐标，i(x，y)表示中心的灰度，x^p，y^q代表点到中心的偏移，则角点的方向表示为：

从角点中心构建这个向量，则这个图像块的方向角θ表示为：

θ＝tan^-1(m01，m10)(23)

由于orb提取的关键点具有方向，因此利用orb提取的特征点具有旋转不变性；

(3.4)特征描述符的匹配

通过ransac算法来去除误匹配点对，ransac反复对数据中的子集进行随机选取，并且将选取的子集假设为局内点，然后进行验证是否符合选取要求，ransac在特征点匹配中的过程如下所示：

3.4.1)从样本集中随机抽选ransac样本，即匹配点对；

3.4.2)根据匹配点对计算变换矩阵；

3.4.3)由样本集、误差度量函数以及矩阵，寻找所有满足当前模型的其他匹配点对，并标定为内点，返回该内点元素个数；

3.4.4)根据局内点个数判定该集合是否属于最优集合；

3.4.5)更新当前错误匹配率，如果大于设置的错误率阈值则重复ransac迭代过程。

所述步骤4)中，当无人机的相机没有进行量测输出时，系统认为图像缺失或者中断，滤波器只进行时间更新；但当相机有量测输出时，滤波器同时进行时间更新和量测更新。

本发明的技术构思为：随着四旋翼飞行器技术的成熟与稳定并且大量地在民用市场上推广，越来越多的人着眼于使用四旋翼飞行器更准确的跟踪目标，本发明就是在四旋翼飞行器实现运动目标跟踪的研究背景下提出的。

基于手机和无人机多传感参数融合的无人机运动跟踪方法主要包括：设计android应用程序将手机imu参数发送至无人机，根据手机和无人机的imu参数构造状态模型和误差状态模型，进一步通过图像信息提取运动目标的坐标，最终通过图像测量信息校正imu的误差，获取准确的相对位姿。

本方法的有益效果主要表现在：针对基于相机的无人机运动跟踪方法存在因图像退化带来的错检和漏检问题，提出一种基于手机和无人机多传感参数融合的无人机运动跟踪方法，大大提高了跟踪的精度和鲁棒性。

附图说明

图1为一种基于手机和无人机多传感器参数融合的无人机运动跟踪方法概括图；

图2为android应用程序获取imu数据并发送的流程图。

具体实施方式

下面结合附图对本发明进一步描述：

参照图1和图2，一种基于手机和无人机多传感参数融合的无人机运动跟踪方法，包括以下步骤：

1)设计android应用程序，获取手机的加速度计和陀螺仪参数，并将imu参数制成ros信息格式，最后通过wi-fi发送至无人机端；

android传感器数据获取时首先需要获取传感器管理对象(sensormanager)，然后通过sensormanager定义传感器类型(以加速度传感器为例)，然后注册监听器监听加速度传感器的数据变化，注册好监听器后，每次数据改变都会触发回调函数，可在回调函数中获取传感器数据，然后将数据制作成ros信息格式，并通过wi-fi将消息发布，无人机端可订阅节点消息。

2)获取手机和无人机的imu参数，建立imu状态模型，误差状态模型，过程如下：

(2.1)建立imu状态模型

imu由三轴陀螺仪和三轴加速度计组成，陀螺仪可以获取到imu自身的旋转角速度，加速度计则可以获取到自身的线性加速度，由于测量误差的存在，给出imu的测量模型：

ωm＝ⁱω+bg+ng(1)

其中，ωm，am分别代表陀螺仪和加速度计的测量值，ⁱω为imu坐标系下的实际角速度值，^ga为世界坐标系下的线加速度值，na，ng为测量高斯白噪声，ba，bg为测量零偏，定义为随机游走噪声，为世界坐标系到imu坐标系的旋转矩阵，^gg则为当地的重力加速度在世界坐标系下的表示；

已知imu的测量模型，可以得到imu的状态向量：

其中，^gp，^gv分别代表世界坐标系下的imu的位置和速度，则表示的是从世界坐标系到imu坐标系的单位旋转四元数，使用的四元数符合hamilton定义，根据运动学方程，可以得到imu的连续时间状态：

其中nba，nbg是均值分别为σba和σbg的高斯白噪声，ω(ⁱω(t))由式(5)所得：

其中，表示反对称矩阵，由式(6)得到：

在无人机运动跟踪过程中，需要时刻估计无人机与运动目标的相对位姿，由式(1)、(2)到角速度和线加速度的估计，不考虑测量高斯白噪声n，分别由式(7)、(8)给出：

再根据式(7)、(8)进行离散化(雅各比矩阵)得到[k,k+1]时刻的状态估计值：

其中，δt＝tk+1-tk代表相邻imu采样时间间隔，值得注意的是，在计算式(9)时，假定在[k,k+1]时刻内角速度和加速度是线性变化的。四元数状态估计公式中代表四元数乘法，同时可以通过将离散化得到；

(2.2)建立imu误差状态模型

在获取到imu的状态估计后，通过imu误差状态转移矩阵fc描述误差在imu状态估计和传播过程中产生的影响，imu误差状态向量由得到，如式(10)示：

代表旋转角度误差，已知四元数误差由一个小角度旋转来表示，如式(11)所示，

由此可得将系统误差状态向量降维，得到15×1的误差状态向量，同时得到角度误差求解公式(12)：

在确定系统误差状态向量后，根据imu运动连续时间状态公式(4)和imu状态估计公式(9)得出imu误差状态连续时间转移矩阵：

式(13)中同时式(13)简化为：

将误差转移方程离散化，得到fd和gd，用于求imu估计过程中的协方差，设协方差p的初始值为零，p的更新方程如式(15)所示：

pk+1＝fd·pk·fd^t+gd·q·gd^t(15)式(15)中，q为噪声矩阵，如式(16)所示：

包含无人机和运动目标的imu状态，这里设无人机和运动目标的imu误差状态分别为且设

则完整的系统误差状态向量为由式(19)给出，

其中δn＝nuav-ntar。

3)根据获取到的图像提取运动目标，过程如下：

(3.1)采集图像

一般而言，采集图像的方法有非常多中，本发明是基于四旋翼飞行器平台的linux开发环境，使用机器人操作系统ros订阅图像主题的方式获取图像的，相机驱动由ros和opencv实现；

(3.2)图像预处理

由于本发明所使用的特征提取方法基于的是图像的纹理光强以及梯度信息，因此采集到的彩色图像首先要进行灰度化，去除无用的图像彩色信息，这里使用的方法是求出每个像素点的r、g、b三个分量的加权平均值即为这个像素点的灰度值，这里不同通道的权值可以根据运行效率进行优化，这里避免浮点运算计算公式为：

gray＝(r×30+g×59+b×11+50)/100(20)

其中gray为像素点的灰度值，r、g、b分别为红、绿、蓝色通道的数值。

(3.3)orb提取特征点

orb也称为rbrief，提取出局部不变的特征，是对brief算法的改进，brief运算速度快，然而没有旋转不变性，并且对噪声比较敏感，orb解决了brief的这两个缺点；为了让算法能有旋转不变性，orb首先利用harris角点检测方法检测角点，之后利用亮度中心(intensitycentroid)来测量旋转方向；假设一个角点的亮度从其中心偏移而来，则合成周围点的方向强度，可以计算角点的方向，定义如下强度矩阵：

mpq＝∑x，yx^py^qi(x，y)(21)

其中x，y为图像块的中心坐标，i(x，y)表示中心的灰度，x^p，y^q代表点到中心的偏移，则角点的方向可以表示为：

从角点中心构建这个向量，则这个图像块的方向角θ可以表示为：

θ＝tan^-1(m01，m10)(23)

由于orb提取的关键点具有方向，因此利用orb提取的特征点具有旋转不变性；

(3.4)特征描述符的匹配

orb算法的特征提取很快，但在实际情况下进行特征匹配时，匹配的效果并不会特别好，尤其当视角发生比较大的变化或者出现之前图像中没有出现的区域，就很容易产生误匹配，如何能够解决这个问题，这就需要通过ransac算法来去除误匹配点对。

ransac是一种不确定算法，它有一定概率获得合理的模型，而提高迭代次数可以提高这个概率。ransac有观测数据、参数化模型和初始参数构成，其观测数据分为满足预设模型的局内点(inlier)以及模型误差超过阈值的局外点(outlier)两类。

ransac反复对数据中的子集进行随机选取，并且将选取的子集假设为局内点，然后进行验证是否符合选取要求。ransac在特征点匹配中的过程如下所示：

3.4.1)从样本集中随机抽选ransac样本(匹配点对)；

3.4.2)根据匹配点对计算变换矩阵；

3.4.3)由样本集、误差度量函数以及矩阵，寻找所有满足当前模型的其他匹配点对，并标定为内点(inlier)，返回该内点元素个数；

3.4.4)根据局内点个数判定该集合是否属于最优集合；

3.4.5)更新当前错误匹配率，如果大于设置的错误率阈值则重复ransac迭代过程。

4)使用多速率扩展卡尔曼滤波对相对位姿进行滤波

常规的扩展卡尔曼滤波包括时间更新和量测更新两个更新过程，并且在一个滤波周期内是一一对应的关系，但多速率扩展卡尔曼滤波器的更新方式不同于常规的方法。以一个量测周期内的更新过程为例，当无人机的相机没有进行量测输出时，系统认为图像缺失或者中断，滤波器只进行时间更新；但当相机有量测输出时，滤波器同时进行时间更新和量测更新。这样的处理方式能够提高数据的更新率，减少了imu信息的浪费，同时相比于基于图像的运动跟踪方法丢失目标的情况，系统更具有鲁棒性。