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

1.本发明属于目标跟踪技术领域，更进一步涉及无人机位姿定位技术领域中一种基于多传感器松紧耦合的无人机位姿估计方法。本发明利用松紧耦合结合的优化方法融合多传感器数据，从而实现对无人机的位姿定位。


背景技术：

2.无人机位姿估计方法的主要任务是，无人机仅对自身携带传感器获取到的数据进行优化处理，实现对无人机的位姿定位。随着信息科学和通信技术的发展，目前已存在大量的位姿估计方法应用于无人机位姿定位。但是，由于没有环境先验信息、无人机携带传感器获取到的数据存在噪声等客观因素，使得实时精确地定位无人机位姿仍然面临极大的挑战。
3.天津大学在其申请的专利文献“视觉传感器激光雷达融合无人机定位与建图装置和方法”(专利申请号202010864051.4，申请公布号cn 112347840 a)中公开了一种基于多传感器融合的无人机位姿定位方法。该方法实现的具体步骤是，采用多状态卡尔曼滤波msckf(multi
‑
state constraint kalman filter)的算法融合双目相机和惯性测量单元imu(inertial measurement unit)的数据，得到高频的无人机位姿、速度信息；再将高频的msckf估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状态信息。该方法存在的不足之处是，在优化求解无人机位姿时，没有充分发挥多传感器数据的特性优势，得到的高频无人机位姿、速度信息准确度不高，且随着算法运行时间增长，其优化得到的无人机位姿中累积误差会增加，导致无人机定位精度下降。
4.中国人民解放军93114部队在其申请的专利文献“gnss和视觉slam融合的轨迹测量方法和装置”(专利申请号202110221070.x，申请公布号cn 112819744 a)中公开了一种无人机位姿估计方法。该方法实现的具体步骤是，首先根据相机的运动轨迹对视觉同步定位与建图slam(simultaneous localization and mapping)进行初始化，后使用图像信息对相机位姿进行扩展局部优化和全局优化，再利用全球导航卫星系统gnss(global navigation satellite system)信息进行回环优化，能够在低成本、高易用性的前提下提升轨迹的测量精度。该方法存在的不足之处是，当全球导航卫星系统gnss的卫星信号被干扰、遮挡以及欺骗攻击时，将导致gnss定位设备无法正常输出导航参数，由此回环优化失败，无法消除无人机位姿中的累积误差；相机在纹理单一，光照条件差的环境中获取到的图像质量较差，使用此图像数据优化得到的无人机定位轨迹精度较低。


技术实现要素：

