CN117685953A - 面向多无人机协同定位的uwb与视觉融合定位方法及系统 - Google Patents

Abstract

本发明公开了无人机定位导航领域的一种面向多无人机协同定位的UWB与视觉融合定位方法及系统，旨在解决多无人机协同定位状态感知和定位精度问题。其包括：在多无人机协同定位过程中，针对撤掉锚点后多无人机系统位姿估计参考系不一致的问题，设计了基于UWB与视觉融合的多无人机协同定位方法，无人机间相互协作和信息共享，通过视觉和UWB两种传感器感知同伴状态，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿，解决了缺少全局参考系的情况下多无人机系统状态感知问题，设计四旋翼无人机平台并运行该定位算法，通过室内三架飞机协同定位实验显示所提出的方法估计精度较高，能够实现多无人机快速精确定位和协同感知，具有极大的应用价值。

Description

面向多无人机协同定位的UWB与视觉融合定位方法及系统

技术领域

本发明涉及一种面向多无人机协同定位的UWB与视觉融合定位方法及系统，属于导航定位技术领域。

背景技术

无人系统近年来获得不断进步，其具有在无人操控的场景下完成自身的移动并完成相应任务的能力，如环境勘探、灾后救援、自动驾驶等。无人机作为无人系统的重要组成部分，包含众多种类，按照飞行方式可分为固定翼无人机、旋翼无人机等，其中四旋翼无人机由于其具有成本低、小型化、动力学建模简单、高机动能力等特性，使其成为在室内或室外环境中广泛应用的理想的机器人平台，一直是学者研究的重点。无人机飞行通常通过全球导航卫星系统(Global Navigation Satellite System，GNSS)进行定位，使无人机的室外大量应用成为可能。然而真实环境通常复杂且未知，并经常出现GNSS信号不佳甚至消失的情况，如室内、走廊、丛林等环境，在上述情况下，GNSS无法提供周围的环境信息，使其无法应用于混乱环境或室内密闭环境中的保障飞行。因此四旋翼无人机需要通过其它传感器在无外界定位信号的情况下实现实时自主定位与建图(Simultaneous Localization AndMapping，SLAM)技术。

为了使SLAM技术应用在不同的场景下，产生了很多在无GNSS的情况下使用不同传感器的自主定位方案，如相机、多线激光雷达、红外传感器、毫米波雷达、惯性测量单元(Inertial Measurement Unit，IMU)等。相机、激光雷达等机载传感器通常用于获取空间中的结构化信息，然后通过获取的信息求解传感器自身相对于空间信息的位姿变化。由于搜索勘探等功能一般均在狭小复杂的环境下完成，因此需要操作较小型的无人机平台，然而小型平台通常会受到尺寸、大小、功率等限制，随着传感器功率的增大其电池载荷重量也会随之增大，对携带重量、体积较大或功率较高的传感器产生了限制。相机和IMU通常能够满足这些功率和有效载荷的限制，因此基于相机和IMU进行自主定位成为重点研究的对象。相机本身具有重量轻、价格便宜、包含信息种类丰富、在不同的环境中能够提供不同的特征等特性，可以从空间提供大量如角点、线段、边缘、平面、曲面、灰度、色彩甚至物体本身的丰富环境信息，扩展很多可用的功能，因此基于视觉的SLAM技术一直是学者研究的重点。同时，IMU可以获取很高频率的无人机机体的加速度和角速度，进而直接解算出位姿信息，然而IMU通常包含高频白噪声及零偏，直接计算会存在大量累计误差。通过两者融合，高频的IMU通过预积分可以快速的求解出两帧之间的状态变化，保证图像帧之间位姿求解的连贯性，不会由于视觉突然明暗交替或通过特征不明显位置造成的定位算法直接失灵；而视觉状态求解的准确性克服了IMU噪声及零偏引起的快速漂移问题，保证了定位的精度。

近年来，由于多无人机系统在战术围捕、搜救等方面的巨大前景，导致多无人机协作成为当下热门的研究主题。在执行协同任务时，为了达到编队飞行、避障等目标，无人机不仅需要明确自身状态，还需要对系统内其他个体的位置进行估计。

通常部署UWB锚点构成全局参考系G，以多传感器融合的方法估计无人机位姿，这种方法延伸至多无人机系统依然适用，无人机只需共享信息即可获取同伴的位置，但是这种依赖锚点的方法限制了多无人机的覆盖范围，不能即刻应用于未知场景。

发明内容

本发明的目的在于克服现有技术中的不足，提供一种解决在GNSS拒止环境下面向多无人机协同定位的UWB与视觉融合定位方法及系统，

为达到上述目的，本发明是采用下述技术方案实现的：

第一方面，本发明提供了一种面向多无人机协同定位的UWB与视觉融合定位方法，包括以下步骤：

步骤1：获取UWB、IMU、双目相机和定高雷达数据；

步骤2：通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；

步骤3：融合IMU和双目相机做数据构成视觉惯性里程计,估计无人机惯性系下的位姿，实现视觉定位，输出得到无人机位姿和环境路标点，同时创建和广播关键帧消息。

步骤4：构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计。

进一步的，步骤3：系统中的每个无人机创建和广播关键帧消息以支持后续的协同定位工作，包括：

为开辟特征提取线程，根据平均视差和特征跟踪的质量提取相机拍摄图像帧的关键帧，当捕获最新的关键帧时，执行以下步骤：

①创建新的关键帧消息

②提取1000个特征并计算对应的BRIEF描述子，将它们写入关键帧消息

③将里程计恢复出3D路标点并将其对应的描述子写入关键帧消息

④将位姿估计矩阵写入关键帧消息

⑤向网络中广播关键帧消息

关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的回环，从而形成回环观测，运动信息用于计算位姿增量，从而形成里程计增量观测。

进一步的，步骤4：构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计，包括：

当无人机采集到最新的关键帧，它的里程计特征提取线程会向系统内广播关键帧信息，询问其他个体是否经历过类似场景，其他个体的回环检测程序会进行场景识别，从图像数据库中筛选出回环候选帧，向询问方答复。

一旦询问方收到答复将计算两无人机相对位姿，这个相对位姿将作为协同定位算法的回环观测送入优化器构建位姿图。采用词袋模型技术快速比较两帧图像的特征向量，然后使用结合RANSAC方法的PnP计算相对位姿；

将所述两无人机相对位姿送入优化器构建位姿图，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿。

进一步的，所述回环检测程序包括：

首先利用词袋模型技术判断是否两帧图像包含类似的场景，从而形成互回环；

采用Kmeans++算法构造字典，对于每一张字典训练图像，图像特征点使用BRIEF描述子表征，Kmeans++算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点，然后对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d。最后会得到一个拥有n个叶子节点的树，每一个叶子节点即为视觉词汇。

使用频率—逆文档频率分配视觉词汇权重。TF认为视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此TF可表达为：

其中m_i表示该词汇ω_i在一幅图像中出现的次数，m为图像中的词汇总数。

IDF认为某个词汇在字典中出现的频率越低，则在分类图像时区分度越高，应该加大权重，因此IDF可表达为：

其中n_i表示建立的字典中词汇ω_i的数量，n是字典中所有视觉词汇的数量。

一个词汇的权重取为TF和IDF的乘积：

