CN113608556A - 一种基于多传感器融合的多机器人相对定位方法 - Google Patents

Abstract

本发明涉及一种基于多传感器融合的多机器人相对定位方法，旨在实现多无人机协同运输时的构型感知功能，为控制模块进行阵型保持提供位姿信息。所采用的技术方案包括以下步骤：单机对视觉可见范围内其他无人机的视觉位姿估计；IMU信息与视觉估计和UWB测距信息融合；多机间定位结果融合。由于采用了确定尺寸的合作标识，使得视觉定位的鲁棒性、精度和计算速度均得以提高，加快观测更新频率；采用了IMU预测+UWB测距+视觉图像观测的多传感器组合解算，使得多机器人间的定位精度得以进一步提高，并且可以在不依靠GPS信息的情况下保持***相对构型。采用了环形网络拓扑进行阵型估计，使得***在部分观测失效的情况下仍能保持运作，具有一定的冗余性。

Description

一种基于多传感器融合的多机器人相对定位方法

技术领域

本发明属于无人机感知与定位研究领域，涉及一种基于多传感器融合的多机器人相对定位方法，具体为一种基于多传感器融合的多无人机协同运输***相互定位方法。

背景技术

多机器人协同***具有方便组合、灵活、冗余等特点，在近年来取得了长足的发展。在空中搬运作业方面，相较于单架大飞机调运，多无人机***更是具有单机成本低、维护方便的优点。根据载荷重量的不同，通过增加单元数量，即可增加***的负载能力。多无人机协同运输***由无人机单元、柔性系绳、载荷连接装置和地面站组成。每架无人机单元上均搭载有相机、惯导传感器、超宽带(UWB)信标、计算单元和控制***，相互间能进行实时通信。在工作时，无人机单元间相距10～20m。地面站主要用于监视无人机状态，在出现安全隐患时及时切换到人工控制。

多无人机协同运输***相互定位方式有多种，目前主流的方法是使用全球定位***(GPS)/北斗卫星导航***(BDS)等卫星定位***定位绝对位置，再进行相对位置解算，目前已经在行业展开应用。GPS等卫星定位***民用精度通常在米级，只能满足单机作业的定位要求。GPS-RTK(GPS-Real-time-kinematic)是一种采用载波相位动态实时差分的定位方法，该方法的定位精度可以达到厘米级，但由于设备价格较为昂贵且需要地面基站等原因，因此无法得到广泛应用。此外，多无人机协同运输相对无人机编队而言，是一个强耦合的***，系绳上的张力分配直接影响整个***的续航时间，因此对相对构型、高度差更为敏感，这就对相对位姿定位提出了更高的要求。为了解决上述问题，我们提出了基于惯导、超宽带(UWB)技术和视觉的多传感器融合定位方案，通过冗余求解使得相对定位误差最小。此外，由于上述传感器都不依赖外部基站，故***能在GPS受干扰的情况下正常工作。

在机器人定位领域，多传感器融合的定位方案已被行业广泛采用。例如，申请号为CN202011388909.0的中国专利提出了一种基于IMU数据、轮速里程计数据、图像数据、激光数据和UWB数据的机器人定位方法，通过对IMU数据预积分、与图像观测对齐等，与激光雷达等传感器数据融合，得到鲁棒的位姿数据。申请号为CN202011071053.4的中国专利提出了一种基于图像、深度图、IMU数据和2D激光雷达的融合定位方法。可见，单机的高精度、高鲁棒性定位方案已经相对成熟，但是多机高精度相对定位方面还存在较大的缺口。无人机在三维欧氏空间内***，且相距较远(大于一般激光雷达的工作范围)，又对传感器的选型和融合方案提出了更多的限制。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于多传感器融合的多机器人相对定位方法，在于设计一种基于多传感器融合的相互定位方法。旨在实现多无人机协同运输时的构型感知功能，为控制模块进行阵型保持提供位姿信息。

技术方案

一种基于多传感器融合的多机器人相对定位方法，其特征在于步骤如下：

步骤1、单元机器人对视觉可见范围内其他机器人的视觉位姿估计：

以AprilTag作为合作标识对每一个机器人进行标识；

***内任一单元机器人只对特定的对象进行检测和跟踪；

任一单元机器人从机载相机读入图像，使用OpenCV将像素信息转化为cv::MAT矩阵形式，并将图像转为灰度图；

位姿初始化：使用AprilTag算法进行全图搜索，找到目标合作标识在图像平面的位置

使用PnP方法求解从相机到合作标识的齐次矩阵

对后续输入图像进行裁剪：根据上一时刻合作标识在图像中的位置

