多无人机协同控制方法和装置

摘要

本发明公开了一种多无人机协同控制方法和装置，该方法包括：获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。本发明提供一种多无人机协同控制方法和装置，解决了无人机在复杂电磁环境中导航和姿态控制的问题。

权利要求

1.一种多无人机协同控制方法，其特征在于，该方法包括：获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；

将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；

通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：

Φ( )‑Φ( )=1‑α；

其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正态分布函数；

t

每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Z (k)∈（t t

， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z (k)为第t维的量测值；

t

当Z (k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值；

为当地磁场与重力方向夹角的标准值，|| ||为当地的地磁场强度值，|| ||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值；所述若 且 ，则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：量测噪声方差阵 满足：

其中，y为加速度的残差，ρ为y的影响因子。

2.根据权利要求1所述的多无人机协同控制方法，其特征在于，所述将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

从多条第一飞行路径刷选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

3.根据权利要求2所述的多无人机协同控制方法，其特征在于，所述将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；

在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。

4.一种多无人机协同控制装置，其特征在于，该装置包括：路径规划模块，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；

协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；

姿态控制模块，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：

Φ( )‑Φ( )=1‑α；

其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正态分布函数；

t

每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Z (k)∈（t t

， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z (k)为第t维的量测值；

t

当Z (k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值；

为当地磁场与重力方向夹角的标准值，|| ||为当地的地磁场强度值，|| ||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值；所述若 且 ，则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：量测噪声方差阵 满足：

其中，y为加速度的残差，ρ为y的影响因子。

5.根据权利要求4所述的多无人机协同控制装置，其特征在于，所述路径规划模块，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：初始路径规划模块，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

路径筛选模块，用于从多条第一飞行路径刷选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

6.根据权利要求4所述的多无人机协同控制装置，其特征在于，所述协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；

构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。

说明书

多无人机协同控制方法和装置

技术领域

[0001] 本发明涉及本发明属于无人机控制技术领域，具体涉及一种多无人机协同控制方法和装置。

背景技术

[0002] 现有的无人机可以用于农业，工业探测以及拍摄等领域，而在复杂的工业探测环境中，在GPS容易失效的情况下，无人机的导航定位成了巨大的问题，现有技术中，可以通过
雷达和相机等进行组合导航，但是此种组合导航的方式，在管道中无法应用，另外在探测过
程中，无人机将面临复杂的电磁环境，无人机的姿态控制将会受到干扰。

发明内容

[0003] 本发明的主要目的在于提供一种多无人机协同控制方法和装置，解决了无人机在复杂电磁环境中导航和姿态控制的问题。

[0004] 本申请的提出了一种多无人机协同控制方法，该方法包括：

[0005] 获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的
飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；

[0006] 将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范
围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；

[0007] 通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并
控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

[0008] 所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和
置信概率的关系可表示为：

[0009] Φ( )‑Φ( )=1‑α；

[0010] 其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正
态分布函数；

[0011] 每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Ztt t
(k)∈（ ， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z
(k)为第t维的量测值；

[0012] 当Zt(k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵
和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，
则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设
值； 为当地磁场与重力方向夹角的标准值， || ||为当地的地磁场强度值，|| ||为
加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地
磁场强度误差的绝对值。

[0013] 优选地，所述将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：

[0014] 通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

[0015] 从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

[0016] 优选地，所述将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保
持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范
围；具体包括：通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的
第一无人机和第二无人机生成的标准构型；

[0017] 在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现
第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线
与水平面之间的倾角保持在预设范围。

[0018] 优选地，所述若 且 ，则将量测噪声方差阵设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：

[0019] 量测噪声方差阵 满足：

[0020]

[0021] 其中，y为加速度的残差，ρ为y的影响因子。

[0022] 本申请还提出了一种多无人机协同控制装置，该装置包括：路径规划模块，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设
时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基
于飞行节点和RRT算法生成第一无人机的目标飞行路径；

[0023] 协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对
位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预
设范围；

[0024] 姿态控制模块，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第
二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

[0025] 所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和
置信概率的关系可表示为：

[0026] Φ( )‑Φ( )=1‑α；

[0027] 其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正
态分布函数；

[0028] 每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Ztt t
(k)∈（ ， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z
(k)为第t维的量测值；
t

[0029] 当Z (k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵
和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，
则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设
值； 为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，|| |
|为当地的地磁场强度值，|| ||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误
差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。

[0030] 优选地，所述路径规划模块，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径
具体包括：

[0031] 初始路径规划模块，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

[0032] 路径筛选模块，用于从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

[0033] 优选地，所述协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二
无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾
角保持在预设范围；具体包括：

[0034] 构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；

[0035] 构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构
型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第
二无人机之间的连线与水平面之间的倾角保持在预设范围。

[0036] 优选地，所述若 且 ，则将量测噪声方差阵设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：

[0037] 量测噪声方差阵 满足：

[0038]

[0039] 其中，y为加速度的残差，ρ为y的影响因子。

[0040] 通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于
管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁
干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同
的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。

附图说明

[0041] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本
发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以
根据这些附图示出的结构获得其他的附图。

[0042] 图1为本发明多无人机协同控制方法的流程图；

[0043] 图2为本发明多无人机协同控制方法另外一个实施例的流程图；

[0044] 图3为本发明多无人机协同控制装置的框图；

[0045] 图4为本发明多无人机协同控制装置中路径规划模块的框图。

[0046] 本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

[0047] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基
于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其
他实施例，都属于本发明保护的范围。

[0048] 需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该
特定姿态发生改变时，则该方向性指示也相应地随之改变。

[0049] 另外，在本发明中涉及第一、第二等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有第一、第二的特征
可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结
合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾
或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

[0050] 本申请的提出了一种多无人机协同控制方法，参考图1‑2，该方法包括：

[0051] 步骤1，获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无
人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；

[0052] 步骤2，将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在
预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；

[0053] 步骤3，通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿
态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

[0054] 通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信
概率的关系可表示为：

[0055] Φ( )‑Φ( )=1‑α；

[0056] 其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正
态分布函数；

[0057] 每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Ztt t
(k)∈（ ， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z
(k)为第t维的量测值；

[0058] 当Zt(k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵
和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，
则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设
值； 为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，|| |
|为当地的地磁场强度值，|| ||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误
差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。

[0059] 通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于
管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁
干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同
的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。

[0060] 具体地，现有的拓展卡尔曼滤波器包括：量测更新部分： 

[0061]

[0062] ；

[0063] 其中， 和 分别为拓展卡尔曼滤波器的进一步预测误差协方差矩阵、状态进一步预测值， 为状态的最优估计值， 和 分别为拓展卡尔
曼滤波器的增益矩阵和量测噪声方差阵； 为k时刻的量测值。

[0064]

[0065] 式中，X(k+1)为系统k时刻对k+1时刻的状态估计值，F(k)、X(k)和G(k)分别为系统k时刻的状态转移矩阵、状态变量和控制矩阵，w(k)为系统的白噪声矩阵，Z(k)、H(k)和v(k)
分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声。

[0066] 对拓展卡尔曼滤波器的任意一个时刻而言，下一时刻状态的预测值为：

[0067] 为判定量测值的可靠度，将预测向量中第t维的预测值 表示 ，计算G(k)w(k)第t维的方差。G(k)w(k)第t维的均值μ为： ；方差 为：
；

[0068] 由系统噪声w（k）的高斯特性，可认为下一时刻的最优估计值 服从均值为t
的高斯分布，即 。引入一个置信概率α，为使得Z (k)处于置信
区间内，根据准正态分布的分布函数Φ(x)，量测值的边界取值、均值、方差和置信概率的关
系可表示为：

[0069] Φ( )‑Φ( )=1‑α；

[0070] 其中，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正态分布函数；因磁力计每次测量时，真实值均服从正态分布，所以根据磁力计的测量特
t t
性，x1和x2应满足 =2 ，当Z (k)∈（ ， 时，则可认为下一时刻的Z(k)属于可靠测
t
量，能够用于接下来的预测环节；当Z (k)∉（ ， 时，则认为量测值属于不可靠测量，量测
值利用观测方程Z(k)=H(k)X(k)+v(k)获得。同时，根据正态分布“3δ”法则，当α设置为0.27%
时，置信检验可以过滤掉磁力计在出厂误差范围之外的量测值，但在实际应用场合中，为提
高融合精度，可通过进行重复试验对α值进行合理选取。

[0071] 优选地，步骤1，将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：

[0072] 步骤11，通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

[0073] 步骤12，从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

[0074] 优选地，步骤2，将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置
保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范
围；具体包括：步骤21，通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质
点模型的第一无人机和第二无人机生成的标准构型；

[0075] 步骤22，在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重
构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机
之间的连线与水平面之间的倾角保持在预设范围。

[0076] 具体地，第一无人机通过发送超声波至第二无人机，第二无人机根据超声波信号来保持与第一无人机的相对位置，从而实现第二无人机的导航与定位。而第一无人机通过
GPS或者其他导航方式进行导航和定位。

[0077] 优选地，步骤3中，若 且 ，则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：量测噪
声方差阵 满足：

[0078]

[0079] 其中，y为加速度的残差，ρ为y的影响因子。

[0080] 具体地，如果 则说明磁倾角与理论值相差较小。若,认定为当地磁场强度与地磁场强度标准值相差较小。由上述两个判
断标准综合得：若 且 ，则磁力计受到的干扰较小，
此时磁力计得测量方差选择 ；此时磁力计受到磁力干扰较小，加速度可
能会对磁力计产生影响，可以根据加速度大小来调整来自适应调整磁力计量测噪声方差
阵，进而调整第二无人机导航参数，提高航姿态控制性能；否则认为磁力计所受干扰较大，
磁力计得测量方差选择0.0015。

[0081] 本申请还提出了一种多无人机协同控制装置，参考图3‑4，该装置包括：

[0082] 路径规划模块1，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点
设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；

[0083] 协同控制模块2，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对
位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预
设范围；

[0084] 姿态控制模块3，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第
二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；

[0085] 通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于
管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁
干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同
的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。

[0086] 通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信
概率的关系可表示为：

[0087] Φ( )‑Φ( )=1‑α；

[0088] 其中，α为量测值的置信概率，x1和x2为量测值的边界取值， =2 ，量测值的，Φ( )代表以 为变量的正态分布函数，Φ( )代表以 为变量的正
态分布函数；

[0089] 每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Ztt t
(k)∈（ ， 时，则可认为下一时刻的Z (k)属于可靠测量，能够用于接下来的预测环节；Z
(k)为第t维的量测值；
t

[0090] 当Z (k)∉（ ， 时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵
和量测噪声；X(k)为系统k时刻的状态变量；若 且 ，
则将量测噪声方差阵 设置为第一预设值，否则将量测噪声方差阵 设置为第二预设
值； 为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，|| |
|为当地的地磁场强度值，|| ||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误
差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。

[0091] 优选地，路径规划模块1，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具
体包括：

[0092] 初始路径规划模块11，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；

[0093] 路径筛选模块12，用于从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。

[0094] 优选地，协同控制模块2，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无
人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角
保持在预设范围；具体包括：

[0095] 构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；

[0096] 构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构
型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第
二无人机之间的连线与水平面之间的倾角保持在预设范围。

[0097] 优选地，若 且 ，则将量测噪声方差阵设置为第一预设值，否则将量测噪声方差阵 设置为第二预设值包括：

[0098] 量测噪声方差阵 满足：

[0099]

[0100] 其中，y为加速度的残差，ρ为y的影响因子。

[0101] 以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用
在其他相关的技术领域均包括在本发明的专利保护范围内。