η_i＝TF_i×IDF_i (3)

计算机在生成k时刻图像KF_k的特征向量时，将图像特征点的BRIEF描述子与构建好的字典进行比对，选择每个层级内使汉明距离最小的中值节点，从树根开始自上而下遍历字典直到叶节点，构造出一幅图像的带权重特征向量：

v_k＝{(ω₁,η₁),(ω₂,η₂),…,(ω_n,η_n)} (4)

进一步的，无人机在交换信息时的通讯策略包括：

某时刻UAV_α将关键帧消息广播至网络中向系统内的无人机询问是否存在回环候选，系统内UAV_β的回环检测程序使用词袋模型查询图像数据库，一旦发现回环候选，会将图像信息打包成消息回复给UAV_α。

询问方收到回复消息，执行带RANSAC算法的PnP求解出进而推导出二者的相对位姿^αT_β。

相对位姿不仅会被送入位姿图优化程序作为视觉回环观测，还会拷贝一份副本发送给UAV_β。

进一步的，执行带RANSAC算法的PnP求解出进而推导出二者的相对位姿^αT_β，包括：

考虑在某时刻UAV_α通过PnP计算出UAV_β的相机系c_β在惯性系w_α下的位姿UAV_α为了得到回环观测，将自身里程计输出值和默认为已知的相机外参矩阵与PnP计算结果相乘，可推导出两无人机机体系间的相对位姿：

这个相对位姿将作为回环观测添加到位姿图优化程序中。

进一步的，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿，包括：

定义位姿图优化中的系统状态矢量为：

其中n为滑动窗口规模。以系统中全部无人机的位姿作为图优化问题的角点，角点之间使用边相连，包含三种边，

①增量边

增量边仅连接单个无人机位姿角点，表示滑动窗口中无人机k_l时刻位姿与k_l-m时刻位姿之间的位姿增量，本例中令m＝3，即一个位姿角点最多与前面三个角点用增量边连接，位姿增量根据视觉惯性里程计的输出结果计算，其与系统状态存在如下关系：

其中i∈{α,β,γ}，表示里程计增量测量，代表SE(3)上的广义加法，n_δT为零均值白噪声。

②回环边

回环边连接多个无人机位姿角点，表示询问方当前帧与应答方数据库中某帧图像存在互回环，回环边实质上是无人机之间的相对位姿，使用PnP计算，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示视觉回环测量，n_T为零均值白噪声。

③测距边

测距边连接多无人机位姿角点，表示某一时刻两机的相对距离，测距值可直接通过UWB传感器读取，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示UWB距离测量，n_d为零均值白噪声。

位姿图优化的中心思想是通过调整角点的位姿以使得代价函数整体最小，优化后的结果是将各无人机的位姿对齐到w_α系。位姿图优化的代价函数可表示为式(10)的形式：

式(10)中S表示系统内所有无人机的增量边的集合，L表示所有回环边的集合，D表示所有测距边的集合。i和j表示无人机的编号，i,j∈{α,β,γ}。

表示里程计位姿增量残差，数学表示为：

表示回环残差，数学表示为：

表示测距残差，数学表示为：

上式中！表示SE(3)上的广义减法。

因为IMU能够测量重力矢量，所以无人机的俯仰角和滚转角完全可观。为减轻计算量，可只优化位置和偏航部分，从而转为4DoF位姿图优化。

求解式(10)，可将系统内所有无人机的位姿对齐到w_α系下，得到优化的UAV_α自身的位姿。

进一步的，步骤2：通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿，包括：

步骤A：获取UWB原始数据和IMU数据；

步骤B：对所述UWB原始数据进行距离校准，得到校准后的UWB数据；

步骤C：判断锚点位置是否已知，若否，根据所述校准后的UWB数据进行锚点标定后跳转步骤D，若是，跳转步骤D；

步骤D：对所述校准后的UWB数据进行离群值检测，得到筛选后的UWB数据；

步骤E：对所述IMU数据进行低通滤波，得到滤波后的IMU数据；

步骤F：将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，得到无人机的位置、速度和姿态信息；所述状态估计器以无迹卡尔曼滤波为核心，对状态的概率分布进行近似，从而估计出均值和协方差；

进一步的，步骤3：系统中的每个无人机采用基于滑动窗口的紧耦合视觉惯性里程计完成自主定位，包括：

对IMU进行预积分，IMU预积分项将在后端优化时用作构建集束调整方程的残差项；

预处理视觉观测的最新帧，利用KLT稀疏光流法跟踪现有特征，同时检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉测量残差定义在单位球面上，视觉测量残差即重投影残差；

用紧耦合融合的方法保证求解的精度，采用边缘化的思想将这些约束信息转换成状态先验条件加入到优化当中，通过构造集束调整方程做非线性最小二乘优化；

以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解。

第二方面，本发明提供一种面向多无人机协同定位的UWB与视觉融合定位系统，包括：

输入模块：获取UWB、IMU、双目相机和定高雷达数据；

UWB模块：用于通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；

里程计模块：用于融合IMU和双目相机做数据构成视觉惯性里程计,估计无人机惯性系下的位姿，实现视觉定位，输出得到无人机位姿和环境路标点，同时创建和广播关键帧消息。

位姿优化模块：用于构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计。

与现有技术相比，本发明所达到的有益效果：

本发明对GNSS拒止环境下四旋翼无人机的定位与导航算法研究具有十分重要的意义。本发明稳定可靠，不受天气和光线等的影响，同时算法节省计算资源，对硬件要求较低，具有很高的理论与实用价值。本发明主要具有以下的特点和优点：

(1)本发明选取美国Time domain公司生产的P440模块实现GNSS拒止环境下无人机的自主定位与建图技术，该方案定位精度高且成本低廉，使用范围广泛。传统GNSS拒止环境下自主导航方案所使用的传感器价格昂贵且稳定性无法得到保证。借助本方法的面向多无人机协同定位的UWB与视觉融合定位方法及系统，可以实现室内室外等多种场景的多机协同定位与建图。

(2)本发明提出了基于UWB和视觉融合的多机协同定位算法，辅以IMU预积分约束，使用基于因子图优化的方式实现多无人机的高精度、稳定自主定位与建图技术。本方法通过视觉和UWB两种传感器感知同伴状态，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿，解决了缺少全局参考系的情况下多无人机系统状态感知问题

(3)本发明采用的位姿求解框架为无迹卡尔曼滤波，该框架易于扩展，除了IMU、UWB和雷达高度计等传感器设备之外，还可根据开发者自行添加传感器设备，如激光雷达等，可进行二次开发。基于因子图的多机协同定位使得每架无人机之间的位姿约束以一种紧耦合的方式联系起来，增加了系统的鲁棒性。

附图说明

附图1面向多无人机协同定位的UWB与视觉融合定位方法及系统流程图

附图2面向多无人机协同定位的UWB与视觉融合定位方法及系统系统硬件架构图

附图3面向多无人机协同定位的UWB与视觉融合定位方法及系统系统算法流程图

附图4词袋模型示意图

附图5字典建立过程示意图

附图6数据库查询示意图

附图7相对位姿计算示意图

附图8多无人机相对位姿图优化示意图

附图9两架无人机室内协同定位轨迹图

