1.一种基于多传感器融合的无人机位姿估计方法,其特征在于,包括如下步骤:
(1)获取多传感器数据:
(1a)获取无人机携带的视觉传感器以10Hz‑30Hz的频率采集M幅不同的地面灰度图像X={Xm|1≤m≤M},IMU传感器以100Hz‑200Hz的频率采集N组不同的无人机加速度数据A={An|1≤n≤N}和角速度数据W={Wn|1≤n≤N},以及GPS传感器以20Hz‑100Hz的频率采集不同的Z组无人机位置数据G={(Oz,Lz,Hz)|1≤z≤Z},其中M≥20,Xm表示第m幅地面灰度图像,N≥200,An、Wn分别表示t时刻无人机的加速度、角速度,Z≥40,Oz、Lz、Hz分别表示t时刻无人机的经度值、纬度值、高度值;
(2)计算每个特征点Fc的视觉残差:
对每幅地面灰度图像Xm进行特征点提取,得到X对应的特征点集合X'={Xm'|1≤m≤M},并计算特征点集合Xm'中每个特征点Fc的视觉残差Ec,其中Xm'表示Xm对应的包括C个特征点的特征点子集合F={Fc|1≤c≤C},C≥30;
(3)计算每幅图像与相邻图像对应无人机位置之间的IMU残差;
采用预积分方法,通过t时刻无人机的加速度An和角速度Wn计算相邻地面灰度图像Xm及相邻图像Xm+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),计算Xm与Xm+1对应无人机位置之间的IMU残差E(m,m+1);
(4)计算每幅图像对应无人机位置的GPS残差Ez:
将GPS采集的Z组无人机位置数据G转换为东北天坐标系下的坐标G'={(Oz',Lz',Hz')|
1≤z≤Z},并计算每幅图像Xm在Gz'下的对应无人机位置的GPS残差Ez;
(5)获取无人机位姿估计结果:
通过每个特征点Fc的视觉残差Ec、每幅图像Xm及相邻图像Xm+1对应无人机位置之间的IMU残差E(m,m+1)和每幅图像Xm对应无人机位置的GPS残差Ez,构建多传感器残差Etight,并采用非线性优化方法对Etight进行优化,得到t时刻无人机的位姿,其中:Etight=Ec+W(m,m+1)·E(m,m+1)+Wz·Ez
其中,W(m,m+1)表示图像Xm及相邻图像Xm+1对应无人机位置之间的IMU残差E(m,m+1)的权重,Wz表示每幅图像Xm对应无人机位置的GPS残差Ez的权重。
2.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法,其特征在于,步骤(2)中所述的计算特征点集合Xm'中每个特征点Fc的视觉残差Ec,计算公式为:其中,xc、yc、zc分别表示第Fc个特征点在第Xm幅图像归一化坐标系下的横坐标值、纵坐标值、竖坐标值,rc和vc分别表示第Fc个特征点在第Xm个图像像素坐标系下的横坐标值和纵坐标值。
3.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法,其特征在于,步骤(3)中所述的相邻地面灰度图像Xm及相邻图像Xm+1之间的位移增量P(m,m+1)、速度增量V(m,m+1)和旋转增量Q(m,m+1),以及Xm与Xm+1对应无人机位置之间的IMU残差E(m,m+1),其计算公式分别为:其中,u1(t)、u2(t)分别表示t时刻无人机加速度和角速度中的噪声,R(m,t)表示从第Xm幅图像对应的无人机位置到t时刻无人机位置的旋转变化量,Pm表示第Xm幅图像与第1幅图像分别对应无人机的位置差值,Vm表示第Xm幅图像对应无人机的速度,qm表示第Xm幅图像与第
1幅图像分别对应无人机的旋转差值,Δt(m,m+1)表示第Xm幅图像与相邻的第Xm+1幅图像之间的时间差值,g表示重力加速度, 表示四元数乘法操作。
4.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法,其特征在于,步骤(4)中所述的计算每幅图像Xm在Gz'下的对应无人机位置的GPS残差Ez,计算公式为:Ez=Pm‑G'z(t)
其中,Pm表示第Xm幅图像与第1幅图像分别对应无人机的位置差值,G'z(t)表示t时刻与第1个图像时刻分别对应在东北天坐标下的坐标差值。
5.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法,其特征在于,步骤(5)中所述的图像Xm及相邻图像Xm+1对应无人机位置之间的IMU残差E(m,m+1)的权重W(m,m+1),以及每幅图像Xm对应无人机位置的GPS残差Ez的权重Wz,计算公式分别为:其中,R'm表示采用非线性优化后的第Xm幅图像所对应无人机的优化位姿,R(m,u)表示通过IMU数据获得的第Xm幅图像的初始位姿,R(m,g)表示通过GPS数据获得的第Xm幅图像的初始位姿,||·||表示取模运算。