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

摘要

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

权利要求

1.一种基于惯性和双目视觉的多无人机协同导航方法，其特征在于，所述方法包括：基于预设的中心无人机和从属无人机的惯导误差模型，建立多无人机的协同导航系统状态模型；所述协同导航系统状态模型的建立方式包括：根据各个从属无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差；根据各个从属无人机的所述惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个从属无人机的所述惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差；根据所述协同导航状态转移矩阵和所述协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型；

其中，所述协同导航系统状态模型的系统状态向量为：表示第架无人机的误差状态向量，

所述协同导航系统状态模型的系统状态预测方程为：表示  时刻的系统状态向量， 表示对 时刻的系统状态向量的预测结果， 为系统状态转移矩阵，

表示第 架无人机的误差状态方程的状态转移矩阵；

由所述中心无人机接收从属无人机发送的惯导解算数据，基于所述协同导航状态模型和所述从属无人机的惯导解算数据，得到多无人机的系统状态预测结果；

当收到从属无人机的双目视觉相对位置观测数据时，由所述中心无人机基于对应从属无人机的双目视觉相对位置观测模型更新所述系统状态预测结果，得到系统内所有无人机的惯导误差估计值；

由系统内所有无人机根据所述惯导误差估计值修正自身的所述惯导数据，得到协同导航数据。

2.根据权利要求1所述的方法，其特征在于，无人机获取惯导数据的方式包括：按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。

3.根据权利要求2所述的方法，其特征在于，由所述中心无人机接收从属无人机发送的惯导解算数据，基于所述协同导航状态模型和所述从属无人机的惯导解算数据，得到多无人机的系统状态预测结果的方式包括：由所述中心无人机按照预设的协同导航周期，根据各个从属无人机的所述无人机状态向量构建对应的系统状态向量，将所述系统状态向量输入所述协同导航状态模型，得到多无人机的系统状态预测结果；其中，所述协同导航周期大于所述惯导解算周期。

4.根据权利要求1所述的方法，其特征在于，由所述中心无人机基于对应从属无人机的双目视觉相对位置观测模型更新所述系统状态预测结果的方式包括：基于从属无人机的双目视觉相对位置观测模型，得到所述从属无人机的观测矩阵；

根据所述从属无人机的双目视觉相对位置观测数据，由所述中心无人机基于所述观测矩阵更新所述系统状态预测结果。

5.根据权利要求1至4中任意一项所述的方法，其特征在于，所述惯导解算数据的获取方式包括：

基于惯导传感器的测量值得到无人机的状态向量的值；所述状态向量包括姿态分量、速度分量和位置分量；

基于气压高度计的高度测量值对所述位置分量的值进行修正。

6.一种基于惯性和双目视觉的多无人机协同导航装置，其特征在于，所述装置包括：协同导航模型建立模块，用于基于预设的中心无人机和从属无人机的惯导误差模型，建立多无人机的协同导航系统状态模型；所述协同导航系统状态模型的建立方式包括：根据各个从属无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差；根据各个从属无人机的所述惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个从属无人机的所述惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差；根据所述协同导航状态转移矩阵和所述协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型；

系统状态预测模块，用于由所述中心无人机接收从属无人机发送的惯导解算数据，基于所述协同导航状态模型和所述从属无人机的惯导解算数据，得到多无人机的系统状态预测结果；

其中，所述协同导航系统状态模型的系统状态向量为：其中， 表示第 架无人机的误差状态向量，所述协同导航系统状态模型的系统状态预测方程为：表示  时刻的系统状态向量， 表示对 时刻的系统状态向量的预测结果， 为系统状态转移矩阵，

表示第 架无人机的误差状态方程的状态转移矩阵；

协同导航模块，用于当收到从属无人机的双目视觉相对位置观测数据时，由所述中心无人机基于对应从属无人机的双目视觉相对位置观测模型更新所述系统状态预测结果，得到系统内所有无人机的惯导误差估计值；

惯导误差修正模块，用于由系统内所有无人机根据所述惯导误差估计值修正自身的所述惯导数据，得到协同导航数据；所述系统状态预测模块和所述协同导航模块均设置在所述中心无人机，所述惯导误差修正模块设置在从属无人机。

