[发明专利]一种多无人机协作SLAM方法与系统

权利要求书

1.一种多无人机协作SLAM方法，其特征在于，包括以下步骤：

S1、检测无人机与地面站之间的通信是否正常,如果正常，则传递无人机的关键帧以及对应的无人机编号；

S2、相似场景判断，如果对比当前的某一图片与另一无人机的某时刻图片相似，则首先计算出两个关键帧对应的相对位姿,再结合单机SLAM结果计算出相对初始位姿；

S3、将所有相似场景的相对初始位姿,纳入到一个集合中，再进行一个自适应数量的聚类，并从中选择最优的聚类，得到关联数据；

S4、将关联数据与单机SLAM结果结合到一起进行优化,最后返回优化后的全局地图信息与飞行轨迹；

在步骤S2中，相似场景判断包括：在单机SLAM中，无人机将实时拍到的图片进行ORB特征提取和进行特征点的描述，将所对应的描述通过预先训练好的词袋模型转换成对应的单词并记录下来；在后续的飞行过程中，任意一无人机将每一帧新拍摄的图片所对应的单词与另一无人机已经存储的单词进行对比，并与所设定的阈值进行对比，如果超过该阈值，则认为其是相似场景；

在步骤S2中，通过ICP算法计算出关键帧对应的相对位姿，再结合单机SLAM中存储的无人机的飞行轨迹信息，求得该相似场景所对应的相对初始位姿；

相似场景所对应的相对初始位姿通过下式求得：

上式中，运算符号代表的是4x4的位姿所进行的运算，X代表单机SLAM的结果，包括地图信息以及飞行轨迹信息，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表两架无人机的相对初始位姿；

在步骤S3中，针对所有相似场景的相对初始位姿,首先进行AP聚类,然后进行聚类选择，得到关联数据；

在步骤S3中，首先将相对初始位姿的数据转换成AP算法能够利用的度量单位：

上式表示为两个不同时刻的相对初始位姿的距离；其中T1和T2代表不同时刻的相对初始位姿，∑代表了4x4维变换矩阵的协方差矩阵，n相对应的取值为4；

将每个聚类的中心视为每个聚类中最具有代表性的相对初始位姿，相对应的就有对应数量的相对初始位姿Ti，i＝1,2,3,...n，针对这生成的n个假设，接下来的目的就是从中选择最优的聚类：

上式表示在已知单机SLAM结果和所有相似场景对应的相对位姿的情况下，从生成的多个聚类h中选择最优聚类的方法；其中h_i代表不同相似场景生成的不同聚类，代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合；

上式后面的式子表示成：

上式代表了应用贝叶斯公式来对进行转换求解的过程；其中h代表不同相似场景生成的不同聚类，代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿；

其中，与假设无关，故不需要计算；先验概率用CRP过程来模拟，单独计算即可；剩余部分则如下：

该部分代表着在某个具体聚类h以及所有的单机SLAM结果的条件下，每个相对初始位姿的概率；其中h代表不同相似场景生成的不同聚类，代表单机SLAM的结果，T代表相对初始位姿，T₀代表该聚类中心所对应的相对初始位姿，Σ₀代表该聚类中心所对应的协方差矩阵；

上式代表了在某个具体聚类h中，在已知单机SLAM结果以及此时的相对初始位姿的条件下，求出来相似场景所对应的相对位姿的概率；其中h代表不同相似场景生成的不同聚类，代表单机SLAM的结果，U代表了识别到相似场景时两架无人机间相对位姿的集合，T代表相对初始位姿，Σ_in代表某个相似场景在该聚类下属于内点时所对应的协方差矩阵，Σ_out代表某个相似场景在该聚类下属于外点时所对应的协方差矩阵；

最后设置一个阈值C，用来判断最后取得的最优聚类是否为数据关联，如果认定为数据关联，则将两架无人机的数据关联起来；

在步骤S4中，将关联数据与单机SLAM结果结合到一起代入到一个协作SLAM的优化公式中：

上式代表了在已知单无人机的观测信息以及多个无人机之间所有“相遇”所对应的相对位姿以后，求得协作SLAM最后优化结果的过程；其中X代表了各个无人机的位姿信息，Z代表了无人机在飞行过程中的观测信息，U代表无人机之间经过验证的“相遇”时刻的相对位姿，X^*代表经过优化过后的各个无人机的位姿信息，r代表不同无人机，k代表在某个时刻的观测值的数量序号，m代表观测值的数量，i代表在飞行过程中“相遇”的序号，相对应的N代表“相遇”的次数；

最后应用光束平差优化算法来对上式进行优化求解，即得到优化后的地图和位姿信息。

2.一种多无人机协作SLAM系统，其特征在于：包括可读存储介质，所述可读存储介质中存储有执行指令，所述执行指令被处理器执行时用于实现如权利要求1所述的方法。