CN118149789A - 一种基于里程计局部一致性的无人机集群定位方法和系统 - Google Patents

一种基于里程计局部一致性的无人机集群定位方法和系统 Download PDF

Info

Publication number
CN118149789A
CN118149789A CN202410155783.4A CN202410155783A CN118149789A CN 118149789 A CN118149789 A CN 118149789A CN 202410155783 A CN202410155783 A CN 202410155783A CN 118149789 A CN118149789 A CN 118149789A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
state variable
moment
ith
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.)
Pending
Application number
CN202410155783.4A
Other languages
English (en)
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.)
Nanhu Research Institute Of Electronic Technology Of China
Original Assignee
Nanhu Research Institute Of Electronic Technology Of China
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 Nanhu Research Institute Of Electronic Technology Of China filed Critical Nanhu Research Institute Of Electronic Technology Of China
Priority to CN202410155783.4A priority Critical patent/CN118149789A/zh
Publication of CN118149789A publication Critical patent/CN118149789A/zh
Pending legal-status Critical Current

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明给出了一种基于里程计局部一致性的无人机集群定位方法和系统，属于无人机集群技术领域，该方法包括：确定无人机集群的关键帧集；构建每架无人机每一时刻的里程计信息损失函数和距离信息损失函数；对关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定每架无人机在对应的关键帧中的像素坐标；构建每架无人机每一时刻的视觉信息损失函数；对里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建每架无人机的总目标函数；对总目标函数进行迭代优化。本发明的无人机集群定位的准确性高、稳定性好和鲁棒性好。

Description