7.一种多无人机系统，包括中心无人机和从属无人机，其特征在于，所述中心无人机用于，基于预先建立的协同导航状态模型，根据所述中心无人机的惯导数据，以及根据收到的由所述从属无人机发送的惯导数据，得到多无人机的系统状态预测结果；所述协同导航状态模型是基于所述中心无人机和所述从属无人机的惯导误差模型建立的；所述协同导航系统状态模型的建立方式包括：根据各个从属无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差；根据各个从属无人机的所述惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个从属无人机的所述惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差；根据所述协同导航状态转移矩阵和所述协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型；

以及用于，接收由所述从属无人机发送的双目视觉相对位置观测数据，基于对应从属无人机的双目视觉相对位置观测模型更新所述系统状态预测结果，得到系统内所有无人机的惯导误差估计值，将所述惯导误差估计值发送给各个从属无人机；

其中，所述协同导航系统状态模型的系统状态向量为：其中， 表示第 架无人机的误差状态向量，所述协同导航系统状态模型的系统状态预测方程为：表示k‑1时刻的系统状态向量， 表示对k时刻的系统状态向量的预测结果，为系统状态转移矩阵，

表示第 架无人机的误差状态方程的状态转移矩阵；

所述从属无人机用于，将本地的惯导解算数据发送给所述中心无人机；

以及用于，接收所述中心无人机发送的惯导误差估计值，根据所述惯导误差估计值修正自身的惯导解算数据，得到协同导航数据。

8.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至5中任一项所述方法的步骤。

9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至5中任一项所述方法的步骤。

说明书

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

技术领域

[0001] 本申请涉及平台协同导航技术领域，特别是涉及一种基于惯性和双目视觉的多无人机协同导航方法和装置。

背景技术

[0002] 目前无人机普遍采用惯性/卫星组合导航技术。然而卫星导航信号一方面在城市巷道、峡谷等环境会受到遮挡，另一方面容易被干扰。因此无人机不能仅依赖卫星作为导航
手段，而是需要在无法获得卫星导航信号的环境中，基于惯性导航等技术实现自主导航，以
适应各种复杂的工作环境。

[0003] 对于无人机集群来说，可以利用无人机之间相对观测的协同导航技术提高无人机的惯性导航定位精度。协同导航是一种利用多运动体之间的相对观测和导航信息交互实现
导航资源共享的新型导航技术。目前针对无人机集群的协同导航技术存在以下特点：第一，
大多局限于协同空中加油、地面目标跟踪等特定的应用场景，无人机的数量多为两架，关注
的多是无人机之间的相对导航信息；第二，无人机集群仍没有完全脱离对卫星导航的依赖，
甚至完全基于卫星导航功能实现协同导航，不适用于无卫星信号的工作环境；第三，无人机
之间的相对观测采用距离或相对方位测量，距离测量传感器常见的是超宽带测距传感器，
相对方位测量传感器为单目相机，能够提供的导航数据精度较低。

发明内容

[0004] 基于此，有必要针对上述技术问题，提供一种能够在无卫星导航信号的工作环境中实现高精度多无人机协同导航的基于惯性和双目视觉的多无人机协同导航方法和装置。

[0005] 一种基于惯性和双目视觉的多无人机协同导航方法，所述方法包括：

[0006] 基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0007] 基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0008] 当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0009] 根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0010] 其中一个实施例中，基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型的方式包括：

[0011] 根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。

[0012] 根据各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方
差。

[0013] 根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。

[0014] 其中一个实施例中，无人机获取惯导解算数据的方式包括：

[0015] 按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。

[0016] 其中一个实施例中，基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果的方式包括：

[0017] 按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状态预测结果。
其中，协同导航周期大于惯导解算周期。

[0018] 其中一个实施例中，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果的方式包括：

[0019] 基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。

[0020] 根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新系统状态预测结果。

[0021] 其中一个实施例中，惯导解算数据的获取方式包括：

[0022] 基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。

[0023] 基于气压高度计的高度测量值对位置分量的值进行修正。

[0024] 一种基于惯性和双目视觉的多无人机协同导航装置，所述装置包括：

[0025] 协同导航模型建立模块，用于基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0026] 系统状态预测模块，用于基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0027] 协同导航模块，用于当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估
计值。

[0028] 惯导误差修正模块，用于根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0029] 一种多无人机系统，包括中心无人机和从属无人机。

[0030] 中心无人机用于，基于预先建立的协同导航系统状态模型，根据中心无人机的惯导解算数据，以及根据收到的由从属无人机发送的惯导解算数据，得到多无人机的系统状
态预测结果。协同导航系统状态模型是基于中心无人机和从属无人机的惯导误差模型建立
的。