附图10两架无人机室内协同定位轨迹绝对位置误差(APE)曲线。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例一：

本实施例提出解决在GNSS拒止环境下面向多无人机协同定位的UWB与视觉融合定位方法及系统。

GNSS拒止环境下面向多无人机协同定位的UWB与视觉融合定位方法及系统单机平台如图2所示。

本章针对撤掉锚点后多无人机系统位姿估计参考系不一致的问题，设计了基于UWB与视觉融合的多无人机协同定位方法，无人机间相互协作和信息共享，通过视觉和UWB两种传感器感知同伴状态，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿，解决了缺少全局参考系的情况下多无人机系统状态感知问题，通过室内三架飞机协同定位实验显示本章提出的方法估计精度较高，相信该协同定位方法能够对GPS拒止环境下多无人机系统的应用起到重要作用。

通过设计四旋翼无人机平台并运行该定位算法，实现无人机快速精确定位，具有极大的应用价值。本发明采取的技术方案包括硬件和软件两部分，硬件包括：包括UWB测距模块，IMU惯性测量传感器、双目相机、雷达高度计、嵌入式机载处理器和无人机平台。软件包括：视觉惯性里程计单元、回环检测单元、多无人机相对位姿计算单元、基于因子图的多无人机紧耦合位姿优化单元。整个算法流程见图3。下面针对各部分做具体说明。

硬件部分：

无人机为轴距380mm的四旋翼飞行器，配备DJI E300电机电调和4S高压电池。传感器搭载了Intel双目相机d435、3DM-GX5-25惯性测量单元(IMU)，P440 UWB测距模块，TFmini激光测距模块(定高雷达)，姿态控制由Pixhawk飞行控制器完成，机载计算机采用Intel第八代NUC(处理器为Core i7-8559U 2.7GHz，四核八线程，内存大小32G)。定位程序是在Linux操作系统下基于ROS(Robot Operating System)机器人开源框架进行开发，所有算法均采用C++语言实现，定位程序采用ROS Message实时接收传感器采集的数据，程序解析出的位置和姿态使用Mavlink协议经由串口发送到Pixhawk飞行控制器做进一步姿态控制。

考虑到系统的可靠性，UWB模块需要有较高的测距精度和较鲁棒的抗多径干扰能力，本平台选择搭载美国Time domain公司生产的P440模块。P440模块是一种波段在3.1GHz到4.8GHz之间的超宽带无线收发器，它主要采用TW-ToF原理，可同时执行四种功能(测距、数据传输、单基地雷达和多基地雷达)，这里仅使用测距功能，测距包括单点测距和组网测距模式，实验中标签和锚点组成测距网络，组网测距可以采用ALOHA(随机)协议或者TDMA(时分多址)协议。此UWB模块的电波发射功率极小(～50uW)，标签使用机载电池经电压转换后供电，锚点使用移动电源直接供电。

UWB测距模块：采用一种波段在3.1GHz到4.8GHz之间的超宽带无线收发器，可同时执行四种功能：测距、数据传输、单基地雷达和多基地雷达，主要使用该模块的测距功能，通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。

IMU惯性测量传感器：主要实现三方面的数据处理，一是通过无迹卡尔曼滤波器融合UWB测距模块和定高雷达数据，估计出无人机在全局坐标系下的位姿，实现UWB的定位功能；二是搭配双目相机实现对视觉观察的环境特征点的检测、跟踪和预处理，实现无人机位姿状态估计；三是创建和广播关键帧消息，便于后续使用视觉传感器通过回环检测感知其他无人机。

双目相机：收集环境信息，将数据传回机载处理器，提取环境特征点，配合IMU惯性测量传感器共同组成视觉惯性里程计。

雷达高度计：测量无人机高度数据，根据实测的高度数据校正和调整UWB定位结果。

嵌入式机载处理器：机载Intel第八代高算力处理器，能够处理各类传感器数据信息，运行回环检测算法和求解位姿图，实现位姿坐标对齐和位姿优化。

无人机平台：包括四旋翼无人机飞行器、电机、电池和飞控部件。

UWB测距模块联合IMU惯性测量传感器和雷达高度计实现基于UWB与IMU融合的鲁棒自主定位与建图技术的定位功能；IMU惯性测量传感器和双目相机组成视觉惯性里程计，通过对飞行环境信息的感知和测量实现基于滑动窗口的紧耦合的自主定位，估计多无人机的位姿状态，同时创建和广播关键帧，实现多人机的回环检测得到相对位姿信息，根据观测量和待优化变量构建位姿图，计算代价函数的最小值，实现多无人机位姿坐标对齐和位姿优化。

软件部分：

(1)UWB模块和视觉惯性里程计单元

使用UWB模块通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；系统中的每个无人机采用基于滑动窗口的紧耦合视觉惯性里程计完成自主定位，它的输出结果包括无人机位姿和环境路标点，本方法中视觉惯性里程计的作用不仅限于状态估计，还负责创建和广播关键帧消息以支持后续的协同定位工作。

里程计位姿增量残差部分是构成角点位姿的代价函数的一项组成部分，主要表示视觉定位过程中无人机位姿的增量，用增量表示是为了摆脱参考系的限制，统一用残差的形式表示是为了减少计算量。

为开辟特征提取线程，根据真的平均视差和特征跟踪的质量提取相机拍摄图像帧的关键帧，当捕获最新的关键帧时，执行以下步骤：

①创建新的关键帧消息

②提取1000个特征并计算对应的BRIEF描述子，将它们写入关键帧消息

③将里程计恢复出3D路标点及其对应的描述子写入关键帧消息

④将位姿估计矩阵写入关键帧消息

⑤向网络中广播关键帧消息

关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的回环，从而形成回环观测，运动信息用于计算位姿增量，从而形成里程计增量观测。

基于视觉惯性的紧耦合里程计框架如图5所示。当相机拍摄的最新帧到达之后，首先进行视觉观测的预处理，步骤包括使用KLT稀疏光流法跟踪现有特征，为了确保特征点的数目维持在特定数量，在跟踪原有特征的基础上同时需要检测新的特征，另外该步骤还负责筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，选择关键帧有两个准则：一是与前一关键帧的平均视差，如果最新帧与前一帧的平均视差大于某一阈值，就将最新帧设置为新关键帧；二是特征跟踪的质量，如果特征跟踪数量低于某一阈值，就将最新帧设置为新关键帧。

字典的图像特征点使用BRIEF描述子表征，Kmeans++算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点，然后对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d。最后会得到一个拥有n个叶子节点的树，每一个叶子节点即为视觉词汇。此处在步骤3第一小点进行了详细描述。

路标点是步骤二中视觉惯性里程计确定的，该处用于关键帧的创建。

本文中所述运动信息主要包括位置和姿态信息。

(2)回环检测单元

使用视觉传感器有效感知其他同伴的方法之一就是回环检测，互回环的含义是由于多个无人机重复飞越了同一场景导致捕获到某些相似的视觉特征。当无人机采集到最新的关键帧，它的里程计特征提取线程会向系统内广播关键帧信息，询问其他个体是否经历过类似场景，其他个体的回环检测程序会进行场景识别，从图像数据库中筛选出回环候选帧，向询问方答复。一旦询问方收到答复将计算两无人机相对位姿，这个相对位姿将作为协同定位算法的回环观测送入优化器构建位姿图。本部分采用词袋模型(Bag ofWord，BOW)技术快速比较两帧图像的特征向量，然后使用结合RANSAC方法的PnP计算相对位姿。

