CN113432602B - 基于多传感器融合的无人机位姿估计方法 - Google Patents

Abstract

本发明提出了一种基于多传感器融合的无人机位姿估计方法，用于复杂环境中无人机的自主定位，实现步骤是：分别获取视觉传感器、IMU和GPS传感器的数据，之后对不同传感器的数据进行处理并构建对应的视觉残差、IMU残差和GPS残差，构建过程中给IMU残差和GPS残差赋予不同的权重，之后融合不同传感器的残差，并使用非线性方法进行优化，得到无人机的位姿。本发明在融合多传感器数据进行无人机位姿求解时，考虑了不同传感器所获数据之间的差异，提高了无人机的位姿估计精度。

Description

基于多传感器融合的无人机位姿估计方法

技术领域

本发明属于目标跟踪技术领域，特别涉及一种基于多传感器融合的无人机位姿估计方法，可用于复杂环境中无人机的自主定位。

背景技术

无人机位姿估计方法的主要任务是，通过对自身携带传感器获取到的数据进行优化处理，从而实现对无人机的定位。在无人机位姿估计中，单目视觉系统通过跟踪多幅图像之间的共同特征，利用多视图几何关系求解图像之间的位置和姿态，在低动态、场景特征丰富的情况下具有较高的精度；而惯性测量单元IMU具有很好的自主性，受环境的影响较小。随着信息科学和通信技术的发展，目前已存在大量的位姿估计方法应用于无人机定位。但是，由于没有环境先验信息以及不同传感器获取到的数据存在不同的噪声等客观因素，使得实时精确的定位无人机仍是一个巨大的挑战。

例如申请公布号为CN111024066A，名称为“一种无人机视觉-惯性融合室内定位方法”的专利申请，公开了一种基于视觉传感器和IMU融合的无人机位姿估计方法，该方法首先对相机获取的图像进行特征点提取，然后对惯性传感器IMU的测量数据进行预积分，得到任意两帧图像间无人机的运动轨迹与姿态，之后紧耦合视觉、惯性里程计对无人机的状态进行估计。该方法存在的不足之处是，在利用两种不同的传感器数据融合求解无人机位姿时，并没有考虑不同传感器数据带来的差异，比如不同传感器中蕴含的噪声程度以及类型都不大相同，因此所求得的无人机位姿精度不是很高。

发明内容

本发明的目的是针对上述现有技术的不足，提出一种基于多传感器融合的无人机位姿估计方法，旨在解决现有技术在融合多传感器数据进行无人机位姿求解时，没有考虑不同传感器数据之间的差异，从而使得求解的无人机位姿精度较低。

为实现上述目的，本发明采取的技术方案包括如下步骤：

(1)获取多传感器数据：

(1a)获取无人机携带的视觉传感器以10Hz-30Hz的频率采集M幅不同的地面灰度图像X＝{X_m|1≤m≤M}，IMU传感器以100Hz-200Hz的频率采集N组不同的无人机加速度数据A＝{A_n|1≤n≤N}和角速度数据W＝{W_n|1≤n≤N}，以及GPS传感器以20Hz-100Hz的频率采集不同的Z组无人机位置数据G＝{(O_z,L_z,H_z)|1≤z≤Z},其中M≥20，X_m表示第m幅地面灰度图像，N≥200，A_n、W_n分别表示t时刻无人机的加速度、角速度，Z≥40，O_z、L_z、H_z分别表示t时刻无人机的经度值、纬度值、高度值；

(2)计算每个特征点F_c的视觉残差E_c：

对每幅地面灰度图像X_m进行特征点提取，得到X对应的特征点集合X'＝{X_m'|1≤m≤M}，并计算特征点集合X_m'中每个特征点F_c的视觉残差E_c，其中X_m'表示X_m对应的包括C个特征点的特征点子集合F＝{F_c|1≤c≤C}，C≥30；

(3)计算每幅图像与相邻图像对应无人机位置之间的IMU残差E_(m,m+1)；