[0031] 以及用于，接收由从属无人机发送的双目视觉相对位置观测数据，基于预设的从属无人机的双目视觉相对位置观测模型，更新系统状态预测结果，得到从属无人机的惯导
误差估计值，将惯导误差估计值发送给从属无人机。

[0032] 从属无人机用于，将本地的惯导解算数据发送给中心无人机。

[0033] 以及用于，接收中心无人机发送的惯导误差估计值，根据惯导误差估计值修正从属无人机的惯导解算数据，得到协同导航数据。

[0034] 一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

[0035] 基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0036] 基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0037] 当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0038] 根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0039] 一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：

[0040] 基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0041] 基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0042] 当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0043] 根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0044] 与现有技术相比，上述一种基于惯性和双目视觉的多无人机协同导航方法、装置、系统、计算机设备和存储介质，基于各个无人机的惯导误差模型建立多无人机的协同导航
系统状态模型，基于协同导航系统状态模型预测多无人机的系统状态，并根据无人机的双
目视觉相对位置观测数据对预测的系统状态进行更新，并估计对应无人机的惯导误差估计
数据，由对应的无人机根据惯导误差估计值修正其惯导解算数据，实现对该无人机的协同
导航。本申请利用双目视觉技术得到无人机之间的相对位置观测数据，并在此基础上修正
各无人机的惯性导航系统误差。由于双目视觉得到的相对位置观测信息包含了无人机间的
距离和相对方位，因此采用双目相机作为机间测量传感器，相当于同时采用了超宽带测距
传感器和单目相机。因此，本申请实现的多无人机协同导航效果优于仅基于距离测量的协
同导航方式或仅基于相对方位测量的协同导航方式。此外，由于本申请对于各架无人机分
别建立其惯导误差模型，因此适用于多架无人机采用不同精度的惯性导航系统的情况。

附图说明

[0045] 图1为一个实施例中一种基于惯性和双目视觉的多无人机协同导航方法的步骤图；

[0046] 图2为一个实施例中一种基于惯性和双目视觉的多无人机协同导航方法的流程示意图；

[0047] 图3为一个实施例中两架无人机的相对位置示意图；

[0048] 图4为一个实施例中计算机设备的内部结构图。

具体实施方式

[0049] 为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不
用于限定本申请。

[0050] 在一个实施例中，如图1所示，提供了一种基于惯性和双目视觉的多无人机协同导航方法，包括以下步骤：

[0051] 步骤102，基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0052] 具体地，各个无人机的惯导误差模型可以根据各个无人机配备的惯导系统，在选定的坐标系下基于现有的各种建模方式得到。惯导误差模型根据上一时刻的惯导解算误差
和惯性传感器数据预测当前时刻的惯导解算误差。

[0053] 协同导航系统状态模型则是基于参与协同导航的各个无人机的惯导误差模型建立的，其输入为各个无人机当前时刻的误差状态以及惯性传感器数据，输出为对下一时刻
各个无人机的误差状态的预测结果，包括均值和方差。

[0054] 步骤104，基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0055] 协同导航系统状态模型则是基于参与协同导航的各个无人机的惯导误差模型建立的，其输入为各个无人机当前时刻的误差状态以及惯性传感器数据，输出为对下一时刻
各个无人机的误差状态的预测结果，包括均值和方差。

[0056] 步骤106，当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0057] 本实施例中的无人机上配有双目视觉传感器，获取该无人机和本无人机系统中其他无人机的相对位置观测数据。对于获取到的相对位置观测数据，可以将其发送到本无人
机系统中指定的、具有相应计算能力的无人机上，也可以发送到本无人机系统外的其他设
备上进行处理，如地面控制中心等。收到双目视觉相对位置观测数据的设备，基于观测无人
机的双目视觉相对位置观测模型，对双目视觉相对位置观测数据进行处理，获得观测无人
机和被观测无人机的相对位置信息，根据这一相对位置信息，更新步骤104中获得的系统状
态预测结果。

[0058] 将更新后的系统状态估计结果分发给各无人机。

[0059] 步骤108，根据惯导误差估计值修正对应无人机的惯导解算结果，得到协同导航结果，实现高精度的多无人机协同导航。