1)回环检测程序首先利用词袋模型技术判断是否两帧图像包含类似的场景，从而形成互回环。词袋模型是一种将图像特征描述为特征向量，从而将图像相似度的比较转换成对特征向量定量比较的技术，词袋模型借鉴了人类对场景的描述方法，比如人类对场景的描述一般是提取某些特征元素进行识别：一个场景中有一个“小提琴”，一台“电脑”，另一场景中有一个“行人”，一辆“自行车”，即图像可看成是若干个“视觉词汇”的集合，视觉词汇之间没有顺序，如图4所示。字典是视觉词汇的全集，计算机根据一张图像的视觉词汇出现情况，可以构造一个以字典为基底的二进制向量，称为特征向量，表征图像中是否有某类特征存在，这种图像描述方法比单纯地灰度描述更加稳定。

2)字典的构建需要从大量图像中提取丰富的特征点，因此字典通常是离线创建的，本节采用Kmeans++算法构造字典，如图5(a)所示。对于每一张字典训练图像，它的图像特征点使用BRIEF描述子表征，Kmeans++算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点，然后对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d。最后会得到一个拥有n个叶子节点的树，每一个叶子节点即为视觉词汇，如图5(b)所示。

在构建字典的时候，还要考虑词汇的权重，这里使用频率—逆文档频率(TermFrequency-Inverse Document Frequency，TF-IDF)分配视觉词汇权重。TF认为视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此TF可表达为：

其中m_i表示该词汇ω_i在一幅图像中出现的次数，m为图像中的词汇总数。

IDF认为某个词汇在字典中出现的频率越低，则在分类图像时区分度越高，应该加大权重，因此IDF可表达为：

其中n_i表示建立的字典中词汇ω_i的数量，n是字典中所有视觉词汇的数量。

一个词汇的权重取为TF和IDF的乘积：

η_i＝TF_i×IDF_i (3)

计算机在生成k时刻图像KF_k的特征向量时，将图像特征点的BRIEF描述子与构建好的字典进行比对，选择每个层级内使汉明距离最小的中值节点，从树根开始自上而下遍历字典直到叶节点，构造出一幅图像的带权重特征向量：

v_k＝{(ω₁,η₁),(ω₂,η₂),…,(ω_n,η_n)} (4)

3)考虑到多无人机系统需要实时交换数据，要满足同时带宽、传输实时性、内存空间等要求，还要考虑到无人机群规模的可扩展性。为此，无人机在交换信息时有必要遵守特定的通讯策略。

以系统中的UAV_α与UAV_β两机为例，如图6所示。某时刻UAV_α将关键帧消息广播至网络中向系统内的无人机询问是否存在回环候选，系统内UAV_β的回环检测程序使用词袋模型查询图像数据库，一旦发现回环候选，会将图像信息打包成消息回复给UAV_α。询问方收到回复消息，执行带RANSAC算法的PnP求解出进而推导出二者的相对位姿^αT_β。相对位姿不仅会被送入位姿图优化程序作为视觉回环观测，还会拷贝一份副本发送给UAV_β，目的是避免UAV_β重复计算回环。这种查询-回复的机制有效避免了类似于集中式地面站那样需要存储所有无人机的视觉信息(空间复杂度O(n))，反而每个无人机仅需存储自身的视觉信息(空间复杂度O(1))，克服了传统方式对无人机群规模的限制。

(3)多无人机相对位姿计算单元

根据一组3D-2D点对计算相机系-惯性系间的相对位姿，这是一个典型的PnP问题，传统的PnP方法解算的精度极易受到离群点和坐标定位噪声的影响，并且传统的PnP问题求解方法只能计算位姿而不能验证回环候选帧的正确性。为此，本方法设计了结合RANSAC方法的PnP求解方式，列出了结合RANSAC方法的PnP求解伪代码，最后利用PnP的结果推导出两无人机机体系间的相对位姿。

当询问方UAV_α收到回环侯选帧，使用上述方法可以验证回环正确性同时计算侯选帧在惯性系w_α下的位姿但是对于协同定位算法，在优化时需要给出的回环观测应是两机的相对位姿^αT_β，为此还需要结合已知条件做一步推导。

如图7所示，考虑在某时刻UAV_α通过PnP计算出UAV_β的相机系c_β在惯性系w_α下的位姿UAV_α为了得到回环观测，将自身里程计输出值和默认为已知的相机外参矩阵与PnP计算结果相乘，可推导出两无人机机体系间的相对位姿：

这个相对位姿将作为回环观测添加到位姿图优化程序中。

(4)基于因子图的多无人机紧耦合位姿优化单元

协同定位算法的核心是位姿图优化程序，位姿图优化程序负责根据三种观测量和待优化变量构建位姿图，实时优化和对齐系统内所有个体的位姿，这样做的结果是每个无人机在确保自身状态估计的同时也可做到对同伴位置的感知，进而能很方便的执行编队飞行、避障等任务。

协同定位的任务是根据传感器的观测值实时将系统内同伴的位姿进行坐标对齐，同时优化自身位姿，由此可定义位姿图优化中的系统状态矢量为：

其中n为滑动窗口规模。以系统中全部无人机的位姿作为图优化问题的角点，角点之间使用边相连，本问题包含三种边，如图8所示。

④增量边

增量边仅连接单个无人机位姿角点，表示滑动窗口中无人机k_l时刻位姿与k_l-m时刻位姿之间的位姿增量，本例中令m＝3，即一个位姿角点最多与前面三个角点用增量边连接，位姿增量根据视觉惯性里程计的输出结果计算，其与系统状态存在如下关系：

其中i∈{α,β,γ}，表示里程计增量测量，代表SE(3)上的广义加法，n_δT为零均值白噪声。

⑤回环边

回环边连接多个无人机位姿角点，表示询问方当前帧与应答方数据库中某帧图像存在互回环，回环边实质上是无人机之间的相对位姿，使用PnP计算，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示视觉回环测量，n_T为零均值白噪声。

⑥测距边

测距边连接多无人机位姿角点，表示某一时刻两机的相对距离，测距值可直接通过UWB传感器读取，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示UWB距离测量，n_d为零均值白噪声。

位姿图优化的中心思想是通过调整角点的位姿以使得代价函数整体最小，优化后的结果是将各无人机的位姿对齐到w_α系。位姿图优化的代价函数可表示为式(10)的形式：

式(10)中S表示系统内所有无人机的增量边的集合，L表示所有回环边的集合，D表示所有测距边的集合。i和j表示无人机的编号，i,j∈{α,β,γ}。

表示里程计位姿增量残差，数学表示为：

表示回环残差，数学表示为：

表示测距残差，数学表示为：

上式中！表示SE(3)上的广义减法。

因为IMU能够测量重力矢量，所以无人机的俯仰角和滚转角完全可观。为减轻计算量，可只优化位置和偏航部分，从而转为4DoF位姿图优化。求解式(10)，可将系统内所有无人机的位姿对齐到w_α系下，从而时时刻刻维护全局一致的位姿，上式除了做到位姿对齐外，对UAV_α自身的位姿做了进一步的优化。

