一种即时定位与地图构建方法

本发明涉及信息处理，特别涉及一种即时定位与地图构建方法。

背景技术：

1、即时定位与地图构建(slam)能够出色的完成在没有gps信号的未知环境中的地图构建和自主导航。目前的slam主要有基于激光雷达、相机、超声波、无人机倾斜摄影、惯性测量单元(i mu)以及里程计等传感器的方法,使用单一传感器进行未知环境的slam存在一定的局限性，如激光雷达slam的全局定位能力差，视觉slam精度易受光照条件影响，而基于多传感器融合的slam可以从硬件结构上充分利用各种传感器的优势，弥补了使用单一传感器的的slam不足，进一步提高了slam自主导航的定位导航精度以及鲁棒性。目前主流的多传感器组合方式包括视觉和惯导融合的slam、激光和惯导融合的slam、激光和视觉融合的slam以及毫米波和激光融合的slam等。

2、多传感器融合slam的前提是多传感器对同一目标的准确描述，由于不同质传感器的数据速率不同，数据节点不一致，故需要进行多传感器的联合标定及数据融合，确保多传感器在同一时刻是对同一物体进行描述。目前主要采用的多传感器融合方法是基于松耦合或某一传感器为主其余为辅，其数据耦合程度较低，且由于数据速率不同，为保证获取的数据的完整性，通常以传输速率较慢的传感器为主进行数据融合，导致在未知环境中的地图构建和自主导航定位的时效性较低。为此，我们提出一种即时定位与地图构建方法。

技术实现思路

1、本发明的主要目的在于提供一种即时定位与地图构建方法，可以有效解决背景技术中的问题。

2、为实现上述目的，本发明采取的技术方案为：

3、一种即时定位与地图构建方法，包括以下步骤，

4、步骤一，对即时定位与地图构建系统所有传感器进行自标定，根据传感器间的变换关系进行多传感器间的联合标定；

5、步骤二，采集时间间隔t内的k个时间节点t1～tk时刻下所有传感器的特征数据值表示为第k个时间节点时第i类的第j个传感器的特征数据值，k为时间节点编号，i为传感器类型编号，j为同类型下传感器编号；

6、步骤三，对采集的传感器的特征数据值进行预处理，若采集的传感器的数据缺失时，记缺失值数据为0，构建数据推演模型，计算时间间隔t的末尾时间节点tk时刻时的所有传感器的特征数据值

7、步骤四，将获取的传感器特征数据值进行配准后，对多传感器的特征数据值进行数据融合，根据数据融合结果建图导航；

8、步骤三中，数据推演模型的构建步骤如下：

9、步骤s1，确定主融合传感器，其中，主融合传感器的确定方法的步骤为，

10、步骤s11,构建层次分析模型，采用标度法比较任意两组传感器对定位精度的重要程度，通过判断矩阵计算出各传感器对定位精度的重要程度的权重值，并进行判断矩阵的一致性检验；

11、步骤s12,通过时间戳查询特征值数据集中各传感器在时间间隔t内的第v个时间节点tv时刻下的数据上传时间，获取各传感器的数据上传时间排序情况；

12、步骤s13,根据步骤s11中各传感器对定位精度的重要程度的权重值计算结果及步骤s12各传感器的数据上传时间排序情况结果，取由公式：αwij+βsnij，计算的结果的最大值所对应的传感器为主融合传感器，其中，α、β分别为权重系数，α∈(0,1)；β∈(0,1)，权重系数α、β由传感器类型及经验值进行确定，一般的取α＝β＝0.5；wij表示为传感器第i类的第j个传感器对定位精度的重要程度的权重值；snij表示为传感器第i类的第j个传感器的数据上传时间排序值；

13、设主融合传感器为第r类传感器，根据特征数据值构建主融合传感器的特征数据值矩阵其中，r∈i，n为第r类传感器的总个数，有，

14、

15、步骤s2，根据特征数据值矩阵构建时间节点t1～tk时刻下所有传感器的特征值数据集其中，

