CN112985391A - 一种基于惯性和双目视觉的多无人机协同导航方法和装置 - Google Patents

一种基于惯性和双目视觉的多无人机协同导航方法和装置 Download PDF

Info

Publication number
CN112985391A
CN112985391A CN202110415745.4A CN202110415745A CN112985391A CN 112985391 A CN112985391 A CN 112985391A CN 202110415745 A CN202110415745 A CN 202110415745A CN 112985391 A CN112985391 A CN 112985391A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
inertial navigation
navigation
collaborative
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
CN202110415745.4A
Other languages
English (en)
Other versions
CN112985391B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110415745.4A priority Critical patent/CN112985391B/zh
Publication of CN112985391A publication Critical patent/CN112985391A/zh
Application granted granted Critical
Publication of CN112985391B publication Critical patent/CN112985391B/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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models

Abstract

本申请涉及一种基于惯性和双目视觉的多无人机协同导航方法和装置。所述方法包括：基于各个无人机的惯导误差模型建立多无人机的协同导航系统状态模型，基于惯导解算和协同导航系统状态模型预测多无人机的系统状态，并根据无人机的双目视觉相对位置观测数据更新系统状态，得到各无人机的惯导解算误差估计，并修正惯导解算结果，实现多无人机协同导航。上述方法基于双目视觉相对位置观测修正各无人机的惯性导航系统误差，利用双目视觉位置观测信息中同时包含无人机间的距离和相对方位的特点，达到高于距离测量或相对方位测量的协同导航精度。此外，上述方法对各架无人机分别建立惯导误差模型，因此适用于多架无人机惯性导航系统精度不同的情况。

Description