裁剪下一时刻输入图像，仅保留合作标识附近的图像区域；对图像区域寻找合作标识，计算合作标识在图像平面的位置

和齐次矩阵

即视觉解算结果，广播在ROS通信网络中；

如果在图像区域中未找到合作标识时，在下一帧图像中扩大剪裁区域直至能够找到合作标识；

步骤2、IMU信息与视觉估计和UWB测距信息融合：

解算任一单元机器人IMU中的信息，获得位姿增量ΔR_ij,Δv_ij,Δp_ij和协方差矩阵δΦ_ij,δv_ij,δp_ij，作为预测单元机器人在下一时刻的位置和协方差即惯导预测结果；使用UWB标签进行DS-TWR测距，获取各机间距离信息；

各单元机器人将解算出本机的惯导数据和UWB测距信息广播在ROS通信网络中；

各单元将步骤1中视觉解算结果和UWB测距信息结果作为观测，与惯导预测结果进行融合滤波，将融合结果广播在ROS通信网络中；如果出现长时间视觉观测失效或结果协方差大于阈值的情况，为滤波结果贴上Exception标签，标记为不可信的数据；

步骤3、多机器人间定位结果融合：

领航的单元机器人根据融合后的相对位姿数据计算当前***相对构型，环形网络拓扑最多容忍一个成员定位失效；

如果存在两个或以上成员定位失效，或通信错误时，***暂停执行当前任务，保持悬停状态并在地面站上报错。

无论何时，地面站具有最高优先级，对当前任务状态进行超控。

所述合作标识采用黑白两色，外边框大小为0.5m。

所述读入图像的分辨率大于1920*1080。

有益效果

本发明提出的一种基于多传感器融合的多机器人相对定位方法，旨在实现多无人机协同运输时的构型感知功能，为控制模块进行阵型保持提供位姿信息。所采用的技术方案包括以下步骤：单机对视觉可见范围内其他无人机的视觉位姿估计；IMU信息与视觉估计和UWB测距信息融合；多机间定位结果融合。

与现有技术相比，本发明具有以下益处：

(1)由于采用了确定尺寸的合作标识，使得视觉定位的鲁棒性、精度和计算速度均得以提高，加快观测更新频率；

(2)由于采用了IMU预测+UWB测距+视觉图像观测的多传感器组合解算，使得多机器人间的定位精度得以进一步提高，并且可以在不依靠GPS信息的情况下保持***相对构型。

(3)由于采用了环形网络拓扑进行阵型估计，使得***在部分观测失效的情况下仍能保持运作，具有一定的冗余性。

附图说明

图1：合作标识加速计算流程图

图2：多传感器融合位姿估计示意图

图3：***拓扑结构示意图

图4：***流程图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明的目的在于设计一种基于多传感器融合的相互定位方法。旨在实现多无人机协同运输时的构型感知功能，为控制模块进行阵型保持提供位姿信息。

为了实现上述目的，本发明所采用的技术方案包括以下步骤:

1)单机对视觉可见范围内其他无人机的视觉位姿估计；

2)IMU信息与视觉估计和UWB测距信息融合；

3)多机间定位结果融合。

所述的步骤1)单机对其他无人机的视觉位姿估计：

1.1)合作标识的设置：使用AprilTag作为合作标识，黑白打印，外边框大小0.5m。

1.2)确定***拓扑：受限于机载计算单元的性能，单元无人机只对特定的对象进行检测和跟踪：设集群数量为n，标号为i的飞机仅由标号为i+1的飞机进行追踪，标号为n-1的飞机由标号为0的飞机进行追踪，组成环形网络。

1.3)图像预处理：从机载相机读入较高分辨率的图像，使用OpenCV将像素信息转化为cv::MAT矩阵形式，并将图像转为灰度图，记灰度图尺寸为gray＝(cols,rows)^T。

1.4)位姿初始化：使用AprilTag算法进行全图搜索，找到目标合作标识在图像平面的位置

使用PnP方法求解从相机到合作标识的齐次矩阵

更新裁剪偏移量

裁剪区域大小size＝k_w×gray，其中k_w<1为人为给定的缩放系数。

1.4)对输入图像进行裁剪：目标不可能在短时间内快速机动，根据k时刻合作标识在图像中的位置，更新裁剪偏移量

对k+1时刻的图像进行裁剪，仅保留合作标识附近的图像区域。

1.5)循环更新视觉观测信息：如图1所示，对输入图像寻找合作标识，利用裁剪信息还原合作标识原来的像素信息，计算合作标识在图像平面的位置

和齐次矩阵

还原公式为：