采用预积分方法，通过t时刻无人机的加速度A_n和角速度W_n计算相邻地面灰度图像X_m及相邻图像X_m+1之间的位移增量P_(m,m+1)、速度增量V_(m,m+1)和旋转增量Q_(m,m+1)，并通过P_(m,m+1)、V_(m,m+1)和Q_(m,m+1)，计算X_m与X_m+1对应无人机位置之间的IMU残差E_(m,m+1)；

(4)计算每幅图像对应无人机位置的GPS残差E_z：

将GPS采集的Z组无人机位置数据G转换为东北天坐标系下的坐标G'＝{(O_z',L_z',H_z')|1≤z≤Z},并计算每幅图像X_m在G_z'下的对应无人机位置的GPS残差E_z；

(5)获取无人机位姿估计结果：

通过每个特征点F_c的视觉残差E_c、每幅图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)和每幅图像X_m对应无人机位置的GPS残差E_z，构建多传感器残差E_tight，并采用非线性优化方法对E_tight进行优化，得到t时刻无人机的位姿，其中：

E_tight＝E_c+W_(m,m+1)·E_(m,m+1)+W_z·E_z

W_(m,m+1)表示图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)的权重，W_z表示每幅图像X_m对应无人机位置的GPS残差E_z的权重。

本发明与现有技术相比，具有以下优点：

1.本发明首先计算每个特征点的视觉残差、每幅图像与相邻图像对应无人机位置之间的IMU残差，以及每幅图像对应无人机位置的GPS残差，并对三个残差进行融合，然后对融合后的多传感器残差进行优化，得到每个时刻无人机的位姿，避免了现有技术中仅采用视觉传感器和IMU传感器导致的长期运行中出现的累计漂移现象对无人机位姿估计精度的影响，有效提高了无人机的定位精度。

2.本发明在获得每个特征点的视觉残差、每幅图像与相邻图像对应无人机位置之间的IMU残差，以及每幅图像对应无人机位置的GPS残差后，对IMU残差和GPS残差赋予不同的权重，之后再对三个残差融合，并对融合后的多传感器残差进行优化，得到每个时刻无人机的位姿，避免了现有技术中对获得的多传感器残差直接融合，从而忽略了不同传感器数据之间的差异对无人机位姿估计精度的影响，有效提高了无人机的定位精度。

附图说明

图1是本发明的实现流程图；

图2为本发明与现有技术在相同EuRoC数据序列下无人机真实位姿的仿真对比图。

具体实施方式

以下结合附图和具体实施例，对本发明作进一步详细描述。

参照图1，本发明包括如下步骤。

步骤1)获取传感器的数据：

获取无人机携带的视觉传感器以10Hz-30Hz的频率采集M幅不同的地面灰度图像X＝{X_m|1≤m≤M}，IMU传感器以100Hz-200Hz的频率采集N组不同的无人机加速度数据A＝{A_n|1≤n≤N}和角速度数据W＝{W_n|1≤n≤N}，以及GPS传感器以20Hz-100Hz的频率采集不同的Z组无人机位置数据G＝{(O_z,L_z,H_z)|1≤z≤Z},其中M≥20，X_m表示第m幅地面灰度图像，N≥200，A_n、W_n分别表示t时刻无人机的加速度、角速度，Z≥40，O_z、L_z、H_z分别表示t时刻无人机的经度值、纬度值、高度值，在本发明实施例中，视觉传感器采用的频率是20Hz，IMU传感器采用的频率是200Hz，GPS传感器采用的频率是20Hz，图像总数M为3040幅，IMU组数N为30401，GPS组数Z为3040。

步骤2)计算每个特征点F_c的视觉残差：

对于视觉传感器获取的图像，因为要尽可能的保存图像的信息，同时当场景和相机视角发生少量改变时，还希望能从图像中判断哪些地方是同一个点，因此采用基于特征点的方式对图像进行特征提取。

对每幅地面灰度图像X_m进行特征点提取，得到X对应的特征点集合X'＝{X_m'|1≤m≤M}，并计算特征点集合X_m'中每个特征点F_c的视觉残差E_c，其中X_m'表示X_m对应的包括C个特征点的特征点子集合F＝{F_c|1≤c≤C}，C≥30；

其中对每幅地面灰度图像X_m进行特征点提取，实现步骤为：

