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

本发明专利技术公开了一种基于多传感器松紧耦合的无人机位姿估计方法，解决在无先验信息的环境中，无人机移动时对无人机进行位姿估计的问题。本发明专利技术的实现步骤是：分别构建与融合视觉、IMU和GPS传感器信息对应的视觉残差、IMU残差和GPS残差，通过紧耦合优化使残差和最小，得到紧耦合优化后的无人机位姿，再使用紧耦合优化后的位姿和GPS信息，分别构建绝对残差和相对残差，进行松耦合优化，使两个残差项的和最小，得到松耦合优化后的无人机位姿。本发明专利技术利用松紧耦合融合多传感器信息的无人机位姿估计方法，有效解决了无人机位姿中存在累积误差的问题，可以鲁棒的估计得到高精度的无人机位姿。可以鲁棒的估计得到高精度的无人机位姿。可以鲁棒的估计得到高精度的无人机位姿。

【技术实现步骤摘要】
基于多传感器松紧耦合的无人机位姿估计方法


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

技术介绍

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

技术实现思路

[0005]本专利技术的目的是针对上述现有技术的不足，提出一种基于多传感器松紧耦合的无人机位姿估计方法，旨在解决当无人机在无任何先验信息的环境中，如何融合多传感器数据，以及如何提升无人机定位精度的问题。
[0006]实现本专利技术目的的思路是，分别构建与融合视觉、IMU和GPS传感器信息对应的视
觉残差、IMU残差和GPS残差，通过紧耦合优化，使三个残差项的和最小，得到紧耦合优化后的无人机位姿，由于紧耦合优化可以充分发挥不同传感器的特性优势，所以可在无任何先验信息的环境中，完成对无人机位姿的初次估计定位。为了克服紧耦合优化后的无人机位姿中存在累积误差的情况，再使用紧耦合优化后的位姿和GPS信息，分别构建绝对残差和相对残差，进行松耦合优化，使两个残差项的和最小，得到松耦合优化后的无人机位姿。引入的松耦合优化融合相对独立的GPS信息，消除紧耦合优化得到的无人机位姿中的累积误差，可提升无人机位姿估计的精度。
[0007]实现本专利技术目的的技术方案包括如下步骤：
[0008](1)获取传感器的数据：
[0009](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个图像分别对应无人机的旋转差值本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多传感器松紧耦合的无人机位姿估计方法，其特征在于，利用融合视觉、IMU和GPS传感器数据，分别构建视觉残差、IMU残差和GPS残差，通过紧耦合优化得到紧耦合优化后的无人机位姿，对紧耦合优化后的无人机位姿进行松耦合得到优化后的无人机位姿；该方法的步骤包括如下：(1)获取传感器的数据：(1a)无人机携带的视觉传感器以采集频率a实时采集至少20帧的黑白图像，a为区间[10，30]赫兹内任选的一个整数；(1b)无人机携带的IMU传感器以采集频率b实时采集至少200组的无人机加速度和角速度读数值，b为区间[100，200]赫兹内任选的一个整数；(1c)无人机携带的GPS传感器以采集频率c实时采集至少40个的无人机经纬度值，c为区间[20，100]赫兹内任选的一个整数；(2)对传感器的数据进行预处理：(2a)从每帧黑白图像中提取至少30，至多200个FAST特征点，计算每个特征点的BRIEF描述子，从该特征点所在图像的相邻图像中寻找描述子相同的特征点；(2b)利用预积分公式，计算相邻图像之间的预积分量；(2c)利用坐标系转换公式，将所有采集到的无人机经度、纬度和高度转换为东北天坐标系下的坐标；(3)对无人机位姿进行紧耦合：(3a)利用重投影公式，计算每个图像中提取特征点的视觉残差；(3b)利用运动学公式，计算每个图像与相邻图像对应无人机位置之间的IMU残差；(3c)按照下式，计算每个图像对应无人机的GPS残差：E
N
＝P
i
‑
N
x
其中，E
N
表示第i个图像对应无人机的GPS残差，P
i
表示第i个图像与第1个图像分别对应无人机的位置差值，N
x
表示x时刻与第1个图像时刻分别对应的GPS差值；(3d)将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的IMU残差和每个图像对应无人机的GPS残差和最小化，即可得到紧耦合优化后的无人机位姿；(4)优化无人机的位姿：(4a)按照下式，计算每个图像对应无人机的绝对误差；其中，E
1，i
表示第i个图像对应无人机的绝对残差，||
·
||表示取模长操作，μ
x
表示无人机携带的GPS传感器在x时刻的置信度；(4b)按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差；其中，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个图像分别对应无人机的旋转差值；(4c)利用下式，得到无人机松耦合优化后的位姿：其中，min{
·
}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。2.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(2a)中所述计算每个特征点的BRIEF描述子是由下述步骤得到的：第一步，在待计算的特征点周围...

【专利技术属性】
技术研发人员：罗楠，王义峰，李晟硕，梁婕，卢波，郑海红，赵辉，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：