p_origin.uv＝p_detect.uv+bias-size/2

若图像区域内未检测到目标合作标识，且当前距离上一次检测到该标识的时间差小于阈值，即t_now-t_lastframe<t_threshold，更新size＝2×size；若超时，重置窗口大小size＝gray，并返回1.3)进行重新初始化。

所述的步骤2)IMU信息与视觉估计和UWB测距信息融合：

2.1)解算IMU信息，获得位姿增量ΔR_ij,Δv_ij,Δp_ij和协方差矩阵δΦ_ij,δv_ij,δp_ij；

2.2)使用UWB标签进行DS-TWR测距，获取各机间距离信息。

2.3)各单元将解算出的惯导数据和UWB测距信息广播在ROS通信网络中。

2.4)各单元根据自身惯导数据和跟踪目标的惯导数据做积分预测。

2.5)如图2所示，各单元将1)中视觉解算结果和UWB测距结果作为观测，与惯导预测结果进行融合滤波。其中，视觉解算结果包含了目标方位角和距离信息，作为主要的观测值，视为主节点；UWB信标仅提供了距离信息，作为从属节点附加在主节点上，施加“弹簧”约束，作为测量距离变化的边界；对IMU数据依照观测进行修正，求取相对位姿

使误差最小。

如果出现长时间视觉观测失效或结果协方差大于阈值的情况，为滤波结果贴上Exception标签，标记为不可信的数据。

所述的步骤3)多机间定位结果融合：

3.1)领航者根据融合后的相对位姿数据估计当前***相对构型。如图3所示，相对位姿矩阵通过环形网络传递，网络最多可以容忍一个成员定位失效，此时依旧可以保证构型估计的完整性。

3.2)如果存在两个或以上成员定位失效，或通信错误时，***暂停执行当前任务，保持悬停状态并在地面站上报错。

3.3)无论何时，地面站具有最高优先级，可以对当前任务状态进行超控。***总体流程图如图4所示。

Claims (4)

1.一种基于多传感器融合的多机器人相对定位方法，其特征在于步骤如下：

步骤1、单元机器人对视觉可见范围内其他机器人的视觉位姿估计：

以AprilTag作为合作标识对每一个机器人进行标识；

***内任一单元机器人只对特定的对象进行检测和跟踪；

任一单元机器人从机载相机读入图像，使用OpenCV将像素信息转化为cv::MAT矩阵形式，并将图像转为灰度图；

位姿初始化：使用AprilTag算法进行全图搜索，找到目标合作标识在图像平面的位置

使用PnP方法求解从相机到合作标识的齐次矩阵

对后续输入图像进行裁剪：根据上一时刻合作标识在图像中的位置

裁剪下一时刻输入图像，仅保留合作标识附近的图像区域；对图像区域寻找合作标识，计算合作标识在图像平面的位置

和齐次矩阵

即视觉解算结果，广播在ROS通信网络中；

如果在图像区域中未找到合作标识时，在下一帧图像中扩大剪裁区域直至能够找到合作标识；

步骤2、IMU信息与视觉估计和UWB测距信息融合：

解算任一单元机器人IMU中的信息，获得位姿增量ΔR_ij,Δv_ij,Δp_ij和协方差矩阵δΦ_ij,δv_ij,δp_ij，作为预测单元机器人在下一时刻的位置和协方差即惯导预测结果；使用UWB标签进行DS-TWR测距，获取各机间距离信息；

各单元机器人将解算出本机的惯导数据和UWB测距信息广播在ROS通信网络中；

各单元将步骤1中视觉解算结果和UWB测距信息结果作为观测，与惯导预测结果进行融合滤波，将融合结果广播在ROS通信网络中；如果出现长时间视觉观测失效或结果协方差大于阈值的情况，为滤波结果贴上Exception标签，标记为不可信的数据；

步骤3、多机器人间定位结果融合：

领航的单元机器人根据融合后的相对位姿数据计算当前***相对构型，环形网络拓扑最多容忍一个成员定位失效；

如果存在两个或以上成员定位失效，或通信错误时，***暂停执行当前任务，保持悬停状态并在地面站上报错。

2.根据权利要求1所述基于多传感器融合的多机器人相对定位方法，其特征在于：无论何时，地面站具有最高优先级，对当前任务状态进行超控。

3.根据权利要求1或2所述基于多传感器融合的多机器人相对定位方法，其特征在于：所述合作标识采用黑白两色，外边框大小为0.5m。

4.根据权利要求1或2所述基于多传感器融合的多机器人相对定位方法，其特征在于：所述读入图像的分辨率大于1920*1080。

Images