步骤2a)对每幅地面灰度图像X_m使用OpenCV提取至少30个FAST特征点；

步骤2b)在每个特征点周围随机选取256对像素点，如果先选取像素点的亮度大于后选取像素点的亮度，则将该特征点的描述子对应位置设为1，否则设置为0，得到该特征点的描述子；

步骤2c)基于灰度不变假设，如果相邻图像X_m+1有与之匹配的特征点，即描述子相同，则将该特征点保留下来，得到X_m对应的特征点子集合F；

计算特征点集合X_m'中每个特征点F_c的视觉残差E_c，计算公式为：

其中，x_c、y_c、z_c分别表示第F_c个特征点在第X_m幅图像归一化坐标系下的横坐标值、纵坐标值、竖坐标值，r_c和v_c分别表示第F_c个特征点在第X_m个图像像素坐标系下的横坐标值和纵坐标值。

步骤3)计算每幅图像与相邻图像对应无人机位置之间的IMU残差；

采用预积分方法，通过t时刻无人机的加速度A_n和角速度W_n计算相邻地面灰度图像X_m及相邻图像X_m+1之间的位移增量P_(m,m+1)、速度增量V_(m,m+1)和旋转增量Q_(m,m+1)，并通过P_(m,m+1)、V_(m,m+1)和Q_(m,m+1)，计算X_m与X_m+1对应无人机位置之间的IMU残差E_(m,m+1)；

上述所述的预积分公式及地面灰度图像X_m与X_m+1对应无人机位置之间的IMU残差E_(m,m+1)，计算公式如下：

其中，u₁(t)、u₂(t)分别表示t时刻无人机加速度和角速度中的噪声，R_(m,t)表示从第X_m幅图像对应的无人机位置到t时刻无人机位置的旋转变化量，P_m表示第X_m幅图像与第1幅图像分别对应无人机的位置差值，V_m表示第X_m幅图像对应无人机的速度，q_m表示第X_m幅图像与第1幅图像分别对应无人机的旋转差值，Δt_(m,m+1)表示第X_m幅图像与相邻的第X_m+1幅图像之间的时间差值，g表示重力加速度，

表示四元数乘法操作。

步骤4)计算每幅图像对应无人机位置的GPS残差E_z：

将GPS采集的Z组无人机位置数据G转换为东北天坐标系下的坐标G'＝{(O_z',L_z',H_z')|1≤z≤Z},并计算每幅图像X_m在G_z'下的对应无人机位置的GPS残差E_z；

上述所述的坐标转换公式如下：

O'_z＝(γ+H_z)cos(O_z)cos(L_z)

L'_z＝(γ+H_z)cos(O_z)sin(L_z)

H'_z＝(γ(1-σ²)+H_z)sin(O_z)

其中，O_z'表示t时刻无人机在东北天坐标系中位姿的横坐标值，L'_z表示t时刻无人机在东北天坐标系中位姿的纵坐标值，H_z'表示t时刻无人机在东北天坐标系中位姿的竖坐标值，O_z表示t时刻无人机的维度值，L_z表示t时刻无人机的经度值，H_z表示t时刻无人机的高度值，γ表示基准椭球体的曲率半径，σ表示基准椭球体的偏心率。

上述每幅图像X_m在G_z'下的对应无人机位置的GPS残差E_z，计算公式为：

E_z＝P_m-G'_z(t)

其中，P_m表示第X_m幅图像与第1幅图像分别对应无人机的位置差值，G'_z(t)表示t时刻与第1个图像时刻分别对应在东北天坐标下的坐标差值。

步骤5)获取无人机位姿估计结果：

视觉传感器和IMU传感器在局部范围内可以获得较高精度的无人机位姿，但是随着无人机长时间的运行，将会出现累计漂移现象，而GPS传感器由于可以提供全局一致的信息，可以大幅度减少这种累计漂移现象，因此在现有技术的基础上加入了GPS传感器。

由于每个传感器获得的残差都包含了无人机的位姿信息，因此对上述获得的多传感器残差进行融合，并使用非线性方法进行优化，得到每个时刻无人机的位姿。