一种基于惯性和双目视觉的多无人机协同导航方法和装置
技术领域
本申请涉及平台协同导航技术领域，特别是涉及一种基于惯性和双目视觉的多无人机协同导航方法和装置。
背景技术
目前无人机普遍采用惯性/卫星组合导航技术。然而卫星导航信号一方面在城市巷道、峡谷等环境会受到遮挡，另一方面容易被干扰。因此无人机不能仅依赖卫星作为导航手段，而是需要在无法获得卫星导航信号的环境中，基于惯性导航等技术实现自主导航，以适应各种复杂的工作环境。
对于无人机集群来说，可以利用无人机之间相对观测的协同导航技术提高无人机的惯性导航定位精度。协同导航是一种利用多运动体之间的相对观测和导航信息交互实现导航资源共享的新型导航技术。目前针对无人机集群的协同导航技术存在以下特点：第一，大多局限于协同空中加油、地面目标跟踪等特定的应用场景，无人机的数量多为两架，关注的多是无人机之间的相对导航信息；第二，无人机集群仍没有完全脱离对卫星导航的依赖，甚至完全基于卫星导航功能实现协同导航，不适用于无卫星信号的工作环境；第三，无人机之间的相对观测采用距离或相对方位测量，距离测量传感器常见的是超宽带测距传感器，相对方位测量传感器为单目相机，能够提供的导航数据精度较低。
发明内容
基于此，有必要针对上述技术问题，提供一种能够在无卫星导航信号的工作环境中实现高精度多无人机协同导航的基于惯性和双目视觉的多无人机协同导航方法和装置。
一种基于惯性和双目视觉的多无人机协同导航方法，所述方法包括：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
其中一个实施例中，基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型的方式包括：
根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。
根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。
根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
其中一个实施例中，无人机获取惯导解算数据的方式包括：
按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。
其中一个实施例中，基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果的方式包括：
按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯导解算周期。
其中一个实施例中，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果的方式包括：
基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。
根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新系统状态预测结果。
其中一个实施例中，惯导解算数据的获取方式包括：
基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。
基于气压高度计的高度测量值对位置分量的值进行修正。
一种基于惯性和双目视觉的多无人机协同导航装置，所述装置包括：
协同导航模型建立模块，用于基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
系统状态预测模块，用于基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
协同导航模块，用于当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
惯导误差修正模块，用于根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
一种多无人机系统，包括中心无人机和从属无人机。
中心无人机用于，基于预先建立的协同导航系统状态模型，根据中心无人机的惯导解算数据，以及根据收到的由从属无人机发送的惯导解算数据，得到多无人机的系统状态预测结果。协同导航系统状态模型是基于中心无人机和从属无人机的惯导误差模型建立的。
以及用于，接收由从属无人机发送的双目视觉相对位置观测数据，基于预设的从属无人机的双目视觉相对位置观测模型，更新系统状态预测结果，得到从属无人机的惯导误差估计值，将惯导误差估计值发送给从属无人机。
从属无人机用于，将本地的惯导解算数据发送给中心无人机。
以及用于，接收中心无人机发送的惯导误差估计值，根据惯导误差估计值修正从属无人机的惯导解算数据，得到协同导航数据。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
与现有技术相比，上述一种基于惯性和双目视觉的多无人机协同导航方法、装置、系统、计算机设备和存储介质，基于各个无人机的惯导误差模型建立多无人机的协同导航系统状态模型，基于协同导航系统状态模型预测多无人机的系统状态，并根据无人机的双目视觉相对位置观测数据对预测的系统状态进行更新，并估计对应无人机的惯导误差估计数据，由对应的无人机根据惯导误差估计值修正其惯导解算数据，实现对该无人机的协同导航。本申请利用双目视觉技术得到无人机之间的相对位置观测数据，并在此基础上修正各无人机的惯性导航系统误差。由于双目视觉得到的相对位置观测信息包含了无人机间的距离和相对方位，因此采用双目相机作为机间测量传感器，相当于同时采用了超宽带测距传感器和单目相机。因此，本申请实现的多无人机协同导航效果优于仅基于距离测量的协同导航方式或仅基于相对方位测量的协同导航方式。此外，由于本申请对于各架无人机分别建立其惯导误差模型，因此适用于多架无人机采用不同精度的惯性导航系统的情况。
附图说明
图1为一个实施例中一种基于惯性和双目视觉的多无人机协同导航方法的步骤图；
图2为一个实施例中一种基于惯性和双目视觉的多无人机协同导航方法的流程示意图；
图3为一个实施例中两架无人机的相对位置示意图；
图4为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种基于惯性和双目视觉的多无人机协同导航方法，包括以下步骤：
步骤102，基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
具体地，各个无人机的惯导误差模型可以根据各个无人机配备的惯导系统，在选定的坐标系下基于现有的各种建模方式得到。惯导误差模型根据上一时刻的惯导解算误差和惯性传感器数据预测当前时刻的惯导解算误差。
协同导航系统状态模型则是基于参与协同导航的各个无人机的惯导误差模型建立的，其输入为各个无人机当前时刻的误差状态以及惯性传感器数据，输出为对下一时刻各个无人机的误差状态的预测结果，包括均值和方差。
步骤104，基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
协同导航系统状态模型则是基于参与协同导航的各个无人机的惯导误差模型建立的，其输入为各个无人机当前时刻的误差状态以及惯性传感器数据，输出为对下一时刻各个无人机的误差状态的预测结果，包括均值和方差。
步骤106，当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
本实施例中的无人机上配有双目视觉传感器，获取该无人机和本无人机系统中其他无人机的相对位置观测数据。对于获取到的相对位置观测数据，可以将其发送到本无人机系统中指定的、具有相应计算能力的无人机上，也可以发送到本无人机系统外的其他设备上进行处理，如地面控制中心等。收到双目视觉相对位置观测数据的设备，基于观测无人机的双目视觉相对位置观测模型，对双目视觉相对位置观测数据进行处理，获得观测无人机和被观测无人机的相对位置信息，根据这一相对位置信息，更新步骤104中获得的系统状态预测结果。
将更新后的系统状态估计结果分发给各无人机。
步骤108，根据惯导误差估计值修正对应无人机的惯导解算结果，得到协同导航结果，实现高精度的多无人机协同导航。
本实施例利用双目视觉技术得到无人机之间的相对位置观测数据，并在此基础上修正各无人机的惯性导航系统误差。由于双目视觉得到的相对位置观测信息包含了无人机间的距离和相对方位，因此采用双目相机作为机间测量传感器，相当于同时采用了超宽带测距传感器和单目相机。因此，本实施例实现的多无人机协同导航效果优于仅基于距离测量的协同导航方式或基于相对方位测量的协同导航方式。
其中一个实施例中，如图2所示，提供了一种基于惯性和双目视觉的多无人机协同导航方法，应用于多无人机系统。该多无人机系统包括中心无人机和从属无人机。所述方法包括以下步骤：
步骤202，获取各个无人机的惯导解算数据。由多无人机系统中的所有无人机分别基于惯导传感器的测量值，按照预设的惯导解算周期T1，各自进行惯导解算，得到各架无人机的状态向量。无人机的状态向量包括姿态分量、速度分量和位置分量。
具体地，在进行惯导解算时，首先需要建立相应的坐标系，包括：
当地地理坐标系N，本实施例中以选取北东地（NED）坐标系为当地地理坐标系N为例进行说明。N坐标系原点与地球表面一点固连，x轴沿当地子午线指向北，z轴沿当地重力方向向下，y轴由右手法则确定。
世界参考坐标系G，也称全局坐标系，一般情况下与地球固连，系统最终输出的导航状态，如位置、速度、姿态角一般在此坐标系中表示。在本实施例中，采用一架无人机初始时刻的当地地理坐标系作为世界参考坐标系。
载体坐标系B，以无人机重心
Figure 923245DEST_PATH_IMAGE001
为原点；
Figure 43647DEST_PATH_IMAGE002
轴沿无人机纵轴指向无人机头部；
Figure 88964DEST_PATH_IMAGE003
轴垂直于无人机纵向对称面，从尾部向头部看过去，指向右方；
Figure 953015DEST_PATH_IMAGE004
轴由右手法则确定，位于无人机纵向对称面内指向下方。
地心地固坐标系E，该坐标系与地球固连，坐标原点位于地球球心，X轴指向赤道与参考子午线的交点，Z轴沿地球自转轴指向北极点，Y轴由右手法则确定。此坐标系是用于G系与N系转换的中间坐标系。
基于上述坐标系的定义方式，本实施例中的坐标系转换关系为：
假设中心无人机的初始位置用纬度
Figure 970649DEST_PATH_IMAGE005
、经度
Figure 511221DEST_PATH_IMAGE006
、高度
Figure 43833DEST_PATH_IMAGE007
表示，将其作为世界坐标系G的原点。其在E系中的坐标位置则为：
Figure 445996DEST_PATH_IMAGE008
，
其中，
Figure 380454DEST_PATH_IMAGE009
为该点的卯酉圈半径，
Figure 842659DEST_PATH_IMAGE010
为地球曲率，
Figure 800251DEST_PATH_IMAGE011
为地球长半轴，
Figure 255372DEST_PATH_IMAGE012
。
E系到G系(即初始点的N系)的旋转矩阵
Figure 247599DEST_PATH_IMAGE013
如下：
Figure 146284DEST_PATH_IMAGE014
则初始点在E系下的位置坐标可用下式转换到G系下：
Figure 653489DEST_PATH_IMAGE015
在确定世界参考坐标系后，
Figure 397454DEST_PATH_IMAGE016
、
Figure 493455DEST_PATH_IMAGE017
为常值矩阵。
E系到N系的旋转矩阵
Figure 94201DEST_PATH_IMAGE018
与
Figure 26385DEST_PATH_IMAGE019
具有相同的形式，设N系原点的经度坐标为
Figure 574041DEST_PATH_IMAGE020
，纬度坐标为
Figure 337597DEST_PATH_IMAGE021
，则旋转矩阵为
Figure 578086DEST_PATH_IMAGE022
设任意点在E系下的坐标为
Figure 981254DEST_PATH_IMAGE023
，则在G系下的坐标为
Figure 129339DEST_PATH_IMAGE024
。
已知无人机在N系下的速度为
Figure 685085DEST_PATH_IMAGE025
，则无人机在G系下的速度
Figure 96475DEST_PATH_IMAGE026
可以表示为
Figure 799989DEST_PATH_IMAGE027
。
设B系按“1-2-3”的顺序依次转动角度
Figure 689447DEST_PATH_IMAGE028
可以与N系对应轴平行，则可以得到旋转矩阵
Figure 614547DEST_PATH_IMAGE029
。
则G系到B系的旋转矩阵为
Figure 993575DEST_PATH_IMAGE030
由
Figure 122068DEST_PATH_IMAGE031
可以得到旋转四元数
Figure 611956DEST_PATH_IMAGE032
。
基于上述坐标系设置，各架无人机在世界参考坐标系下进行惯导解算。具体过程为：假设无人机k-1时刻在G系下的位置为
Figure 142294DEST_PATH_IMAGE033
，速度为
Figure 895486DEST_PATH_IMAGE034
，姿态四元数为
Figure 760543DEST_PATH_IMAGE035
，k时刻捷联式陀螺输出角速度为
Figure 54121DEST_PATH_IMAGE036
，加速度计输出比力为
Figure 704545DEST_PATH_IMAGE037
，解算周期为
Figure 97481DEST_PATH_IMAGE038
。
姿态分量的获取方式为：由
Figure 262883DEST_PATH_IMAGE039
可以得到旋转矩阵
Figure 297835DEST_PATH_IMAGE040
。地球自转角速度为
Figure 52033DEST_PATH_IMAGE041
。
Figure 678187DEST_PATH_IMAGE042
Figure 534147DEST_PATH_IMAGE043
Figure 107211DEST_PATH_IMAGE044
Figure 263386DEST_PATH_IMAGE045
，
Figure 529282DEST_PATH_IMAGE046
为四元数乘法运算符。
速度分量的获取方式为：由更新后的四元数
Figure 590648DEST_PATH_IMAGE047
可以得到更新后的旋转矩阵
Figure 29719DEST_PATH_IMAGE048
。E系下的坐标为
Figure 509242DEST_PATH_IMAGE049
，由此可以先得到纬经高坐标，再可以得到
Figure 680461DEST_PATH_IMAGE050
。重力加速度
Figure 245434DEST_PATH_IMAGE051
。
Figure 222617DEST_PATH_IMAGE052
Figure 805914DEST_PATH_IMAGE053
位置分量的获取方式为：
Figure 148034DEST_PATH_IMAGE054
进一步地，为修正惯导解算高度通道的误差，各架无人机上均配备气压高度计。步骤202之后，还包括：
步骤203，基于气压高度计的高度测量值对位置分量的值进行修正。本步骤的目的是引入其他高度源对高度进行修正，以解决惯导解算的高度通道发散问题。假设气压高度计的高度测量值为
Figure 997041DEST_PATH_IMAGE055
，则根据坐标转换关系更新状态向量
Figure 450019DEST_PATH_IMAGE056
中的
Figure 904134DEST_PATH_IMAGE057
。
步骤204，根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
具体地，根据步骤202-步骤203的结算结果，可以将每个无人机的状态向量定义为：
Figure 479472DEST_PATH_IMAGE058
则误差状态向量为：
Figure 2726DEST_PATH_IMAGE059
其中，
Figure 524975DEST_PATH_IMAGE060
是角度误差向量，由误差四元数定义：
Figure 568017DEST_PATH_IMAGE061
基于上述定义，协同导航系统状态模型的连续系统状态方程为：
Figure 766786DEST_PATH_IMAGE062
其中，
Figure 262489DEST_PATH_IMAGE063
Figure 588428DEST_PATH_IMAGE064
Figure 735245DEST_PATH_IMAGE065
为由姿态四元数
Figure 590068DEST_PATH_IMAGE066
得到的旋转矩阵，
Figure 900964DEST_PATH_IMAGE067
、
Figure 765015DEST_PATH_IMAGE068
分别为陀螺和加速度计的输出
Figure 517070DEST_PATH_IMAGE069
、
Figure 323221DEST_PATH_IMAGE070
在G系下的投影，
Figure 855834DEST_PATH_IMAGE071
表示反对称矩阵，
Figure 257996DEST_PATH_IMAGE072
为陀螺噪声、加速度计噪声。
离散化的协同导航系统状态模型为：
Figure 130137DEST_PATH_IMAGE073
差分方程的状态递推矩阵
Figure 654659DEST_PATH_IMAGE074
采用二阶近似计算
Figure 877830DEST_PATH_IMAGE075
假设连续状态下状态方程输入噪声
Figure 332951DEST_PATH_IMAGE076
的方差为
Figure 262861DEST_PATH_IMAGE077
，则离散化后系统噪声方差
Figure 237246DEST_PATH_IMAGE078
的计算方法为：
Figure 478871DEST_PATH_IMAGE079
，
Figure 222836DEST_PATH_IMAGE080
，
Figure 335149DEST_PATH_IMAGE081
，
Figure 935895DEST_PATH_IMAGE082
Figure 117346DEST_PATH_IMAGE083
假设协同导航系统内有n架无人机，则总的状态向量为：
Figure 665002DEST_PATH_IMAGE084
对应的状态转移矩阵为：
Figure 271302DEST_PATH_IMAGE085
离散化后的系统输入噪声方差为：
Figure 246211DEST_PATH_IMAGE086
步骤206，按照预设的协同导航周期T2，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期T2>惯导解算周期T1。
基于协同导航系统状态模型进行状态预测，基于k-1时刻的各个无人机的状态向量构建对应的系统状态向量
Figure 914959DEST_PATH_IMAGE087
，得到对下一时刻（k时刻）的系统状态向量预测结果
Figure 63043DEST_PATH_IMAGE088
：
Figure 884369DEST_PATH_IMAGE089
步骤208，建立无人机的双目视觉相对位置观测模型。
以无人机i（即观测者）利用双目视觉传感器对无人机j（即被观测者）进行观测，获得无人机j的相对位置为例进行说明（图2中，i=2）。如图3所示，设无人机i，j在全局坐标系（G系）下的位置分别为
Figure 30179DEST_PATH_IMAGE090
，无人机i的姿态四元数为
Figure 733693DEST_PATH_IMAGE091
，则无人机i对于无人机j的双目视觉相对位置观测模型为：
Figure 623152DEST_PATH_IMAGE092
线性化误差方程为：
Figure 813830DEST_PATH_IMAGE093
其中，
Figure 864963DEST_PATH_IMAGE094
Figure 55773DEST_PATH_IMAGE095
Figure 748922DEST_PATH_IMAGE096
步骤210，当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
基于步骤208中所述的双目视觉相对位置观测模型，得到无人机的观测矩阵，并更新系统状态预测结果。具体地，系统状态预测结果的预测方差计算方式为：
Figure 279261DEST_PATH_IMAGE097
Figure 829191DEST_PATH_IMAGE098
表示状态转移矩阵，并进行卡尔曼增益计算
Figure 959827DEST_PATH_IMAGE099
，
Figure 925509DEST_PATH_IMAGE100
表示观测矩阵，
Figure 575933DEST_PATH_IMAGE101
为测量值噪声方差：
Figure 296764DEST_PATH_IMAGE102
步骤212，无人机i根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
则对无人机i的惯导误差估计值的计算方式为：
Figure 665429DEST_PATH_IMAGE103
后验估计值方差的计算方式为：
Figure 684069DEST_PATH_IMAGE104
由于惯导解算周期T1和协同导航周期T2的长度不同，中心无人机在没有收到相对位置观测时仅进行状态预测，当有相对位置观测时，进行卡尔曼滤波更新。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
其中一个实施例中，提供了一种基于惯性和双目视觉的多无人机协同导航装置，所述装置包括：
协同导航模型建立模块，用于基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
系统状态预测模块，用于基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
协同导航模块，用于当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
惯导误差修正模块，用于根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
其中一个实施例中，协同导航模型建立模块用于根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
其中一个实施例中，还包括惯导解算数据获取模块，用于按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。
其中一个实施例中，系统状态预测模块用于按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯导解算周期。
其中一个实施例中，协同导航模块用于基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新系统状态预测结果。
其中一个实施例中，惯导解算数据获取模块用于基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于气压高度计的高度测量值对位置分量的值进行修正。
其中一个实施例中，提供了一种多无人机系统，包括中心无人机和从属无人机。
中心无人机用于，基于预先建立的协同导航系统状态模型，根据中心无人机的惯导解算数据，以及根据收到的由从属无人机发送的惯导解算数据，得到多无人机的系统状态预测结果。协同导航系统状态模型是基于中心无人机和从属无人机的惯导误差模型建立的。
以及用于，接收由从属无人机发送的双目视觉相对位置观测数据，基于预设的从属无人机的双目视觉相对位置观测模型，更新系统状态预测结果，得到从属无人机的惯导误差估计值，将惯导误差估计值发送给从属无人机。
从属无人机用于，将本地的惯导解算数据发送给中心无人机。
以及用于，接收中心无人机发送的惯导误差估计值，根据惯导误差估计值修正从属无人机的惯导解算数据，得到协同导航数据。
关于基于惯性和双目视觉的多无人机协同导航装置和系统的具体限定可以参见上文中对于基于惯性和双目视觉的多无人机协同导航方法的限定，在此不再赘述。上述基于惯性和双目视觉的多无人机协同导航装置和系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图4所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储各个无人机的惯导误差模型、多无人机的协同导航系统状态模型和无人机的双目视觉相对位置观测模型。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于惯性和双目视觉的多无人机协同导航方法。
本领域技术人员可以理解，图4中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯导解算周期。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新系统状态预测结果。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于气压高度计的高度测量值对位置分量的值进行修正。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。
基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。
根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯导解算周期。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新系统状态预测结果。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于气压高度计的高度测量值对位置分量的值进行修正。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种基于惯性和双目视觉的多无人机协同导航方法，其特征在于，所述方法包括：
基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型；
基于所述协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果；
当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新所述系统状态预测结果，得到各个无人机的惯导误差估计值；
根据所述惯导误差估计值修正对应无人机的所述惯导解算数据，得到协同导航数据。
2.根据权利要求1所述的方法，其特征在于，基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型的方式包括：
根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差；
根据各个无人机的所述惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的所述惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差；
根据所述协同导航状态转移矩阵和所述协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。
3.根据权利要求1所述的方法，其特征在于，无人机获取惯导解算数据的方式包括：
按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。
4.根据权利要求3所述的方法，其特征在于，基于所述协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果的方式包括：
按照预设的协同导航周期，根据各个无人机的所述无人机状态向量构建对应的系统状态向量，将所述系统状态向量输入所述协同导航系统状态模型，得到多无人机的系统状态预测结果；其中，所述协同导航周期大于所述惯导解算周期。
5.根据权利要求1所述的方法，其特征在于，基于对应无人机的双目视觉相对位置观测模型更新所述系统状态预测结果的方式包括：
基于无人机的双目视觉相对位置观测模型，得到所述无人机的观测矩阵；
根据所述无人机的双目视觉相对位置观测数据，基于所述观测矩阵更新所述系统状态预测结果。
6.根据权利要求1至5中任意一项所述的方法，其特征在于，所述惯导解算数据的获取方式包括：
基于惯导传感器的测量值得到无人机的状态向量的值；所述状态向量包括姿态分量、速度分量和位置分量；
基于气压高度计的高度测量值对所述位置分量的值进行修正。
7.一种基于惯性和双目视觉的多无人机协同导航装置，其特征在于，所述装置包括：
协同导航模型建立模块，用于基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型；
系统状态预测模块，用于基于所述协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果；
协同导航模块，用于当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新所述系统状态预测结果，得到各个无人机的惯导误差估计值；
惯导误差修正模块，用于根据所述惯导误差估计值修正对应无人机的所述惯导解算数据，得到协同导航数据。
8.一种多无人机系统，包括中心无人机和从属无人机，其特征在于，
所述中心无人机用于，基于预先建立的协同导航系统状态模型，根据所述中心无人机的惯导解算数据，以及根据收到的由所述从属无人机发送的惯导解算数据，得到多无人机的系统状态预测结果；所述协同导航系统状态模型是基于所述中心无人机和所述从属无人机的惯导误差模型建立的；
以及用于，接收由所述从属无人机发送的双目视觉相对位置观测数据，基于预设的所述从属无人机的双目视觉相对位置观测模型，更新所述系统状态预测结果，得到所述从属无人机的惯导误差估计值，将所述惯导误差估计值发送给所述从属无人机；
所述从属无人机用于，将本地的惯导解算数据发送给所述中心无人机；
以及用于，接收所述中心无人机发送的惯导误差估计值，根据所述惯导误差估计值修正所述从属无人机的惯导解算数据，得到协同导航数据。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述方法的步骤。
CN202110415745.4A 2021-04-19 2021-04-19 一种基于惯性和双目视觉的多无人机协同导航方法和装置 Active CN112985391B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110415745.4A CN112985391B (zh) 2021-04-19 2021-04-19 一种基于惯性和双目视觉的多无人机协同导航方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110415745.4A CN112985391B (zh) 2021-04-19 2021-04-19 一种基于惯性和双目视觉的多无人机协同导航方法和装置

