局部地图匹配与端到端测距多无人机协同定位系统和方法与流程

1.本发明涉及多无人机编队飞行与避碰避障领域，尤其涉及一种无人机编队成员协同定位与状态估计领域。具体讲，涉及基于局部地图匹配与端到端测距的多无人机协同定位方法。


背景技术：

2.小型四旋翼无人机由于机动性高，具有俯瞰视角等优点，已被逐渐应用于军事甚至是民用领域。随着使用场景日益丰富，为提升作战效率和任务成功率，多无人机系统开始进入人们的视线。多无人机系统拥有极高的互相协作能力，能够在更多的环境和场景下提供更大的覆盖面积、更优的感知能力，在进行复杂且危险的任务时，能够更好的降低风险。直至目前多无人机协作任务多局限于可通过全球导航卫星系统(gnss)为无人机提供绝对位置的环境，而导航定位拒止环境下的多无人机感知和避碰避障仍存在极大的挑战性，特别是在城市峡谷、室内或森林中作业时，导航定位拒止环境会严重影响无人机的任务执行效率。
3.多无人机在执行协作任务时，为了达到编队飞行、内部避碰避障等目标，需要在明确自身位置的基础上，进一步感知和估计同伴位置。在导航定位拒止环境下可为多无人机系统提供所需位置信息的方法有室内定位系统，其通过在室内布置多个动作捕捉相机跟踪目标运动，然后定位系统将目标位置反馈到地面站，由地面站广播各无人机位置，这种方法的定位精度能够达到厘米级，但缺点是造价昂贵和有限的覆盖范围。超宽带(ultra
‑
wideband，uwb)是近年来一种新颖的电波测距技术，它通过测量电波的飞行时间、到达时间差或到达角计算出两个模块之间的距离。由于发射电波的波段在3.1ghz
‑
4.8ghz之间，能够有效克服其他电波信号的干扰。此外其较高的带宽能够轻易克服多径效应、减弱非视距测量的影响。超宽带技术可替代昂贵的室内定位系统来支持多无人机定位，基本原理是在环境四周部署已知全局位置的uwb锚点，每个飞机上固连一个uwb标签，标签与锚点间周期性测距，再通过三边测量或卡尔曼滤波算法定位，系统中的个体通过共享位置信息即可感知同伴。目前，新加坡南洋理工大学谢立华团队、新加坡国立大学陈本美团队已经使用这种方法完成了多无人机编队飞行，但是这种部署锚点的方法同样限制了覆盖范围，不能使多无人机系统即刻应用于未知环境。
4.相对于部署锚点等外部设施，无人机搭载相机和imu等机载传感器就能够在未知场景中完成自主定位，然后通过相互协作和交换信息感知同伴(协同定位)，基本原理是无人机通过提取和跟踪环境特征完成自身位置估计和特征深度恢复，然后无人机间通过共享的环境特征与存储的稀疏地图匹配，可获得同伴相对于自身惯性参考系的位置。香港科技大学aerial robotics group、苏黎世联邦理工学院vision for robotics lab已经使用这种方法进行了多无人机编队飞行实验，但是这种方法也存在着明显的不足，首先是要求无人机飞行轨迹具有部分重叠且相机的拍摄视角基本相同以满足地图匹配的要求，其次是地图可能会误匹配导致错误的位姿对齐结果，另外视觉定位方法由于边缘化等原因具有长时
间位姿估计漂移问题，导致整个多无人机系统随着时间增长协同定位精度下降。
5.综上所述，国内外学者针对多无人机系统位置感知问题进行了卓有成效的研究，并取得了丰硕的研究成果。然而，无论是基于uwb锚点还是基于局部地图匹配的方法，均不能使无人机在未知环境下长时间、高精度地完成对自身和同伴的位置估计。


技术实现要素：

