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

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

Info

Publication number
CN113375665A
CN113375665A CN202110675571.5A CN202110675571A CN113375665A CN 113375665 A CN113375665 A CN 113375665A CN 202110675571 A CN202110675571 A CN 202110675571A CN 113375665 A CN113375665 A CN 113375665A
Authority
CN
China
Prior art keywords
aerial vehicle
unmanned aerial
image
pose
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110675571.5A
Other languages
English (en)
Other versions
CN113375665B (zh
Inventor
罗楠
王义峰
李晟硕
梁婕
卢波
郑海红
赵辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xidian University
Original Assignee
Xidian University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xidian University filed Critical Xidian University
Priority to CN202110675571.5A priority Critical patent/CN113375665B/zh
Publication of CN113375665A publication Critical patent/CN113375665A/zh
Application granted granted Critical
Publication of CN113375665B publication Critical patent/CN113375665B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Processing (AREA)

Abstract

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

Description

基于多传感器松紧耦合的无人机位姿估计方法
技术领域
本发明属于目标跟踪技术领域，更进一步涉及无人机位姿定位技术领域中一种基于多传感器松紧耦合的无人机位姿估计方法。本发明利用松紧耦合结合的优化方法融合多传感器数据，从而实现对无人机的位姿定位。
背景技术
无人机位姿估计方法的主要任务是，无人机仅对自身携带传感器获取到的数据进行优化处理，实现对无人机的位姿定位。随着信息科学和通信技术的发展，目前已存在大量的位姿估计方法应用于无人机位姿定位。但是，由于没有环境先验信息、无人机携带传感器获取到的数据存在噪声等客观因素，使得实时精确地定位无人机位姿仍然面临极大的挑战。
天津大学在其申请的专利文献“视觉传感器激光雷达融合无人机定位与建图装置和方法”(专利申请号202010864051.4，申请公布号CN 112347840 A)中公开了一种基于多传感器融合的无人机位姿定位方法。该方法实现的具体步骤是，采用多状态卡尔曼滤波MSCKF(Multi-State Constraint Kalman Filter)的算法融合双目相机和惯性测量单元IMU(Inertial Measurement Unit)的数据，得到高频的无人机位姿、速度信息；再将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状态信息。该方法存在的不足之处是，在优化求解无人机位姿时，没有充分发挥多传感器数据的特性优势，得到的高频无人机位姿、速度信息准确度不高，且随着算法运行时间增长，其优化得到的无人机位姿中累积误差会增加，导致无人机定位精度下降。
中国人民解放军93114部队在其申请的专利文献“GNSS和视觉SLAM融合的轨迹测量方法和装置”(专利申请号202110221070.X，申请公布号CN 112819744 A)中公开了一种无人机位姿估计方法。该方法实现的具体步骤是，首先根据相机的运动轨迹对视觉同步定位与建图SLAM(Simultaneous Localization and Mapping)进行初始化，后使用图像信息对相机位姿进行扩展局部优化和全局优化，再利用全球导航卫星系统GNSS(GlobalNavigation Satellite System)信息进行回环优化，能够在低成本、高易用性的前提下提升轨迹的测量精度。该方法存在的不足之处是，当全球导航卫星系统GNSS的卫星信号被干扰、遮挡以及欺骗攻击时，将导致GNSS定位设备无法正常输出导航参数，由此回环优化失败，无法消除无人机位姿中的累积误差；相机在纹理单一，光照条件差的环境中获取到的图像质量较差，使用此图像数据优化得到的无人机定位轨迹精度较低。
发明内容
本发明的目的是针对上述现有技术的不足，提出一种基于多传感器松紧耦合的无人机位姿估计方法，旨在解决当无人机在无任何先验信息的环境中，如何融合多传感器数据，以及如何提升无人机定位精度的问题。
实现本发明目的的思路是，分别构建与融合视觉、IMU和GPS传感器信息对应的视觉残差、IMU残差和GPS残差，通过紧耦合优化，使三个残差项的和最小，得到紧耦合优化后的无人机位姿，由于紧耦合优化可以充分发挥不同传感器的特性优势，所以可在无任何先验信息的环境中，完成对无人机位姿的初次估计定位。为了克服紧耦合优化后的无人机位姿中存在累积误差的情况，再使用紧耦合优化后的位姿和GPS信息，分别构建绝对残差和相对残差，进行松耦合优化，使两个残差项的和最小，得到松耦合优化后的无人机位姿。引入的松耦合优化融合相对独立的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残差：
EN＝Pi-Nx
其中，EN表示第i个图像对应无人机的GPS残差，Pi表示第i个图像与第1个图像分别对应无人机的位置差值，Nx表示x时刻与第1个图像时刻分别对应的GPS差值；
(3d)将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的IMU残差和每个图像对应无人机的GPS残差和最小化，即可得到紧耦合优化后的无人机位姿；
(4)优化无人机的位姿：
(4a)按照下式，计算每个图像对应无人机的绝对误差；
Figure BDA0003120881760000031
其中，E1,i表示第i个图像对应无人机的绝对残差，‖·‖表示取模长操作，μx表示无人机携带的GPS传感器在x时刻的置信度；
(4b)按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差；
Figure BDA0003120881760000032
其中，E2,(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表示矩阵的转置操作，Ri表示紧耦合优化后第i个图像与第1个图像分别对应无人机的旋转差值，Ri+1紧耦合优化后表示第i+1个图像与第1个图像分别对应无人机的旋转差值。
(4c)利用下式，得到无人机松耦合优化后的位姿：
Figure BDA0003120881760000041
其中，min{·}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。
发明与现有的技术相比具有以下优点：
第一，本发明对无人机位姿进行紧耦合，融合多传感器信息，充分发挥不同传感器的特性优势，克服了现有技术无法在环境复杂，传感器噪声变化大的情况下完成无人机位姿估计的问题，使得本发明可在无任何先验信息的环境中完成无人机位姿的估计。
第二，本发明优化无人机的位姿，将紧耦合优化后的无人机位姿和相对独立的GPS信息进行松耦合优化，克服了现有技术在视觉信息失效时，无法完成消除无人机位姿中存在的累积误差的问题，使得本发明可以鲁棒的消除无人机位姿中的累积误差，进一步提升无人机位姿的定位精度。
附图说明
图1是本发明的流程图；
图2为本发明仿真实验结果图。
具体实施方式
以下结合附图和具体实施例，对本发明作进一步详细描述。
参照图1，对本发明的实施步骤做进一步描述。
步骤1，获取传感器的数据。
无人机携带的视觉传感器以采集频率a实时采集至少20帧的黑白图像，a为区间[10，30]赫兹内任选的一个整数。
无人机携带的IMU传感器以采集频率b实时采集至少200组的无人机加速度和角速度读数值，b为区间[100，200]赫兹内任选的一个整数。
无人机携带的GPS传感器以采集频率c实时采集至少40个的无人机经纬度值，c为区间[20，100]赫兹内任选的一个整数。
步骤2，对传感器的数据进行预处理。
从每帧黑白图像中提取至少30，至多200个FAST特征点，计算每个特征点的BRIEF描述子，从该特征点所在图像的相邻图像中寻找描述子相同的特征点。
所述计算每个特征点的BRIEF描述子是由下述步骤得到的。
第一步，在待计算的特征点周围随机选取256对像素点。
第二步，对于每一对像素点，如果先选取像素点的亮度大于后选取像素点的亮度，则将待计算的特征点的描述子对应位置为1，否则设置为0，得到该像素点对的二进制数。
第三步，将256对像素点的二进制数组合成一个二进制串，作为该特征点的BRIEF描述子。
利用下述的预积分公式，计算相邻图像之间的预积分量。
Figure BDA0003120881760000051
Figure BDA0003120881760000052
Figure BDA0003120881760000053
其中，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时刻无人机位置的旋转变化量。
利用下述的坐标系转换公式，将所有采集到的无人机经度、纬度和高度转换为东北天坐标系下的坐标。
x(t)＝(N+alt(t))cos(lat(t))cos(lon(t))
y(t)＝(N+alt(t))cos(lat(t))sin(lon(t))
z(t)＝(N(1-u2)+alt(t))sinlat(t))
其中，x(t)表示t时刻无人机在东北天坐标系中位姿的横坐标值，N表示基准椭球体的曲率半径，alt(t)表示t时刻无人机的高度，lat(t)表示t时刻无人机的纬度，lon(t)表示t时刻无人机的经度，y(t)表示t时刻无人机在东北天坐标系中位姿的纵坐标值，z+t)表示t时刻无人机在东北天坐标系中位姿的竖坐标值，u表示基准椭球体的偏心率。
步骤3，对无人机位姿进行紧耦合优化。
利用下述的重投影公式，计算每个图像中提取特征点的视觉残差。
Figure BDA0003120881760000061
其中，Eh表示第h个特征点的视觉残差，xh表示第h个特征点在第i个图像中归一化坐标系的横坐标值，zh表示第h个特征点在第i个图像中归一化坐标系的竖坐标值，uh表示第h个特征点在第i个图像中像素坐标系的横坐标值，yh表示第h个特征点在第i个图像中归一化坐标系的纵坐标值，vh表示第h个特征点在第i个图像中像素坐标系中的纵坐标值。
利用下述的运动学公式，计算每个图像与相邻图像对应无人机位置之间的IMU残差。
Figure BDA0003120881760000062
其中，Ei表示第i个图像与相邻的第i+1个图像分别对应无人机位姿之间的IMU残差，Pi+1表示第i+1个图像与第1个图像分别对应无人机的位置差值，Vi表示第i个图像对应无人机的速度，Δt(i,i+1)表示第i个图像与相邻的第i+1个图像之间的时间差值，g表示重力加速度，Vi+1表示第i+1个图像对应无人机的速度，qi+1表示第i+1个图像与第1个图像分别对应无人机的旋转差值，
Figure BDA0003120881760000063
表示四元数乘法操作，qi第i个图像与第1个图像分别对应无人机的旋转差值。
按照下式，计算每个图像对应无人机的GPS残差。
EN＝Pi-Nx
其中，EN表示第i个图像对应无人机的GPS残差，Pi表示第i个图像与第1个图像分别对应无人机的位置差值，Nx表示x时刻与第1个图像时刻分别对应的GPS差值。
将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的IMU残差和每个图像对应无人机的GPS残差项求和，利用最小二乘法，令残差项的总和按照梯度下降的方向逐步变小，最终使残差项和最小，即可得到紧耦合优化后的无人机位姿。
步骤4，优化无人机的位姿。
按照下式，计算每个图像对应无人机的绝对误差。
Figure BDA0003120881760000071
其中，E1,i表示第i个图像对应无人机的绝对残差，‖·‖表示取模长操作，μx表示无人机携带的GPS传感器在x时刻的置信度。
按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差。
Figure BDA0003120881760000072
其中，E2,(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表示矩阵的转置操作，Ri表示紧耦合优化后第i个图像与第1个图像分别对应无人机的旋转差值，Ri+1紧耦合优化后表示第i+1个图像与第1个图像分别对应无人机的旋转差值。
利用下式，得到无人机松耦合优化后的位姿。
Figure BDA0003120881760000073
其中，min{·}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。
下面结合仿真实验对本发明的效果做进一步说明。
1.仿真实验条件：
本发明仿真实验的硬件测试平台是：处理器为CPU intel Core i7-4790，主频为3.6GHz，内存16GB。
本发明的仿真实验的软件平台为：Ubuntu 18.04LTS，64位操作系统和Melodic版本的ROS(Robot Operating System)。
本发明的仿真实验数据来自于EuRoC数据集，该数据集是由携带双目相机、IMU、VICON0以及LEICA0等设备的六旋翼微型飞行器在苏黎世联邦理工学院的一个厂房和一个普通房间内采集的数据。该系列数据集上的每条数据都可以提供频率为20HZ的黑白图像序列，频率为200HZ的惯性传感器数据(加速度计和陀螺仪读数)，以及模拟生成的频率为20赫兹的无人机经纬度信息，并且提供亚毫米级的运动轨迹真值。本发明在仿真实验中使用EuRoC数据集中的MH_02_easy和MH_05_difficult数据序列。
3.仿真内容仿真结果分析：
本发明仿真实验是采用本发明的方法，根据上述EuRoC数据集中的MH_02_easy和MH_05_difficult数据序列，对无人机的位姿进行估计。
下面结合图2的仿真图对本发明的效果做一步的描述。
图2(a)是无人机在数据序列MH_02_easy上的估计位姿轨迹曲线和真实位姿曲线对比图。图2(a)中的无人机估计位姿轨迹曲线是通过采用本发明方法计算得到的，无人机真实位姿轨迹曲线是由数据集提供的，具有亚毫米级的精度。图2(a)中的横坐标表示在二维空间中无人机的位置坐标沿着x轴移动对应的值，纵坐标表示在二维空间中无人机的位置坐标沿着y轴移动对应的值，单位为米m。图2(a)中以圆形-实线-圆形标示的曲线表示无人机估计的位姿轨迹曲线，以五角星-虚线-五角星标示的曲线表示无人机的真实位姿轨迹曲线。
图2(b)是无人机在数据序列MH_05_difficult上的估计位姿轨迹曲线和真实位姿曲线对比图。图2(b)中的无人机估计位姿轨迹曲线是通过采用本发明方法计算得到的，无人机真实位姿轨迹曲线是由数据集提供的，具有亚毫米级的精度。图2(b)中的横坐标表示在二维空间中无人机的位置坐标沿着x轴移动对应的值，纵坐标表示在二维空间中无人机的位置坐标沿着y轴移动对应的值，单位为米m。图2(b)中以圆形-实线-圆形标示的曲线表示无人机的估计位姿轨迹曲线，以五角星-虚线-五角星标示的曲线表示无人机的真实位姿轨迹曲线。
图2(c)是数据序列MH_02_easy上的无人机估计位姿的绝对误差随时间变化曲线图。图2(c)是通过本发明方法计算得到的无人机估计位姿和数据集提供的无人机真实位姿对比得到的。图2(c)中的横坐标表示飞行时间，单位为秒s，纵坐标表示无人机估计位姿的绝对误差，单位为米m。图2(c)中以黑色实线标示的曲线表示无人机估计位姿的绝对误差。
图2(d)是数据序列MH_05_difficult上的无人机估计位姿的绝对误差随时间变化曲线图。图2(d)是通过本发明方法计算得到的无人机估计位姿和数据集提供的无人机真实位姿对比得到的。图2(d)中的横坐标表示飞行时间，单位为秒s，纵坐标表示无人机估计位姿的绝对误差，单位为米m。图2(d)中以黑色实线标示的曲线表示无人机估计位姿的绝对误差。
由图2(a)和图2(b)可以看出，本发明方法估计得到的无人机位姿轨迹曲线与真实的无人机位姿轨迹曲线几乎趋于一致，表明本发明方法估计的无人机位姿轨迹曲线具有较高精度。
由图2(c)和图2(d)可以看出，本发明方法估计得到的无人机位姿的绝对误差整体较小，随着运行时间增加，估计的无人机位姿的绝对误差没有明显增加，表明本发明方法估计的无人机位姿可以有效消除累积误差。

Claims (6)

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残差：
EN＝Pi-Nx
其中，EN表示第i个图像对应无人机的GPS残差，Pi表示第i个图像与第1个图像分别对应无人机的位置差值，Nx表示x时刻与第1个图像时刻分别对应的GPS差值；
(3d)将每个图像中每个特征点的视觉残差、每个图像与相邻图像对应无人机位置之间的IMU残差和每个图像对应无人机的GPS残差和最小化，即可得到紧耦合优化后的无人机位姿；
(4)优化无人机的位姿：
(4a)按照下式，计算每个图像对应无人机的绝对误差；
Figure FDA0003120881750000021
其中，E1，i表示第i个图像对应无人机的绝对残差，||·||表示取模长操作，μx表示无人机携带的GPS传感器在x时刻的置信度；
(4b)按照下式，计算每个图像与相邻图像分别对应无人机之间的相对误差；
Figure FDA0003120881750000022
其中，E2，(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表示矩阵的转置操作，Ri表示紧耦合优化后第i个图像与第1个图像分别对应无人机的旋转差值，Ri+1紧耦合优化后表示第i+1个图像与第1个图像分别对应无人机的旋转差值；
(4c)利用下式，得到无人机松耦合优化后的位姿：
Figure FDA0003120881750000023
其中，min{·}表示最小化操作，n表示无人机携带的视觉传感器在飞行过程中获取到的所有图像的总数量，∑表示求和操作。
2.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(2a)中所述计算每个特征点的BRIEF描述子是由下述步骤得到的：
第一步，在待计算的特征点周围随机选取256对像素点；
第二步，对于每一对像素点，如果先选取像素点的亮度大于后选取像素点的亮度，则将待计算的特征点的描述子对应位置为1，否则设置为0，得到该像素点对的二进制数；
第三步，将256对像素点的二进制数组合成一个二进制串，作为该特征点的BRIEF描述子。
3.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(2b)中所述预积分公式如下：
Figure FDA0003120881750000031
Figure FDA0003120881750000032
Figure FDA0003120881750000033
其中，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时刻无人机位置的旋转变化量。
4.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(2c)中所述坐标系转换公式如下：
x(t)＝(N+alt(t))cos(lat(t))cos(lon(t))
y(t)＝(N+alt(t))cos(lat(t))sin(lon(t))
z(t)＝(N(1-u2)+alt(t))sin(lat(t))
其中，x(t)表示t时刻无人机在东北天坐标系中位姿的横坐标值，N表示基准椭球体的曲率半径，alt(t)表示t时刻无人机的高度，lat(t)表示t时刻无人机的纬度，lon(t)表示t时刻无人机的经度，y(t)表示t时刻无人机在东北天坐标系中位姿的纵坐标值，z(t)表示t时刻无人机在东北天坐标系中位姿的竖坐标值，u表示基准椭球体的偏心率。
5.根据权利要求1所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(3a)中所述重投影公式如下：
Figure FDA0003120881750000041
其中，Eh表示第h个特征点的视觉残差，xh表示第h个特征点在第i个图像中归一化坐标系的横坐标值，zh表示第h个特征点在第i个图像中归一化坐标系的竖坐标值，uh表示第h个特征点在第i个图像中像素坐标系的横坐标值，yh表示第h个特征点在第i个图像中归一化坐标系的纵坐标值，vh表示第h个特征点在第i个图像中像素坐标系中的纵坐标值。
6.根据权利要求3所述的基于多传感器融合的无人机位姿估计方法，其特征在于，步骤(3b)中所述运动学公式如下：
Figure FDA0003120881750000042
其中，Ei表示第i个图像与相邻的第i+1个图像分别对应无人机位姿之间的IMU残差，Pi+1表示第i+1个图像与第1个图像分别对应无人机的位置差值，Vi表示第i个图像对应无人机的速度，Δt(i，i+1)表示第i个图像与相邻的第i+1个图像之间的时间差值，g表示重力加速度，Vi+1表示第i+1个图像对应无人机的速度，qi+1表示第i+1个图像与第1个图像分别对应无人机的旋转差值，
Figure FDA0003120881750000043
表示四元数乘法操作，qi第i个图像与第1个图像分别对应无人机的旋转差值。
CN202110675571.5A 2021-06-18 2021-06-18 基于多传感器松紧耦合的无人机位姿估计方法 Active CN113375665B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110675571.5A CN113375665B (zh) 2021-06-18 2021-06-18 基于多传感器松紧耦合的无人机位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110675571.5A CN113375665B (zh) 2021-06-18 2021-06-18 基于多传感器松紧耦合的无人机位姿估计方法

Publications (2)

Publication Number Publication Date
CN113375665A true CN113375665A (zh) 2021-09-10
CN113375665B CN113375665B (zh) 2022-12-02

Family

ID=77577661

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110675571.5A Active CN113375665B (zh) 2021-06-18 2021-06-18 基于多传感器松紧耦合的无人机位姿估计方法

Country Status (1)

Country Link
CN (1) CN113375665B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114355961A (zh) * 2021-11-24 2022-04-15 普宙科技(深圳)有限公司 无人机基于多视角视觉同步紧耦合视觉辅助悬停方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
US20200240793A1 (en) * 2019-01-28 2020-07-30 Qfeeltech (Beijing) Co., Ltd. Methods, apparatus, and systems for localization and mapping
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN112731503A (zh) * 2020-12-25 2021-04-30 中国科学技术大学 一种基于前端紧耦合的位姿估计方法及系统
CN114322996A (zh) * 2020-09-30 2022-04-12 阿里巴巴集团控股有限公司 一种多传感器融合定位系统的位姿优化方法和装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200240793A1 (en) * 2019-01-28 2020-07-30 Qfeeltech (Beijing) Co., Ltd. Methods, apparatus, and systems for localization and mapping
CN109991636A (zh) * 2019-03-25 2019-07-09 启明信息技术股份有限公司 基于gps、imu以及双目视觉的地图构建方法及系统
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN114322996A (zh) * 2020-09-30 2022-04-12 阿里巴巴集团控股有限公司 一种多传感器融合定位系统的位姿优化方法和装置
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN112731503A (zh) * 2020-12-25 2021-04-30 中国科学技术大学 一种基于前端紧耦合的位姿估计方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
TONG QIN ET.AL: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》 *
彭滨等: "融合全局位姿信息的视觉惯性SLAM算法", 《桂林电子科技大学学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114355961A (zh) * 2021-11-24 2022-04-15 普宙科技(深圳)有限公司 无人机基于多视角视觉同步紧耦合视觉辅助悬停方法

Also Published As

Publication number Publication date
CN113375665B (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN110095116A (zh) 一种基于lift的视觉定位和惯性导航组合的定位方法
CN108051002A (zh) 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN109522832B (zh) 基于点云片段匹配约束和轨迹漂移优化的回环检测方法
CN109708649B (zh) 一种遥感卫星的姿态确定方法及系统
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN107449444A (zh) 一种多星图姿态关联的星敏感器内参数标定方法及其装置
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN110044377B (zh) 一种基于Vicon的IMU离线标定方法
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN112085003A (zh) 公共场所异常行为自动识别方法及装置、摄像机设备
CN108613675B (zh) 低成本无人飞行器移动测量方法及系统
CN112797985A (zh) 基于加权扩展卡尔曼滤波的室内定位方法及室内定位系统
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
Choi et al. Position-based augmented reality platform for aiding construction and inspection of offshore plants
CN113375665B (zh) 基于多传感器松紧耦合的无人机位姿估计方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN112197765B (zh) 一种实现水下机器人精细导航的方法
Weng et al. Visual–inertial structural acceleration measurement
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN115144867A (zh) 一种基于无人机搭载三轴云台相机的目标检测定位方法
CN113432602B (zh) 基于多传感器融合的无人机位姿估计方法
CN116026316B (zh) 一种耦合视觉惯性里程计与gnss的无人船航迹推算方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant