卫星拒止环境下多无人机分布式协同定位与建图方法

1.本发明涉及多无人机导航技术领域，尤其是涉及卫星拒止环境下多无人机分布式协同定位与建图方法。


背景技术：

2.近年来，无人机被广泛的应用于地下洞穴探测、森林监测、复杂工业园区重建等领域。上述应用均依赖稳定、实时的多无人机定位系统。基于全球导航卫星系统（global navigation satellite system, gnss）、运动捕捉系统(motion capture system, mocap)、超宽带测距（ultra-wideband, uwb）等外部传感器的无人机定位方法在过去的几十年中被广泛的应用。然而，外部传感器通常依赖离线部署与标定，无法在一些狭小的非结构化区域为无人机提供稳定的定位信息，这在很大程度上限制了多无人机的应用范围。因此，在卫星拒止环境下实现多无人机的协同定位与建图仍是一项具有挑战的工作。
3.卫星拒止环境下多无人机分布式协同定位与建图方法，按照传感器类型可以分为基于视觉的多无人机协同定位与建图方法、基于超宽带（ultra-wideband，uwb）的多无人机协同定位与建图方法、基于激光雷达的多无人机协同定位与建图方法。
4.基于视觉的多无人机协同定位与建图方法：利用无人机机载相机拍摄到的图片，在多无人机间做多视图几何匹配，实现多无人机的协同定位。但是，受限于机载相机的有限视场角，当多个无人机间的视场角不存在重叠区域时，会产生较大累计误差。
5.基于uwb的多无人机协同定位与建图方法：uwb是近年来一种新颖的电波测距技术，超宽带技术通过测量电波的到达时间、到达时间差或到达角计算出两个模块之间的距离。无人机利用自身定位信息配合uwb测距信息可以实现多无人机的协同定位功能。但是，uwb测距信息受其体积与功率的影响，存在测距范围小、易受障碍物遮挡的缺点，容易导致多无人机协同定位失效。
6.基于激光雷达的多无人机协同定位与建图方法：随着，近年来激光雷达体积、重量、成本的持续降低，使基于激光雷达的无人机自主导航技术成为了可能。得益于激光的主动深度测量特性，激光雷达可以提供不受光照影响的点云信息，从而实现多无人机的协同定位与建图。然而，激光雷达产生的大量点云信息导致基于激光雷达的多无人机协同定位与建图方法的计算复杂度较高，通常依赖集中式地面站实现协同定位与建图功能。
7.综上所述，目前针对卫星拒止环境下多无人机协同定位与建图方面的相关工作，多受限于相机视场角、uwb测距范围、地面集中式运算平台，难以适应计算资源受限的多无人机平台和对实时性要求高的任务。因此，研究卫星拒止环境下多无人机分布式协同定位与建图方法对于多无人机系统的实际落地应用至关重要。


技术实现要素：

