[发明专利]局部地图匹配与端到端测距多无人机协同定位系统和方法

权利要求书

1.一种局部地图匹配与端到端测距多无人机协同定位系统，其特征是，包括：四旋翼结构无人机，进行高频图像采集的工业级灰度相机作为机载环境特征感知设备，左右目相机型号相同，左右目相机通过固连方式连接在无人机机架上，并通过硬件触发方式同步触发采集图像；IMU惯性测量单元用于高频测量无人机的加速度和角速度信息，内部由一个三轴陀螺仪和三轴加速度计组成；机载计算机内置wifi模块用于支持无人机间通讯；机载超宽带UWB模块进行测距组网，每个模块按照时分多址TDMA协议依次向组网内的节点测距；机载飞行控制器作为无人机的姿态控制器实现无人机的完整闭环飞行；机载计算机中进行：

(1)使用视觉惯性里程计进行自主定位和状态估计

使用视觉惯性里程计将双目相机和IMU测量值做融合进行自主定位和状态估计进而反馈到控制器完成自主飞行，视觉惯性里程计基本的输出结果包括无人机位姿和稀疏环境地图，为支持协同定位工作，视觉惯性里程计还负责创建、打包和广播关键帧消息作为无人机间的信息传递；

(2)局部地图匹配

当集群中某一无人机UAV_i,i∈{α,β,γ,}收到询问方广播的关键帧消息时，使用词袋模型技术将图像特征描述子转换为特征向量，对自己的数据库内各帧图像做相似性评分，选出评分最高的图像作为匹配帧并将图像信息回复给询问方，询问方使用当前帧恢复出的3D局部稀疏地图与匹配帧的2D特征点做匹配，计算出两无人机在产生互回环时刻的相对位姿；

(3)位姿图优化

位姿图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以高斯-牛顿G-N法或最小二乘估计L-M迭代变量使得代价函数最小，这样做的结果是每个无人机在确保自身准确的位置估计同时也可做到对同伴位置的感知协同定位，进而方便的执行编队飞行、避碰避障任务；其中：

第一部分，使用视觉惯性里程计详细步骤：双目相机与IMU融合可组成较为鲁棒的视觉惯性里程计，用于实时估计无人机在自身惯性参考系下的状态，采用基于关键帧方法的紧耦合优化视觉惯性里程计，在滑动窗口中将相机关键帧和IMU测量值组成集束调整方程，里程计的输出结果包括自身位姿和稀疏局部地图点，位姿信息用于反馈到控制器保持闭环，为支持后续的协同定位工作，里程计在原有基础上额外增加特征提取环节，当UAV_i,i∈{α,β,γ}在k时刻捕获到最新的关键帧KF_k时，特征提取环节执行以下步骤：

1)创建新的关键帧消息

2)对该关键帧提取1000个特征点并计算BRIEF描述子，将它们按顺序写入关键帧消息

3)将里程计位姿估计矩阵写入关键帧消息

4)对系统中的无人机广播关键帧消息

关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的地图匹配，运动信息用于计算运动增量；

第二部分，局部地图匹配详细步骤：关键帧消息中的视觉特征信息用于无人机进行局部地图匹配，如果多个无人机重复飞越了同一场景导致捕获了某些相似的视觉特征，这种情况称为互回环，互回环可用于无人机间进行局部地图匹配，从而计算出互回环时刻的相对位姿，当无人机采集到最新时刻的关键帧时，它的视觉惯性里程计会向局域网络内广播关键帧信息，询问同伴是否经历过相似场景，同伴收到消息后，它的局部地图匹配环节会生成最新帧的特征向量然后在图像数据库搜寻匹配帧，向询问方答复，一旦询问方收到答复将使用PnP算法计算出二者的相对位姿，这个相对位姿将被送入图优化环节以对齐同伴的位姿至自身的惯性参考系：

1)特征向量生成

特征向量的生成使用词袋模型BOW技术，词袋模型技术可将图像特征描述为特征向量，无人机对两帧图像相似度的比较可进而转换成对特征向量的定量比较，词袋模型将图像看成若干个“视觉词汇”的集合，视觉词汇之间没有顺序；

使用词袋模型技术首先需要构造视觉字典，字典是视觉词汇的全集，无人机根据一张图像的视觉词汇出现情况，构造一个以字典为基底的二进制向量，称为特征向量，表征图像中是否有某类特征存在，首先采用kmeans++算法离线构造字典，然后再将字典内置于每一个无人机中，其基本步骤如下：

1)在相似环境中采集大量训练图像，对于每一张字典训练图像，提取不少于1000个的特征点，并且使用BRIEF描述子表征特征点；

2)使用kmeans++算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点；

3)对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d，最后会得到一个拥有n个叶子节点的字典树，每一个叶子节点即为视觉词汇；

在构建字典时还要分配词汇的权重，采用频率—逆文档频率TF-IDF分配视觉词汇权重，TF表示视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此TF可表达为：

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

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

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

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

η_i＝TF_i×IDF_i

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

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

2)数据库搜索

当字典是在相似环境下训练好的模型时，无人机搭载的机载计算机可实时生成特征向量并进行相似图像搜索，UAV_α在k时刻广播的关键帧消息被UAV_β收到，UAV_β的数据库搜索环节使用视觉字典创建图像特征向量，随后向图像数据库查找匹配帧，设UAV_α的关键帧KF_α特征向量为v_α，UAV_β数据库中某一帧图像KF_β特征向量为v_β：

v_α＝{(ω₁,η₁),(ω₂,η₂),…,(ω_n,η_n)}

v_β＝{(ω₁,η₁),(ω₂,η₂),…,(ω_n,η_n)}

式中ω为视觉词汇，η为对应的权值，UAV_β为了评估两帧图像的相似性需要对特征向量进行相似性评分，鉴于向量的维数较大，本系统选择计算量较低的L1范数来计算相似性评分：

如相似性评分大于某一阈值，则UAV_β认为KF_α和KF_β包含相似的场景，即KF_β是KF_α匹配帧，此时，UAV_β会将匹配帧相关视觉特征打包成消息发送回询问方，以便询问方UAV_α的PnP环节计算二者的相对位姿；

3)计算相对位姿

当询问方UAV_α收到了UAV_β回复的匹配帧消息，便可进一步计算两无人机在当前关键帧时刻的相对位姿矩阵为后续位姿图优化环节提供局部地图匹配观测，具体来说，一旦收到匹配帧，询问方UAV_α的PnP环节用当前关键帧KF_α恢复出的稀疏局部地图与匹配帧KF_β中的特征点进行匹配，通过比较BRIEF描述子寻找最小汉明距离可确认两帧间的一组3D地图点-2D特征点集，通过这组3D-2D点集可计算出匹配帧相机系c_β在当前帧惯性参考系w_α下的位姿再通过已知参数可进一步推导出两无人机机体系间的相对位姿^αT_β；

根据一组3D-2D点集计算相机系-惯性系间的位姿，采用结合RANSAC方法的PnP求解方式，PnP的计算结果为为得到两无人机的相对位姿，UAV_α将自身的里程计输出默认为已知的相机外参矩阵与PnP计算结果相乘，推导出两无人机机体系的相对位姿：

其中是两无人机的相对位姿；

第三部分，位姿图优化：协同定位方法的核心是位姿图优化环节，图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以G-N法或L-M迭代变量使得代价函数最小，详细步骤如下：

图优化环节把待求解的问题表示成图Graph简写为G，一个图由若干个角点Vertex，以及连接着这些角点的边Edge组成，图G可以用数学表示为G＝{V,E}，其中V为角点集，角点表示待优化的变量；E为边集，边表示角点之间的联系，角点为所有个体在自身惯性参考系w_α下的位姿边为无人机的观测量，包括运动增量观测既增量边，局部地图匹配观测既地图匹配边，UWB距离观测既测距边：

1)运动增量观测构建

关键帧消息包含视觉特征信息和无人机运动信息，视觉特征信息用于局部地图匹配，运动信息用于计算运动增量，当无人机接收到任一同伴的关键帧消息，首先将其存入到消息队列，然后提取当前时刻k与较早某时刻k-l关键帧消息中的里程计部分计算运动增量，过程如下：

其中是UAV_i,i∈{α,β,γ}在k时刻的运动增量，表示在b_k-l系下，和来自UAV_i关键帧消息中的里程计部分；

2)局部地图匹配观测构建

局部地图匹配观测来自PnP环节的输出结果，表达式如下：

其中i,j∈{α,β,γ}，其具体构建过程参考局部地图匹配原理；

3)UWB距离观测构建

UWB模块作为机载传感器安装在每一个无人机上，负责周期性地测量无人机之间的相对距离，其测量过程列写为：

其中表示搭载于UAV_i上的UWB模块在k时刻对UAV_j的距离测量，t_ij为超宽带电波的飞行时间，c为光速；

4)离群值检测方法

如果局部地图匹配观测或UWB距离观测满足以下三条之一则认为产生了离群值，离群值将不会用于构建位姿图：

其中i,j∈{α,β,γ}，条件1表示前后两次地图匹配计算的相对位姿应当与运动增量形成一个闭环，这里默认第一次地图匹配观测是正确的；条件2表示某时刻的UWB距离观测应该与地图匹配产生的相对位姿保持一致，这里(·)_p表示取变换矩阵中的3维位置部分；条件3表示前后两次距离测量不应超过某一阈值，v_max表示两机最大相对速度；

5)位姿图构建

对于系统中包含三个无人机UAV_α、UAV_β和UAV_γ，对于UAV_α来说，其根据无人机间的相互观测构建位姿图从而转化为代价函数，通过调整位姿角点使得代价函数整体最小，在一个大小为n的滑动窗口内，定义待优化位姿角点为：

角点之间以各种边相连，边包括增量边、地图匹配边和测距边；

1)增量边

增量边仅连接单个无人机位姿角点，表示滑动窗口中某无人机k_l时刻位姿与k_l-m时刻位姿之间的运动增量，令m＝3，即一个位姿角点最多与前面三个角点用增量边连接，其与位姿角点存在如下关系：

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

2)地图匹配边

地图匹配边连接多个无人机位姿角点，表示询问方当前帧与应答方匹配帧存在局部地图匹配关系，其与位姿角点存在如下关系：

其中i,j∈{α,β,γ}，表示局部地图匹配观测，n_T为零均值白噪声；

3)测距边

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

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

由上述位姿图转化的代价函数表示为：

式中表示系统内所有无人机的增量边的集合，表示所有地图匹配边的集合，表示所有测距边的集合，i和j表示无人机的编号，i,j∈{α,β,γ}；

表示运动增量残差，数学表示为：

表示地图匹配残差，数学表示为：

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

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

通过求解代价函数，无人机UAV_α再次优化了自身位姿，同时将同伴的位姿对齐至惯性参考系w_α下，从而完成协同定位工作；

对于多于3架无人机，按照3架无人机位姿图构建步骤类推。