一种基于里程计局部一致性的无人机集群定位方法和系统
技术领域
本发明属于无人机集群技术领域，尤其涉及一种基于里程计局部一致性的无人机集群定位方法和系统。
背景技术
随着无人机技术的发展，无人机集群技术已成为人们关注的研究和应用领域之一。在无人配送、无人仓储以及在无人集群作战等均有广阔的应用前景。其中高精度无人机集群定位技术是实现无人集群自主化、协同化的关键。
目前无人机定位方案可以分为借助外部传感器和自身传感器两大类方案。传统的基于全球定位系统(GPS、GNSS)或惯性导航系统(INS)的无人机定位方法存在精度不足、易受环境影响等问题，难以满足复杂环境下的任务需求。光学动作捕捉系统虽然可以实现较高的定位精度，但也有成本高昂、安装布置复杂等缺点。较低成本的UWB基站，也需要提前布置，不能满足复杂未知环境下的应用场景。另一种应用广泛的是利用无人机自身携带传感器(惯性测量单元IMU、RGB相机、RGB-D相机、激光雷达等)进行自主定位，通过单机定位技术可以获取无人机的位置、姿态和运动状态等信息，该技术具有成本低、信息量大、适应性强等优点。但在现实应用场景中，无人机集群通常需要完成多种复杂任务，例如搜救、探索、建图等，这些任务需求往往超出了传统单机定位技术的能力。视觉里程计和激光里程计是单机定位中的主流方法。视觉里程计利用视觉信息进行匹配和计算，从而获得无人机的位置和姿态信息。然而，由于视觉信息受到环境光照强度、背景、运动模糊等因素的影响，容易产生误差和漂移。同样地，激光里程计也依赖于激光雷达对环境的扫描，也会受到误差和噪声、定位区域的遮挡和复杂性等影响。
综上，面对空间复杂、环境变化等多种挑战，基于自身传感器的无人机集群定位的稳定性差、准确性低和鲁棒性差。
发明内容
本发明的目的之一，在于提供一种基于里程计局部一致性的无人机集群定位方法，该方法提高了无人机集群定位的准确性，改善了无人机集群定位的稳定性和鲁棒性，且成本低，无需提前布置定位基站，满足复杂未知环境下的应用场景中的定位需求。
本发明的目的之二，在于提供一种基于里程计局部一致性的无人机集群定位系统。
为了达到上述目的之一，本发明采用如下技术方案实现：
一种基于里程计局部一致性的无人机集群定位方法，所述无人机集群定位方法包括如下步骤：
步骤S1、获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集；
步骤S2、利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数；
步骤S3、利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数；
步骤S4、利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标；
步骤S5、利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数；
步骤S6、对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；
步骤S7、对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
进一步的，在所述步骤S1中，所述里程计数据包括每架无人机每一时刻的里程计绕Z轴旋转的角度和里程计平移向量；
所述距离传感器数据包括无人机集群中任意两架无人机在每一时刻的距离信息；
所述视觉信息数据包括每架无人机每一时刻所能观察到的无人机图像信息。
进一步的，在所述步骤S1中，所述确定无人机集群的关键帧集的具体过程包括：
步骤S11、利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
步骤S12、对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
进一步的，在所述步骤S2中，所述构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数的具体过程包括：
步骤S21、利用所述里程计绕Z轴旋转的角度，构建所述关键帧集中每架无人机每一时刻里程计绕Z轴旋转矩阵分量；
步骤S22、采用所述里程计平移向量和里程计绕Z轴旋转矩阵分量，对所述关键帧集中每架无人机在相邻时刻的相对位姿变换矩阵进行表征；
步骤S23、利用无人机的平移状态向量和绕Z轴旋转矩阵，对每一时刻的每架无人机的位姿状态变量进行预估；
步骤S24、采用所述相对位姿变换矩阵以及位姿状态变量预估结果，对所述关键帧集中每架无人机每一时刻的里程计信息损失函数进行表征。
进一步的，在所述步骤S5中，所述构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数的具体过程包括：
步骤S51、对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
步骤S52、利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
步骤S53、利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
进一步的，在所述步骤S7中，所述迭代优化的具体过程包括：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值；
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值；
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
为了达到上述目的之二，本发明采用如下技术方案实现：
一种基于里程计局部一致性的无人机集群定位系统，所述无人机集群定位系统包括：
数据获取模块，用于获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集；
位姿修正模块，用于利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数；
距离修正模块，用于利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数；
检测和跟踪模块，用于利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标；
构建模块，用于利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数；
时间累加模块，用于对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；
迭代优化模块，用于对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
进一步的，所述数据获取模块包括：
时间对齐子模块，用于利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
关键帧识别子模块，用于对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
进一步的，所述构建模块包括：
变换子模块，用于对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
第一确定子模块，用于利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
累加处理子模块，用于利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
进一步的，所述迭代优化模块执行如下步骤：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值；
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值；
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
综上，本发明的技术方案具有如下有益效果：
本发明通过无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，确定无人机集群的关键帧集；利用里程计数据，对每架无人机的位姿进行修正，以构建每架无人机每一时刻的里程计信息损失函数；利用距离传感器数据，对任意两架无人机之间的距离进行修正，以构建每架无人机每一时刻的距离信息损失函数；利用视觉信息数据，对每架无人机依次进行检测和跟踪，以确定无人机集群中每架无人机在对应的关键帧中的像素坐标；利用里程计数据和像素坐标，构建每架无人机每一时刻的视觉信息损失函数；对里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；通过总目标函数的迭代优化，以确定每架无人机的位姿，实现了利用里程计局部一致性、集群间信息共享、多传感信息融合机制的无人机集群定位，利用集群中的无人机之间的共享自身的位置信息，提高了无人机集群的定位准确性、鲁棒性和稳定性；本发明结合多种传感器信息(如GPS、惯性测量单元(IMU)和视觉传感器等)，以更全面、多角度地获取定位信息和环境信息，以应对复杂环境中的不确定性和变化性，有效解决了单机定位的误差和漂移问题；本发明的无人机集群定位方法，对于实现无人机集群自主化、协同化有着重要意义，具有广阔的应用前景和研究价值。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为根据本发明实施例的基于里程计局部一致性的无人机集群定位方法流程示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例只是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本实施例给出了一种基于里程计局部一致性的无人机集群定位方法，参考图1，该无人机集群定位方法包括如下步骤：
步骤S1、获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集。
本实施例中的里程计数据包括每架无人机每一时刻的里程计绕Z轴旋转的角度和里程计平移向量。距离传感器数据包括无人机集群中任意两架无人机在每一时刻的距离信息。视觉信息数据包括每架无人机每一时刻所能观察到的无人机图像信息。
由于无人机集群间通讯会受到信号干扰、距离限制、设备偶发故障等影响出现通讯故障，在对信息进行融合处理时，充分利用传感器数据的时间戳信息；在进行优化求解前，进行时间对齐，保证整个无人机集群中各个传感器信息(集群帧)在同一时刻，确保集群状态的时间一致性。确定无人机集群的关键帧集的具体过程包括：
步骤S11、利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
步骤S12、对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
步骤S2、利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数。
由于里程计(如视觉里程计和激光里程计等)具有较好的局部一致性(即里程计可采用IMU信息和重力矢量，估计翻滚角和俯仰角，减少了两个位姿估计自由度)，进而可根据四自由度的相对位姿变换矩阵，建立局部一致性测量损失函数(即里程计信息损失函数)。本实施例中的构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数的具体过程包括：
步骤S21、利用所述里程计绕Z轴旋转的角度，构建所述关键帧集中每架无人机每一时刻里程计绕Z轴旋转矩阵分量；
步骤S22、采用所述里程计平移向量和里程计绕Z轴旋转矩阵分量，对所述关键帧集中每架无人机在相邻时刻的相对位姿变换矩阵进行表征；
步骤S23、利用无人机的平移状态向量和绕Z轴旋转矩阵，对每一时刻的每架无人机的位姿状态变量进行预估；
步骤S24、采用所述相对位姿变换矩阵以及位姿状态变量预估结果，对所述关键帧集中每架无人机每一时刻的里程计信息损失函数进行表征。
本实施例中的关键帧集中每架无人机每一时刻的里程计信息损失函数为：
其中，为关键帧集中第i架无人机在t时刻的相对位姿变换矩阵；/>和分别为关键帧集中第i架无人机在t-1时刻和t时刻的里程计绕Z轴旋转矩阵分量；和/>分别为关键帧集中第i架无人机在t-1时刻和t时刻的里程计平移向量；/>和/>分别为关键帧集中第i架无人机在t-1时刻和t时刻的位姿状态变量；/>和分别为关键帧集中第i架无人机在t-1时刻和t时刻的平移状态向量；/>和/>分别为关键帧集中第i架无人机在t-1时刻和t时刻的绕Z轴旋转状态矩阵；/>为关键帧集中第i架无人机在t时刻的绕Z轴旋转的角度；( )T为转置矩阵；|| ||2为2范数运算。
步骤S3、利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数。
测距传感器提供无人机集群之间的距离信息，可通过无人机间的距离信息对定位信息(即两架无人机之间的距离)的修正，构建距离信息损失函数。
本实施例中的关键帧集中每架无人机每一时刻的距离信息损失函数为：
其中，和/>分别为关键帧集中第i架无人机和第j架无人机在t时刻的平移状态向量；ri，j，t为关键帧集中第i架无人机和第j架无人机在t时刻之间的传感器距离信息(即距离信息)；j＝1,2，...，U，U为关键帧集中第i架无人机在t时刻所能观察到的含有距离信息的无人机。
步骤S4、利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标。
当无人机集群中某个无人机观测到其他无人机时，可以利用目标检测算法得到无人机的二维检测框，因无人机并不是一直出现在传感器视野中，需要通过目标跟踪算法，得到图像中无人机的ID信息。通过目标检测算法，得到无人机在像素坐标系下的像素坐标。
步骤S5、利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数。
本实施例中的关键帧集中每架无人机每一时刻的视觉信息损失函数为：
其中，n1,h和n2,h分别第一方向向量和第二方向向量；和/>分别为关键帧集中第h架无人机和第i架无人机在t时刻的平移状态向量；/>为关键帧集中第h架无人机在t时刻的里程计绕Z轴旋转矩阵分量，h＝1,2，...，D，D为关键帧集中所有检测到的无人机；π-1(u,v)为相机坐标系变化函数；(u,v)为像素坐标。
本实施例中的构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数的具体过程包括：
步骤S51、对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
步骤S52、利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
步骤S53、利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
步骤S6、对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数。
考虑使用关键帧对集群状态进行刻画，利用集群状态的时间序列特性进行刻画，利用多个集群帧集合进行联合优化，集群帧长度可以根据硬件资源和计算精度的需求进行选择。
本实施例中的关键帧集中每架无人机的总目标函数为：
其中，SF为关键帧集，SF＝{SF1，SF2，SF3，...，SFm},m为关键帧集中关键帧长度。
步骤S7、对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
为了实现对内存消耗占用的优化以及求解效率的提升，可以采用无约束优化方法中的限制内存拟牛顿法(L-BFGS)进行求解，不仅能够高效地求解优化问题，而且具有较低的内存资源消耗，可在小型计算平台上稳定求解。迭代优化的具体过程包括：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
每架无人机第k次迭代优化的位姿状态变量修正值为：
其中，Sk为每架无人机第k次迭代优化的位姿状态变量修正值；和Hk分别为每架无人机第k次迭代优化的梯度值和海塞矩阵；Pk为每架无人机t时刻的位姿状态变量初始值；αk满足弱沃尔夫条件(Wolfe condition)，可以预先设定；ρk、Vk和H0为中间变量，I为单位矩阵。
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机第k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
本实施例中的每架无人机第k+1次迭代优化的位姿状态变量值为：
其中，Pk+1为每架无人机第k+1次迭代优化的位姿状态变量值。
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值。
本实施例中的第i架无人机第k+1次迭代优化的梯度值为第i架无人机的总目标函数在所述位姿状态变量值的梯度值。
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值。
本实施例中的每架无人机第k次迭代优化的梯度增量值yk为：
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
本实施例通过无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，确定无人机集群的关键帧集；利用里程计数据，对每架无人机的位姿进行修正，以构建每架无人机每一时刻的里程计信息损失函数；利用距离传感器数据，对任意两架无人机之间的距离进行修正，以构建每架无人机每一时刻的距离信息损失函数；利用视觉信息数据，对每架无人机依次进行检测和跟踪，以确定无人机集群中每架无人机在对应的关键帧中的像素坐标；利用里程计数据和像素坐标，构建每架无人机每一时刻的视觉信息损失函数；对里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建关键帧集中每架无人机的总目标函数；通过总目标函数的迭代优化，以确定每架无人机的位姿，实现了利用里程计局部一致性、集群间信息共享、多传感信息融合机制的无人机集群定位，利用集群中的无人机之间的共享自身的位置信息，提高了无人机集群的定位准确性、鲁棒性和稳定性；本发明结合多种传感器信息(如GPS、惯性测量单元(IMU)和视觉传感器等)，以更全面、多角度地获取定位信息和环境信息，以应对复杂环境中的不确定性和变化性，有效解决了单机定位的误差和漂移问题；本实施例的无人机集群定位方法，对于实现无人机集群自主化、协同化有着重要意义，具有广阔的应用前景和研究价值。
上述实施例可通过如下实施例给出的技术方案实现：
另一实施例给出了一种基于里程计局部一致性的无人机集群定位系统，该无人机集群定位系统包括：
数据获取模块，用于获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集；
位姿修正模块，用于利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数；
距离修正模块，用于利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数；
检测和跟踪模块，用于利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标；
构建模块，用于利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数；
时间累加模块，用于对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；
迭代优化模块，用于对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
进一步的，所述数据获取模块包括：
时间对齐子模块，用于利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
关键帧识别子模块，用于对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
进一步的，所述位姿修正模块包括：
第一构建子模块，用于利用所述里程计绕Z轴旋转的角度，构建所述关键帧集中每架无人机每一时刻里程计绕Z轴旋转矩阵分量；
第一表征子模块，用于采用所述里程计平移向量和里程计绕Z轴旋转矩阵分量，对所述关键帧集中每架无人机在相邻时刻的相对位姿变换矩阵进行表征；
预估子模块，用于利用无人机的平移状态向量和绕Z轴旋转矩阵，对每一时刻的每架无人机的位姿状态变量进行预估；
第二表征子模块，用于采用所述相对位姿变换矩阵以及位姿状态变量预估结果，对所述关键帧集中每架无人机每一时刻的里程计信息损失函数进行表征。
进一步的，所述构建模块包括：
变换子模块，用于对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
第一确定子模块，用于利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
累加处理子模块，用于利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
进一步的，所述迭代优化模块执行如下步骤：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值；
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值；
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
请注意，以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。以上实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种基于里程计局部一致性的无人机集群定位方法，其特征在于，所述无人机集群定位方法包括如下步骤：
步骤S1、获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集；
步骤S2、利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数；
步骤S3、利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数；
步骤S4、利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标；
步骤S5、利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数；
步骤S6、对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；
步骤S7、对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
2.根据权利要求1所述的无人机集群定位方法，其特征在于，在所述步骤S1中，所述里程计数据包括每架无人机每一时刻的里程计绕Z轴旋转的角度和里程计平移向量；
所述距离传感器数据包括无人机集群中任意两架无人机在每一时刻的距离信息；
所述视觉信息数据包括每架无人机每一时刻所能观察到的无人机图像信息。
3.根据权利要求2所述的无人机集群定位方法，其特征在于，在所述步骤S1中，所述确定无人机集群的关键帧集的具体过程包括：
步骤S11、利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
步骤S12、对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
4.根据权利要求3所述的无人机集群定位方法，其特征在于，在所述步骤S2中，所述构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数的具体过程包括：
步骤S21、利用所述里程计绕Z轴旋转的角度，构建所述关键帧集中每架无人机每一时刻里程计绕Z轴旋转矩阵分量；
步骤S22、采用所述里程计平移向量和里程计绕Z轴旋转矩阵分量，对所述关键帧集中每架无人机在相邻时刻的相对位姿变换矩阵进行表征；
步骤S23、利用无人机的平移状态向量和绕Z轴旋转矩阵，对每一时刻的每架无人机的位姿状态变量进行预估；
步骤S24、采用所述相对位姿变换矩阵以及位姿状态变量预估结果，对所述关键帧集中每架无人机每一时刻的里程计信息损失函数进行表征。
5.根据权利要求1～4中任意一项所述的无人机集群定位方法，其特征在于，在所述步骤S5中，所述构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数的具体过程包括：
步骤S51、对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
步骤S52、利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
步骤S53、利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
6.根据权利要求5所述的无人机集群定位方法，其特征在于，在所述步骤S7中，所述迭代优化的具体过程包括：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机第k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值；
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值；
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
7.一种基于里程计局部一致性的无人机集群定位系统，其特征在于，所述无人机集群定位系统包括：
数据获取模块，用于获取无人机集群中各架无人机的里程计数据、距离传感器数据、视觉信息数据及其对应的时间戳，以确定无人机集群的关键帧集；
位姿修正模块，用于利用所述里程计数据，对所述关键帧集中每架无人机的位姿进行修正，以构建所述关键帧集中每架无人机每一时刻的里程计信息损失函数；
距离修正模块，用于利用所述距离传感器数据，对所述关键帧集中任意两架无人机之间的距离进行修正，以构建所述关键帧集中每架无人机每一时刻的距离信息损失函数；
检测和跟踪模块，用于利用所述视觉信息数据，对所述关键帧集中每个关键帧中每架无人机依次进行检测和跟踪，以确定所述无人机集群中每架无人机在对应的关键帧中的像素坐标；
构建模块，用于利用所述里程计数据和像素坐标，构建所述关键帧集中每架无人机每一时刻的视觉信息损失函数；
时间累加模块，用于对所述关键帧集中每架无人机每一时刻的里程计信息损失函数、距离信息损失函数和视觉信息损失函数分别进行时间累加，以构建所述关键帧集中每架无人机的总目标函数；
迭代优化模块，用于对所述关键帧集中每架无人机的总目标函数进行迭代优化，以确定每架无人机的位姿。
8.根据权利要求7所述的无人机集群定位系统，其特征在于，所述数据获取模块包括：
时间对齐子模块，用于利用所述时间戳，对无人机集群中各架无人机自身的里程计数据、距离传感器数据、视觉信息数据进行时间对齐，以获取所述无人机集群的集群帧；
关键帧识别子模块，用于对所述无人机集群的集群帧进行关键帧识别，以获取无人机集群的关键帧集。
9.根据权利要求8所述的无人机集群定位系统，其特征在于，所述构建模块包括：
变换子模块，用于对所述无人机集群中每架无人机在对应的关键帧中的像素坐标依次进行相机坐标系和世界坐标系变换，以确定每架无人机与所能观察到的其余无人机在世界坐标下的第一方向向量；
第一确定子模块，用于利用所述每架无人机每一时刻的平移状态向量，确定每一时刻的每架无人机与所能观察到的其余无人机在世界坐标下的第二方向向量；
累加处理子模块，用于利用所述第一方向向量和第二方向向量，确定每一时刻的每架无人机相对于对应其余无人机的视觉损失函数后进行累加处理，以对所述关键帧集中每架无人机每一时刻的视觉信息损失函数进行表征。
10.根据权利要求9所述的无人机集群定位系统，其特征在于，所述迭代优化模块执行如下步骤：
步骤S701、设置迭代优化次数k＝0，迭代优化结果存储个数l＝0；
步骤S702、设置第i架无人机t时刻的位姿状态变量初始值为所述关键帧集中第i架无人机t时刻的里程计数据；
步骤S703、判断l是否小于迭代优化结果存储个数阈值，如是，则进入步骤S704；如否，则利用缓存器中所有的位姿状态变量修正值和梯度增量值，计算所述第i架无人机第k次迭代优化的位姿状态变量修正值，进入步骤S708；
步骤S704、计算所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值；
步骤S705、判断k是否为零，若是，则设置所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵为单位矩阵，进入步骤S706；若否，则利用存储器最后一次存储的位姿状态变量修正值和梯度增量值，对所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵进行更新，以将更新后的海塞矩阵作为所述第i架无人机的总目标函数在所述位姿状态变量初始值的海塞矩阵，进入步骤S706；
步骤S706、将所述关键帧集中第i架无人机的总目标函数在所述位姿状态变量初始值的梯度值和海塞矩阵分别作为第i架无人机第k次迭代优化的梯度值和海塞矩阵；
步骤S707、利用所述第i架无人机第k次迭代优化的梯度值和海塞矩阵，确定所述第i架无人机k次迭代优化的位姿状态变量修正值；
步骤S708、利用所述位姿状态变量修正值，对所述第i架无人机t时刻的位姿状态变量初始值进行修正，并将修正结果作为所述第i架无人机第k+1次迭代优化的位姿状态变量值；
步骤S709、利用所述第i架无人机第k+1次迭代优化的位姿状态变量值，计算所述第i架无人机第k+1次迭代优化的梯度值；
步骤S710、计算所述第i架无人机第k+1次迭代优化的梯度值和第k次迭代优化的梯度值的差值，并将所述差值作为所述第i架无人机第k次迭代优化的梯度增量值；
步骤S711、将所述第i架无人机第k次迭代优化的位姿状态变量修正值和梯度增量值存储到缓存器中，并判断l是否小于迭代优化结果存储个数阈值，如是，则令l＝l+1，进入步骤S712；如否，则丢弃缓存器中所述第i架无人机k-l次迭代优化的位姿状态变量修正值和梯度增量值，进入步骤S712；
步骤S712、判断所述梯度增量值是否大于精度阈值，如是，则并将所述第i架无人机第k+1次迭代优化的位姿状态变量值作为所述第i架无人机t时刻的位姿状态变量初始值，令k＝k+1，返回步骤S703；如否，则将所述第i架无人机t时刻的位姿状态变量初始值作为所述第i架无人机t时刻的位姿，结束。
CN202410155783.4A 2024-02-04 2024-02-04 一种基于里程计局部一致性的无人机集群定位方法和系统 Pending CN118149789A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410155783.4A CN118149789A (zh) 2024-02-04 2024-02-04 一种基于里程计局部一致性的无人机集群定位方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410155783.4A CN118149789A (zh) 2024-02-04 2024-02-04 一种基于里程计局部一致性的无人机集群定位方法和系统

