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

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

背景技术：

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

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

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

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

技术实现思路

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

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

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

4、步骤1：获取uwb、imu、双目相机和定高雷达数据；

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

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

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

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

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

10、①创建新的关键帧消息

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

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

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

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

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

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

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

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

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

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

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

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

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

24、

25、其中mi表示该词汇ωi在一幅图像中出现的次数，m为图像中的词汇总数。

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

27、

28、其中ni表示建立的字典中词汇ωi的数量，n是字典中所有视觉词汇的数量。

29、一个词汇的权重取为tf和idf的乘积：

30、ηi＝tfi×idfi (3)

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

32、vk＝{(ω1,η1),(ω2,η2),…,(ωn,ηn)} (4)

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

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

35、询问方收到回复消息，执行带ransac算法的pnp求解出进而推导出二者的相对位姿αtβ。

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

37、进一步的，执行带ransac算法的pnp求解出进而推导出二者的相对位姿αtβ，包括：

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

39、

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

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

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

43、

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

45、①增量边

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

47、

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

49、②回环边

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

51、

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

53、③测距边

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

55、

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

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

58、

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

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

61、

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

63、

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

65、

66、上式中！表示se(3)上的广义减法。

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

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

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

70、步骤a：获取uwb原始数据和imu数据；

71、步骤b：对所述uwb原始数据进行距离校准，得到校准后的uwb数据；

72、步骤c：判断锚点位置是否已知，若否，根据所述校准后的uwb数据进行锚点标定后跳转步骤d，若是，跳转步骤d；

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

74、步骤e：对所述imu数据进行低通滤波，得到滤波后的imu数据；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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