6.为克服现有技术的不足，本发明旨在解决导航定位拒止环境下多无人机系统位置感知问题，提出一种基于局部地图匹配与端到端测距的多无人机协同定位方法，该方法不需要在环境中部署任何设施，多无人机间仅需相互测距和信息共享，就可在保证高精度的自身位置估计同时计算同伴位置。同时，该方法具有良好的鲁棒性和抗干扰能力，可以在测距产生离群值或者地图匹配暂时失效时仍能做到对同伴的位置估计。为此，本发明采取的技术方案是，局部地图匹配与端到端测距多无人机协同定位系统，包括：四旋翼结构无人机，进行高频图像采集的工业级灰度相机作为机载环境特征感知设备，左右目相机型号相同，左右目相机通过固连方式连接在无人机机架上，并通过硬件触发方式同步触发采集图像；imu惯性测量单元用于高频测量无人机的加速度和角速度信息，内部由一个三轴陀螺仪和三轴加速度计组成；机载计算机内置wifi模块用于支持无人机间通讯；机载uwb模块进行测距组网，每个模块按照时分多址tdma协议依次向组网内的节点测距；机载飞行控制器作为无人机的姿态控制器实现无人机的完整闭环飞行；机载计算机中进行：
7.(1)视觉惯性里程计
8.使用视觉惯性里程计将双目相机和imu测量值做融合进行自主定位和状态估计进而反馈到控制器完成自主飞行，视觉惯性里程计基本的输出结果包括无人机位姿和稀疏环境地图，为支持协同定位工作，视觉惯性里程计还负责创建、打包和广播关键帧消息作为无人机间的信息传递；
9.(2)局部地图匹配
10.当集群中某一无人机uav
i
,i∈{α,β,γ,
…
}收到询问方广播的关键帧消息时，使用词袋模型技术将图像特征描述子转换为特征向量，对自己的数据库内各帧图像做相似性评分，选出评分最高的图像作为匹配帧并将图像信息回复给询问方，询问方使用当前帧恢复出的3d局部稀疏地图与匹配帧的2d特征点做匹配，计算出两无人机在产生互回环时刻的相对位姿；
11.(3)位姿图优化
12.位姿图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以高斯
‑
牛顿g
‑
n法或最小二乘估计l
‑
m迭代变量使得代价函数最小，这样做的结果是每个无人机在确保自身准确的位置估计同时也可做到对同伴位置的感知协同定位，进而方便的执行编队飞行、避碰避障任务。
13.局部地图匹配与端到端测距多无人机协同定位方法，利用前述系统实现，利用机载计算机进行：
14.(1)视觉惯性里程计
15.使用视觉惯性里程计将双目相机和imu测量值做融合进行自主定位和状态估计进而反馈到控制器完成自主飞行，视觉惯性里程计基本的输出结果包括无人机位姿和稀疏环
境地图，为支持协同定位工作，视觉惯性里程计还负责创建、打包和广播关键帧消息作为无人机间的信息传递；
16.(2)局部地图匹配
17.当集群中某一无人机uav
i
,i∈{α,β,γ,
…
}收到询问方广播的关键帧消息时，使用词袋模型技术将图像特征描述子转换为特征向量，对自己的数据库内各帧图像做相似性评分，选出评分最高的图像作为匹配帧并将图像信息回复给询问方，询问方使用当前帧恢复出的3d局部稀疏地图与匹配帧的2d特征点做匹配，计算出两无人机在产生互回环时刻的相对位姿；
18.(3)位姿图优化
19.位姿图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以g
‑
n法或l
‑
m迭代变量使得代价函数最小，这样做的结果是每个无人机在确保自身准确的位置估计同时也可做到对同伴位置的感知协同定位，进而方便的执行编队飞行、避碰避障任务。
20.其中：
21.第一部分，视觉惯性里程计详细步骤：双目相机与imu融合可组成较为鲁棒的视觉惯性里程计，用于实时估计无人机在自身惯性参考系下的状态，采用基于关键帧方法的紧耦合优化视觉惯性里程计，在滑动窗口中将相机关键帧和imu测量值组成集束调整方程，里程计的输出结果包括自身位姿和稀疏局部地图点，位姿信息用于反馈到控制器保持闭环，为支持后续的协同定位工作，里程计在原有基础上额外增加特征提取环节，当uav
i
,i∈{α,β,γ}在k时刻捕获到最新的关键帧kf
k
时，特征提取环节执行以下步骤：
22.1)创建新的关键帧消息
23.2)对该关键帧提取1000个特征点并计算brief描述子，将它们按顺序写入关键帧消息
24.3)将里程计位姿估计矩阵写入关键帧消息
25.4)对系统中的无人机广播关键帧消息
26.关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的地图匹配，运动信息用于计算运动增量；
27.第二部分，局部地图匹配详细步骤：关键帧消息中的视觉特征信息用于无人机进行局部地图匹配，如果多个无人机重复飞越了同一场景导致捕获了某些相似的视觉特征，这种情况称为互回环，互回环可用于无人机间进行局部地图匹配，从而计算出互回环时刻的相对位姿，当无人机采集到最新时刻的关键帧时，它的视觉惯性里程计会向局域网络内广播关键帧信息，询问同伴是否经历过相似场景，同伴收到消息后，它的局部地图匹配环节会生成最新帧的特征向量然后在图像数据库搜寻匹配帧，向询问方答复，一旦询问方收到答复将使用pnp算法计算出二者的相对位姿，这个相对位姿将被送入图优化环节以对齐同伴的位姿至自身的惯性参考系：
28.1)特征向量生成
29.特征向量的生成使用词袋模型bow技术，词袋模型技术可将图像特征描述为特征向量，无人机对两帧图像相似度的比较可进而转换成对特征向量的定量比较，词袋模型将
图像看成若干个“视觉词汇”的集合，视觉词汇之间没有顺序；
30.使用词袋模型技术首先需要构造视觉字典，字典是视觉词汇的全集，无人机根据一张图像的视觉词汇出现情况，构造一个以字典为基底的二进制向量，称为特征向量，表征图像中是否有某类特征存在，首先采用kmeans 算法离线构造字典，然后再将字典内置于每一个无人机中，其基本步骤如下：
31.1)在相似环境中采集大量训练图像，对于每一张字典训练图像，提取不少于1000个的特征点，并且使用brief描述子表征特征点；
32.2)使用kmeans 算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点；
33.3)对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d，最后会得到一个拥有n个叶子节点的字典树，每一个叶子节点即为视觉词汇；
34.在构建字典时还要分配词汇的权重，采用频率—逆文档频率tf
‑
idf分配视觉词汇权重，tf表示视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此tf可表达为：
[0035][0036]
其中m
i
表示该词汇ω
i
在一幅图像中出现的次数，m为图像中的词汇总数；
[0037]
idf认为某个词汇在字典中出现的频率越低，则在分类图像时区分度越高，应该加大权重，因此idf表达为：
[0038][0039]
其中n
i
表示建立的字典中词汇ω
i
的数量，n是字典中所有视觉词汇的数量；
[0040]
一个词汇的权重取为tf和idf的乘积：
[0041]
η
i
＝tf
i
×
idf
i
[0042]
无人机在生成k时刻图像kf
k
的特征向量时，将图像特征点的brief描述子与构建好的字典进行比对，选择每个层级内使汉明距离最小的中值节点，从树根开始自上而下遍历字典直到叶节点，构造出一幅图像的带权重特征向量：
[0043]
v
k
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0044]
2)数据库搜索
[0045]
当字典是在相似环境下训练好的模型时，无人机搭载的机载计算机可实时生成特征向量并进行相似图像搜索，uav
α
在k时刻广播的关键帧消息被uav
β
收到，uav
β
的数据库搜索环节使用视觉字典创建图像特征向量，随后向图像数据库查找匹配帧，设uav
α
的关键帧kf
α
特征向量为v
α
，uav
β
数据库中某一帧图像kf
β
特征向量为v
β
：
[0046]
v
α
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0047]
v
β
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0048]
式中ω为视觉词汇，η为对应的权值。uav
β
为了评估两帧图像的相似性需要对特征向量进行相似性评分，鉴于向量的维数较大，本系统选择计算量较低的l1范数来计算相似性评分：
[0049][0050]
如相似性评分大于某一阈值，则uav
β
认为kf
α
和kf
β
包含相似的场景，即kf
β
是kf
α
匹配帧。此时，uav
β
会将匹配帧相关视觉特征打包成消息发送回询问方，以便询问方uav
α
的pnp环节计算二者的相对位姿；
[0051]
3)计算相对位姿
[0052]
当询问方uav
α
收到了uav
β
回复的匹配帧消息，便可进一步计算两无人机在当前关键帧时刻的相对位姿矩阵为后续位姿图优化环节提供局部地图匹配观测，具体来说，一旦收到匹配帧，询问方uav
α
的pnp环节用当前关键帧kf
α
恢复出的稀疏局部地图与匹配帧kf
β
中的特征点进行匹配，通过比较brief描述子寻找最小汉明距离可确认两帧间的一组3d地图点
‑
2d特征点集，通过这组3d
‑
2d点集可计算出匹配帧相机系c
β
在当前帧惯性参考系w
α
下的位姿再通过已知参数可进一步推导出两无人机机体系间的相对位姿
α
t
β
；
[0053]
根据一组3d
‑
2d点集计算相机系
‑
惯性系间的位姿，采用结合ransac方法的pnp求解方式，pnp的计算结果为为得到两无人机的相对位姿，uav
α
将自身的里程计输出默认为已知的相机外参矩阵与pnp计算结果相乘，推导出两无人机机体系的相对位姿：
[0054][0055]
其中是两无人机的相对位姿；
[0056]
第三部分，位姿图优化：协同定位方法的核心是位姿图优化环节，图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以g
‑
n法或l
‑
m迭代变量使得代价函数最小，详细步骤如下：
[0057]
图优化环节把待求解的问题表示成图graph简写为g，一个图由若干个角点vertex，以及连接着这些角点的边edge组成，图g可以用数学表示为g＝{v,e}，其中v为角点集，角点表示待优化的变量；e为边集，边表示角点之间的联系，角点为所有个体在自身惯性参考系w
α
下的位姿边为无人机的观测量，包括运动增量观测既增量边，局部地图匹配观测既地图匹配边，uwb距离观测既测距边：
[0058]
1)运动增量观测构建
[0059]
关键帧消息包含视觉特征信息和无人机运动信息，视觉特征信息用于局部地图匹配，运动信息用于计算运动增量，当无人机接收到任一同伴的关键帧消息，首先将其存入到消息队列，然后提取当前时刻k与较早某时刻k
‑
l关键帧消息中的里程计部分计算运动增量，过程如下：
[0060][0061]
其中是uav
i
,i∈{α,β,γ}在k时刻的运动增量，表示在b
k
‑
l
系下，和来自uav
i
关键帧消息中的里程计部分；
[0062]
2)局部地图匹配观测构建
[0063]
局部地图匹配观测来自pnp环节的输出结果，表达式如下：
[0064][0065]
其中i,j∈{α,β,γ}，其具体构建过程参考局部地图匹配原理；
[0066]
3)uwb距离观测构建
[0067]
uwb模块作为机载传感器安装在每一个无人机上，负责周期性地测量无人机之间的相对距离，其测量过程列写为：
[0068][0069]
其中i,j∈{α,β,γ}表示搭载于uav
i
上的uwb模块在k时刻对uav
j
的距离测量，t
ij
为超宽带电波的飞行时间，c为光速；
[0070]
4)离群值检测方法
[0071]
如果局部地图匹配观测或uwb距离观测满足以下三条之一则认为产生了离群值，离群值将不会用于构建位姿图：
[0072][0073][0074][0075]
其中i,j∈{α,β,γ}，条件1表示前后两次地图匹配计算的相对位姿应当与运动增量形成一个闭环，这里默认第一次地图匹配观测是正确的；条件2表示某时刻的uwb距离观测应该与地图匹配产生的相对位姿保持一致，这里(
·
)
p
表示取变换矩阵中的3维位置部分；条件3表示前后两次距离测量不应超过某一阈值，v
max
表示两机最大相对速度；
[0076]
5)位姿图构建
[0077]
对于系统中包含三个无人机uav
α
、uav
β
和uav
γ
，对于uav
α
来说，其根据无人机间的相互观测构建位姿图从而转化为代价函数，通过调整位姿角点使得代价函数整体最小，在一个大小为n的滑动窗口内，定义待优化位姿角点为：
[0078][0079]
角点之间以各种边相连，边包括增量边、地图匹配边和测距边；
[0080]
1)增量边
[0081]
增量边仅连接单个无人机位姿角点，表示滑动窗口中某无人机k
l
时刻位姿与k
l
‑
m
时刻位姿之间的运动增量，令m＝3，即一个位姿角点最多与前面三个角点用增量边连接，其与位姿角点存在如下关系：
[0082][0083]
其中i∈{α,β,γ}，表示运动增量观测，代表se(3)上的广义加法，n
δt
为零均值白噪声。
[0084]
2)地图匹配边
[0085]
地图匹配边连接多个无人机位姿角点，表示询问方当前帧与应答方匹配帧存在局部地图匹配关系，其与位姿角点存在如下关系：
[0086][0087]
其中i,j∈{α,β,γ}，表示局部地图匹配观测，n
t
为零均值白噪声；
[0088]
3)测距边
[0089]
测距边连接多个无人机位姿角点，表示某一时刻两机的相对距离，测距值可直接通过uwb传感器读取，其与位姿角点存在如下关系：
[0090][0091]
其中i,j∈{α,β,γ}，表示uwb距离观测，n
d
为零均值白噪声；
[0092]
由上述位姿图转化的代价函数表示为：
[0093][0094]
式中表示系统内所有无人机的增量边的集合，表示所有地图匹配边的集合，表示所有测距边的集合。i和j表示无人机的编号，i,j∈{α,β,γ}；
[0095]
表示运动增量残差，数学表示为：
[0096][0097]
表示地图匹配残差，数学表示为：
[0098][0099]
表示测距残差，数学表示为：
[0100][0101]
上式中表示se(3)上的广义减法；
[0102]
通过求解代价函数，无人机uav
α
再次优化了自身位姿，同时将同伴的位姿对齐至惯性参考系w
α
下，从而完成协同定位工作；
[0103]
对于多于3架无人机，按照3架无人机位姿图构建步骤类推。
[0104]
由于局部地图匹配环节需要无人机实时交换数据，要综合考虑网络带宽、通信次序、空间时间复杂度等要求，还要考虑到无人机群规模的可扩展性，设计特定的通讯策略：
[0105]
某时刻uav
α
将关键帧消息广播至网络中向系统内的无人机询问是否存在匹配帧，uav
β
的数据库搜索环节程序使用词袋模型技术搜索图像数据库，一旦发现匹配帧会将图像信息打包成消息回复给uav
α
，询问方收到回复消息，执行带ransac算法的pnp求解出进而推导出二者的相对位姿相对位姿不仅会被送入图优化环节构建位姿图，还会
拷贝一份副本发送给uav
β
，目的是避免uav
β
重复计算。
[0106]
结合ransac方法的pnp求解方式步骤如下：针对每一次迭代，首先在3d
‑
2d点集c中随机选择n对数据构成猜测点集c
guess
，利用c
guess
进行pnp求解得到猜测位姿t
guess
,将c
guess
赋给一个临时变量c
temp
，对c中除了c
guess
的所有元素，若某元素与t
guess
之间的重投影误差小于一定阈值τ，则将该元素加入临时变量c
temp
；如果c
temp
中元素数量大于一定阈值d，则利用c
temp
进行pnp求解得到更加精确的位姿t
better
，若此次操作得到的位姿t
better
与c
temp
之间的重投影误差e
better
比上一次得到的最佳误差e
best
要小，则对最佳位姿估计t
best
、重投影误差e
best
、最佳匹配点集c
best
进行更新；重复执行以上操作直至达到迭代次数即可；pnp的计算结果为为得到两无人机的相对位姿，uav
α
将自身的里程计输出默认为已知的相机外参矩阵与pnp计算结果相乘，推导出两无人机机体系的相对位姿：
[0107][0108]
其中是两无人机的相对位姿。
[0109]
本发明的特点及有益效果是：
[0110]
本发明对解决导航定位拒止环境下多无人机系统位置感知问题具有十分重要的意义，本发明稳定可靠，不易受到环境和离群值的影响，同时算法节省计算资源，对硬件计算能力要求较低，具有很高的理论与使用价值。本发明主要具有以下的特点和优点：
[0111]
(1)本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法精度高、成本低廉、实用性强。传统导航定位拒止环境下的多无人机编队状态估计与避碰避障问题常采用室内定位系统等解决方法造价昂贵，而且需要在室内布置多个标定好的高精度相机导致覆盖范围有限，不能即刻应用于未知场景。而本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法不需要预先在环境中部署任何设施，仅靠无人机间相互测距与信息交换即可高精度的感知同伴位置。
[0112]
(2)本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法鲁棒性高、通用性强、不受集群个体数量限制。传统的基于稀疏地图匹配的协同定位方法要求多无人机飞行轨迹有相当部分的重叠，且要求相机的拍摄视角基本相同，一旦无法找到匹配特征则这种方法的估计精度随时间的增长开始下降，不能保证多无人机系统长期运行。而本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法在地图匹配暂时失效时仍用uwb距离测量持续优化位姿以保证定位精度，确保无人机高精度、持续稳定地估计同伴位置。
[0113]
(3)本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法节省网络带宽，无人机间仅需少量的信息交换。传统的基于稀疏地图匹配的协同定位方法需要无人机发送未压缩的关键帧图像，而传输图像需要消耗大量的网络资源，当集群规模扩大到一定程度时会导致局域网络陷入堵塞，导致算法无法实时运行。而本发明提出的基于局部地图匹配与端到端测距的多无人机协同定位方法仅提取图像中的少量特征点和描述子，连同里程计估计值打包成关键帧消息发送至局域网络，这个过程几乎没有带宽消耗。
[0114]
(4)本发明搭建多无人机协同定位硬件平台可扩展性强。除了imu、双目相机和uwb等传感器设备之外，开发者还可自行添加传感器设备，如激光雷达等，在软件算法流程基本
不变的情况下进行二次开发。
附图说明：
[0115]
附图1多无人机协同定位方法硬件平台架构图。
[0116]
附图2多无人机协同定位方法软件算法流程图。
[0117]
附图3视觉字典建立过程图。
[0118]
附图4结合ransac方法的pnp求解流程图。
[0119]
附图5局部地图匹配示意图。
[0120]
附图6数据交换机制示意图。
[0121]
附图7离群值检测方法示意图。
[0122]
附图8位姿图建立示意图。
[0123]
附图9 uav
α
将uav
β
和uav
γ
的位姿对齐至w
a
系的轨迹图。
[0124]
附图10 uav
α
将uav
β
和uav
γ
的位姿对齐至w
a
系的曲线图。
具体实施方式
[0125]
本发明旨在解决导航定位拒止环境下多无人机系统位置感知问题，提出一种基于局部地图匹配与端到端测距的多无人机协同定位方法，该方法不需要在环境中部署任何设施，多无人机间仅需相互测距和信息共享，就可在保证高精度的自身位置估计同时计算同伴位置。同时，该方法具有良好的鲁棒性和抗干扰能力，可以在测距产生离群值或者地图匹配暂时失效时仍能做到对同伴的位置估计。本发明采用的技术方案包括硬件和软件两部分，硬件部分为多无人机感知、通信和计算提供设备支持，包括：机载双目相机、imu惯性测量传感器、uwb测距模块、机载计算机和无人机底层飞行控制平台，其具体的硬件平台架构图如附图1所示。软件部分通过传感器测量值解算出自身和同伴位置，为无人机控制器和航迹规划提供反馈信息，包括：视觉惯性里程计、局部地图匹配和位姿图优化，其具体的软件流程图如图2所示。
[0126]
下面针对本发明中的硬件和软件部分做具体说明。
[0127]
硬件部分：
[0128]
综合考虑各类传感器的特性，平衡无人机平台的动力、功能与功耗等各方面具体指标，再根据各部分算法的要求设计了如附图1所示的硬件平台。平台采用轴距为650mm的四旋翼结构无人机，选择可进行高频图像采集的工业级灰度相机作为机载环境特征感知设备，左右目相机型号均为flir chameleon3，分辨率采用752*480px，采集频率设为20hz，左右目相机通过固连方式连接在无人机机架上，并通过硬件触发方式同步触发采集图像。imu惯性测量单元用于高频测量无人机的加速度和角速度信息，内部由一个三轴陀螺仪和三轴加速度计组成，本发明选择microstrain 3dm
‑
gx5
‑
15型号imu，频率设定为500hz。机载计算机选用intel nuc8i7beh，其包含四核cpu及八个系统级线程和32g内存，主频最高达4.5ghz，计算机内置wifi模块用于支持无人机间通讯。uwb选择搭载美国time domain公司生产的p440模块，此uwb模块的功耗极小(～50uw)，且有较高的测距精度和较鲁棒的抗多径干扰能力，多无人机系统内的uwb模块构成测距组网，每个模块按照tdma(时分多址)协议依次向组网内的节点测距。pixhawk飞行控制器作为无人机的姿态控制器实现无人机的完整
闭环飞行。
[0129]
软件部分：
[0130]
(1)视觉惯性里程计
[0131]
每个无人机使用视觉惯性里程计将双目相机和imu测量值做融合进行自主定位和状态估计进而反馈到控制器完成自主飞行，协同定位部分作为后续模块置于里程计之后独立运行，将里程计与协同定位部分独立开来的好处是即使协同定位部分暂时失效或发生错误，单个无人机仍能维持闭环，保持自主飞行能力。视觉惯性里程计基本的输出结果包括无人机位姿和稀疏环境地图，为支持协同定位工作，视觉惯性里程计还负责创建、打包和广播关键帧消息作为无人机间的信息传递。
[0132]
(2)局部地图匹配
[0133]
当集群中某一无人机uav
i
,i∈{α,β,γ,
…
}收到询问方广播的关键帧消息时，使用词袋模型技术将图像特征描述子转换为特征向量，对自己的数据库内各帧图像做相似性评分，选出评分最高的图像作为匹配帧并将图像信息回复给询问方。询问方使用当前帧恢复出的3d局部稀疏地图与匹配帧的2d特征点做匹配，计算出两无人机在产生互回环时刻的相对位姿。
[0134]
(3)位姿图优化
[0135]
协同定位方法的核心是位姿图优化环节，位姿图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以以高斯
‑
牛顿g
‑
n(gauss
‑
newton)法或最小二乘估计l
‑
m(levenberg
‑
marquardt)迭代变量使得代价函数最小，这样做的结果是每个无人机在确保自身准确的位置估计同时也可做到对同伴位置的感知(协同定位)，进而方便的执行编队飞行、避碰避障等任务。
[0136]
本发明针对导航定位拒止环境下多无人机系统位置感知问题，在无人机相互观测与信息共享的基础上，首次提出基于局部地图匹配与端到端测距的多无人机协同定位方法，使无人机在保证高精度的位置估计同时感知同伴状态。
[0137]
下面结合附图对基于局部地图匹配与端到端测距的多无人机协同定位方法做进一步说明。
[0138]
协同定位方法硬件平台如附图1所示，各传感器通过usb或者串口的方式连接到机载计算机，计算机解析出的位姿信息经由串口发送到pixhawk飞行控制器做进一步姿态控制，以驱动电机电调完成无人机飞行或悬停功能。
[0139]
协同定位软件算法是在linux操作系统下基于ros(robot operating system)机器人开源框架以c 语言程序实现，算法流程如附图2所示。以下针对各部分流程做具体说明，为叙述清晰，这里设多无人机系统由三架飞机uav
α
,uav
β
,uav
γ
组成。
[0140]
第一部分，视觉惯性里程计：双目相机与imu融合可组成较为鲁棒的视觉惯性里程计，用于实时估计无人机在自身惯性参考系下的状态，本发明采用基于关键帧方法的紧耦合优化视觉惯性里程计，在滑动窗口中将相机关键帧和imu测量值组成集束调整方程，里程计的输出结果包括自身位姿和稀疏局部地图点，位姿信息用于反馈到控制器保持闭环。为支持后续的协同定位工作，里程计在原有基础上额外增加特征提取环节。当uav
i
,i∈{α,β,γ}在k时刻捕获到最新的关键帧kf
k
时，它的特征提取环节执行以下步骤：
[0141]
1)创建新的关键帧消息
[0142]
2)对该关键帧提取1000个特征点并计算brief描述子，将它们按顺序写入关键帧消息
[0143]
3)将里程计位姿估计矩阵写入关键帧消息
[0144]
4)对系统中的无人机广播关键帧消息
[0145]
关键帧消息包含丰富的视觉信息和运动信息，视觉信息用于查找无人机之间的地图匹配，运动信息用于计算运动增量。
[0146]
第二部分，局部地图匹配：关键帧消息中的视觉特征信息用于无人机进行局部地图匹配，如果多个无人机重复飞越了同一场景导致捕获了某些相似的视觉特征，这种情况称为互回环。互回环可用于无人机间进行局部地图匹配，从而计算出互回环时刻的相对位姿。当无人机采集到最新时刻的关键帧时，它的视觉惯性里程计会向局域网络内广播关键帧信息，询问同伴是否经历过相似场景，同伴收到消息后，它的局部地图匹配环节会生成最新帧的特征向量然后在图像数据库搜寻匹配帧，向询问方答复。一旦询问方收到答复将使用pnp算法计算出二者的相对位姿，这个相对位姿将被送入图优化环节以对齐同伴的位姿至自身的惯性参考系。
[0147]
1)特征向量生成
[0148]
特征向量的生成使用词袋模型(bag of word，bow)技术，词袋模型技术可将图像特征描述为特征向量，无人机对两帧图像相似度的比较可进而转换成对特征向量的定量比较，词袋模型将图像看成若干个“视觉词汇”的集合，视觉词汇之间没有顺序。
[0149]
使用词袋模型技术首先需要构造视觉字典，字典是视觉词汇的全集，无人机根据一张图像的视觉词汇出现情况，可以构造一个以字典为基底的二进制向量，称为特征向量，表征图像中是否有某类特征存在。本发明首先采用kmeans 算法离线构造字典，然后再将字典内置于每一个无人机中。字典构建过程附图3所示，其基本步骤如下：
[0150]
1)在相似环境中采集大量训练图像，对于每一张字典训练图像，提取不少于1000个的特征点，并且使用brief描述子表征特征点
[0151]
2)使用kmeans 算法对描述子进行k中值聚类，把描述子空间离散化为k个二进制聚类，这些聚类形成字典树中的第一层节点
[0152]
3)对第一层中每个节点相关联的描述子重复聚类操作再聚成k类得到第二层节点，这个步骤的递归深度为d，最后会得到一个拥有n个叶子节点的字典树，每一个叶子节点即为视觉词汇
[0153]
在构建字典时还要分配词汇的权重，本系统采用频率—逆文档频率(termfrequency
‑
inverse document frequency,tf
‑
idf)分配视觉词汇权重。tf表示视觉词汇在一幅图像中出现的频率越高，这个词汇的区分度就高，应该加大权重，因此tf可表达为：
[0154][0155]
其中m
i
表示该词汇ω
i
在一幅图像中出现的次数，m为图像中的词汇总数。
[0156]
idf认为某个词汇在字典中出现的频率越低，则在分类图像时区分度越高，应该加大权重，因此idf可表达为：
[0157][0158]
其中n
i
表示建立的字典中词汇ω
i
的数量，n是字典中所有视觉词汇的数量。
[0159]
一个词汇的权重取为tf和idf的乘积：
[0160]
η
i
＝tf
i
×
idf
i
[0161]
无人机在生成k时刻图像kf
k
的特征向量时，将图像特征点的brief描述子与构建好的字典进行比对，选择每个层级内使汉明距离最小的中值节点，从树根开始自上而下遍历字典直到叶节点，构造出一幅图像的带权重特征向量：
[0162]
v
k
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0163]
2)数据库搜索
[0164]
当字典是在相似环境下训练好的模型时，无人机搭载的机载计算机可实时生成特征向量并进行相似图像搜索，这里以系统中的uav
α
向uav
β
查询为例。uav
α
在k时刻广播的关键帧消息被uav
β
收到，uav
β
的数据库搜索环节使用视觉字典创建图像特征向量，随后向图像数据库查找匹配帧，设uav
α
的关键帧kf
α
特征向量为v
α
，uav
β
数据库中某一帧图像kf
β
特征向量为v
β
：
[0165]
v
α
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0166]
v
β
＝{(ω1,η1),(ω2,η2),
…
,(ω
n
,η
n
)}
[0167]
式中ω为视觉词汇，η为对应的权值。uav
β
为了评估两帧图像的相似性需要对特征向量进行相似性评分，鉴于向量的维数较大，本系统选择计算量较低的l1范数来计算相似性评分：
[0168][0169]
如相似性评分大于某一阈值，则uav
β
认为kf
α
和kf
β
包含相似的场景，即kf
β
是kf
α
匹配帧。此时，uav
β
会将匹配帧相关视觉特征打包成消息发送回询问方，以便询问方uav
α
的pnp环节计算二者的相对位姿。
[0170]
3)计算相对位姿
[0171]
当询问方uav
α
收到了uav
β
回复的匹配帧消息，便可进一步计算两无人机在当前关键帧时刻的相对位姿矩阵为后续位姿图优化环节提供局部地图匹配观测。具体来说，一旦收到匹配帧，询问方uav
α
的pnp环节用当前关键帧kf
α
恢复出的稀疏局部地图与匹配帧kf
β
中的特征点进行匹配，通过比较brief描述子寻找最小汉明距离可确认两帧间的一组3d地图点
‑
2d特征点集，通过这组3d
‑
2d点集可计算出匹配帧相机系c
β
在当前帧惯性参考系w
α
下的位姿再通过已知参数可进一步推导出两无人机机体系间的相对位姿
α
t
β
。
[0172]
根据一组3d
‑
2d点集计算相机系
‑
惯性系间的位姿，可用pnp方法求解。传统的pnp方法解算的精度极易受到离群点和坐标定位噪声的影响，另外传统的pnp只能计算位姿而不能验证匹配帧的正确性。为此本发明提出结合ransac方法的pnp求解方式，基本过程如附图4所示：针对每一次迭代，首先在3d
‑
2d点集c中随机选择n对数据构成猜测点集c
guess
，利用c
guess
进行pnp求解得到猜测位姿t
guess
,将c
guess
赋给一个临时变量c
temp
，对c中除了c
guess
的所有元素，若某元素与t
guess
之间的重投影误差小于一定阈值τ，则将该元素加入临时变量
c
temp
。如果c
temp
中元素数量大于一定阈值d，则利用c
temp
进行pnp求解得到更加精确的位姿t
better
，若此次操作得到的位姿t
better
与c
temp
之间的重投影误差e
better
比上一次得到的最佳误差e
best
要小，则对最佳位姿估计t
best
、重投影误差e
best
、最佳匹配点集c
best
进行更新。重复执行以上操作直至达到迭代次数即可。pnp的计算结果为为得到两无人机的相对位姿，uav
α
将自身的里程计输出默认为已知的相机外参矩阵与pnp计算结果相乘，推导出两无人机机体系的相对位姿：
[0173][0174]
其中是两无人机的相对位姿，局部地图匹配示意图如附图5所示。
[0175]
由于局部地图匹配环节需要无人机实时交换数据，要综合考虑网络带宽、通信次序、空间时间复杂度等要求，还要考虑到无人机群规模的可扩展性。为此，本发明为无人机交换信息设计了特定的通讯策略。
[0176]
以系统中的uav
α
与uav
β
两机为例，如附图6所示。某时刻uav
α
将关键帧消息广播至网络中向系统内的无人机询问是否存在匹配帧，uav
β
的数据库搜索环节程序使用词袋模型技术搜索图像数据库，一旦发现匹配帧会将图像信息打包成消息回复给uav
α
。询问方收到回复消息，执行带ransac算法的pnp求解出进而推导出二者的相对位姿相对位姿不仅会被送入图优化环节构建位姿图，还会拷贝一份副本发送给uav
β
，目的是避免uav
β
重复计算。这种查询
‑
回复的机制有效避免了类似于集中式地面站那样需要存储所有无人机的视觉信息(空间复杂度o(n))，反而每个无人机仅需存储自身的视觉信息(空间复杂度o(1))，克服了传统方式对无人机群规模的限制。
[0177]
第三部分，位姿图优化：协同定位方法的核心是位姿图优化环节，图优化环节负责根据无人机间的相互观测、共享信息和待优化、待对齐的位姿变量建立联系，以g
‑
n法或l
‑
m迭代变量使得代价函数最小，无人机不仅再次优化了自身位姿，同时将同伴的位姿对齐至自身的惯性参考系下，从而完成协同定位工作。
[0178]
图优化环节把待求解的问题表示成图(graph)，一个图由若干个角点(vertex)，以及连接着这些角点的边(edge)组成，图g可以用数学表示为g＝{v,e}，其中v为角点集，角点表示待优化的变量；e为边集，边表示角点之间的联系。以uav
α
构建的图为例，角点为所有个体在自身惯性参考系w
α
下的位姿边为无人机的观测量，包括运动增量观测(增量边)，局部地图匹配观测(地图匹配边)，uwb距离观测(测距边)。
[0179]
1)运动增量观测构建
[0180]
关键帧消息包含视觉特征信息和无人机运动信息，视觉特征信息用于局部地图匹配，运动信息用于计算运动增量。当无人机接收到任一同伴的关键帧消息，首先将其存入到消息队列，然后提取当前时刻k与较早某时刻k
‑
l关键帧消息中的里程计部分计算运动增量，过程如下：
[0181]
[0182]
其中是uav
i
,i∈{α,β,γ}在k时刻的运动增量，表示在b
k
‑
l
系下，和来自uav
i
关键帧消息中的里程计部分。
[0183]
2)局部地图匹配观测构建
[0184]
局部地图匹配观测来自pnp环节的输出结果，表达式如下：
[0185][0186]
其中i,j∈{α,β,γ}，其具体构建过程参考局部地图匹配原理。
[0187]
3)uwb距离观测构建
[0188]
uwb模块作为机载传感器安装在每一个无人机上，负责周期性地测量无人机之间的相对距离，本系统选择搭载美国time domain公司生产的p440 uwb模块，该模块具备良好的抗多径干扰和在非视距情况下的工作能力，其测量过程可以列写为：
[0189][0190]
其中i,j∈{α,β,γ}表示搭载于uav
i
上的uwb模块在k时刻对uav
j
的距离测量，t
ij
为超宽带电波的飞行时间，c为光速。
[0191]
4)离群值检测方法
[0192]
本系统中的观测离群值主要有两种，其一是因为误匹配导致错误的局部地图匹配观测，其二是因为uwb模块在非视距的情况工作或者遭受多径效应时导致错误的距离观测。根据图优化的基本原理，如果对离群值不予以识别而直接用作构建位姿图，等同于加入了一条原本不应存在的边，势必会导致协同定位精度下降甚至估计错误。本节针对离群值提出了检测方法，示意图如附图7所示，如果局部地图匹配观测或uwb距离观测满足以下三条之一则认为产生了离群值，离群值将不会用于构建位姿图：
[0193][0194][0195][0196]
其中i,j∈{α,β,γ}。条件1表示前后两次地图匹配计算的相对位姿应当与运动增量形成一个闭环，这里默认第一次地图匹配观测是正确的；条件2表示某时刻的uwb距离观测应该与地图匹配产生的相对位姿保持一致，这里(
·
)
p
表示取变换矩阵中的3维位置部分；条件3表示前后两次距离测量不应超过某一阈值，v
max
表示两机最大相对速度。
[0197]
5)位姿图构建
[0198]
本发明提出的协同定位方法可不限制集群规模，但为了叙述方便假设系统中包含三个无人机uav
α
、uav
β
和uav
γ
。对于uav
α
来说，其根据无人机间的相互观测构建位姿图从而转化为代价函数，通过调整位姿角点使得代价函数整体最小。在一个大小为n的滑动窗口内，定义待优化位姿角点为：
[0199][0200]
角点之间以各种边相连，边包括增量边、地图匹配边和测距边，位姿图建立过程如
附图8所示。
[0201]
1)增量边
[0202]
增量边仅连接单个无人机位姿角点，表示滑动窗口中某无人机k
l
时刻位姿与k
l
‑
m
时刻位姿之间的运动增量，本例中令m＝3，即一个位姿角点最多与前面三个角点用增量边连接，其与位姿角点存在如下关系：
[0203][0204]
其中i∈{α,β,γ}，表示运动增量观测，代表se(3)上的广义加法，n
δt
为零均值白噪声。
[0205]
2)地图匹配边
[0206]
地图匹配边连接多个无人机位姿角点，表示询问方当前帧与应答方匹配帧存在局部地图匹配关系，其与位姿角点存在如下关系：
[0207][0208]
其中i,j∈{α,β,γ}，表示局部地图匹配观测，n
t
为零均值白噪声。
[0209]
3)测距边
[0210]
测距边连接多个无人机位姿角点，表示某一时刻两机的相对距离，测距值可直接通过uwb传感器读取，其与位姿角点存在如下关系：
[0211][0212]
其中i,j∈{α,β,γ}，表示uwb距离观测，n
d
为零均值白噪声。
[0213]
由上述位姿图转化的代价函数表示为：
[0214][0215]
式中表示系统内所有无人机的增量边的集合，表示所有地图匹配边的集合，表示所有测距边的集合。i和j表示无人机的编号，i,j∈{α,β,γ}。
[0216]
表示运动增量残差，数学表示为：
[0217][0218]
表示地图匹配残差，数学表示为：
[0219][0220]
表示测距残差，数学表示为：
[0221][0222]
上式中表示se(3)上的广义减法。
[0223]
通过求解代价函数，无人机uav
α
再次优化了自身位姿，同时将同伴的位姿对齐至
惯性参考系w
α
下，从而完成协同定位工作。
[0224]
本发明实例结果
[0225]
本案例给出室内环境下三架无人机的协同定位实验结果，每个无人机均采用前述的硬件配置，质量、体积基本相同，他们的标号为uav
α
，uav
β
和uav
γ
，实验以uav
α
的视角验证协同定位算法的有效性。
[0226]
附图9为uav
α
运行协同定位算法后估计出uav
β
或uav
γ
的轨迹，蓝色曲线为真值。图中红色轨迹有部分缺失是因为协同定位程序需要进行初始化，uav
α
只有在首次发现地图匹配后才会把该无人机纳入位姿图从而进行优化和对齐，因而在初始化前不会有位姿发布。经过观察发现uav
α
几乎可以良好的还原其他个体的运动轨迹。
[0227]
附图10为uav
α
运行协同定位算法后计算出uav
β
或uav
γ
在w
α
系下的位姿，蓝色曲线为真值。通过定性和定量比较得出以下结论：uav
α
对同伴uav
β
和uav
γ
的轨迹跟踪和位姿估计较为准确，协同定位精度优于0.1m，对齐到w
α
系中的位姿基本贴合真值，算法整体表现良好。
[0228]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。