下面给出实验结果

本案例对上述定位系统进行了多组基于多传感器融合的GNSS拒止环境下面向多无人机协同定位的UWB与视觉融合定位方法及系统实验。

图9和图10分别为GNSS拒止环境下两架无人机室内协同定位轨迹跟踪效果和两架无人机室内协同定位轨迹绝对位置误差(APE)曲线，该实验的场景为室内，起点与终点为同一地点。根据结果可以看出基于本方法的轨迹几乎与真值重合，误差较小。

协同定位算法的详细性能指标如表1所示：

表1协同定位算法性能指标

根据表格可以看出，本方法提出的多无人机协同定位方法解决了缺少全局参考系的情况下多无人机系统状态感知问题，UAV_α对同伴UAV_β和UAV_γ的轨迹跟踪和位姿估计较为准确，协同定位精度优于0.1m，算法整体表现良好。

本实施例中，使用UWB模块通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿的具体方法包括：

UWB定位部分采用“基于UWB与IMU融合的鲁棒自主定位与建图技术”来实现，与此同时，IMU和双目相机做数据融合构成视觉自主定位部分，一般称为视觉惯性里程计，可估计出无人机在惯性系下的位姿，视觉部分的数据融合算法为基于滑动窗口的紧耦合非线性优化。然后采用模块间松耦合的方式实现UWB定位与视觉自主定位的融合，即UWB定位与视觉自主定位两部分单独运行，最后通过全局位姿图优化融合二者的估计结果，这样做的好处是即使视觉跟踪暂时失效或者UWB测量出现离群值，定位算法仍能保持较稳定的状态估计。

本部分包括对UWB原始数据的测距信息校准、离群值剔除以及IMU噪声处理三部分。处理后的IMU数据能够有效地降低噪声污染。其处理后的结果作为输入送入卡尔曼滤波单元。

理想情况下UWB距离测量的精度能够达到厘米级，但实际上测量值常常因为某种原因带有静差和误差。为了尽可能减少影响定位精度的因素，本方法在测量值送入卡尔曼滤波器之前加入了距离校准环节，经由距离校准之后可认为是更贴近真值的测量值。

将UWB距离测量与距离d真值建模为线性关系，即：

其中n_d为零均值白噪声。距离校准环节采用线性回归法，通过将两个UWB模块摆放于不同的位置测距，得到数个真值-测量样本每个样本中的测量值为UWB距离的读数，距离真值由高精度的室内定位系统获得。经过大量样本拟合模型，确定当前模型下的尺度因子a与静差b。

其中a为距离真值的均值，为距离测量的均值。

每当采集到UWB传感器测量数据时，默认通过距离校准环节进行线性回归处理，推算出更合理的距离值。

虽然UWB具备的优良特性能最大限度的避免多径效应、非视距的影响，但在障碍物密集的场景下仍有可能引入离群值，离群值指明显有悖于事实的测量，如读数出现负数，突然发生很大跳变等情况，离群值的引入会导致错误的状态估计。这里采用添加阈值方法检测离群值，一旦认为本次距离测量为离群值，将直接被剔除。

若p_k为k时刻估计器输出的无人机位置，当k+1时刻无人机上的标签收到与锚点A_i的距离测量如果满足

则本次距离测量被判定为离群值。这里γ是可调阈值,v_max为无人机最大飞行速度，f是UWB模块测距更新速率。如果本次距离测量被判定为离群值，则直接舍弃，若为正常情况，则将作为距离测量送入估计器做状态更新。

对于IMU而言，由于其采用捷联方式直接固定在机体上，无人机在飞行过程中受到螺旋桨震动的影响，导致测量数据受到较大的噪声污染。根据IMU测量噪声主要来源是机体的高频震动，本发明设计了截止频率约为30Hz的IIR数字低通滤波器，通过低通滤波处理的IMU数据能够有效地降低噪声污染。IIR低通滤波算法频域表示如式(4)所示：

其中，m和n，a_i,i＝1,…,m和b_j,j＝1,…,n均为滤波器参数。该低通滤波器的时域形式为：

y(t)＝a₀u(t)+a₁u(t-1)+…+a_mu(t-m)-b₁y(t-1)-…-b_ny(t-n) (2-5)

其中u代表IMU原生测量数据，y代表低通滤波处理结果，通过低通滤波后的数据作为IMU测量值再进行多传感器融合。

(2)UWB锚点在线标定单元

本节采用非线性最小二乘理论设计了锚点标定算法，在创建全局系的同时标定出环境四周锚点的位置，锚点标定的结果被送入状态估计器保存起来，以便后续的算法顺利进行。锚点标定环节在整个流程中仅被执行一次。

锚点标定：首先建立全局坐标系，从俯视角度看，该坐标系以锚点A₁所在位置为坐标系原点，锚点A₂与A₁的连线为x轴所在方向，A₁的高度方向为z轴，这样就建立了右手系的两个坐标轴，而y轴方向则可以根据右手定则确定。为了简便，可将A₃放置在近似y轴的方向上，A₄可放置在任意位置。标定阶段令无人机保持静止，标签按照设定好的时隙表周期性的向锚点发出测距请求，根据TW-ToF原理解析出标签到每个锚点的距离。此外，开启UWB模块的回声测距功能，即标签会收到锚点与锚点之间的距离信息。其原理可解释为：锚点与锚点之间也要开启相互测距，距离测量值存储在锚点的缓存中。当标签向锚点发送测距请求后，锚点向标签回复时会将回声测距值加入响应信息中，这样标签就收到了两锚点间的距离测量。

锚点标定阶段的待估计量为标签在全局系G下的位置p_T和锚点位置p_Ai：

可构建非线性最小二乘问题：

式(7)中C是锚点的集合，通过求解代价函数可标定出标签和每个锚点的位置。标签与机体系原点的偏移量被认为是已知的先验信息，则可推导出此时无人机在全局系下的位置p，这个值将作为状态估计器的迭代初值p₀。

(3)无迹卡尔曼滤波单元

卡尔曼(R.E.Kalman)在1960年首次提出卡尔曼滤波(Kalman Filter,KF)，KF是一种基于模型线性最小方差估计，其标准形式具有递推计算等优点。但是KF只适用于系统方程和观测方程均为线性的情况，即系统是线性高斯的。在线性高斯系统中使用KF可将状态与噪声分别进行传播然后构成状态传播后的线性高斯系统，从而完成递推估计，这个结论在非线性系统时却不能成立。扩展卡尔曼滤波(EKF)通过对方程做线性近似可解决系统和观测为非线性时的估计问题，但是线性化过程舍弃了二阶以上的高阶项，导致针对强非线性系统的状态估计精度不足。

本节设计的状态估计器以无迹卡尔曼滤波(UKF)为核心，状态估计器以无迹卡尔曼滤波为理论基础进行设计，UKF不同于EKF那样对系统进行线性近似，而是对状态的概率分布进行近似，其核心思想是在状态传播前对状态进行采样，将采样后的点集(称为Sigma点)按照非线性的系统方程和观测方程进行状态传播，然后对传播后的Sigma点按照高斯分布去估计其分布函数，从而估计出均值和协方差。尽管还是在高斯分布的假设下，但避免了EKF线性化过程带来的误差，同时通过增加采样点使传播后的状态的分布更接近真实分布。UKF与EKF几乎具有相同的时间复杂度，但是UKF的估计精度更高，在实现上也比EKF更为简单。