[0060] 本实施例利用双目视觉技术得到无人机之间的相对位置观测数据，并在此基础上修正各无人机的惯性导航系统误差。由于双目视觉得到的相对位置观测信息包含了无人机
间的距离和相对方位，因此采用双目相机作为机间测量传感器，相当于同时采用了超宽带
测距传感器和单目相机。因此，本实施例实现的多无人机协同导航效果优于仅基于距离测
量的协同导航方式或基于相对方位测量的协同导航方式。

[0061] 其中一个实施例中，如图2所示，提供了一种基于惯性和双目视觉的多无人机协同导航方法，应用于多无人机系统。该多无人机系统包括中心无人机和从属无人机。所述方法
包括以下步骤：

[0062] 步骤202，获取各个无人机的惯导解算数据。由多无人机系统中的所有无人机分别基于惯导传感器的测量值，按照预设的惯导解算周期T1，各自进行惯导解算，得到各架无人
机的状态向量。无人机的状态向量包括姿态分量、速度分量和位置分量。

[0063] 具体地，在进行惯导解算时，首先需要建立相应的坐标系，包括：

[0064] 当地地理坐标系N，本实施例中以选取北东地（NED）坐标系为当地地理坐标系N为例进行说明。N坐标系原点与地球表面一点固连，x轴沿当地子午线指向北，z轴沿当地重力
方向向下，y轴由右手法则确定。

[0065] 世界参考坐标系G，也称全局坐标系，一般情况下与地球固连，系统最终输出的导航状态，如位置、速度、姿态角一般在此坐标系中表示。在本实施例中，采用一架无人机初始
时刻的当地地理坐标系作为世界参考坐标系。

[0066] 载体坐标系B，以无人机重心 为原点； 轴沿无人机纵轴指向无人机头部；轴垂直于无人机纵向对称面，从尾部向头部看过去，指向右方； 轴由右手法则确定，位于
无人机纵向对称面内指向下方。

[0067] 地心地固坐标系E，该坐标系与地球固连，坐标原点位于地球球心，X轴指向赤道与参考子午线的交点，Z轴沿地球自转轴指向北极点，Y轴由右手法则确定。此坐标系是用于G
系与N系转换的中间坐标系。

[0068] 基于上述坐标系的定义方式，本实施例中的坐标系转换关系为：

[0069] 假设中心无人机的初始位置用纬度 、经度 、高度 表示，将其作为世界坐标系G的原点。其在E系中的坐标位置则为：

[0070] ，

[0071] 其中， 为该点的卯酉圈半径， 为地球曲率， 为地球长半轴，

[0072] 。

[0073] E系到G系(即初始点的N系)的旋转矩阵 如下：

[0074]

[0075] 则初始点在E系下的位置坐标可用下式转换到G系下：

[0076]

[0077] 在确定世界参考坐标系后， 、 为常值矩阵。

[0078] E系到N系的旋转矩阵 与 具有相同的形式，设N系原点的经度坐标为 ，纬度坐标为 ，则旋转矩阵为

[0079]

[0080] 设任意点在E系下的坐标为 ，则在G系下的坐标为

[0081] 。

[0082] 已知无人机在N系下的速度为 ，则无人机在G系下的速度 可以表示为

[0083] 。

[0084] 设B系按“1‑2‑3”的顺序依次转动角度 可以与N系对应轴平行，则可以得到旋转矩阵 。

[0085] 则G系到B系的旋转矩阵为

[0086]

[0087] 由 可以得到旋转四元数 。

[0088] 基于上述坐标系设置，各架无人机在世界参考坐标系下进行惯导解算。具体过程为：假设无人机k‑1时刻在G系下的位置为 ，速度为 ，姿态四元数为
，k时刻捷联式陀螺输出角速度为 ，加速度计输出比力为 ，解算周期为 。

[0089] 姿态分量的获取方式为：由 可以得到旋转矩阵 。地球自转角速度为 。

[0090]

[0091]

[0092]

[0093] ， 为四元数乘法运算符。

[0094] 速度分量的获取方式为：由更新后的四元数 可以得到更新后的旋转矩阵。E系下的坐标为 ，由此可以先得到纬经高坐标，
再可以得到 。重力加速度 。

[0095]

[0096]

[0097] 位置分量的获取方式为：

[0098]

[0099] 进一步地，为修正惯导解算高度通道的误差，各架无人机上均配备气压高度计。步骤202之后，还包括：

[0100] 步骤203，基于气压高度计的高度测量值对位置分量的值进行修正。本步骤的目的是引入其他高度源对高度进行修正，以解决惯导解算的高度通道发散问题。假设气压高度
计的高度测量值为 ，则根据坐标转换关系更新状态向量 中的 。

