CN105022401B - 基于视觉的多四旋翼无人机协同slam方法 - Google Patents

Abstract

本发明公开了一种基于视觉的多四旋翼无人机协同SLAM的方法，该方法针对多架安装有采集图像信息的摄像头和获取无人机的姿态角和加速度信息的惯性导航单元且可相互通信的四旋翼无人机，其中一架为主无人机，其余为辅助无人机。主无人机负责完成SLAM任务，辅助无人机悬停于固定位置对主无人机进行相对位置测量，帮助主无人机进行实时定位修正。本发明主要应用于多四旋翼无人机自主飞行定位，有助于主无人机实时调整位姿估计，提高无人机位姿估计及路标位置估计的准确性；另外，在主无人机产生意外无法完成SLAM的情况下，辅助无人机能迅速替补，继续执行任务，具有良好的机动性。

Description

基于视觉的多四旋翼无人机协同SLAM方法

技术领域

本发明属于无人机技术领域，尤其涉及一种基于视觉的多四旋翼无人机协同SLAM方法。

背景技术

无人机现已广泛应用于军事及民用等众多领域。在军事上，无人机可完成空中预警、侦察监视、通讯中继及小范围内攻击等任务。在民用方面，无人机可以完成环境监测、遥感探测、灾区搜救、农药喷洒等任务。无人机位姿的准确估计是无人机自主安全飞行的基础。目前普遍使用的定位方式是GPS定位，但在某些特殊环境下，GPS信号会缺失或受到干扰，需要不依赖GPS的定位方法。基于视觉的SLAM方法使用机载摄像头采集图像信息，惯性导航单元获取无人机的姿态角和加速度信息，是无人机自主飞行定位的一个重要研究方向。

四旋翼无人机是一种有四个螺旋桨的无人飞行器，依靠改变螺旋桨的速度来实现飞行的各种姿态，具有结构简单，体型小巧，控制灵活等优势，适合用于室内及复杂环境。在复杂环境下，单架无人机在准确率和效率上很难满足要求，可采用多架无人机相互辅助，共同完成任务，即协同。多无人机协同SLAM利用无人机之间的相对观测获得的距离和相对方位信息，进行资源共享和定位修正，从而能够获得比单架无人机定位更精确的结果。

目前SLAM技术已广泛应用于地面移动机器人领域，水下机器人与无人机领域也有了一定的研究成果。基于局部子地图等思想，在地面移动机器人的二维空间已经进行了多机器人协同SLAM的研究，而在无人机活动的三维空间内，协同SLAM技术的研究工作还有待进一步展开，将是未来国内外一个重要研究方向。

发明内容

本发明所要解决的技术问题是针对背景技术中所涉及到的缺陷，提供一种基于视觉的多四旋翼无人机协同SLAM方法。

本发明为解决上述技术问题采用以下技术方案:

基于视觉的多四旋翼无人机协同SLAM方法，包含以下步骤：

步骤1)，在四旋翼无人机集群中选取一架四旋翼无人机作为主无人机，其余作为辅助无人机；

步骤2)，利用惯性导航单元获取主无人机的姿态角和加速度信息，计算主无人机在地面坐标系下的位置；

步骤3)，辅助无人机观察主无人机的飞行变化，根据辅助无人机在地面坐标系下的位置及其与主无人机的相对位置计算得到主无人机在地面坐标系下的位置变化量；

步骤4)，根据步骤3)中得到的主无人机在地面坐标系下的位置变化量对主无人机和路标位置进行校正；

步骤5)，当主无人机将与辅助无人机之间的距离超过预设的第一距离值时，辅助无人机调整位置接近主无人机，使得其与主无人机之间的距离为预设的第二距离值；

步骤6)，当主无人机产生故障时，选取距离主无人机最近的辅助无人机作为新的主无人机，继续完成SLAM任务。

作为本发明基于视觉的多四旋翼无人机协同SLAM方法进一步的优化方案，所述步骤3)根据以下公式计算主无人机在地面坐标系下的位置变化量:

式中，分别为辅助无人机i在t-1时刻和t时刻观测与主无人机之间的相对位置，i＝1,2,...,N-1；

N-1为辅助无人机的数量；

Δt为t-1时刻和t时刻之间的间隔时间；

Δx_i为辅助无人机i观测到的主无人机在地面坐标系下的位置变化量。

作为本发明基于视觉的多四旋翼无人机协同SLAM方法进一步的优化方案，所述步骤4)根据以下公式对主无人机位置进行校正：

式中，分别为主无人机在t时刻和t-1时刻的校正位置，x_t为主无人机t时刻根据惯性导航单元得到的自身位置。

作为本发明基于视觉的多四旋翼无人机协同SLAM方法进一步的优化方案，所述步骤5)的详细步骤如下：

若在t时刻，辅助无人机i与主无人机之间的相对距离超过摄像头最大观测距离的4/5，则辅助无人机i首先向主无人机发送暂停信号，然后根据该时刻得到的相对位置信息沿最短距离迅速飞行至距主无人机最大观测距离1/5处悬停，向主无人机发送继续执行信号。

作为本发明基于视觉的多四旋翼无人机协同SLAM方法进一步的优化方案，步骤6)的详细步骤如下：

主无人机产生故障时，比较该t时刻各辅助无人机对于主无人机的相对距离选取辅助无人机n作为新的主无人机，迅速移至原主无人机位置，继续完成SLAM任务，其余辅助无人机任务保持不变。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明将N架四旋翼无人机中的一架作为主无人机，其余N-1架作为辅助无人机，辅助无人机保持悬停状态，与路标一起估计和更新，通过观察与主无人机的相对位置计算主无人机在地面坐标系下的绝对位置，有助于主无人机实时调整位姿估计，提高无人机位姿估计及路标位置估计的准确性；另外，在主无人机产生意外无法完成SLAM的情况下，辅助无人机能迅速替补，继续执行任务，具有良好的机动性。

附图说明

图1是本发明基于视觉的多四旋翼无人机SLAM方法流程图；

图2是本发明中辅助无人机i观测主无人机相对位置示意图；

图3是主无人机与辅助无人机间通信过程流程图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

如图1所示，本发明基于视觉的多四旋翼协同SLAM方法如下：

选取一架四旋翼无人机UAV1作为主无人机，其余两架四旋翼无人机UAV2和UAV3作为辅助无人机，并确保UAV2和UAV3在UAV1的观测范围以内。三架四旋翼无人机均携带有摄像头和惯性导航单元。

UAV1进行SLAM任务，路标的总个数为M，将UAV2和UAV3也作为路标进行估计和更新，利用机载摄像机采集图像信息，利用机载惯性导航单元获取无人机的姿态角和加速度信息。公式(1)为主无人机SLAM的联合概率分布

式中，x^t＝{x₁,x₂,...,x_t}，z^t＝{z₁,z₂,...,z_t}，u^t＝{u₁,u₂,...,u_t}，n^t＝{n₁,n₂,...,n_t}分别表示从初始时刻到t时刻主无人机的状态量、观测量、控制量和数据关联量的历史信息集合；M＝{m₁,m₂,...,m_N}表示N个路标构成的环境地图，各路标之间相互独立；m_N+1和m_N+2分别表示作为路标的辅助无人机UAV2和UAV3。

间隔时间Δt前后，各辅助无人机分别观测与主无人机之间的相对位置，如图2所示，得到式(2)～(5)，分别为UAV2在t-1时刻和t时刻观测到的与主无人机UAV1的相对位置和以及UAV3在t-1时刻和t时刻观测到的与主无人机UAV1的相对位置和

则UAV2和UAV3在两次观测后分别得到的UAV1在地面坐标系下飞行的位置变化Δx₁和Δx₂

