CN113608555B - 多无人机协同控制方法和装置 - Google Patents

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

Info

Publication number
CN113608555B
CN113608555B CN202111168058.3A CN202111168058A CN113608555B CN 113608555 B CN113608555 B CN 113608555B CN 202111168058 A CN202111168058 A CN 202111168058A CN 113608555 B CN113608555 B CN 113608555B
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
value
measurement
attitude
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.)
Active
Application number
CN202111168058.3A
Other languages
English (en)
Other versions
CN113608555A (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.)
Guangdong Bochuang Jiahe Technology Co ltd
Original Assignee
Guangdong Bochuang Jiahe Technology Co ltd
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 Guangdong Bochuang Jiahe Technology Co ltd filed Critical Guangdong Bochuang Jiahe Technology Co ltd
Priority to CN202111168058.3A priority Critical patent/CN113608555B/zh
Publication of CN113608555A publication Critical patent/CN113608555A/zh
Application granted granted Critical
Publication of CN113608555B publication Critical patent/CN113608555B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104—Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying

Landscapes

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

Abstract

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

Description

多无人机协同控制方法和装置
技术领域
本发明涉及本发明属于无人机控制技术领域，具体涉及一种多无人机协同控制方法和装置。
背景技术
现有的无人机可以用于农业，工业探测以及拍摄等领域，而在复杂的工业探测环境中，在GPS容易失效的情况下，无人机的导航定位成了巨大的问题，现有技术中，可以通过雷达和相机等进行组合导航，但是此种组合导航的方式，在管道中无法应用，另外在探测过程中，无人机将面临复杂的电磁环境，无人机的姿态控制将会受到干扰。
发明内容
本发明的主要目的在于提供一种多无人机协同控制方法和装置，解决了无人机在复杂电磁环境中导航和姿态控制的问题。
本申请的提出了一种多无人机协同控制方法，该方法包括：
获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure 852878DEST_PATH_IMAGE001
)-Φ(
Figure 436175DEST_PATH_IMAGE002
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure 309453DEST_PATH_IMAGE003
=2
Figure 627302DEST_PATH_IMAGE004
，量测值的
Figure 611438DEST_PATH_IMAGE005
，Φ(
Figure 799974DEST_PATH_IMAGE001
)代表以
Figure 844154DEST_PATH_IMAGE001
为变量的正态分布函数，Φ(
Figure 632987DEST_PATH_IMAGE002
)代表以
Figure 420814DEST_PATH_IMAGE002
为变量的正 态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt (k)∈（
Figure 729436DEST_PATH_IMAGE006
，
Figure 944517DEST_PATH_IMAGE007
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt (k)为第t维的量测值；
当Zt(k)∉（
Figure 236958DEST_PATH_IMAGE006
，
Figure 546585DEST_PATH_IMAGE007
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H (k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵 和量测噪声；X(k)为系统k时刻的状态变量；若
Figure 975292DEST_PATH_IMAGE008
且
Figure 361274DEST_PATH_IMAGE009
， 则将量测噪声方差阵
Figure 875432DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 739483DEST_PATH_IMAGE010
设置为第二预设 值；
Figure 288276DEST_PATH_IMAGE011
为当地磁场与重力方向夹角的标准值， ||
Figure 828848DEST_PATH_IMAGE012
||为当地的地磁场强度值，||
Figure 830302DEST_PATH_IMAGE013
||为 加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地 磁场强度误差的绝对值。
优选地，所述将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
优选地，所述将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
优选地，所述若
Figure 498044DEST_PATH_IMAGE008
且
Figure 635764DEST_PATH_IMAGE009
，则将量测噪声方差阵
Figure 347237DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 835987DEST_PATH_IMAGE010
设置为第二预设值包括：
量测噪声方差阵
Figure 41840DEST_PATH_IMAGE010
满足：
Figure 34067DEST_PATH_IMAGE014
其中，y为加速度的残差，ρ为y的影响因子。
本申请还提出了一种多无人机协同控制装置，该装置包括：路径规划模块，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
姿态控制模块，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure 198332DEST_PATH_IMAGE001
)-Φ(
Figure 158067DEST_PATH_IMAGE002
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure 167611DEST_PATH_IMAGE003
=2
Figure 279924DEST_PATH_IMAGE004
，量测值的
Figure 349511DEST_PATH_IMAGE005
，Φ(
Figure 547274DEST_PATH_IMAGE001
)代表以
Figure 360509DEST_PATH_IMAGE001
为变量的正态分布函数，Φ(
Figure 576596DEST_PATH_IMAGE002
)代表以
Figure 82663DEST_PATH_IMAGE002
为变量的正 态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt (k)∈（
Figure 502143DEST_PATH_IMAGE006
，
Figure 119070DEST_PATH_IMAGE007
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt (k)为第t维的量测值；
当Zt(k)∉（
Figure 205974DEST_PATH_IMAGE006
，
Figure 866632DEST_PATH_IMAGE007
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H (k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵 和量测噪声；X(k)为系统k时刻的状态变量；若
Figure 38987DEST_PATH_IMAGE008
且
Figure 928446DEST_PATH_IMAGE009
， 则将量测噪声方差阵
Figure 135436DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 717727DEST_PATH_IMAGE010
设置为第二预设 值；
Figure 361067DEST_PATH_IMAGE011
为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，||
Figure 319796DEST_PATH_IMAGE012
| |为当地的地磁场强度值，||
Figure 115713DEST_PATH_IMAGE013
||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误 差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。
优选地，所述路径规划模块，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
初始路径规划模块，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
路径筛选模块，用于从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
优选地，所述协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：
构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
优选地，所述若
Figure 868906DEST_PATH_IMAGE008
且
Figure 750274DEST_PATH_IMAGE009
，则将量测噪声方差阵
Figure 512694DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 412385DEST_PATH_IMAGE010
设置为第二预设值包括：
量测噪声方差阵
Figure 336479DEST_PATH_IMAGE010
满足：
Figure 705144DEST_PATH_IMAGE014
其中，y为加速度的残差，ρ为y的影响因子。
通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1为本发明多无人机协同控制方法的流程图；
图2为本发明多无人机协同控制方法另外一个实施例的流程图；
图3为本发明多无人机协同控制装置的框图；
图4为本发明多无人机协同控制装置中路径规划模块的框图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中涉及第一、第二等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有第一、第二的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
本申请的提出了一种多无人机协同控制方法，参考图1-2，该方法包括：
步骤1，获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
步骤2，将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
步骤3，通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure 5675DEST_PATH_IMAGE001
)-Φ(
Figure 776185DEST_PATH_IMAGE002
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure 854868DEST_PATH_IMAGE003
=2
Figure 976408DEST_PATH_IMAGE004
，量测值的
Figure 815051DEST_PATH_IMAGE005
，Φ(
Figure 174488DEST_PATH_IMAGE001
)代表以
Figure 705963DEST_PATH_IMAGE001
为变量的正态分布函数，Φ(
Figure 32908DEST_PATH_IMAGE002
)代表以
Figure 940822DEST_PATH_IMAGE002
为变量的正 态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt (k)∈（
Figure 420345DEST_PATH_IMAGE006
，
Figure 857142DEST_PATH_IMAGE007
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt (k)为第t维的量测值；
当Zt(k)∉（
Figure 671383DEST_PATH_IMAGE006
，
Figure 117408DEST_PATH_IMAGE007
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H (k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵 和量测噪声；X(k)为系统k时刻的状态变量；若
Figure 451437DEST_PATH_IMAGE008
且
Figure 59136DEST_PATH_IMAGE009
， 则将量测噪声方差阵
Figure 111406DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 344810DEST_PATH_IMAGE010
设置为第二预设 值；
Figure 798925DEST_PATH_IMAGE011
为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，||
Figure 843104DEST_PATH_IMAGE012
| |为当地的地磁场强度值，||
Figure 382670DEST_PATH_IMAGE013
||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误 差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。
通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。
具体地，现有的拓展卡尔曼滤波器包括：量测更新部分：
Figure 904918DEST_PATH_IMAGE015
Figure 564324DEST_PATH_IMAGE016
；
其中，
Figure 513826DEST_PATH_IMAGE017
和
Figure 540688DEST_PATH_IMAGE018
分别为拓展卡尔曼滤波器的进一步预测误差协方差 矩阵、状态进一步预测值，
Figure 866627DEST_PATH_IMAGE019
为状态的最优估计值，
Figure 295334DEST_PATH_IMAGE020
和
Figure 681316DEST_PATH_IMAGE010
分别为拓展卡尔 曼滤波器的增益矩阵和量测噪声方差阵；
Figure 444742DEST_PATH_IMAGE021
为k时刻的量测值。
Figure 574372DEST_PATH_IMAGE022
式中，X(k+1)为系统k时刻对k+1时刻的状态估计值，F(k)、X(k)和G(k)分别为系统k时刻的状态转移矩阵、状态变量和控制矩阵，w(k)为系统的白噪声矩阵，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声。
对拓展卡尔曼滤波器的任意一个时刻而言，下一时刻状态的预测值为：
Figure 857585DEST_PATH_IMAGE023
为判定量测值的可靠度，将预测向量中第t维的预测值
Figure 414469DEST_PATH_IMAGE024
表示
Figure 415923DEST_PATH_IMAGE025
，计算G (k)w(k)第t维的方差。G(k)w(k)第t维的均值μ为：
Figure 349244DEST_PATH_IMAGE026
；方差
Figure 470652DEST_PATH_IMAGE027
为：
Figure 464016DEST_PATH_IMAGE028
；
由系统噪声w（k）的高斯特性，可认为下一时刻的最优估计值
Figure 687187DEST_PATH_IMAGE029
服从均值为
Figure 158620DEST_PATH_IMAGE030
的高斯分布，即
Figure 150846DEST_PATH_IMAGE031
。引入一个置信概率α，为使得Zt(k)处于置信 区间内，根据准正态分布的分布函数Φ(x)，量测值的边界取值、均值、方差和置信概率的关 系可表示为：
Φ(
Figure 564379DEST_PATH_IMAGE001
)-Φ(
Figure 274846DEST_PATH_IMAGE002
)=1-α；
其中，Φ(
Figure 284390DEST_PATH_IMAGE001
)代表以
Figure 662282DEST_PATH_IMAGE001
为变量的正态分布函数，Φ(
Figure 466290DEST_PATH_IMAGE002
)代表以
Figure 913321DEST_PATH_IMAGE002
为变量 的正态分布函数；因磁力计每次测量时，真实值均服从正态分布，所以根据磁力计的测量特 性，x1和x2应满足
Figure 460977DEST_PATH_IMAGE003
=2
Figure 693375DEST_PATH_IMAGE004
，当Zt(k)∈（
Figure 933864DEST_PATH_IMAGE006
，
Figure 618923DEST_PATH_IMAGE007
时，则可认为下一时刻的Zt(k)属于可靠测 量，能够用于接下来的预测环节；当Zt(k)∉（
Figure 235849DEST_PATH_IMAGE006
，
Figure 572021DEST_PATH_IMAGE007
时，则认为量测值属于不可靠测量，量测 值利用观测方程Z(k)=H(k)X(k)+v(k)获得。同时，根据正态分布“3δ”法则，当α设置为0.27% 时，置信检验可以过滤掉磁力计在出厂误差范围之外的量测值，但在实际应用场合中，为提 高融合精度，可通过进行重复试验对α值进行合理选取。
优选地，步骤1，将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
步骤11，通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
步骤12，从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
优选地，步骤2，将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：步骤21，通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
步骤22，在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
具体地，第一无人机通过发送超声波至第二无人机，第二无人机根据超声波信号来保持与第一无人机的相对位置，从而实现第二无人机的导航与定位。而第一无人机通过GPS或者其他导航方式进行导航和定位。
优选地，步骤3中，若
Figure 248990DEST_PATH_IMAGE008
且
Figure 155766DEST_PATH_IMAGE009
，则将量测噪声方 差阵
Figure 310804DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 252215DEST_PATH_IMAGE010
设置为第二预设值包括：量测噪 声方差阵
Figure 834506DEST_PATH_IMAGE010
满足：
Figure 743425DEST_PATH_IMAGE014
其中，y为加速度的残差，ρ为y的影响因子。
具体地，如果
Figure 436575DEST_PATH_IMAGE008
则说明磁倾角与理论值相差较小。若
Figure 498072DEST_PATH_IMAGE009
,认定为当地磁场强度与地磁场强度标准值相差较小。由上述两个判 断标准综合得：若
Figure 251264DEST_PATH_IMAGE008
且
Figure 132633DEST_PATH_IMAGE009
，则磁力计受到的干扰较小， 此时磁力计得测量方差选择
Figure 878741DEST_PATH_IMAGE032
；此时磁力计受到磁力干扰较小，加速度可 能会对磁力计产生影响，可以根据加速度大小来调整来自适应调整磁力计量测噪声方差 阵，进而调整第二无人机导航参数，提高航姿态控制性能；否则认为磁力计所受干扰较大， 磁力计得测量方差选择0.0015。
本申请还提出了一种多无人机协同控制装置，参考图3-4，该装置包括：
路径规划模块1，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
协同控制模块2，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
姿态控制模块3，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
通过第一无人机的领航和第二无人机的检测，可以检测管道的损伤和其他问题，构建置信检验模型，有效地滤除磁力计中低置信度的量测值，保证量测值的准确性；而对于管道中第二无人机的飞行姿态调节和现有的噪声方差矩阵为定值矩阵，不能应对不同的磁干扰环境。本发明采用可调节的测量噪声方差矩阵，即所述磁力计误差协方差矩阵，对不同的磁场强度采用不同的噪声方差矩阵，以减小磁干扰对偏航角的影响。
通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure 529165DEST_PATH_IMAGE001
)-Φ(
Figure 718838DEST_PATH_IMAGE002
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure 87502DEST_PATH_IMAGE003
=2
Figure 388033DEST_PATH_IMAGE004
，量测值的
Figure 158543DEST_PATH_IMAGE005
，Φ(
Figure 502806DEST_PATH_IMAGE001
)代表以
Figure 624346DEST_PATH_IMAGE001
为变量的正态分布函数，Φ(
Figure 197409DEST_PATH_IMAGE002
)代表以
Figure 88005DEST_PATH_IMAGE002
为变量的正 态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt (k)∈（
Figure 619480DEST_PATH_IMAGE006
，
Figure 962737DEST_PATH_IMAGE007
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt (k)为第t维的量测值；
当Zt(k)∉（
Figure 854339DEST_PATH_IMAGE006
，
Figure 599441DEST_PATH_IMAGE007
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H (k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵 和量测噪声；X(k)为系统k时刻的状态变量；若
Figure 36238DEST_PATH_IMAGE008
且
Figure 866791DEST_PATH_IMAGE009
， 则将量测噪声方差阵
Figure 47237DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 896113DEST_PATH_IMAGE010
设置为第二预设 值；
Figure 769391DEST_PATH_IMAGE011
为当地磁场与重力方向夹角的标准值，Z(k)为系统滤波器在k时刻的量测值，||
Figure 87240DEST_PATH_IMAGE012
| |为当地的地磁场强度值，||
Figure 805797DEST_PATH_IMAGE013
||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误 差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。
优选地，路径规划模块1，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
初始路径规划模块11，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
路径筛选模块12，用于从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
优选地，协同控制模块2，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：
构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
优选地，若
Figure 525491DEST_PATH_IMAGE008
且
Figure 304092DEST_PATH_IMAGE009
，则将量测噪声方差阵
Figure 358504DEST_PATH_IMAGE010
设置为第一预设值，否则将量测噪声方差阵
Figure 146332DEST_PATH_IMAGE010
设置为第二预设值包括：
量测噪声方差阵
Figure 454953DEST_PATH_IMAGE010
满足：
Figure 670034DEST_PATH_IMAGE014
其中，y为加速度的残差，ρ为y的影响因子。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (6)

1.一种多无人机协同控制方法，其特征在于，该方法包括：
获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure DEST_PATH_IMAGE002
)-Φ(
Figure DEST_PATH_IMAGE004
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure DEST_PATH_IMAGE006
=2
Figure DEST_PATH_IMAGE008
，量测值的
Figure DEST_PATH_IMAGE010
，Φ(
Figure 570706DEST_PATH_IMAGE002
)代表以
Figure 554359DEST_PATH_IMAGE002
为变量的正态分布函数，Φ(
Figure 130834DEST_PATH_IMAGE004
)代表以
Figure 73513DEST_PATH_IMAGE004
为变量的正态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt(k)∈（
Figure DEST_PATH_IMAGE012
，
Figure DEST_PATH_IMAGE014
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt(k)为第t维的量测值；
当Zt(k)∉（
Figure 261787DEST_PATH_IMAGE012
，
Figure 880987DEST_PATH_IMAGE014
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声；X(k)为系统k时刻的状态变量；若
Figure DEST_PATH_IMAGE016
且
Figure DEST_PATH_IMAGE018
，则将量测噪声方差阵
Figure DEST_PATH_IMAGE020
设置为第一预设值，否则将量测噪声方差阵
Figure 820124DEST_PATH_IMAGE020
设置为第二预设值；
Figure DEST_PATH_IMAGE022
为当地磁场与重力方向夹角的标准值，||
Figure DEST_PATH_IMAGE024
||为当地的地磁场强度值，||
Figure DEST_PATH_IMAGE026
||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值；所述若
Figure 943326DEST_PATH_IMAGE016
且
Figure 408942DEST_PATH_IMAGE018
，则将量测噪声方差阵
Figure 199043DEST_PATH_IMAGE020
设置为第一预设值，否则将量测噪声方差阵
Figure 999378DEST_PATH_IMAGE020
设置为第二预设值包括：
量测噪声方差阵
Figure 533128DEST_PATH_IMAGE020
满足：
Figure DEST_PATH_IMAGE028
其中，y为加速度的残差，ρ为y的影响因子。
2.根据权利要求1所述的多无人机协同控制方法，其特征在于，所述将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
从多条第一飞行路径刷选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
3.根据权利要求2所述的多无人机协同控制方法，其特征在于，所述将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
4.一种多无人机协同控制装置，其特征在于，该装置包括：
路径规划模块，用于获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；
协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；
姿态控制模块，用于通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；
所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：
Φ(
Figure 197458DEST_PATH_IMAGE002
)-Φ(
Figure 670378DEST_PATH_IMAGE004
)=1-α；
其中，α为量测值的置信概率，x 1和x 2为量测值的边界取值，
Figure 708741DEST_PATH_IMAGE006
=2
Figure 515023DEST_PATH_IMAGE008
，量测值的
Figure 705964DEST_PATH_IMAGE010
，Φ(
Figure 103448DEST_PATH_IMAGE002
)代表以
Figure 878374DEST_PATH_IMAGE002
为变量的正态分布函数，Φ(
Figure 753927DEST_PATH_IMAGE004
)代表以
Figure 330532DEST_PATH_IMAGE004
为变量的正态分布函数；
每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt(k)∈（
Figure 164496DEST_PATH_IMAGE012
，
Figure 911872DEST_PATH_IMAGE014
时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt(k)为第t维的量测值；
当Zt(k)∉（
Figure 108892DEST_PATH_IMAGE012
，
Figure 258114DEST_PATH_IMAGE014
时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声；X(k)为系统k时刻的状态变量；若
Figure 482553DEST_PATH_IMAGE016
且
Figure 717225DEST_PATH_IMAGE018
，则将量测噪声方差阵
Figure 934580DEST_PATH_IMAGE020
设置为第一预设值，否则将量测噪声方差阵
Figure 187575DEST_PATH_IMAGE020
设置为第二预设值；
Figure 97762DEST_PATH_IMAGE022
为当地磁场与重力方向夹角的标准值，||
Figure 819731DEST_PATH_IMAGE024
||为当地的地磁场强度值，||
Figure 325930DEST_PATH_IMAGE026
||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值；所述若
Figure 918585DEST_PATH_IMAGE016
且
Figure 999673DEST_PATH_IMAGE018
，则将量测噪声方差阵
Figure 943359DEST_PATH_IMAGE020
设置为第一预设值，否则将量测噪声方差阵
Figure 14433DEST_PATH_IMAGE020
设置为第二预设值包括：
量测噪声方差阵
Figure 992753DEST_PATH_IMAGE020
满足：
Figure DEST_PATH_IMAGE028A
其中，y为加速度的残差，ρ为y的影响因子。
5.根据权利要求4所述的多无人机协同控制装置，其特征在于，所述路径规划模块，用于将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：
初始路径规划模块，用于通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；
路径筛选模块，用于从多条第一飞行路径刷选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。
6.根据权利要求4所述的多无人机协同控制装置，其特征在于，所述协同控制模块，用于将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：
构型构造模块，用于通过拟态物理法定义第一无人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；
构型重构模块，用于在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。
CN202111168058.3A 2021-10-08 2021-10-08 多无人机协同控制方法和装置 Active CN113608555B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111168058.3A CN113608555B (zh) 2021-10-08 2021-10-08 多无人机协同控制方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111168058.3A CN113608555B (zh) 2021-10-08 2021-10-08 多无人机协同控制方法和装置

Publications (2)

Publication Number Publication Date
CN113608555A CN113608555A (zh) 2021-11-05
CN113608555B true CN113608555B (zh) 2021-12-21

Family

ID=78310741

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111168058.3A Active CN113608555B (zh) 2021-10-08 2021-10-08 多无人机协同控制方法和装置

Country Status (1)

Country Link
CN (1) CN113608555B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN118376206B (zh) * 2024-06-19 2024-10-18 中交第一航务工程局有限公司 沉管浮运安装时船管连接姿态的监测方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104359473A (zh) * 2014-10-24 2015-02-18 南京航空航天大学 一种动态环境下无人机编队飞行的协同航迹智能规划方法
US9711851B1 (en) * 2016-02-04 2017-07-18 Proxy Technologies, Inc. Unmanned vehicle, system and method for transmitting signals
CN108459612B (zh) * 2017-02-21 2021-03-23 北京航空航天大学 基于人工势场法的无人机编队控制方法及装置
CN107608372B (zh) * 2017-08-14 2020-12-04 广西师范大学 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法
CN108536171B (zh) * 2018-03-21 2020-12-29 电子科技大学 一种多约束下多无人机协同跟踪的路径规划方法
CN109917811A (zh) * 2019-04-12 2019-06-21 中国人民解放军国防科技大学 一种无人机集群协同避障-重构处理方法
CN110243377B (zh) * 2019-07-19 2022-09-30 南京航空航天大学 一种基于分层式结构的集群飞行器协同导航方法

Also Published As

Publication number Publication date
CN113608555A (zh) 2021-11-05

Similar Documents

Publication Publication Date Title
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN106597017B (zh) 一种基于扩展卡尔曼滤波的无人机角加速度估计方法及装置
CN107289933A (zh) 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN104296776A (zh) 用于磁力计校准和补偿的系统和方法
CN104932522A (zh) 一种飞行器的自主降落方法和系统
EP1280457A1 (en) Motion-tracking
CN110955261A (zh) 一种基于ros的固定翼无人机自动驾驶仪的仿真方法
WO2017161304A1 (en) Systems, methods, and apparatus for airflow sensing and close formation flight
CN111983936A (zh) 一种无人机半物理仿真系统及测评方法
JP2016126001A (ja) 慣性航法システム及び慣性航法システムにおける磁気近点離角検出支援を提供する方法
CN111288989A (zh) 一种小型无人机视觉定位方法
Kan et al. Development of drone capable of autonomous flight using GPS
CN107289932A (zh) 基于mems传感器和vlc定位融合的单卡尔曼滤波导航装置和方法
US20200141969A1 (en) System and method for determining airspeed
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN113608555B (zh) 多无人机协同控制方法和装置
CN107246872B (zh) 基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法
Goppert et al. Invariant Kalman filter application to optical flow based visual odometry for UAVs
CN111220932B (zh) 无人机磁干扰标定方法及分布式磁异常探测系统
CN113465596B (zh) 一种基于多传感器融合的四旋翼无人机定位方法
Madany et al. Modelling and simulation of robust navigation for unmanned air systems (UASs) based on integration of multiple sensors fusion architecture
KR101821992B1 (ko) 무인비행체를 이용한 목표물의 3차원 위치 산출 방법 및 장치
CN108897333A (zh) 姿态估算方法、装置和无人机
US20230051574A1 (en) Uav nevigation calibration method, non-transitory computer-readable storage medium and uav implementing the same

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