[0101] 步骤204，根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的惯导误差状态转移矩阵，构建多无人机的
协同导航状态转移矩阵，根据各个无人机的惯导状态输入噪声方差，构建多无人机的协同
导航系统输入噪声方差。根据协同导航状态转移矩阵和协同导航系统输入噪声方差，建立
多无人机的协同导航系统状态模型。

[0102] 具体地，根据步骤202‑步骤203的结算结果，可以将每个无人机的状态向量定义为：

[0103]

[0104] 则误差状态向量为：

[0105]

[0106] 其中， 是角度误差向量，由误差四元数定义：

[0107]

[0108] 基于上述定义，协同导航系统状态模型的连续系统状态方程为：

[0109]

[0110] 其中，

[0111]

[0112]

[0113] 为由姿态四元数 得到的旋转矩阵， 、 分别为陀螺和加速度计的输出 、 在G系下的投影， 表示反对称矩阵， 为陀螺噪声、加速
度计噪声。

[0114] 离散化的协同导航系统状态模型为：

[0115]

[0116] 差分方程的状态递推矩阵 采用二阶近似计算

[0117]

[0118] 假设连续状态下状态方程输入噪声 的方差为 ，则离散化后系统噪声方差  的计算方法为：

[0119] ， ， ，

[0120]

[0121] 假设协同导航系统内有n架无人机，则总的状态向量为：

[0122]

[0123] 对应的状态转移矩阵为：

[0124]

[0125] 离散化后的系统输入噪声方差为：

[0126]

[0127] 步骤206，按照预设的协同导航周期T2，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状态模型，得到多无人机的系统状
态预测结果。其中，协同导航周期T2>惯导解算周期T1。

[0128] 基于协同导航系统状态模型进行状态预测，基于k‑1时刻的各个无人机的状态向量构建对应的系统状态向量 ，得到对下一时刻（k时刻）的系统状态向量预测结果
：

[0129]

[0130] 步骤208，建立无人机的双目视觉相对位置观测模型。

[0131] 以无人机i（即观测者）利用双目视觉传感器对无人机j（即被观测者）进行观测，获得无人机j的相对位置为例进行说明（图2中，i=2）。如图3所示，设无人机i，j在全局坐标系
（G系）下的位置分别为 ，无人机i的姿态四元数为 ，则无人机i对于无人机j
的双目视觉相对位置观测模型为：

[0132]

[0133] 线性化误差方程为：

[0134]

[0135] 其中，

[0136]

[0137]

[0138]

[0139] 步骤210，当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0140] 基于步骤208中所述的双目视觉相对位置观测模型，得到无人机的观测矩阵，并更新系统状态预测结果。具体地，系统状态预测结果的预测方差计算方式为：

[0141]

[0142] 表示状态转移矩阵，并进行卡尔曼增益计算 ， 表示观测矩阵， 为测量值噪声方差：

[0143]

[0144] 步骤212，无人机i根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0145] 则对无人机i的惯导误差估计值的计算方式为：

[0146]

[0147] 后验估计值方差的计算方式为：

[0148]

[0149] 由于惯导解算周期T1和协同导航周期T2的长度不同，中心无人机在没有收到相对位置观测时仅进行状态预测，当有相对位置观测时，进行卡尔曼滤波更新。

[0150] 应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的
执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分
步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行
完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，
而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

[0151] 其中一个实施例中，提供了一种基于惯性和双目视觉的多无人机协同导航装置，所述装置包括：

[0152] 协同导航模型建立模块，用于基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0153] 系统状态预测模块，用于基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0154] 协同导航模块，用于当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估
计值。

[0155] 惯导误差修正模块，用于根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0156] 其中一个实施例中，协同导航模型建立模块用于根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各个无人机的
惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无人机的惯导
状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导航状态转移
矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。

[0157] 其中一个实施例中，还包括惯导解算数据获取模块，用于按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。

[0158] 其中一个实施例中，系统状态预测模块用于按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输入协同导航系统状
态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯导解算周期。

[0159] 其中一个实施例中，协同导航模块用于基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测数据，基于观测矩阵更新
系统状态预测结果。

[0160] 其中一个实施例中，惯导解算数据获取模块用于基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于气压高度计的高
度测量值对位置分量的值进行修正。

[0161] 其中一个实施例中，提供了一种多无人机系统，包括中心无人机和从属无人机。