16、步骤s3，提取特征数据值矩阵中的第u列及第u+1列元素构建初始向量点集pu及末尾向量点集pu+1,其中u为正整数，且u+1∈k；

17、步骤s3，计算初始向量点集pu至末尾向量点集pu+1的向量增量点集δp，其中，

18、步骤s4，构建以向量增量点集δp的计算结果为输入，以预测的增量点集δp'为输出的神经网络模型，并对建立的神经网络模型进行训练，根据训练结果和实际报警结果的误差调整模型的隐含层数量至准确率不低于期望值，并通过构建的神经网络模型输出预测的增量点集δp'，其中，

19、步骤s5，根据获取的增量点集δp'计算数据融合节点tv时刻下主融合传感器的特征数据值其中，

20、步骤s6，重复步骤s1～步骤s5，确定数据融合节点tv时刻下所有传感器的特征数据值获取数据融合节点tv时刻下的特征值数据集

21、进一步的，在步骤s4中，神经网络模型的期望值计算公式为，

22、

23、其中，e(y)表示期望值；n表示神经网络模型输入样本的数量；f(xi)表示神经网络模型的输出函数；xi表示神经网络模型的第i个输出样本。

24、本发明具有如下有益效果：

25、(1)与现有技术相比，本发明技术方案通过对即时定位与地图构建系统所有传感器进行自标定，根据传感器间的变换关系进行多传感器间的联合标定，采集时间间隔t内的k个时间节点t1～tk时刻下所有传感器的特征数据值对采集的传感器的特征数据值进行预处理，构建数据推演模型，计算时间间隔t的末尾时间节点tk时刻时的所有传感器的特征数据值将获取的传感器特征数据值进行配准后，对多传感器的特征数据值进行数据融合，根据数据融合结果建图导航，通过数据推演模型确定主融合传感器，并获取数据融合节点tv时刻下所有传感器的特征数据值及数据融合节点tv时刻下的特征值数据集能够解决现有技术中以传输速率较慢的传感器为主进行数据融合，导致在未知环境中的地图构建和自主导航定位的时效性较低的技术问题。

技术特征：

1.一种即时定位与地图构建方法，其特征在于，包括以下步骤，

2.根据权利要求1所述的一种即时定位与地图构建方法，其特征在于，步骤s1中，主融合传感器的确定方法的步骤为，

3.根据权利要求1所述的一种即时定位与地图构建方法，其特征在于：在步骤s13中，主融合传感器为由公式：αwij+βsnij的计算结果的最大值所对应的传感器。

4.根据权利要求1所述的一种即时定位与地图构建方法，其特征在于，在步骤三中，若采集的传感器的数据缺失时，记缺失值数据为0。

5.根据权利要求1所述的一种即时定位与地图构建方法，其特征在于，在步骤s4中，神经网络模型的期望值计算公式为，

6.根据权利要求1所述的一种即时定位与地图构建方法，其特征在于：在步骤s13中，权重系数α、β由传感器类型及经验值进行确定，一般的取α＝β＝0.5。

技术总结
本发明公开了一种即时定位与地图构建方法，涉及信息处理技术领域，通过对即时定位与地图构建系统所有传感器进行自标定，根据传感器间的变换关系进行多传感器间的联合标定，采集时间间隔t内的k个时间节点t<subgt;1</subgt;～t<subgt;k</subgt;时刻下所有传感器的特征数据值对采集的传感器的特征数据值进行预处理，构建数据推演模型，计算时间间隔t的末尾时间节点t<subgt;k</subgt;时刻时的所有传感器的特征数据值将获取的传感器特征数据值进行配准后，对多传感器的特征数据值进行数据融合，根据数据融合结果建图导航，能够解决现有技术中以传输速率较慢的传感器为主进行数据融合，导致在未知环境中的地图构建和自主导航定位的时效性较低的技术问题。

技术研发人员：张瑞菊,王坚,赵江洪,王志涛,薛雅倩
受保护的技术使用者：北京建筑大学
技术研发日：
技术公布日：2024/1/15