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

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN113608555A(43)申请公布日2021.11.05(21)申请号202111168058.3(22)申请日2021.10.08(71)申请人广东博创佳禾科技有限公司地址528000广东省佛山市禅城区塱宝西路82号509室(72)发明人周定叶(74)专利代理机构佛山粤进知识产权代理事务所(普通合伙)44463代理人王余钱(51)Int.Cl.G05D1/10(2006.01)权利要求书3页说明书9页附图2页(54)发明名称多无人机协同控制方法和装置(57)摘要本发明公开了一种多无人机协同控制方法和装置，该方法包括：获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。本发明提供一种多无人机协同控制方法和装置，解决了无人机在复杂电磁环境中导航和姿态控制的问题。CN113608555ACN113608555A权利要求书1/3页1.一种多无人机协同控制方法，其特征在于，该方法包括：获取当前时间段用户输入的第一控制指令和用户输入的第二控制指令；当前时间段小于预设时间段；将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径；将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；通过加速度计、陀螺仪和磁力计进行第二无人机的姿态数据采集，并根据采集的第二无人机的姿态数据，通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态，并控制第二无人机在飞行至飞行节点时，保持稳定的姿态；所述通过拓展卡尔曼滤波器姿态解算算法得到第二无人机的姿态包括：构建置信检验模型，滤除低置信度的量测值，保证量测值的准确性；量测值的边界取值、均值、方差和置信概率的关系可表示为：Φ()‑Φ()=1‑α；其中，α为量测值的置信概率，x1和x2为量测值的边界取值，=2，量测值的，Φ()代表以为变量的正态分布函数，Φ()代表以为变量的正态分布函数；每次测量时，姿态测量的真实值均服从正态分布，根据磁力计的测量特性，当Zt(k)∈（，时，则可认为下一时刻的Zt(k)属于可靠测量，能够用于接下来的预测环节；Zt(k)为第t维的量测值；当Zt(k)∉（，时，则认为量测值属于不可靠测量，量测值利用观测方程Z(k)=H(k)X(k)+v(k)获得；其中，Z(k)、H(k)和v(k)分别为系统滤波器在k时刻的量测值、量测矩阵和量测噪声；X(k)为系统k时刻的状态变量；若且，则将量测噪声方差阵设置为第一预设值，否则将量测噪声方差阵设置为第二预设值；为当地磁场与重力方向夹角的标准值，||||为当地的地磁场强度值，||||为加速度计的测量值，δd为所测磁倾角与理论磁倾角误差的绝对值，δm为所测磁场强度与地磁场强度误差的绝对值。2.根据权利要求1所述的多无人机协同控制方法，其特征在于，所述将待检测的管道标记多个监测点，将多个监测点设置为第一无人机的飞行节点，基于飞行节点和RRT算法生成第一无人机的目标飞行路径具体包括：通过RRT算法生成通过第一无人机的起始点和目标点的多条第一飞行路径；从多条第一飞行路径筛选出一条经过所有飞行节点的规划路径，将此规划路径作为第一无人机的目标飞行路径。3.根据权利要求2所述的多无人机协同控制方法，其特征在于，所述将第一无人机设置为领航无人机，第二无人机设置为跟随无人机，第一无人机在管道外飞行，第二无人机在管道内飞行，第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围；具体包括：通过拟态物理法定义第一无2CN113608555A权利要求书2/3页人机和第二无人机的引力和斥力,将质点模型的第一无人机和第二无人机生成的标准构型；在第一无人机和第二无人机遇到障碍物时，通过双射变换改变第一无人机和第二无人机的引力和斥力的大小和方向；将第一无人机和第二无人机的构型进行重构，以实现第一无人机和第二无人机相对位置保持在预设范围，第一无人机和第二无人机之间的连线与水平面之间的倾角保持在预设范围。4.根据权利要求1所述的多无人机协同控制方法，其特征在