[0162] 中心无人机用于，基于预先建立的协同导航系统状态模型，根据中心无人机的惯导解算数据，以及根据收到的由从属无人机发送的惯导解算数据，得到多无人机的系统状
态预测结果。协同导航系统状态模型是基于中心无人机和从属无人机的惯导误差模型建立
的。

[0163] 以及用于，接收由从属无人机发送的双目视觉相对位置观测数据，基于预设的从属无人机的双目视觉相对位置观测模型，更新系统状态预测结果，得到从属无人机的惯导
误差估计值，将惯导误差估计值发送给从属无人机。

[0164] 从属无人机用于，将本地的惯导解算数据发送给中心无人机。

[0165] 以及用于，接收中心无人机发送的惯导误差估计值，根据惯导误差估计值修正从属无人机的惯导解算数据，得到协同导航数据。

[0166] 关于基于惯性和双目视觉的多无人机协同导航装置和系统的具体限定可以参见上文中对于基于惯性和双目视觉的多无人机协同导航方法的限定，在此不再赘述。上述基
于惯性和双目视觉的多无人机协同导航装置和系统中的各个模块可全部或部分通过软件、
硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器
中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个
模块对应的操作。

[0167] 在一个实施例中，提供了一种计算机设备，该计算机设备可以是服务器，其内部结构图可以如图4所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和
数据库。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包
括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据
库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算
机设备的数据库用于存储各个无人机的惯导误差模型、多无人机的协同导航系统状态模型
和无人机的双目视觉相对位置观测模型。该计算机设备的网络接口用于与外部的终端通过
网络连接通信。该计算机程序被处理器执行时以实现一种基于惯性和双目视觉的多无人机
协同导航方法。

[0168] 本领域技术人员可以理解，图4中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备
可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。

[0169] 在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：

[0170] 基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0171] 基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0172] 当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0173] 根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0174] 在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据各
个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个无
人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同导
航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模型。

[0175] 在一个实施例中，处理器执行计算机程序时还实现以下步骤：按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。

[0176] 在一个实施例中，处理器执行计算机程序时还实现以下步骤：按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量输
入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于惯
导解算周期。

[0177] 在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测数
据，基于观测矩阵更新系统状态预测结果。

[0178] 在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于气
压高度计的高度测量值对位置分量的值进行修正。

[0179] 在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：

[0180] 基于各个无人机的惯导误差模型，建立多无人机的协同导航系统状态模型。

[0181] 基于协同导航系统状态模型和各个无人机的惯导解算数据，得到多无人机的系统状态预测结果。

[0182] 当收到无人机的双目视觉相对位置观测数据时，基于对应无人机的双目视觉相对位置观测模型更新系统状态预测结果，得到各个无人机的惯导误差估计值。

[0183] 根据惯导误差估计值修正对应无人机的惯导解算数据，得到协同导航数据。

[0184] 在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据各个无人机的惯导误差模型，分别得到对应的惯导误差状态转移矩阵和惯导状态输入噪声方差。根据
各个无人机的惯导误差状态转移矩阵，构建多无人机的协同导航状态转移矩阵，根据各个
无人机的惯导状态输入噪声方差，构建多无人机的协同导航系统输入噪声方差。根据协同
导航状态转移矩阵和协同导航系统输入噪声方差，建立多无人机的协同导航系统状态模
型。

[0185] 在一个实施例中，计算机程序被处理器执行时还实现以下步骤：按照预设的惯导解算周期，由无人机进行惯导解算，得到对应的无人机状态向量。

[0186] 在一个实施例中，计算机程序被处理器执行时还实现以下步骤：按照预设的协同导航周期，根据各个无人机的无人机状态向量构建对应的系统状态向量，将系统状态向量
输入协同导航系统状态模型，得到多无人机的系统状态预测结果。其中，协同导航周期大于
惯导解算周期。

[0187] 在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于无人机的双目视觉相对位置观测模型，得到无人机的观测矩阵。根据无人机的双目视觉相对位置观测
数据，基于观测矩阵更新系统状态预测结果。

[0188] 在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于惯导传感器的测量值得到无人机的状态向量的值。状态向量包括姿态分量、速度分量和位置分量。基于
气压高度计的高度测量值对位置分量的值进行修正。

[0189] 本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机
可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，
本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可
包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（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）等。

[0190] 以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛
盾，都应当认为是本说明书记载的范围。

[0191] 以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来
说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护
范围。因此，本申请专利的保护范围应以所附权利要求为准。