融合手机和无人机多传感参数的运动目标跟踪方法

，，

(浙江工业大学 信息工程学院，杭州 310023)

0 引言

四旋翼无人机具有自由度高，环境适应能力强等优点，在航空拍摄、交通巡逻、军事侦察等领域有广阔的应用场景。无人机运动跟踪的本质是通过估计无人机与运动目标之间的相对位姿来实现无人机对运动目标的跟踪。相对位姿的估计在航天器对接[1]、空中自主加油[2]、无人机编队飞行[3]等领域应用广泛，相对于依靠单传感器数据的相对位姿估计方法，基于多传感器数据融合[4-5]的方法能够提高位姿估计的准确性。随着半导体技术的发展，传感器的成本和体积大大减少，以及CPU处理性能的提升，使得融合多传感器数据的相对位姿估计方法在小型旋翼无人机的运动跟踪场景中应用成为可能。探究更高效准确的多传感数据融合的无人机跟踪方法，是当前的一个研究热点。

基于单传感器的相对位姿估计方法主要采用GPS、激光、视觉传感器等。Shao Mingqin等人[6]研究了位置差分，伪距差分和相位差分3种常用的差分GPS方法，发现位置差分方法最容易实现但精度最低，伪距差分方法精度较高且限制较少，所以得到广泛应用，相位差分方法精度最高但由于限制较多而应用有限。基于差分GPS的方法有一个明显的缺点，当在GPS信号弱或者缺失的地方，将会失效。Karl E.Wenzel等人[7]提出了基于视觉传感器的相对位姿方法，跟随无人机通过PnP方法计算得到与领航无人机的相对位姿，实现无人机的编队飞行。文献[8]中给出了另外一种相对位姿的估计方法POSIT，该方法可以在只有少数特征点的情况下，通过迭代求出物体姿态的近似解。但是基于图像的方案仍面临两个主要问题：1)因相机运动而产生的图像退化造成的运动目标错检的情况，由于在目标跟踪中需要控制相机或者相机载体运动，因此获取到的图像也经常会产生图像退化的情况从而对运动目标的提取和识别产生了影响；2)大量的图像计算对处理器平台的计算能力要求高，基于图像的目标跟踪包含了大量图像运算和图像的运动补偿，因此对于计算平台的要求较高，普通的嵌入式平台难以胜任。

融合多传感器数据的相对位姿估计方法，通过在单传感器方法的基础上引入其他传感器加以辅助以得到更精确的相对位姿估计。Zhu Yunfen等人[9]提出了基于SINS/GPS/IRST的无人机空中加油方法，该方法通过计算获得加油机与无人机的相对惯导信息和相对GPS信息，同时通过无人机上红外线传感器得到无人机与加油机之间的距离，然后将三者数据进行融合获取到无人机与加油机的相对位姿，实现无人机的空中加油。Jeremy Hardy等人[10]针对GPS信号部分地区无法使用且易受恶意劫持的情况，提出了一种基于视觉-IMU数据融合的无人机编队飞行方法，该方法使用点对点的无线电传输无人机的IMU数据，计算得到无人机之间的相对位姿，然后通过固定在无人机上朝下的相机提出特征信息来计算无人机之间的相对位姿，最后通过卡尔曼滤波融合数据得到无人机之间的相对位姿关系。王龙等人[11]提出了紧耦合的视觉/INS相对位姿测量方法，并通过实验证明紧耦合的方法相比于松耦合，有更高的实时性和测量精度。

本文结合相对位姿中多传感器融合的方法，提出了基于EKF(Extended Kalman Filter，扩展卡尔曼滤波)的融合手机和无人机多传感参数的运动跟踪方法。

1 基于EKF的多传感参数的融合方法

如图1所示，无人机上装有视觉传感器和惯性传感器，运动目标(本文以人体运动为例)持有具备IMU的移动设备，手机通过Wi-Fi将IMU数据发送给无人机，然后无人机将3个传感器的数据作为输入进行数据融合，最后实现无人机对运动目标的运动跟踪。IMU有采样频率快的特点，能够在快速剧烈运动的情况下得到运动状态，但是由于陀螺仪和加速度计本身存在测量误差，在长时间的计算过程中会产生漂移的情况，融合视觉传感器数据能够有效的抑制漂移；同时在视觉传感器出现遮挡或者错检情况时，可暂时依靠IMU数据进行跟踪，从而提高系统的鲁棒性。本文通过将视觉传感器获取到的运动目标特征点坐标直接与IMU获取到的相位位姿进行融合，简化相对位姿求解过程，实现多传感器融合的无人机实时运动跟踪。

图1 无人机运动跟踪系统示意图

EKF方法通常包括状态估计和测量更新两个部分，本小节将详细介绍系统的状态估计部分。基于视觉传感器数据的状态估计计算量大，且相对于IMU的更新频率太低，所以使用IMU来进行状态估计更符合系统要求。

同时由于多传感器的存在，需要先将所有数据转换到同一坐标系，再进行数据的融合。本文坐标系定义如图2所示，世界坐标系为G，无人机IMU坐标系Iu，无人机相机坐标系C以及运动目标IMU坐标系It。

图2 系统坐标系定义

1.1 IMU状态模型

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

ωm=Iω+bg+ng