8.为克服现有技术的不足，本发明旨在提出一种适用于卫星拒止环境的多无人机分布式协同定位与建图方法，无人机通过确定自身定位信息以及其他无人机的相对定位信
息，从而为多无人机系统提供全局一致的环境地图信息。
9.本发明的卫星拒止环境下多无人机分布式协同定位与建图方法，包括：无人机自主定位，每架无人机利用激光雷达提供的原始点云数据提取自身的点云特征信息，然后估计自身的自主定位信息；多无人机协同定位，每架无人机利用其他无人机发布的点云特征信息在自身的特征地图上做匹配，从而计算其他无人机在自身参考坐标系中的相对位姿信息；多无人机协同建图，每架无人机在自身参考坐标系下，利用自主定位信息、相对位姿信息、点云特征信息维护一张全局特征点云地图。
10.进一步的，无人机自主定位的具体步骤包括：s11、从激光雷达点云中提取特征点集，包括位于平坦区域的面特征点集和位于边缘或尖锐区域的线特征点集；s12、通过特征点集与特征地图的匹配关系构建雷达约束残差；特征点在无人机自身参考坐标系下的位置：线特征点集的匹配残差：面特征点集的匹配残差：式中，为特征点云中的特征点，为无人机在自身参考坐标系下的位姿初值；为线特征点集中的特征点，与为无人机自身特征地图中距离最近且不在同一条扫描线上的任意两个点；为面特征点集中的特征点，、、为无人机自身特征地图中距离最近的任意三个点；s13、根据特征点集的匹配残差，将基于雷达特征-地图匹配的无人机自主定位问题转化为非线性最小二乘问题；通过高斯-牛顿法对非线性最小二乘问题的迭代求解，即可实现多无人机系统中每架无人机对自身自主定位的估计；式中，为无人机在自身参考坐标系中的位姿，为通过无人机在自身参考坐标系中位姿的估计值，为线特征点集中特征点的数量，为面特征点集中特征点的数量。
11.进一步的，s11步骤中依据平滑度确定特征点集，具体地，平滑度：式中，为激光雷达点云中的点，为的邻域点集，为的邻域点集中点的数
量，为向量的范数，为的邻域点集中包含的点；依据平滑度值对点云排序，最大的数十个点即为线特征点集，最小的数十个点即为面特征点集，且规定一个特征点周围10个邻近点不能作为特征点。
12.进一步的，多无人机协同定位的具体步骤包括：s21、构建无人机的自主定位因子；无人机自主定位观测：位姿增量：位姿观测增量：自主定位因子：式中，表示时刻的无人机自主定位观测矩阵，表示时刻的无人机自主定位观测矩阵，表示至间无人机的位姿增量，表示无人机自主定位观测协方差；表示时刻在参考坐标系下的位姿的逆，表示时刻在参考坐标系下的位姿；表示时刻自主定位观测矩阵的逆矩阵；表示时刻相对于参考坐标系位姿矩阵的估计矩阵对应的逆矩阵，表示时刻相对于参考坐标系位姿矩阵的估计矩阵；s22、构建无人机的相对定位因子；无人机的特征点在无人机的参考坐标系下的位置：无人机的线特征点集到无人机特征地图的匹配残差：无人机的面特征点集到无人机特征地图的匹配残差：无人机相对于无人机参考坐标系的相对定位因子：式中，为在的参考坐标系下的位姿初值，为的特征点；与为特征地图中距离的特征点的投影最近且不在同一条扫描线上
的任意两个点；、、为特征地图中距离的特征点的投影最近的任意三个点；为线特征点集中特征点的数量，为面特征点集中特征点的数量；s23、将自主定位因子和相对定位因子融合，然后通过高斯噪声假设将多无人机分布式相对位姿图优化问题转化为最小二乘问题；式中，为相对于参考坐标系的位姿，为相对于参考坐标系的位姿的估计值。
13.进一步的，多无人机协同建图中，全局特征点云地图包括线特征地图与面特征地图，其表示如下：式中，分别为时刻无人机在自身参考坐标系中的位姿，分别为无人机在时刻提取的线特征，分别为无人机在时刻提取的面特征，分别为时刻无人机在无人机参考坐标系中的位姿，分别为无人机在时刻提取的线特征，分别为无人机在时刻提取的面特征。
14.综上，本发明采用上述多无人机分布式协同定位与建图方法，具备如下优势：1、本发明提出了无人机自主定位方法，该方法仅依赖机载激光雷达与无人机机载计算机，无需外部传感器或高性能地面处理器，可用于支撑无人机在卫星导航拒止环境下的自主飞行与相关应用。
15.2、本发明提出了基于特征-地图匹配的多无人机分布式协同定位方法，该方法利用激光雷达点云特征与地图的匹配关系实现了对无人机自身位姿与多无人机系统内部其他无人机的相对位姿的求解，可用于支撑多无人机在卫星导航拒止环境下的协同飞行。
16.3、本发明提出了多无人机协同建图方法，该方法将每架无人机自身提取的特征点云与其他无人机发布的特征点云投影到无人机自身的参考坐标系，使多无人机系统中的每架无人机均可以维护一张全局一致的特征地图。
附图说明
17.图1为本发明实施例中卫星拒止环境下多无人机协同定位方法总体结构图；图2为本发明实施例中分布式相对位姿图优化方法示意图；图3为本发明实施例中卫星拒止环境下无人机自主定位对比图；图4为本发明实施例中室内运动捕捉系统环境下协同定位对比图；图5为本发明实施例中丛林环境下三架无人机协同定位轨迹与建图效果；图6为本发明实施例中楼宇环境下三架无人机协同建图效果。
具体实施方式
18.以下结合附图和实施例对本发明的技术方案作进一步说明。
19.本方案所选用的无人机均搭载有三维激光雷达和机载处理器。三维激光雷达用于采集无人机周围环境的环境点云信息，测量精度高，覆盖范围大，能够将环境信息以三维点云数据形式发布出来。机载处理器用于处理三维激光雷达点云数据与卫星拒止环境下多无人机协同定位方法运行，获得无人机自身位姿及系统内其他无人机位姿。
20.如图1所示，本方案所设计的卫星拒止环境下多无人机协同定位算法主要包含三部分，卫星拒止环境下无人机自主定位、卫星拒止环境下多无人机相对定位以及卫星拒止环境下多无人机协同建图。
21.一、卫星拒止环境下无人机自主定位。
22.基于雷达特征-地图匹配的无人机自主定位方法利用激光雷达提供的原始点云数据估计多无人机系统中的每架无人机自身的定位信息。
23.s11、点云特征提取。从激光雷达点云中提取两种特征点，包括位于平坦区域的面特征点和位于边缘或尖锐区域的线特征点。为提取这两种特征点，对点云中每个点定义平滑度，其计算公式如下：
ꢀꢀꢀ
（1）式中，为激光雷达点云中的点，代表的邻域点集，表示的邻域点集中点的数量，表示向量的范数，表示的邻域点集中包含的点。
24.根据平滑度的定义，如果点分布在平面上，的值接近0；若点分布在边缘上，的值很大。先计算每个点云的平滑度值，排序后取最大和最小的数十个点，即为特征点。在特征提取时为了避免特征点分布过于集中，规定一个特征点周围10个邻近点不能作为特征点，最终特征提取环节获得了线特征点集与面特征点集。因此，特征点集可以定义为，其中表示两个集合的并集。
25.s12、雷达特征-地图匹配残差函数构建。特征-地图匹配通过特征点云与特征地图的匹配关系构建雷达约束残差。
26.对于特征点云中的每个特征点，通过无人机位姿初值（可直接转化为无人机在自身坐标系中的位姿初值）将投影至无人机自身坐标系，其投影公式如下：
ꢀꢀꢀ
（2）式中，表示特征点在世界坐标系下的位置。
27.对于线特征点集中的每个点，找到特征地图中距离在参考坐标系下的投影最近且不在同一条扫描线上的个点（）。假设与为特征地图中距离最近且不在同一条扫描线上的任意2个点。与、连成的线构成线特征对应关系，定义线特征点集中的第个点到地图的匹配残差为：
ꢀꢀꢀ
（3）式中，分子为向量与向量叉乘的模，其数值等于以两向量为边的平行四边形的面积。分母是向量的模，其数值等于三角形底边的长。因此，数值上等于特征点到、连成的线的垂直距离。
28.对于平面特征点集中的每个点，找到特征地图中距离在参考坐标系下的投影最近的个点，以张成一个平面。假设、、为特征地图中距离最近的任意3个点。与、、张成的平面构成平面特征对应关系，定义平面特征点集中的第个点到地图的匹配残差为：
ꢀꢀꢀ
（4）式中，分子为以向量与向量为边的平行四边形沿向量拉伸所构成的三维物体体积。分母是以向量与向量为边的平行四边形的面积。因此，在数值上等于特征点到、、张成的平面的垂直距离。
29.s13、非线性优化求解：根据线特征点集中的第个点到地图的匹配残差与平面特征点集中的第个点到地图的匹配残差的定义，将基于雷达特征-地图匹配的无人机自主定位问题转化为非线性最小二乘问题。
30.ꢀꢀꢀ
（5）式中，表示无人机在自身参考坐标系中的位姿，表示通过无人机在自身参考坐标系中位姿的估计值，表示线特征点集中线特征的数量，表示面特征点集中面特征的数量。
31.通过高斯-牛顿法对公式（5）中定义的非线性最小二乘问题的迭代求解，即可实现多无人机系统中每架无人机对自身位姿的估计，如图3所示。
32.二、卫星拒止环境下多无人机相对定位。
33.多无人机相对定位方法利用多无人机系统中其他无人机发布的自主定位信息与点云特征信息构建因子图，从而计算其他无人机在每架无人机参考坐标系中的位姿。因子图中包括2种因子：1）无人机自主定位因子；2）无人机相对定位因子，如图2所示。
34.下面以2架无人机与组成的多无人机系统为例，介绍卫星拒止环境下在的参考坐标系中的位姿的解算方法。
35.s21、无人机自主定位因子构建。无人机自主定位因子用于描述因子图中两相邻节点间的位姿关系。对于一个多无人机系统，每架无人机可以估计自身的位姿并将其发布至其他无人机。定义无人机自主定位观测如下：
ꢀꢀꢀ
（6）式中，表示时刻的无人机自主定位观测矩阵，表示时刻的无人机自主定位观测矩阵，表示至间无人机的位姿增量，表示无人机自主定位观测协方差。
36.根据位姿增量的坐标系转换不变性，对于相对于的位姿估计问题，位姿增量可以写为如下形式：
ꢀꢀꢀ
（7）式中，表示时刻在参考坐标系下的位姿的逆，表示时刻在参考坐标系下的位姿。
37.将公式（7）代入公式（6）中，可得：
ꢀꢀꢀ
（8）式中，表示至间的位姿观测增量，表示时刻自主定位观测矩阵的逆矩阵。
38.因此，定义相对于参考坐标系的自主定位因子为：
ꢀꢀꢀ
（9）式中，表示时刻相对于参考坐标系位姿矩阵的估计矩阵对应的逆矩阵，表示时刻相对于参考坐标系位姿矩阵的估计矩阵。
39.s22、无人机相对定位因子构建。利用发布的激光雷达点云特征在自身特征地图中做匹配。对于发布的激光雷达点云特征中的每个特征点，通过无人机位姿初值将投影至的参考坐标系，其投影公式如下：
ꢀꢀꢀ
（10）式中，为为在的参考坐标系下的位姿初值，表示特征点在的参考坐标系下的位置。
40.对于发布的线特征点集中的每个点，找到特征地图中距离在参考坐标系下的投影最近且不在同一条扫描线上的个点（）。假设与为特征地图中距离最近且不在同一条扫描线上的任意2个点。与、连成的线构成线特征对应关系，定义发布的线特征点集的第个点到地图的匹配残差为：
ꢀꢀꢀ
（11）式中，分子为向量与向量叉乘的模，其数值等于以两向量为边的平行四边形的面积。分母是向量的模，其数值等于三角形底边的长。因此，数值上等于特征点到、连成的线的垂直距离。
41.对于发布的面特征点集中的每个点，找到特征地图中距离在参考坐标系下的投影最近的个点，以张成一个平面。假设、、为特征地图中距离最近的任意3个点。与、、张成的平面构成平面特征对应关系，定义平面特征点集中的第个点到地图的匹配残差为：
ꢀꢀꢀ
（12）式中，分子为以向量与向量为边的平行四边形沿向量拉伸所构成的三维物体体积。分母是以向量与向量为边的平行四边形的面积。因此，在数值上等于特征点在参考坐标系下的投影到、、张成的平面的垂直距离。
42.据发布的线特征点集中的第个点到特征地图的匹配残差与发布的平面特征点集中的第个点到特征地图的匹配残差的定义，将相对于参考坐标系的相对定位因子定义为：
ꢀꢀꢀ
（13）式中，表示发布的线特征点集中线特征的数量，表示发布的面特征点集中面特征的数量。
43.s23、分布式相对位姿图优化方法。分布式相对位姿图优化方法对无人机自主定位信息与多无人机相对定位信息进行融合，实现精确、鲁棒、实时的分布式多无人机协同定位。
44.本方案采用增量式位姿图优化求解器分布式求解多无人机位姿，以求解相对于参考坐标系的位姿为例，通过高斯噪声假设将多无人机分布式相对位姿图优化问题转化为最小二乘问题：
ꢀꢀꢀ
（14）通过开源求解器gtsam (georgia tech smoothing and mapping)求解公式（14）中定义的相对定位位姿图优化问题即可实现对相对于参考坐标系的位姿的求解，如图4所示。
45.三、卫星拒止环境下多无人机协同建图方法。
46.多无人机系统中的每架无人机都需要维护在其自身参考坐标系下维护一张特征点云地图。特征点云地图构建就是利用激光雷达扫描的特征数据构建地图的过程，激光雷达特征与特征点云地图是通过位姿关系进行关联的。
47.以和两架无人机组成的多无人机系统为例，时刻维护的特征地图是个单帧特征点云在世界坐标系下投影的集合，其包含两个子集，时刻维护的线特征地图与面特征地图，公式如下：
ꢀꢀꢀ
（15）式中，分别为时刻无人机在自身参考坐标系中的位姿，分别为无人机在时刻提取的线特征，分别为无人机在时刻提取的面特征，分别为时刻无人机在无人机参考坐标系中的位姿，分别为无人机在时刻提取的线特征，分别为无人机在时刻提取的
面特征。
48.根据公式(15)中特征点云地图的定义可以，本发明提出的卫星拒止环境下多无人机协同定位与建图方法可以在多无人机系统中包含的每架无人机上维护一张全局一致的特征地图，如图5、图6所示。
49.以上是本发明的具体实施方式，但本发明的保护范围不应局限于此。任何熟悉本领域的技术人员在本发明所揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内，因此本发明的保护范围应以权利要求书所限定的保护范围为准。