Publications (2)

Publication Number Publication Date
CN112985391A true CN112985391A (zh) 2021-06-18
CN112985391B CN112985391B (zh) 2021-08-10

Family

ID=76340964

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110415745.4A Active CN112985391B (zh) 2021-04-19 2021-04-19 一种基于惯性和双目视觉的多无人机协同导航方法和装置

Country Status (1)

Country Link
CN (1) CN112985391B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114963879A (zh) * 2022-05-20 2022-08-30 中国电子科技集团公司电子科学研究院 无人机综合反制系统及综合反制方法
CN115979261A (zh) * 2023-03-17 2023-04-18 中国人民解放军火箭军工程大学 一种多惯导系统的轮转调度方法、系统、设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108151737A (zh) * 2017-12-19 2018-06-12 南京航空航天大学 一种动态互观测关系条件下的无人机蜂群协同导航方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法
CN110068335A (zh) * 2019-04-23 2019-07-30 中国人民解放军国防科技大学 一种gps拒止环境下无人机集群实时定位方法及系统
CN110147118A (zh) * 2019-05-28 2019-08-20 中国工程物理研究院电子工程研究所 无人机定位方法、控制方法、装置及无人机集群
CN111238469A (zh) * 2019-12-13 2020-06-05 南京航空航天大学 一种基于惯性/数据链的无人机编队相对导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108151737A (zh) * 2017-12-19 2018-06-12 南京航空航天大学 一种动态互观测关系条件下的无人机蜂群协同导航方法
CN109974706A (zh) * 2019-03-08 2019-07-05 哈尔滨工程大学 一种基于双运动模型的主从式多auv协同导航方法
CN110068335A (zh) * 2019-04-23 2019-07-30 中国人民解放军国防科技大学 一种gps拒止环境下无人机集群实时定位方法及系统
CN110147118A (zh) * 2019-05-28 2019-08-20 中国工程物理研究院电子工程研究所 无人机定位方法、控制方法、装置及无人机集群
CN111238469A (zh) * 2019-12-13 2020-06-05 南京航空航天大学 一种基于惯性/数据链的无人机编队相对导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
FLAVIA CAUSA.ET AL: "Multi-UAV Formation Geometries for Cooperative Navigation in GNSS-challenging Environments", 《2018 IEEE/ION POSITION，LOCATION AND NAVIGATION SYMPOSIUM（PLANS）》 *
JUAN-CARLOS TRUJILLO.ET AL: "Navigation in GNSS-challenging Environments Systems in GPS-Denied Environments", 《SENSORS 2018》 *
田俊曦: "GNSS拒止环境下的多飞行器协同导航关键技术研究", 《中国优秀硕士学位论文全文数据库·工程科技Ⅱ辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114963879A (zh) * 2022-05-20 2022-08-30 中国电子科技集团公司电子科学研究院 无人机综合反制系统及综合反制方法
CN114963879B (zh) * 2022-05-20 2023-11-17 中国电子科技集团公司电子科学研究院 无人机综合反制系统及综合反制方法
CN115979261A (zh) * 2023-03-17 2023-04-18 中国人民解放军火箭军工程大学 一种多惯导系统的轮转调度方法、系统、设备及介质