Publications (1)

Publication Number Publication Date
CN118149789A true CN118149789A (zh) 2024-06-07

Family

ID=91285894

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410155783.4A Pending CN118149789A (zh) 2024-02-04 2024-02-04 一种基于里程计局部一致性的无人机集群定位方法和系统

Country Status (1)

Country Link
CN (1) CN118149789A (zh)

Similar Documents

Publication Publication Date Title
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN109887057B (zh) 生成高精度地图的方法和装置
CN109506642B (zh) 一种机器人多相机视觉惯性实时定位方法及装置
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN109522832B (zh) 基于点云片段匹配约束和轨迹漂移优化的回环检测方法
US20200300637A1 (en) Collaborative navigation and mapping
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112184824B (zh) 一种相机外参标定方法、装置
CN111156998A (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
Seiskari et al. HybVIO: Pushing the limits of real-time visual-inertial odometry
CN113763548B (zh) 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统
CN112577493A (zh) 一种基于遥感地图辅助的无人机自主定位方法及系统
Miao et al. UniVIO: Unified direct and feature-based underwater stereo visual-inertial odometry
CN108780577A (zh) 图像处理方法和设备
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN114964276B (zh) 一种融合惯导的动态视觉slam方法
CN111812978B (zh) 一种多无人机协作slam方法与系统
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN116007609A (zh) 一种多光谱图像和惯导融合的定位方法和计算系统
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN112268564B (zh) 一种无人机降落空间位置和姿态端到端估计方法
CN117710476A (zh) 基于单目视觉的无人机位姿估计与稠密建图方法
CN118149789A (zh) 一种基于里程计局部一致性的无人机集群定位方法和系统

Legal Events

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