阅读说明：本技术 一种基于多传感器融合的多机器人相对定位方法 (Multi-robot relative positioning method based on multi-sensor fusion ) 是由 黄攀峰 杨立 张帆 张夷斋 于 2021-07-19 设计创作，主要内容包括：本发明涉及一种基于多传感器融合的多机器人相对定位方法,旨在实现多无人机协同运输时的构型感知功能,为控制模块进行阵型保持提供位姿信息。所采用的技术方案包括以下步骤：单机对视觉可见范围内其他无人机的视觉位姿估计；IMU信息与视觉估计和UWB测距信息融合；多机间定位结果融合。由于采用了确定尺寸的合作标识,使得视觉定位的鲁棒性、精度和计算速度均得以提高,加快观测更新频率；采用了IMU预测+UWB测距+视觉图像观测的多传感器组合解算,使得多机器人间的定位精度得以进一步提高,并且可以在不依靠GPS信息的情况下保持系统相对构型。采用了环形网络拓扑进行阵型估计,使得系统在部分观测失效的情况下仍能保持运作,具有一定的冗余性。(The invention relates to a multi-robot relative positioning method based on multi-sensor fusion, aiming at realizing a configuration sensing function during multi-unmanned aerial vehicle collaborative transportation and providing pose information for a control module to carry out matrix maintenance. The adopted technical scheme comprises the following steps: estimating the visual pose of other unmanned aerial vehicles in the visual visible range by the single machine; IMU information is fused with visual estimation and UWB ranging information; and fusing the positioning results among multiple machines. Due to the adoption of the cooperative identification with the determined size, the robustness, the precision and the calculation speed of the visual positioning are improved, and the observation updating frequency is increased; the method adopts the multi-sensor combined calculation of IMU prediction, UWB ranging and visual image observation, so that the positioning accuracy among multiple robots is further improved, and the relative configuration of the system can be kept without depending on GPS information. The ring network topology is adopted for array type estimation, so that the system can still keep running under the condition of partial observation failure, and certain redundancy is realized.)

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

技术领域

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

背景技术

多机器人协同系统具有方便组合、灵活、冗余等特点，在近年来取得了长足的发展。在空中搬运作业方面，相较于单架大飞机调运，多无人机系统更是具有单机成本低、维护方便的优点。根据载荷重量的不同，通过增加单元数量，即可增加系统的负载能力。多无人机协同运输系统由无人机单元、柔性系绳、载荷连接装置和地面站组成。每架无人机单元上均搭载有相机、惯导传感器、超宽带(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所示。