通过每个特征点F_c的视觉残差E_c、每幅图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)和每幅图像X_m对应无人机位置的GPS残差E_z，构建多传感器残差E_tight，并采用非线性优化方法对E_tight进行优化，得到t时刻无人机的位姿，其中：

E_tight＝E_c+W_(m,m+1)·E_(m,m+1)+W_z·E_z

W_(m,m+1)表示图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)的权重，W_z表示每幅图像X_m对应无人机位置的GPS残差E_z的权重。

由于不同传感器中蕴含的噪声程度以及类型都不大相同，因此在构造IMU残差E_(m,m+1)和每幅图像X_m对应无人机位置的GPS残差E_z时，给每个残差添加不同的权重，使得最后融合的多传感器残差考虑了不同传感器数据之间的差异。

上述所述的W_(m,m+1)和W_z，其计算公式如下：

其中，R'_m表示采用非线性优化后的第X_m幅图像所对应无人机的优化位姿，R_(m,u)表示通过IMU数据获得的第X_m幅图像的初始位姿，R_(m,g)表示通过GPS数据获得的第X_m幅图像的初始位姿，||·||表示取模运算。

下面结合仿真实验对本发明的技术效果作进一步说明。

1.仿真条件和内容：

仿真实验的硬件测试平台是：处理器为CPU intel Core i7-4790，主频为3.6GHz，内存12GB。

仿真实验的软件平台为：Ubuntu 18.04LTS，64位操作系统和Melodic版本的ROS(Robot Operating System)。

仿真实验数据来自于EuRoC数据集，该数据集是由携带双目相机、IMU、VICON0以及LEICA0等设备的六旋翼微型飞行器在苏黎世联邦理工学院的一个厂房和一个普通房间内采集的数据。该系列数据集上的每条数据都可以提供频率为20Hz的灰度图像序列，频率为200Hz的惯性传感器数据(加速度计和陀螺仪读数)，以及模拟生成的频率为20Hz的无人机经纬度信息，并且提供亚毫米级的运动轨迹真值。使用EuRoC数据集中的MH_02_easy和MH_05_difficult数据序列，对本发明与现有的基于视觉传感器和IMU融合的无人机位姿估计方法无人机真实位姿进行对比仿真，其结果如图2所示。

2.仿真结果分析：

参照图2，其中图2(a)是本发明方法与现有技术方法在数据序列MH_02_easy上估计的位姿与数据序列真实的位姿对比曲线图。图2(a)中的横坐标表示在二维空间中无人机的位置坐标沿着x轴移动对应的值，纵坐标表示在二维空间中无人机的位置坐标沿着y轴移动对应的值，单位为米m。图2(a)中以虚线标示的曲线表示用本发明方法估计的无人机位姿轨迹曲线，以点线标示的曲线表示用现有技术方法估计的无人机位姿轨迹曲线，以实线标示的曲线表示此数据序列上无人机的真实位姿轨迹曲线。

图2(b)表示本发明方法与现有技术方法在数据序列MH_05_difficult上估计的位姿与数据序列真实的位姿对比曲线图。

由图2(a)和图2(b)可以看出，本发明方法估计得到的无人机位姿轨迹曲线与真实的无人机位姿轨迹曲线几乎趋于一致，并且明显优于用现有技术方法得到的无人机位姿轨迹曲线，表明本发明方法估计的无人机位姿轨迹曲线具有较高的精度。

Claims (5)

1.一种基于多传感器融合的无人机位姿估计方法，其特征在于，包括如下步骤：

(1)获取多传感器数据：

(1a)获取无人机携带的视觉传感器以10Hz-30Hz的频率采集M幅不同的地面灰度图像X＝{X_m|1≤m≤M}，IMU传感器以100Hz-200Hz的频率采集N组不同的无人机加速度数据A＝{A_n|1≤n≤N}和角速度数据W＝{W_n|1≤n≤N}，以及GPS传感器以20Hz-100Hz的频率采集不同的Z组无人机位置数据G＝{(O_z,L_z,H_z)|1≤z≤Z},其中M≥20，X_m表示第m幅地面灰度图像，N≥200，A_n、W_n分别表示t时刻无人机的加速度、角速度，Z≥40，O_z、L_z、H_z分别表示t时刻无人机的经度值、纬度值、高度值；