5.本发明的目的是针对上述现有技术的不足，提出一种基于多传感器松紧耦合的无人机位姿估计方法，旨在解决当无人机在无任何先验信息的环境中，如何融合多传感器数据，以及如何提升无人机定位精度的问题。
6.实现本发明目的的思路是，分别构建与融合视觉、imu和gps传感器信息对应的视
觉残差、imu残差和gps残差，通过紧耦合优化，使三个残差项的和最小，得到紧耦合优化后的无人机位姿，由于紧耦合优化可以充分发挥不同传感器的特性优势，所以可在无任何先验信息的环境中，完成对无人机位姿的初次估计定位。为了克服紧耦合优化后的无人机位姿中存在累积误差的情况，再使用紧耦合优化后的位姿和gps信息，分别构建绝对残差和相对残差，进行松耦合优化，使两个残差项的和最小，得到松耦合优化后的无人机位姿。引入的松耦合优化融合相对独立的gps信息，消除紧耦合优化得到的无人机位姿中的累积误差，可提升无人机位姿估计的精度。
7.实现本发明目的的技术方案包括如下步骤：
8.(1)获取传感器的数据：
9.(1a)无人机携带的视觉传感器以采集频率a实时采集至少20帧的黑白图像，a为区间[10，30]赫兹内任选的一个整数；
[0010]
(1b)无人机携带的imu传感器以采集频率b实时采集至少200组的无人机加速度和角速度读数值，b为区间[100，200]赫兹内任选的一个整数；
[0011]
(1c)无人机携带的gps传感器以采集频率c实时采集至少40个的无人机经纬度值，c为区间[20，100]赫兹内任选的一个整数；
[0012]
(2)对传感器的数据进行预处理：
[0013]
(2a)从每帧黑白图像中提取至少30，至多200个fast特征点，计算每个特征点的brief描述子，从该特征点所在图像的相邻图像中寻找描述子相同的特征点；
[0014]
(2b)利用预积分公式，计算相邻图像之间的预积分量；
[0015]
(2c)利用坐标系转换公式，将所有采集到的无人机经度、纬度和高度转换为东北天坐标系下的坐标；
[0016]
(3)对无人机位姿进行紧耦合：
[0017]
(3a)利用重投影公式，计算每个图像中提取特征点的视觉残差；
[0018]
(3b)利用运动学公式，计算每个图像与相邻图像对应无人机位置之间的imu残差；
[0019]
(3c)按照下式，计算每个图像对应无人机的gps残差：
[0020]
e
n
＝p
i
‑
n
x
[0021]
其中，e
n
表示第i个图像对应无人机的gps残差，p
i
表示第i个图像与第1个图像分别对应无人机的位置差值，n
x
表示x时刻与第1个图像时刻分别对应的gps差值；
[0022]
(3d)将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的imu残差和每个图像对应无人机的gps残差和最小化，即可得到紧耦合优化后的无人机位姿；
[0023]
(4)优化无人机的位姿：
[0024]
(4a)按照下式，计算每个图像对应无人机的绝对误差；
[0025][0026]
其中，e
1,i
表示第i个图像对应无人机的绝对残差，‖
·
‖表示取模长操作，μ
x
表示无人机携带的gps传感器在x时刻的置信度；
[0027]
(4b)按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差；
[0028][0029]
其中，e
2,(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机之间的相对误差，t
(i,i+1)
表示紧耦合优化后第i个图像与相邻的第i+1个图像分别对应无人机的位置差值，n
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机在东北天坐标系中的坐标差值，r
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机的旋转差值，t表示矩阵的转置操作，r
i
表示紧耦合优化后第i个图像与第1个图像分别对应无人机的旋转差值，r
i+1
紧耦合优化后表示第i+1个图像与第1个图像分别对应无人机的旋转差值。
[0030]
(4c)利用下式，得到无人机松耦合优化后的位姿：
[0031][0032]
其中，min{
·
}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。
[0033]
发明与现有的技术相比具有以下优点：
[0034]
第一，本发明对无人机位姿进行紧耦合，融合多传感器信息，充分发挥不同传感器的特性优势，克服了现有技术无法在环境复杂，传感器噪声变化大的情况下完成无人机位姿估计的问题，使得本发明可在无任何先验信息的环境中完成无人机位姿的估计。
[0035]
第二，本发明优化无人机的位姿，将紧耦合优化后的无人机位姿和相对独立的gps信息进行松耦合优化，克服了现有技术在视觉信息失效时，无法完成消除无人机位姿中存在的累积误差的问题，使得本发明可以鲁棒的消除无人机位姿中的累积误差，进一步提升无人机位姿的定位精度。
附图说明
[0036]
图1是本发明的流程图；
[0037]
图2为本发明仿真实验结果图。
具体实施方式
[0038]
以下结合附图和具体实施例，对本发明作进一步详细描述。
[0039]
参照图1，对本发明的实施步骤做进一步描述。
[0040]
步骤1，获取传感器的数据。
[0041]
无人机携带的视觉传感器以采集频率a实时采集至少20帧的黑白图像，a为区间[10，30]赫兹内任选的一个整数。
[0042]
无人机携带的imu传感器以采集频率b实时采集至少200组的无人机加速度和角速度读数值，b为区间[100，200]赫兹内任选的一个整数。
[0043]
无人机携带的gps传感器以采集频率c实时采集至少40个的无人机经纬度值，c为区间[20，100]赫兹内任选的一个整数。
[0044]
步骤2，对传感器的数据进行预处理。
[0045]
从每帧黑白图像中提取至少30，至多200个fast特征点，计算每个特征点的brief
描述子，从该特征点所在图像的相邻图像中寻找描述子相同的特征点。
[0046]
所述计算每个特征点的brief描述子是由下述步骤得到的。
[0047]
第一步，在待计算的特征点周围随机选取256对像素点。
[0048]
第二步，对于每一对像素点，如果先选取像素点的亮度大于后选取像素点的亮度，则将待计算的特征点的描述子对应位置为1，否则设置为0，得到该像素点对的二进制数。
[0049]
第三步，将256对像素点的二进制数组合成一个二进制串，作为该特征点的brief描述子。
[0050]
利用下述的预积分公式，计算相邻图像之间的预积分量。
[0051][0052][0053][0054]
其中，p
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应的无人机位置之间的预积分量中加速度对位移的增量，a(t)表示t时刻无人机的加速度，n1(t)表示t时刻无人机加速度中的噪声，v
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应的无人机位置之间的预积分量中加速度对速度的增量，q
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应的无人机位置之间的预积分量中角速度对旋转的增量，w(t)表示t时刻无人机的角速度，n2(t)表示t时刻无人机角速度中的噪声，r
(i,t)
表示从第i个图像对应的无人机位置到t时刻无人机位置的旋转变化量。
[0055]
利用下述的坐标系转换公式，将所有采集到的无人机经度、纬度和高度转换为东北天坐标系下的坐标。
[0056]
x(t)＝(n+alt(t))cos(lat(t))cos(lon(t))
[0057]
y(t)＝(n+alt(t))cos(lat(t))sin(lon(t))
[0058]
z(t)＝(n(1
‑
u2)+alt(t))sinlat(t))
[0059]
其中，x(t)表示t时刻无人机在东北天坐标系中位姿的横坐标值，n表示基准椭球体的曲率半径，alt(t)表示t时刻无人机的高度，lat(t)表示t时刻无人机的纬度，lon(t)表示t时刻无人机的经度，y(t)表示t时刻无人机在东北天坐标系中位姿的纵坐标值，z+t)表示t时刻无人机在东北天坐标系中位姿的竖坐标值，u表示基准椭球体的偏心率。
[0060]
步骤3，对无人机位姿进行紧耦合优化。
[0061]
利用下述的重投影公式，计算每个图像中提取特征点的视觉残差。
[0062][0063]
其中，e
h
表示第h个特征点的视觉残差，x
h
表示第h个特征点在第i个图像中归一化坐标系的横坐标值，z
h
表示第h个特征点在第i个图像中归一化坐标系的竖坐标值，u
h
表示第
h个特征点在第i个图像中像素坐标系的横坐标值，y
h
表示第h个特征点在第i个图像中归一化坐标系的纵坐标值，v
h
表示第h个特征点在第i个图像中像素坐标系中的纵坐标值。
[0064]
利用下述的运动学公式，计算每个图像与相邻图像对应无人机位置之间的imu残差。
[0065][0066]
其中，e
i
表示第i个图像与相邻的第i+1个图像分别对应无人机位姿之间的imu残差，p
i+1
表示第i+1个图像与第1个图像分别对应无人机的位置差值，v
i
表示第i个图像对应无人机的速度，δt
(i,i+1)
表示第i个图像与相邻的第i+1个图像之间的时间差值，g表示重力加速度，v
i+1
表示第i+1个图像对应无人机的速度，q
i+1
表示第i+1个图像与第1个图像分别对应无人机的旋转差值，表示四元数乘法操作，q
i
第i个图像与第1个图像分别对应无人机的旋转差值。
[0067]
按照下式，计算每个图像对应无人机的gps残差。
[0068]
e
n
＝p
i
‑
n
x
[0069]
其中，e
n
表示第i个图像对应无人机的gps残差，p
i
表示第i个图像与第1个图像分别对应无人机的位置差值，n
x
表示x时刻与第1个图像时刻分别对应的gps差值。
[0070]
将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的imu残差和每个图像对应无人机的gps残差项求和，利用最小二乘法，令残差项的总和按照梯度下降的方向逐步变小，最终使残差项和最小，即可得到紧耦合优化后的无人机位姿。
[0071]
步骤4，优化无人机的位姿。
[0072]
按照下式，计算每个图像对应无人机的绝对误差。
[0073][0074]
其中，e
1,i
表示第i个图像对应无人机的绝对残差，‖
·
‖表示取模长操作，μ
x
表示无人机携带的gps传感器在x时刻的置信度。
[0075]
按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差。
[0076][0077]
其中，e
2,(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机之间的相对误差，t
(i,i+1)
表示紧耦合优化后第i个图像与相邻的第i+1个图像分别对应无人机的位置差值，n
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机在东北天坐标系中的坐标差值，r
(i,i+1)
表示第i个图像与相邻的第i+1个图像分别对应无人机的旋转差值，t表示矩阵的转置操作，r
i
表示紧耦合优化后第i个图像与第1个图像分别对应无人机的旋转差值，r
i+1
紧耦合优化后表示第i+1个图像与第1个图像分别对应无人机的旋转差值。
[0078]
利用下式，得到无人机松耦合优化后的位姿。
[0079][0080]
其中，min{
·
}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。
[0081]
下面结合仿真实验对本发明的效果做进一步说明。
[0082]
1.仿真实验条件：
[0083]
本发明仿真实验的硬件测试平台是：处理器为cpu intel core i7
‑
4790，主频为3.6ghz，内存16gb。
[0084]
本发明的仿真实验的软件平台为：ubuntu 18.04lts，64位操作系统和melodic版本的ros(robot operating system)。
[0085]
本发明的仿真实验数据来自于euroc数据集，该数据集是由携带双目相机、imu、vicon0以及leica0等设备的六旋翼微型飞行器在苏黎世联邦理工学院的一个厂房和一个普通房间内采集的数据。该系列数据集上的每条数据都可以提供频率为20hz的黑白图像序列，频率为200hz的惯性传感器数据(加速度计和陀螺仪读数)，以及模拟生成的频率为20赫兹的无人机经纬度信息，并且提供亚毫米级的运动轨迹真值。本发明在仿真实验中使用euroc数据集中的mh_02_easy和mh_05_difficult数据序列。
[0086]
3.仿真内容仿真结果分析：
[0087]
本发明仿真实验是采用本发明的方法，根据上述euroc数据集中的mh_02_easy和mh_05_difficult数据序列，对无人机的位姿进行估计。
[0088]
下面结合图2的仿真图对本发明的效果做一步的描述。
[0089]
图2(a)是无人机在数据序列mh_02_easy上的估计位姿轨迹曲线和真实位姿曲线对比图。图2(a)中的无人机估计位姿轨迹曲线是通过采用本发明方法计算得到的，无人机真实位姿轨迹曲线是由数据集提供的，具有亚毫米级的精度。图2(a)中的横坐标表示在二维空间中无人机的位置坐标沿着x轴移动对应的值，纵坐标表示在二维空间中无人机的位置坐标沿着y轴移动对应的值，单位为米m。图2(a)中以圆形
‑
实线
‑
圆形标示的曲线表示无人机估计的位姿轨迹曲线，以五角星
‑
虚线
‑
五角星标示的曲线表示无人机的真实位姿轨迹曲线。
[0090]
图2(b)是无人机在数据序列mh_05_difficult上的估计位姿轨迹曲线和真实位姿曲线对比图。图2(b)中的无人机估计位姿轨迹曲线是通过采用本发明方法计算得到的，无人机真实位姿轨迹曲线是由数据集提供的，具有亚毫米级的精度。图2(b)中的横坐标表示在二维空间中无人机的位置坐标沿着x轴移动对应的值，纵坐标表示在二维空间中无人机的位置坐标沿着y轴移动对应的值，单位为米m。图2(b)中以圆形
‑
实线
‑
圆形标示的曲线表示无人机的估计位姿轨迹曲线，以五角星
‑
虚线
‑
五角星标示的曲线表示无人机的真实位姿轨迹曲线。
[0091]
图2(c)是数据序列mh_02_easy上的无人机估计位姿的绝对误差随时间变化曲线图。图2(c)是通过本发明方法计算得到的无人机估计位姿和数据集提供的无人机真实位姿对比得到的。图2(c)中的横坐标表示飞行时间，单位为秒s，纵坐标表示无人机估计位姿的绝对误差，单位为米m。图2(c)中以黑色实线标示的曲线表示无人机估计位姿的绝对误差。
[0092]
图2(d)是数据序列mh_05_difficult上的无人机估计位姿的绝对误差随时间变化曲线图。图2(d)是通过本发明方法计算得到的无人机估计位姿和数据集提供的无人机真实位姿对比得到的。图2(d)中的横坐标表示飞行时间，单位为秒s，纵坐标表示无人机估计位姿的绝对误差，单位为米m。图2(d)中以黑色实线标示的曲线表示无人机估计位姿的绝对误差。
[0093]
由图2(a)和图2(b)可以看出，本发明方法估计得到的无人机位姿轨迹曲线与真实的无人机位姿轨迹曲线几乎趋于一致，表明本发明方法估计的无人机位姿轨迹曲线具有较高精度。
[0094]
由图2(c)和图2(d)可以看出，本发明方法估计得到的无人机位姿的绝对误差整体较小，随着运行时间增加，估计的无人机位姿的绝对误差没有明显增加，表明本发明方法估计的无人机位姿可以有效消除累积误差。