无迹卡尔曼滤波是基于模型状态估计方法，因此在进行传感器融合和状态估计之前需要确定系统方程和观测方程，选择估计器的系统状态矢量。一般来说系统方程和观测方程具有如下形式：

其中u_k为系统的输入，w_k零均值的系统噪声。y_k为状态的观测，η_k为零均值的观测噪声。w_k和η_k不相关，方差分别为和设x_k，w_k，η_k的维数为分别为n，p，q。

估计器主要对无人机位置、速度和姿态等进行估计，因此选择系统的状态矢量为：

其中p为无人机在全局系下的位置，v为速度，q为单位四元数，表示无人机的姿态，b_a和b_ω为IMU零偏。

加入了IMU传感器后，系统方程可列写为如下形式：

这里IMU的读数作为系统方程的输入系统噪声矢量为

当k＝0时启动状态估计器，首先对系统状态迭代初值进行初始化，在静置状态下采集400组IMU加速度计和陀螺仪测量数据后取平均值，对状态中的加速度零偏初值和角速度零偏初值进行估计:

位置初值p₀按照锚点标定步骤的结果推导,其他状态变量初始化为零，协方差矩阵按经验值初始化:

无人机起飞后的k时刻采集到IMU数据时，利用此数据对k+1时刻系统状态做预测。假设此时系统状态x_k的均值和协方差为和P_k，系统噪声w_k的均值和协方差为和计L＝n+p维的增广状态与对应的协方差为：

通过对系统状态应用UT变换得到Sigma点的集合矩阵：

其中，λ是标量参数，λ＝α²(L+k)-L，常量α表征Sigma点在状态附近的分散程度，1e^-4≤α≤1，k为辅助缩放参数。(·)_i表示在矩阵·中取第i列。

然后将Sigma点按照非线性的系统方程进行传播得到新的Sigma点，而不是对系统进行线性化近似：

其中为Sigma点中的状态部分，为Sigma点中的系统噪声部分。经过非线性系统方程传播的Sigma点集χ_ik+1|k可以用来预测传播后的状态均值：

这里W_i ^m，W_i ^c为标量权重

上述为估计器的预测步骤，当k+1时刻有UWB或定高雷达的观测量y_k+1到达时，执行更新步骤。首先将预测步骤得出的状态均值与观测噪声η_k组成L＝n+q维增广状态：

然后按照UT变换生成新的Sigma点集χ_ik+1|k，χ_ik+1|k通过非线性的观测方程h(x_k,η_k)传播生成点集y_ik+1|k，y_ik+1|k用来生成估计的观测值

由此，可计算卡尔曼增益K进行状态的更新，得到后验状态的均值与协方差P_k+1：

多传感器融合主要是指UWB测距模块和IMU惯性测量传感器融合，还加入雷达高度计辅助测量z轴方向的高度，使无人机状态估计更加准确，步骤六介绍了融合的过程。

UWB定位方法只需部署低成本的传感器即可完成在GPS拒止环境下的位置估计，不受光照和天气的影响，更重要的这种方法具备全局参考系，为多无人机的直接应用提供了便利。但UWB传感器单独作用时的定位效果并不理想，首先是UWB信号容易受到干扰，在障碍物密集的非视距传播场景下定位精度有所下降，稳定性一般；其次因为采用常值速度假设导致UWB定位方法的精度不高；第三是位置估计更新速率受限于UWB测量频率，大约在20Hz左右，要保持无人机的稳定性，位置更新速率至少要100Hz以上，显然这并不满足控制器对状态估计的要求。IMU可以高频测量机体的三轴的加速度和角速度，恰好可以弥补UWB单独作用时的缺陷。因此可以将UWB与IMU测量进行融合，从而提升无人机在GPS拒止环境下的定位性能。

(4)多传感器冗余单元

本方法通过融合定高雷达用来提升z轴估计精度，传感器安装在机架背面。

为了使用定高雷达对系统状态中位置p的z轴分量进行修正，在将定高雷达测量值送入估计器更新前需要对雷达测量数据进行一步处理，当读取雷达测量数据时，利用无人机的姿态将投影到全局系的z轴上再用于状态更新，定高雷达的投影方程与观测方程如式(2-24)和(2-25)：

式(24)即为投影方程，R_k为姿态四元数的旋转矩阵形式，表示k时刻机体系到全局系的旋转关系，(·)_z表示取矢量中的z轴分量。式(25)为观测方程，为全局系下位置p_k+1的z轴分量的观测值，是定高雷达的测量噪声。

定高雷达的测量原理类似于UWB，雷达的激光发射器向地面发射一束激光，传感器根据发射时间与接收时间计算到地面的直线距离。当地面始终水平时，定高雷达的测量通过投影方程后可直接送入估计器进行状态更新，而当地形发生变化时(如地面某处凹凸不平)，定高雷达测量结果还蕴含了地形信息。为此，本节提出了针对地形估计的多传感器冗余策略，当地形发生变化时，无人机可估计地形并适当改变飞行高度，以免出现意外情况。地形估计器的原理如下：

当获取到定高雷达测量时，利用式(24)根据当前姿态将测量值投影到全局系的z轴上，得到高度测量结果

当时，不对z轴高度进行更新，根据定高雷达的使用说明，定高雷达的最佳使用距离为0.2～20m。且传感器安装在机架背面，当飞机静置于地面时传感器读数大约在0.2左右，当读数小于0.2时可认为产生了离群值，因此直接剔除。

当时，取当前位置估计中的高度分量与高度测量作差计算相对偏差l_err，当|l_err|＞0.3时，认为有地形突变，不使用去更新状态量p_k中的高度，而是直接估计地形：

当但|l_err|≤0.3时，地形没有突变，但可能有缓慢变化，此时先用去更新高度，然后再按照式(2-26)估计地形。

具体来说，系统中的每个无人机采用基于滑动窗口的紧耦合视觉惯性里程计完成自主定位，它的输出结果包括无人机位姿和环境路标点，包括：

对IMU进行预积分，IMU预积分项将在后端优化时用作构建集束调整方程的残差项；

预处理视觉观测的最新帧，利用KLT稀疏光流法跟踪现有特征，同时检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉测量残差定义在单位球面上，视觉测量残差即重投影残差；

用紧耦合融合的方法保证求解的精度，采用边缘化的思想将这些约束信息转换成状态先验条件加入到优化当中，通过构造集束调整方程做非线性最小二乘优化；

以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解。

对IMU进行预积分的方式达到减少计算量，IMU预积分项将在后端优化时用作构建集束调整方程的残差项，包括：

对于两帧图像之间的时间区间，存在着较多IMU的测量值，为了避免重复传播，采用对IMU进行预积分的方式：

对于滑动窗口中第k帧图像和其相邻的第k+1帧图像之间的IMU测量值，其IMU预积分的残差项可定义为：