UAV1根据自身惯导系统测量计算，得到t-1时刻和t时刻在地面坐标系的位置x_t-1和x_t，结合UAV2和UAV3估计位置变化，修正UAV1位置，得UAV1t时刻的最终位置估计

UAV1再对t-1时刻到t时刻观察到的路标进行校正和更新。

如图3所示，若UAV2与UAV1之间的相对距离超过摄像头最大观测距离的4/5，则UAV2向UAV1发送暂停信号，并迅速飞行至距UAV1最大观测距离1/5处悬停，向UAV1发送继续执行信号；若UAV3与UAV1之间的相对距离超过摄像头最大观测距离的4/5，同上述步骤调整UAV3悬停位置。

在UAV1完成SLAM任务前，重复上述估计、校正和调整步骤。若任务过程中UAV1产生故障，则比较该t时刻UAV2和UAV3对于UAV1的相对距离和若则将UAV2作为替补主无人机，迅速沿方向飞行至原主无人机处，取代UAV1继续完成SLAM任务；反之，则将UAV3作为替补主无人机，迅速沿方向飞行至原主无人机处，取代UAV1继续完成SLAM任务。

本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.基于视觉的多四旋翼无人机协同SLAM方法，其特征在于，包含以下步骤：

步骤1)，在四旋翼无人机集群中选取一架四旋翼无人机作为主无人机，其余作为辅助无人机；

步骤2)，利用惯性导航单元获取主无人机的姿态角和加速度信息，计算主无人机在地面坐标系下的位置；

步骤3)，辅助无人机观察主无人机的飞行变化，根据辅助无人机在地面坐标系下的位置及其与主无人机的相对位置计算得到主无人机在地面坐标系下的位置变化量，计算公式如下：

式中，分别为辅助无人机i在t-1时刻和t时刻观测与主无人机之间的相对位置，i＝1,2,…,N-1；

分别为辅助无人机i在t、t-1时刻与主无人机间的直线距离；分别为辅助无人机i与主无人机在t、t-1时刻的连线与水平面的夹角；分别为辅助无人机i与主无人机在t、t-1时刻的连线在水平面的投影与地面坐标系x轴正方向的夹角；

N-1为辅助无人机的数量；

Δt为t-1时刻和t时刻之间的间隔时间；

Δx_i为辅助无人机i观测到的主无人机在地面坐标系下的位置变化量；

步骤4)，根据步骤3)中得到的主无人机在地面坐标系下的位置变化量对主无人机和路标位置进行校正；

步骤5)，当主无人机将与辅助无人机之间的距离超过预设的第一距离值时，辅助无人机调整位置接近主无人机，使得其与主无人机之间的距离为预设的第二距离值；

步骤6)，当主无人机产生故障时，选取距离主无人机最近的辅助无人机作为新的主无人机，继续完成SLAM任务。

2.根据权利要求1所述的基于视觉的多四旋翼无人机协同SLAM方法，其特征在于，所述步骤4)根据以下公式对主无人机位置进行校正：

式中，分别为主无人机在t时刻和t-1时刻的校正位置，x_t为主无人机t时刻根据惯性导航单元得到的自身位置。

3.根据权利要求2所述的基于视觉的多四旋翼无人机协同SLAM方法，其特征在于，所述步骤5)的详细步骤如下：

若在t时刻，辅助无人机i与主无人机之间的相对距离超过摄像头最大观测距离的4/5，则辅助无人机i首先向主无人机发送暂停信号，然后根据该时刻得到的相对位置信息沿最短距离迅速飞行至距主无人机最大观测距离1/5处悬停，向主无人机发送继续执行信号。

4.根据权利要求3所述的基于视觉的多四旋翼无人机协同SLAM方法，其特征在于，步骤6)的详细步骤如下：

主无人机产生故障时，比较该t时刻各辅助无人机对于主无人机的相对距离选取辅助无人机n作为新的主无人机，迅速移至原主无人机位置，继续完成SLAM任务，其余辅助无人机任务保持不变。