(2)计算每个特征点F_c的视觉残差：

对每幅地面灰度图像X_m进行特征点提取，得到X对应的特征点集合X'＝{X_m'|1≤m≤M}，并计算特征点集合X_m'中每个特征点F_c的视觉残差E_c，其中X_m'表示X_m对应的包括C个特征点的特征点子集合F＝{F_c|1≤c≤C}，C≥30；

(3)计算每幅图像与相邻图像对应无人机位置之间的IMU残差；

采用预积分方法，通过t时刻无人机的加速度A_n和角速度W_n计算相邻地面灰度图像X_m及相邻图像X_m+1之间的位移增量P_(m,m+1)、速度增量V_(m,m+1)和旋转增量Q_(m,m+1)，并通过P_(m,m+1)、V_(m,m+1)和Q_(m,m+1)，计算X_m与X_m+1对应无人机位置之间的IMU残差E_(m,m+1)；

(4)计算每幅图像对应无人机位置的GPS残差E_z：

将GPS采集的Z组无人机位置数据G转换为东北天坐标系下的坐标G'＝{(O_z',L_z',H_z')|1≤z≤Z},并计算每幅图像X_m在G_z'下的对应无人机位置的GPS残差E_z；

(5)获取无人机位姿估计结果：

通过每个特征点F_c的视觉残差E_c、每幅图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)和每幅图像X_m对应无人机位置的GPS残差E_z，构建多传感器残差E_tight，并采用非线性优化方法对E_tight进行优化，得到t时刻无人机的位姿，其中：

E_tight＝E_c+W_(m,m+1)·E_(m,m+1)+W_z·E_z

其中，W_(m,m+1)表示图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)的权重，W_z表示每幅图像X_m对应无人机位置的GPS残差E_z的权重。

2.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(2)中所述的计算特征点集合X_m'中每个特征点F_c的视觉残差E_c，计算公式为：

其中，x_c、y_c、z_c分别表示第F_c个特征点在第X_m幅图像归一化坐标系下的横坐标值、纵坐标值、竖坐标值，r_c和v_c分别表示第F_c个特征点在第X_m个图像像素坐标系下的横坐标值和纵坐标值。

3.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(3)中所述的相邻地面灰度图像X_m及相邻图像X_m+1之间的位移增量P_(m,m+1)、速度增量V_(m,m+1)和旋转增量Q_(m,m+1)，以及X_m与X_m+1对应无人机位置之间的IMU残差E_(m,m+1)，其计算公式分别为：

其中，u₁(t)、u₂(t)分别表示t时刻无人机加速度和角速度中的噪声，R_(m,t)表示从第X_m幅图像对应的无人机位置到t时刻无人机位置的旋转变化量，P_m表示第X_m幅图像与第1幅图像分别对应无人机的位置差值，V_m表示第X_m幅图像对应无人机的速度，q_m表示第X_m幅图像与第1幅图像分别对应无人机的旋转差值，Δt_(m,m+1)表示第X_m幅图像与相邻的第X_m+1幅图像之间的时间差值，g表示重力加速度，

表示四元数乘法操作。

4.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(4)中所述的计算每幅图像X_m在G_z'下的对应无人机位置的GPS残差E_z，计算公式为：

E_z＝P_m-G'_z(t)

其中，P_m表示第X_m幅图像与第1幅图像分别对应无人机的位置差值，G'_z(t)表示t时刻与第1个图像时刻分别对应在东北天坐标下的坐标差值。

5.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(5)中所述的图像X_m及相邻图像X_m+1对应无人机位置之间的IMU残差E_(m,m+1)的权重W_(m,m+1)，以及每幅图像X_m对应无人机位置的GPS残差E_z的权重W_z，计算公式分别为：

其中，R'_m表示采用非线性优化后的第X_m幅图像所对应无人机的优化位姿，R_(m,u)表示通过IMU数据获得的第X_m幅图像的初始位姿，R_(m,g)表示通过GPS数据获得的第X_m幅图像的初始位姿，||·||表示取模运算。

Images