是两个连续帧间利用带有噪声的陀螺仪和加速度计测量值得到的IMU预积分观测。表示四元数的三维误差状态，[·]_xyz表示提取四元数q的虚部。加速度计和陀螺仪零偏也包括在在线修正的残差项中。

预处理视觉观测的最新帧，利用KLT稀疏光流法跟踪现有特征，同时需要检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉观测残差定义在单位球面上，视觉测量残差即重投影残差，包括：

当相机拍摄的最新帧到达之后，首先进行视觉观测的预处理，步骤包括使用KLT稀疏光流法跟踪现有特征，为了确保特征点的数目维持在特定数量，在跟踪原有特征的基础上同时需要检测新的特征，另外该步骤还负责筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，选择关键帧有两个准则：一是与前一关键帧的平均视差，如果最新帧与前一帧的平均视差大于某一阈值κ₁，就将最新帧设置为新关键帧；二是特征跟踪的质量，如果特征跟踪数量低于某一阈值κ₂，就将最新帧设置为新关键帧。

视觉测量残差即重投影残差，假设在第i帧图像时首次观测到了第l个特征点，它的像素坐标记为随着无人机的运动，特征点l在第j帧图像中被再次观测到，由此可定义视觉测量残差，这里将视觉测量残差定义在单位球面上，则视觉测量残差定义为：

其中是在第i帧中首次观测到的第l个特征点的像素坐标，是该特征点在第j帧图像中的像素坐标，是包含相机内参的反投影函数。

由于视觉测量残差只有两个自由度，而残差矢量是个三维矢量，这里将残差矢量投影到正切平面上，从而保证正确的自由度。b₁，b₂是两个任意选择的正交基，它们能够张成的正切平面。

进一步的，以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解，包括：

以无人机在全局系下位姿作为角点，即图中的待优化变量是无人机全局位姿。图中的边包括一元边和二元边。

采用SE(3)的变换矩阵形式：

其中，R_k表示无人机在k时刻的姿态，p_k表示无人机在k时刻的位置。

构建的图，以无人机在全局系下位姿作为角点，即图中的待优化变量是无人机全局位姿。图中的边包括一元边和二元边。

(1)增量边

由于视觉惯性里程计测量的是无人机在惯性系下的位姿而待优化变量是无人机在全局系下的位姿T_k。为了摆脱参考系的限制，这里使用位姿增量的形式，表示无人机k时刻位姿与k-l时刻位姿之间的增量，表示在k-l时刻的机体系下。位姿增量可根据视觉惯性里程计的估计结果推导：

因此图中的增量边连接两个时刻的位姿角点，表示两时刻的位姿的增量，增量边是二元边。

(2)全局位姿边

全局位姿边来自UWB定位的结果，表示无人机在某时刻的全局位姿测量，全局位姿边是一元边。

构建的图G＝{V,E}，k时刻的某一角点用增量边连接较早时刻的角点，黑色连线代表全局位姿边，全局位姿边是一元边。

现定义滑窗中的系统状态矢量为：

X＝[T₀,T₁,…,T_n] (3-5)

在滑窗内的某一时刻k，无人机获取到新的测量值后，定义图优化问题的代价函数如下：

式(6)中T表示滑窗中全局位姿边的集合，表示全局位姿残差：

S表示滑窗中所有增量边的集合，表示位姿增量残差：

其中符号！代表SE(3)上的广义减法，通过整理代价函数(3-6)中的每一项得到：

使代价函数(3-9)最小化，可求出滑窗中的状态矢量的最优估计

定义代价函数时在残差项外加入了鲁棒核函数(Robust Kernel)。使用Huber核函数来抑制离群值的影响，表达式为：

实施例二：

本实施例提供提供一种面向多无人机协同定位的UWB与视觉融合定位系统，包括：

输入模块：获取UWB、IMU、双目相机和定高雷达数据；

UWB模块：用于通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；

里程计模块：用于融合IMU和双目相机做数据构成视觉惯性里程计,估计无人机惯性系下的位姿，实现视觉定位，输出得到无人机位姿和环境路标点，同时创建和广播关键帧消息。

位姿优化模块：用于构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计。

具体硬件包括：

UWB测距模块：采用一种波段在3.1GHz到4.8GHz之间的超宽带无线收发器，可同时执行四种功能：测距、数据传输、单基地雷达和多基地雷达，主要使用该模块的测距功能，通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。

IMU惯性测量传感器：主要实现三方面的数据处理，一是通过无迹卡尔曼滤波器融合UWB测距模块和定高雷达数据，估计出无人机在全局坐标系下的位姿，实现UWB的定位功能；二是搭配双目相机实现对视觉观察的环境特征点的检测、跟踪和预处理，实现无人机位姿状态估计；三是创建和广播关键帧消息，便于后续使用视觉传感器通过回环检测感知其他无人机。

双目相机：收集环境信息，将数据传回机载处理器，提取环境特征点，配合IMU惯性测量传感器共同组成视觉惯性里程计。

雷达高度计：测量无人机高度数据，根据实测的高度数据校正和调整UWB定位结果。

嵌入式机载处理器：机载Intel第八代高算力处理器，能够处理各类传感器数据信息，运行回环检测算法和求解位姿图，实现位姿坐标对齐和位姿优化。

无人机平台：包括四旋翼无人机飞行器、电机、电池和飞控部件。

UWB测距模块联合IMU惯性测量传感器和雷达高度计实现基于UWB与IMU融合的鲁棒自主定位与建图技术的定位功能；IMU惯性测量传感器和双目相机组成视觉惯性里程计，通过对飞行环境信息的感知和测量实现基于滑动窗口的紧耦合的自主定位，估计多无人机的位姿状态，同时创建和广播关键帧，实现多人机的回环检测得到相对位姿信息，根据观测量和待优化变量构建位姿图，计算代价函数的最小值，实现多无人机位姿坐标对齐和位姿优化。

本实施例的装置可用于实现实施例一所述的方法。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，包括以下步骤：

获取UWB、IMU、双目相机和定高雷达数据；

通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；

融合IMU和双目相机做数据构成视觉惯性里程计,估计无人机惯性系下的位姿，实现视觉定位，输出得到无人机位姿和环境路标点，同时创建和广播关键帧消息；

构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计。

2.根据权利要求1所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，系统中的每个无人机创建和广播关键帧消息以支持后续的协同定位工作，包括：

为开辟特征提取线程，根据平均视差和特征跟踪的质量提取相机拍摄图像帧的关键帧，当捕获最新的关键帧时，执行以下步骤：

创建新的关键帧消息；

提取1000个特征并计算对应的BRIEF描述子，将它们写入关键帧消息；

将里程计恢复出3D路标点并将其对应的描述子写入关键帧消息；

将位姿估计矩阵写入关键帧消息；

向网络中广播关键帧消息；

所述关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的回环，从而形成回环观测，运动信息用于计算位姿增量，从而形成里程计增量观测。

3.根据权利要求1所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计，包括：

当无人机采集到最新的关键帧，它的里程计特征提取线程会向系统内广播关键帧信息，询问其他个体是否经历过类似场景，其他个体的回环检测程序会进行场景识别，从图像数据库中筛选出回环候选帧，向询问方答复；