(1)

(2)

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

(3)

(4)

(5)

其中：|Iω(t)×|表示反对称矩阵，可由式(6)得到：

(6)

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

(7)

(8)

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

(9)

1.2 IMU误差状态模型

(10)

⊗δq

(11)

(12)

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

(13)

(14)

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

Pk + 1=Fd·Pk·FdT+Gd·Q·GdT

(15)

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

(16)

(17)

(18)

(19)

2 视觉测量更新

在获取到无人机和运动物体的状态估计后，需要通过观测量对状态估计进行更新。本文将视觉传感器的输入作为观测量，通过特征点检测算法检测运动物体，建立测量更新方程来对IMU状态估计进行更新，最后将修正后的状态发送给无人机控制系统。本文选择将检测到的运动目标图像坐标直接作为EKF的输入，避免需要大量计算负载的PnP算法求解，提高系统的实时性。

图像数据的处理使用ORB算法[14]，ORB算法采用运算速度非常快的FAST角点检测子，并针对FAST特征不具备方向的问题，加入了FAST特征的方向信息；特征点描述部分则是在BRIEF的基础上改进了其对图像噪声敏感和不具备旋转不比性的特点；虽然在特征点的描述细致程度上不如SIFT[15]和SURF[16]算法，但是在计算速度上，ORB性能明显优于两者，针对嵌入式平台计算能力有限和本文对实时性的要求，本文选择ORB算法提取特征点。

2.1 测量方程

(20)

(21)

(22)

CPtar=ICR·GIR(GPtar-GPuav) +ICR·IPC

(23)

(24)

其中：Hf为测量雅克比矩阵，由h(x)求导获得，见式(25):

(25)

2.2 状态更新

在确定测量更新模型之后，可以得到完整EKF系统方程：

(26)

可根据EKF迭代得到无人机和运动目标的误差状态估计值，然后用于校正IMU状态估计。

3 实验分析

本文中多传感器的时间戳同步是数据融合的关键，无人机与移动设备上的IMU采样频率存在差异，无人机IMU的采样频率可以达到200 Hz或更高，而移动设备上的IMU受限于硬件性能，采样频率普遍在50～100 Hz，且两者无法做到绝对的时间戳同步，本文针对这个问题，采用ROS[18](Robot Operating System，机器人操作系统)的消息同步机制，将无人机与移动设备的IMU数据进行相对时间同步，然后将获取到无人机和手机的状态估计。本文中相机的采样频率为30 Hz，图像分辨率为752×480，通过使用文献[19]的方法，在相机采样时刻，选取距当前时刻最近的IMU状态估计，进行测量更新，校正状态估计。

作为实验所需条件之一，本文编写了一个Android APP用于采集手机中IMU的数据，并将数据转化为ROS消息格式，然后通过Wi-Fi传输给无人机，实验中使用的手机是三星Galaxy S7 edge，该机使用的IMU传感器为LSM6DS3，同时无人机将使用LP-RESEARCH公司的LPMS-USBAL传感器，实验平台搭建如图3所示。

图3 无人机运动跟踪实验平台搭建

获取到需要的传感器参数后，本文针对应用场景制作了两类数据集，一类模拟无人机悬停，运动物体运动的场景；另一类则模拟无人机跟随运动物体运动的场景。实验中本文方法将通过与基于图像的POSIT方法进行比较，验证本文方法的准确性。

图4 无人机悬停场景下的相对位置

图5 无人机悬停场景下的相对位置误差

其中图4给出了无人机悬停场景下无人机与运动目标的相对位置，其中虚线部分为运动目标走过的真实路线，实线部分和点划线则分别是本文方法计算得到的相对位置和POSIT方法计算得到的相对位置，图5则表示无人机悬停场景下本文方法和POSIT方法计算的相对位置误差；无人机跟随运动场景下无人机与运动目标保持同一速度和方向做直线运动6 m，同时保持无人机与运动物体之间的距离不变，图6代表无人机运动跟踪场景下的产生相对位置误差，同样实线代表本文方法，点划线为POSIT方法。两种场景下的相对位置误差如表1所示，从表中可以得出，不论无人机悬停或者跟踪场景下，本文方法得到的相对位置误差都要优于POSIT方法。无人机悬停情况下，本文方法得到的相对位置误差为0.67 m，跟踪场景下为0.273 m。

图6 无人机跟踪场景下的相对位置误差

表1 相对位置误差

4 结论

本文提出融合手机和无人机多传感器参数的无人机运动目标跟踪方法用于解决基于单目视觉传感器运动跟踪中存在的问题。通过设计Android APP将手机中的IMU数据通过Wi-Fi传输给无人机，使得无人机可以通过使用手机和无人机的IMU数据用于系统状态估计，然后通过视觉传感器获取的运动目标图像坐标作为扩展卡尔曼滤波的测量更新修正IMU状态估计产生的误差。最后通过实测数据集模拟无人机悬停和无人机跟踪场景，证明本文设计的方法在无人机的运动目标跟踪中能够达到0.67m的定位精度，相对于单目视觉的方法有更高的精度。为了验证该方法在无人机上的实际效果，未来的工作需要在无人机上运行本方法，并进行调试，同时将针对以人体为运动目标的场景扩展到其他应用场景。