Also Published As

Publication number Publication date
CN112985391B (zh) 2021-08-10

Similar Documents

Publication Publication Date Title
CN111208492B (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
CN110873888B (zh) 定位方法、定位装置、定位设备和计算机存储介质
Campbell et al. Cooperative tracking using vision measurements on seascan UAVs
US8666661B2 (en) Video navigation
CN109059907B (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN112985391B (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN112556696B (zh) 一种对象定位方法、装置、计算机设备以及存储介质
CN111338383A (zh) 基于gaas的自主飞行方法及系统、存储介质
US20160379365A1 (en) Camera calibration device, camera calibration method, and camera calibration program
CN114689047B (zh) 基于深度学习的组合导航方法、装置、系统及存储介质
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
WO2020135183A1 (zh) 点云地图的构建方法、装置、计算机设备和存储介质
CN110598370B (zh) 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计
CN113009816B (zh) 时间同步误差的确定方法及装置、存储介质及电子装置
CN106886037B (zh) 适用于弱gnss信号条件的pos数据纠偏方法
Mostafa et al. Optical flow based approach for vision aided inertial navigation using regression trees
CN110411449B (zh) 一种航空侦察载荷目标定位方法、系统及终端设备
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
CN114897942A (zh) 点云地图的生成方法、设备及相关存储介质
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN114061570A (zh) 车辆定位方法、装置、计算机设备和存储介质
CN114264301A (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