一旦询问方收到答复将计算两无人机相对位姿，这个相对位姿将作为协同定位算法的回环观测送入优化器构建位姿图；采用词袋模型技术快速比较两帧图像的特征向量，然后使用结合RANSAC方法的PnP计算相对位姿；

将所述两无人机相对位姿送入优化器构建位姿图，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿。

4.根据权利要求3所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，所述回环检测程序包括：

首先利用词袋模型技术判断是否两帧图像包含类似的场景，从而形成互回环；

采用Kmeans++算法构造字典，对于每一张字典训练图像，图像特征点使用BRIEF描述子表征，Kmeans++算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点，然后对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d；最后会得到一个拥有n个叶子节点的树，每一个叶子节点即为视觉词汇；

使用频率—逆文档频率分配视觉词汇权重；TF认为视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此TF可表达为：

其中m_i表示该词汇ω_i在一幅图像中出现的次数，m为图像中的词汇总数；

IDF认为某个词汇在字典中出现的频率越低，则在分类图像时区分度越高，应该加大权重，因此IDF可表达为：

其中n_i表示建立的字典中词汇ω_i的数量，n是字典中所有视觉词汇的数量；

一个词汇的权重取为TF和IDF的乘积：

η_i＝TF_i×IDF_i (3)

计算机在生成k时刻图像KF_k的特征向量时，将图像特征点的BRIEF描述子与构建好的字典进行比对，选择每个层级内使汉明距离最小的中值节点，从树根开始自上而下遍历字典直到叶节点，构造出一幅图像的带权重特征向量：

v_k＝{(ω₁,η₁),(ω₂,η₂),…,(ω_n,η_n)} (4)。

5.根据权利要求3所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，无人机在交换信息时的通讯策略包括：

某时刻UAV_α将关键帧消息广播至网络中向系统内的无人机询问是否存在回环候选，系统内UAV_β的回环检测程序使用词袋模型查询图像数据库，一旦发现回环候选，会将图像信息打包成消息回复给UAV_α；

询问方收到回复消息，执行带RANSAC算法的PnP求解出进而推导出二者的相对位姿^αT_β；

相对位姿不仅会被送入位姿图优化程序作为视觉回环观测，还会拷贝一份副本发送给UAV_β。

6.根据权利要求5所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，执行带RANSAC算法的PnP求解出进而推导出二者的相对位姿^αT_β，包括：

考虑在某时刻UAV_α通过PnP计算出UAV_β的相机系c_β在惯性系w_α下的位姿UAV_α为了得到回环观测，将自身里程计输出值和默认为已知的相机外参矩阵与PnP计算结果相乘，可推导出两无人机机体系间的相对位姿：

这个相对位姿将作为回环观测添加到位姿图优化程序中。

7.根据权利要求3所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，以构建和求解位姿图的方式实时优化和对齐系统内所有个体的位姿，包括：

定义位姿图优化中的系统状态矢量为：

其中n为滑动窗口规模；以系统中全部无人机的位姿作为图优化问题的角点，角点之间使用边相连，包含三种边：

增量边：

增量边仅连接单个无人机位姿角点，表示滑动窗口中无人机k_l时刻位姿与k_l-m时刻位姿之间的位姿增量，一个位姿角点最多与前面三个角点用增量边连接，位姿增量根据视觉惯性里程计的输出结果计算，其与系统状态存在如下关系：

其中i∈{α,β,γ}，表示里程计增量测量，代表SE(3)上的广义加法，n_δT为零均值白噪声；

回环边：

回环边连接多个无人机位姿角点，表示询问方当前帧与应答方数据库中某帧图像存在互回环，回环边是无人机之间的相对位姿，使用PnP计算，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示视觉回环测量，n_T为零均值白噪声；

测距边：

测距边连接多无人机位姿角点，表示某一时刻两机的相对距离，测距值可直接通过UWB传感器读取，其与系统状态存在如下关系：

其中i,j∈{α,β,γ}，表示UWB距离测量，n_d为零均值白噪声；

通过调整角点的位姿以使得代价函数整体最小，优化后的结果是将各无人机的位姿对齐到w_α系；位姿图优化的代价函数可表示为式(10)的形式：

式(10)中S表示系统内所有无人机的增量边的集合，L表示所有回环边的集合，D表示所有测距边的集合；i和j表示无人机的编号，i,j∈{α,β,γ}；

表示里程计位姿增量残差，数学表示为：

表示回环残差，数学表示为：

表示测距残差，数学表示为：

上式中！表示SE(3)上的广义减法；

为减轻计算量，只优化位置和偏航部分，从而转为4DoF位姿图优化；

求解式(10)，可将系统内所有无人机的位姿对齐到w_α系下，得到优化的UAV_α自身的位姿。

8.根据权利要求1所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿，包括：

步骤A：获取UWB原始数据和IMU数据；

步骤B：对所述UWB原始数据进行距离校准，得到校准后的UWB数据；

步骤C：判断锚点位置是否已知，若否，根据所述校准后的UWB数据进行锚点标定后跳转步骤D，若是，跳转步骤D；

步骤D：对所述校准后的UWB数据进行离群值检测，得到筛选后的UWB数据；

步骤E：对所述IMU数据进行低通滤波，得到滤波后的IMU数据；

步骤F：将所述筛选后的UWB数据和滤波后的IMU数据输入状态估计器，得到无人机的位置、速度和姿态信息；所述状态估计器以无迹卡尔曼滤波为核心，对状态的概率分布进行近似，从而估计出均值和协方差；

所述状态估计器的输入是经信息校准和离群值剔除后的UWB测距数据和通过低通滤波处理噪声后的IMU数据；输出是无人机的位置、速度、姿态信息。

9.根据权利要求1所述的面向多无人机协同定位的UWB与视觉融合定位方法，其特征在于，系统中的每个无人机采用基于滑动窗口的紧耦合视觉惯性里程计完成自主定位，包括：

对IMU进行预积分，IMU预积分项将在后端优化时用作构建集束调整方程的残差项；

预处理视觉观测的最新帧，利用KLT稀疏光流法跟踪现有特征，同时检测新的特征，筛选出关键帧，关键帧将被送入后端优化器做状态估计和深度恢复，将视觉测量残差定义在单位球面上，视觉测量残差即重投影残差；

用紧耦合融合的方法保证求解的精度，采用边缘化的思想将这些约束信息转换成状态先验条件加入到优化当中，通过构造集束调整方程做非线性最小二乘优化；

以无人机在全局系下位姿作为角点构建图，即图中的待优化变量是无人机全局位姿，包括一元边和二元边，基于图优化求解。

10.一种面向多无人机协同定位的UWB与视觉融合定位系统，包括：

输入模块：获取UWB、IMU、双目相机和定高雷达数据；

UWB模块：用于通过无迹卡尔曼滤波器融合UWB、IMU和定高雷达数据，估计出无人机在全局坐标系下的位姿；

里程计模块：用于融合IMU和双目相机做数据构成视觉惯性里程计,估计无人机惯性系下的位姿，实现视觉定位，输出得到无人机位姿和环境路标点，同时创建和广播关键帧消息；

位姿优化模块：用于构建位姿优化图，采用全局一元位姿边表示UWB定位测量结果，采用二元增量边的形式表示视觉惯性里程计测量的位姿结果，计算代价函数的最小值，求出位姿状态矢量的最优估计。