CN115774280A - 一种多源融合定位导航方法、电子设备及存储介质 - Google Patents

Abstract

一种多源融合定位导航方法、电子设备及存储介质。为提高导航定位的精度。本发明对三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系进行坐标系的转换；对采集的三维激光雷达点云数据进行位姿的构建；判断采集的载波相位差分测量仪测量数据是否满足设定的时间阈值，判断为是，则对步骤S3构建的位姿添加载波相位差分测量仪约束进行多源融合定位导航，判断为否，则进行下一步；启动蓝牙扫描模块进行扫描，采集蓝牙数据，然后判断是否采集到蓝牙数据，判断为是，则将采集的蓝牙数据进行解析获取UUID值，将得到的获取UUID值用于进行多源融合定位导航实现多源融合定位导航。本发明提高了定位精度。

Description

一种多源融合定位导航方法、电子设备及存储介质

技术领域

本发明属于导航定位技术领域，具体涉及一种多源融合定位导航方法、电子设备及存储介质。

背景技术

激光雷达定位主要通过测量激光雷达与周围可使激光发生反射的物体间间距从而反推出激光雷达位置。激光雷达通过精密的光束控制系统发出一束或多束激光扫描其视野环境。激光束经环境反射回扫描仪，返回的信号由光电探测器接收。通过对比分析发射信号与接收信号的差异测算出反射点距激光雷达的距离。激光定位其优点在于是目前最稳定、最可靠、最高性能的定位导航方法，并且其连续使用的寿命长，对于后期的改造成本也较低。但其易受到天气的影响，激光一般在晴朗的天气里衰减较小，传播距离较远。而在大雨、浓烟、浓雾等天气里，衰减急剧加大，传播距离也会受到影响，进而降低定位精度。

GNSS是所有导航定位卫星的总称，即全球导航卫星系统(Global NavigationSatellite System)凡是可以通过捕获跟踪其卫星信号实现定位的系统，均可纳入GNSS系统的范围，其中包含了美国的GPS、中国的BDS、俄罗斯的GLONASS和欧洲的GALILEO。GNSS定位原理则是根据无线电波的传播速度是恒定的，传播路径的线性性质，通过测量空间中的电波的传播时间来确定距离差的卫星和用户接收机天线之间的距离差、距离和测量值，再以这些距离差为半径进行三球交汇，根据联立方程求解用户位置。GNSS系统目前已经具备极高的响应速度和定位精度，也有非常可靠的稳定性。在无人驾驶方面，借助GNSS定位技术，无人车可以定位获取到高精度的位置，从而正确导航并有效地做出驾驶决策。但是，单独靠GNSS定位也存在一定的问题，当进入了隧道或是高楼林立的都市区时，导航很容易失去信号导致无法定位或存在一定范围的误差。

实时动态(RTK)定位技术是以载波相位观测值为根据的实时差分GPS技术，它是GPS测量技术发展的一个新突破。众所周知，无论静态定位，还是准动态定位等定位模式，由于数据处理滞后，所以无法实时解算出定位结果，也无法对观测数据进行检核，这就难以保证观测数据的质量。目前，解决这一问题的主要方法就是延长观测时间、选择作业窗口来保证测量数据的可靠性，这样一来就降低了GPS测量的工作效率。而实时动态定位(RTK)系统由基准站、流动站和数据链组成，建立无线数据通讯是实时动态测量的保证，其原理是取点位精度较高的首级控制点作为基准点，安置一台接收机作为参考站，对卫星进行连续观测，流动站上的接收机在接收卫星信号的同时，通过无线电传输设备接收基准站上的观测数据，流动站上根据相对定位的原理实时计算显示出流动站的三维坐标和测量精度。实现实时监测待测点的数据观测质量和基线解算结果的收敛情况，根据待测点的精度指标，确定观测时间，从而减少冗余观测，提高工作效率。RTK具有全天候观测、作业效率高、定位精度高、没有误差积累等优点，同时其信号强度、定位精度会受到卫星状况的限制、电离层以及数据链电台传输距离等方面的影响。

无人车在军事、工业、民生等各个领域都已经有了广泛应用，在全球已经形成一个庞大的产业体系，导航定位是无人车必须具备的功能，是无人车完成各项任务的前提和基础。从目前用于无人车的导航定位技术来看，多是以多种传感器融合的方式实现，多源融合定位导航指的是多个导航系统之间存在可获取的相对观测信息或者间接相对观测信息，通过一定的信息交换融合手段，实现多个系统之间导航资源共享，最终使导航系统性能更优，精度更高，提升多无人车的导航定位精度具有重要意义。

发明内容

本发明要解决的问题是提高导航定位的精度，提出一种多源融合定位导航方法、电子设备及存储介质。

为实现上述目的，本发明通过以下技术方案实现：

一种多源融合定位导航方法，包括如下步骤：

S1、使用搭载三维激光雷达、载波相位差分测量仪、蓝牙扫描模块的移动设备在室外进行数据采集，首先采集同一时间段内的三维激光雷达点云数据、载波相位差分测量仪测量数据，待用；

S2、对三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系进行坐标系的转换；

S3、对采集的三维激光雷达点云数据进行位姿的构建；

S4、判断步骤S1采集的载波相位差分测量仪测量数据是否满足设定的时间阈值，判断为是，则对步骤S3构建的位姿添加载波相位差分测量仪约束进行多源融合定位导航，判断为否，则进行下一步；

S5、启动蓝牙扫描模块进行扫描，采集蓝牙数据，然后判断是否采集到蓝牙数据，判断为是，则将采集的蓝牙数据进行解析获取UUID值，将得到的获取UUID值用于进行多源融合定位导航，判断为否，则进行下一步；

S6、重复步骤S1-S5，实现多源融合定位导航。

进一步的，步骤S2的具体实现方法包括如下步骤：

S2.1、首先对步骤S1采集的三维激光雷达点云数据进行位姿估算，并记录下三维激光雷达的运动位姿数据，同时记录下对应时间段内的载波相位差分测量仪测量数据，采用遍历搜索的方法寻找三维激光雷达位姿和载波相位差分测量仪测量值时间最接近的坐标数据，最后得到时间对应的三维激光雷达坐标数据、载波相位差分测量仪测量坐标数据用于坐标系转换，其中三维激光雷达坐标数据为K＝{k1，…ki，…kn}，载波相位差分测量仪测量坐标数据为G＝{g1，…gi，…gn}，其中n为坐标数据的个数，i为n中的任意一个；

S2.2、设置旋转矩阵为R，平移矩阵为W，对于第i个三维激光雷达坐标数据ki则有计算公式为：

ki＝R×gi+W；

其中，ki为第i个三维激光雷达坐标数据，gi为第i个载波相位差分测量仪测量坐标数据；

然后，设置第i个三维激光雷达坐标数据的误差为wi，则有计算公式为：

wi＝ki-(R×gi+W)；

S2.3、根据步骤S2.2设置的三维激光雷达坐标数据的误差，构建最小二乘误差minH，则有计算公式为：

S2.4、然后进行优化计算得到最优旋转矩阵R’为：

通过最优旋转矩阵求得最优平移矩阵W’：

W’＝ki-R’×gi；

确定最优旋转矩阵R’、最优平移矩阵W’，完成三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系的坐标系转换。

进一步的，步骤S3基于正态分布变换匹配方法对采集的三维激光雷达点云数据进行位姿的构建，具体实现方法包括如下步骤：

S3.1、将采集的三维激光雷达点云数据分为参考点云数据，目标点云数据；

S3.2、将参考点云数据按照所占的空间划分成指定大小的网格，并计算每个网格的均值和协方差矩阵：

其中，q为网格的均值，m为网格中数据点的个数，xj为网格中的参考点云数据，j为m中的任意一个；

则协方差矩阵C为：

S3.2、对于目标点云数据Xj，通过变换初始位姿变换矩阵将其转换到参考点云的网格中，计算公式为：

Xj＝T(xj，a)

其中，a为初始化变换参数，T为初始位姿变换矩阵；

计算目标点云数据在各网格内的响应概率密度p：

其中，p(Xj)为目标点云数据Xj在网格内的响应概率密度；

S3.3、通过步骤S3.2的目标点云数据在各网格内的响应概率密度计算正态分布变换匹配方法的配准得分：

S3.4、根据牛顿优化算法对目标函数Score(p)进行优化，寻找初始化变换参数使得Score(p)的值最大，完成三维激光雷达点云数据的位姿构建。

进一步的，步骤S4中的时间阈值范围为5-10s。

进一步的，步骤S4中对步骤S3构建的位姿添加载波相位差分测量仪约束的具体实现方法为通过因子图融合三维激光雷达点云数据、载波相位差分测量仪测量数据；

S4.1、设置三维激光雷达通过点云配准得到位姿的因子为f_L，则有：

f_L＝x_t+1-h(x_t)

其中，x_k+1为t+1时刻的位姿状态值，h(x_k)为t时刻递推的位姿状态值；

载波相位差分测量仪位置的因子为f_r，则有：

f_r＝g_t+1-h_r(g_t)

其中，g_t+1为t+1时刻的载波相位差分测量仪的测量值，h_r(g_t)为t时刻载波相位差分测量仪的测量方程；

S4.2、通过最大后验概率约束得到目标函数X^*：

其中，Xi为因子图中的第i个数据，fi为第i个因子；

S4.3、最后通过GTSAM求解，完成三维激光雷达点云数据、载波相位差分测量仪测量数据的融合，进行多源融合定位导航。

进一步的，步骤S5的具体实现方法包括如下步骤：

S5.1、蓝牙扫描模块进行iBeacon设备扫描；

S5.2、扫描结束，判断是否采集到蓝牙数据，判断为是，则将采集到的蓝牙数据根据信号强度值RSSI，蓝牙扫描模块定位3个距离最近的iBeacon设备，分别解析3个距离最近的iBeacon设备的广播数据，根据BLE标准的广播数据格式，解析获取UUID值，为移动设备的位置，用于多源融合定位导航。

电子设备，包括存储器和处理器，存储器存储有计算机程序，所述的处理器执行所述计算机程序时实现所述的一种多源融合定位导航方法的步骤。

计算机可读存储介质，其上存储有计算机程序所述计算机程序被处理器执行时实现所述的一种多源融合定位导航方法。

本发明的有益效果：

本发明所述的一种多源融合定位导航方法，根据移动设备上安装的载波相位差分测量仪的测量和激光雷达的测量，计算得到指定目标的实时位置信息，利用载波相位差分测量仪测量数据对采集的三维激光雷达点云数据进行位姿的构建并通过因子图融合，能够降低室外环境下长时间运行造成的累积误差，提高定位的精度。

本发明所述的一种多源融合定位导航方法，当载波相位差分测量仪的测量和激光雷达的测量由于干扰或无信号导致无法实现直接卫星导航定位环境中的高精度在线定位时，启动蓝牙扫描模块进行定位补偿，能有效解决指定目标被遮挡、干扰等无法实现直接卫星导航定位环境中的高精度在线定位问题。

本发明所述的一种多源融合定位导航方法，提高了移动设备定位精度，本发明用于无人车的定位导航能够有效提高无人车的导航定位的工作性能。

附图说明

图1为本发明所述的一种多源融合定位导航方法的流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施方式，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施方式仅用以解释本发明，并不用于限定本发明，即所描述的具体实施方式仅仅是本发明一部分实施方式，而不是全部的具体实施方式。通常在此处附图中描述和展示的本发明具体实施方式的组件可以以各种不同的配置来布置和设计，本发明还可以具有其他实施方式。

因此，以下对在附图中提供的本发明的具体实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定具体实施方式。基于本发明的具体实施方式，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他具体实施方式，都属于本发明保护的范围。

为能进一步了解本发明的发明内容、特点及功效，兹例举以下具体实施方式，并配合附图1详细说明如下：

具体实施方式一：

一种多源融合定位导航方法，包括如下步骤：

S1、使用搭载三维激光雷达、载波相位差分测量仪、蓝牙扫描模块的移动设备在室外进行数据采集，首先采集同一时间段内的三维激光雷达点云数据、载波相位差分测量仪测量数据，待用；

S2、对三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系进行坐标系的转换；

进一步的，步骤S2的具体实现方法包括如下步骤：

S2.1、首先对步骤S1采集的三维激光雷达点云数据进行位姿估算，并记录下三维激光雷达的运动位姿数据，同时记录下对应时间段内的载波相位差分测量仪测量数据，采用遍历搜索的方法寻找三维激光雷达位姿和载波相位差分测量仪测量值时间最接近的坐标数据，最后得到时间对应的三维激光雷达坐标数据、载波相位差分测量仪测量坐标数据用于坐标系转换，其中三维激光雷达坐标数据为K＝{k1，…ki，…kn}，载波相位差分测量仪测量坐标数据为G＝{g1，…gi，…gn}，其中n为坐标数据的个数，i为n中的任意一个；

S2.2、设置旋转矩阵为R，平移矩阵为W，对于第i个三维激光雷达坐标数据ki则有计算公式为：

ki＝R×gi+W；

其中，ki为第i个三维激光雷达坐标数据，gi为第i个载波相位差分测量仪测量坐标数据；

然后，设置第i个三维激光雷达坐标数据的误差为wi，则有计算公式为：

wi＝ki-(R×gi+W)；

S2.3、根据步骤S2.2设置的三维激光雷达坐标数据的误差，构建最小二乘误差minH，则有计算公式为：

S2.4、然后进行优化计算得到最优旋转矩阵R’为：

通过最优旋转矩阵求得最优平移矩阵W’：

W’＝ki-R’×gi；

确定最优旋转矩阵R’、最优平移矩阵W’，完成三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系的坐标系转换；

S3、对采集的三维激光雷达点云数据进行位姿的构建；

进一步的，步骤S3基于正态分布变换匹配方法对采集的三维激光雷达点云数据进行位姿的构建，具体实现方法包括如下步骤：

S3.1、将采集的三维激光雷达点云数据分为参考点云数据，目标点云数据；

S3.2、将参考点云数据按照所占的空间划分成指定大小的网格，并计算每个网格的均值和协方差矩阵：

其中，q为网格的均值，m为网格中数据点的个数，xj为网格中的参考点云数据，j为m中的任意一个；

则协方差矩阵C为：

S3.2、对于目标点云数据Xj，通过变换初始位姿变换矩阵将其转换到参考点云的网格中，计算公式为：

Xj＝T(xj，a)

其中，a为初始化变换参数，T为初始位姿变换矩阵；

计算目标点云数据在各网格内的响应概率密度p：

其中，p(Xj)为目标点云数据Xj在网格内的响应概率密度；

S3.3、通过步骤S3.2的目标点云数据在各网格内的响应概率密度计算正态分布变换匹配方法的配准得分：

S3.4、根据牛顿优化算法对目标函数Score(p)进行优化，寻找初始化变换参数使得Score(p)的值最大，完成三维激光雷达点云数据的位姿构建；

S4、判断步骤S1采集的载波相位差分测量仪测量数据是否满足设定的时间阈值，判断为是，则对步骤S3构建的位姿添加载波相位差分测量仪约束进行多源融合定位导航，判断为否，则进行下一步；

进一步的，步骤S4中对步骤S3构建的位姿添加载波相位差分测量仪约束的具体实现方法为通过因子图融合三维激光雷达点云数据、载波相位差分测量仪测量数据；

S4.1、设置三维激光雷达通过点云配准得到位姿的因子为f_L，则有：

f_L＝x_t+1-h(x_t)

其中，x_k+1为t+1时刻的位姿状态值，h(x_k)为t时刻递推的位姿状态值；

载波相位差分测量仪位置的因子为f_r，则有：

f_r＝g_t+1-h_r(g_t)

其中，g_t+1为t+1时刻的载波相位差分测量仪的测量值，h_r(g_t)为t时刻载波相位差分测量仪的测量方程；

S4.2、通过最大后验概率约束得到目标函数X^*：

其中，Xi为因子图中的第i个数据，fi为第i个因子；

S4.3、最后通过GTSAM求解，完成三维激光雷达点云数据、载波相位差分测量仪测量数据的融合，进行多源融合定位导航；

S5、启动蓝牙扫描模块进行扫描，采集蓝牙数据，然后判断是否采集到蓝牙数据，判断为是，则将采集的蓝牙数据进行解析获取UUID值，将得到的获取UUID值用于进行多源融合定位导航，判断为否，则进行下一步；

进一步的，步骤S5的具体实现方法包括如下步骤：

S5.1、蓝牙扫描模块进行iBeacon设备扫描；

S5.2、扫描结束，判断是否采集到蓝牙数据，判断为是，则将采集到的蓝牙数据根据信号强度值RSSI，蓝牙扫描模块定位3个距离最近的iBeacon设备，分别解析3个距离最近的iBeacon设备的广播数据，根据BLE标准的广播数据格式，解析获取UUID值，为移动设备的位置，用于多源融合定位导航；

进一步的，设置3个距离最近的iBeacon设备的坐标为(x_i,y_i)，i＝1,2,3，设置蓝牙扫描模块的坐标为(x,y)，则蓝牙扫描模块到对应iBeacon设备的距离设置为d_i，则有计算公式为：

(x-x_i)²+(y-y_i)²＝d_i ²

然后，对于3个距离最近的iBeacon设备经过以上公式进行计算，能够得到以下方程为：

得到的坐标为移动设备的位置，用于多源融合定位导航；

S6、重复步骤S1-S5，实现多源融合定位导航。

具体实施方式二：

电子设备，包括存储器和处理器，存储器存储有计算机程序，所述的处理器执行所述计算机程序时实现所述的一种多源融合定位导航方法的步骤。

本发明的计算机装置可以是包括有处理器以及存储器等装置，例如包含中央处理器的单片机等。并且，处理器用于执行存储器中存储的计算机程序时实现上述的基于CREO软件的可修改由关系驱动的推荐数据的推荐方法的步骤。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

具体实施方式三：

计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的一种多源融合定位导航方法。

本发明的计算机可读存储介质可以是被计算机装置的处理器所读取的任何形式的存储介质，包括但不限于非易失性存储器、易失性存储器、铁电存储器等，计算机可读存储介质上存储有计算机程序，当计算机装置的处理器读取并执行存储器中所存储的计算机程序时，可以实现上述的基于CREO软件的可修改由关系驱动的建模数据的建模方法的步骤。

所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

需要说明的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

虽然在上文中已经参考具体实施方式对本申请进行了描述，然而在不脱离本申请的范围的情况下，可以对其进行各种改进并且可以用等效物替换其中的部件。尤其是，只要不存在结构冲突，本申请所披露的具体实施方式中的各项特征均可通过任意方式相互结合起来使用，在本说明书中未对这些组合的情况进行穷举性的描述仅仅是出于省略篇幅和节约资源的考虑。因此，本申请并不局限于文中公开的特定具体实施方式，而是包括落入权利要求的范围内的所有技术方案。

Claims (8)

1.一种多源融合定位导航方法，其特征在于：包括如下步骤：

S1、使用搭载三维激光雷达、载波相位差分测量仪、蓝牙扫描模块的移动设备在室外进行数据采集，首先采集同一时间段内的三维激光雷达点云数据、载波相位差分测量仪测量数据，待用；

S2、对三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系进行坐标系的转换；

S3、对采集的三维激光雷达点云数据进行位姿的构建；

S4、判断步骤S1采集的载波相位差分测量仪测量数据是否满足设定的时间阈值，判断为是，则对步骤S3构建的位姿添加载波相位差分测量仪约束进行多源融合定位导航，判断为否，则进行下一步；

S5、启动蓝牙扫描模块进行扫描，采集蓝牙数据，然后判断是否采集到蓝牙数据，判断为是，则将采集的蓝牙数据进行解析获取UUID值，将得到的获取UUID值用于进行多源融合定位导航，判断为否，则进行下一步；

S6、重复步骤S1-S5，实现多源融合定位导航。

2.根据权利要求1所述的一种多源融合定位导航方法，其特征在于：步骤S2的具体实现方法包括如下步骤：

S2.1、首先对步骤S1采集的三维激光雷达点云数据进行位姿估算，并记录下三维激光雷达的运动位姿数据，同时记录下对应时间段内的载波相位差分测量仪测量数据，采用遍历搜索的方法寻找三维激光雷达位姿和载波相位差分测量仪测量值时间最接近的坐标数据，最后得到时间对应的三维激光雷达坐标数据、载波相位差分测量仪测量坐标数据用于坐标系转换，其中三维激光雷达坐标数据为K＝{k1，…ki，…kn}，载波相位差分测量仪测量坐标数据为G＝{g1，…gi，…gn}，其中n为坐标数据的个数，i为n中的任意一个；

S2.2、设置旋转矩阵为R，平移矩阵为W，对于第i个三维激光雷达坐标数据ki则有计算公式为：

ki＝R×gi+W；

其中，ki为第i个三维激光雷达坐标数据，gi为第i个载波相位差分测量仪测量坐标数据；

然后，设置第i个三维激光雷达坐标数据的误差为wi，则有计算公式为：

wi＝ki-(R×gi+W)；

S2.3、根据步骤S2.2设置的三维激光雷达坐标数据的误差，构建最小二乘误差minH，则有计算公式为：

S2.4、然后进行优化计算得到最优旋转矩阵R’为：

通过最优旋转矩阵求得最优平移矩阵W’：

W’＝ki-R’×gi；

确定最优旋转矩阵R’、最优平移矩阵W’，完成三维激光雷达点云数据的激光坐标系、载波相位差分测量仪测量数据的载波相位差分测量仪坐标系的坐标系转换。

3.根据权利要求2所述的一种多源融合定位导航方法，其特征在于：步骤S3基于正态分布变换匹配方法对采集的三维激光雷达点云数据进行位姿的构建，具体实现方法包括如下步骤：

S3.1、将采集的三维激光雷达点云数据分为参考点云数据，目标点云数据；

S3.2、将参考点云数据按照所占的空间划分成指定大小的网格，并计算每个网格的均值和协方差矩阵：

其中，q为网格的均值，m为网格中数据点的个数，xj为网格中的参考点云数据，j为m中的任意一个；

则协方差矩阵C为：

S3.2、对于目标点云数据Xj，通过变换初始位姿变换矩阵将其转换到参考点云的网格中，计算公式为：

Xj＝T(xj，a)

其中，a为初始化变换参数，T为初始位姿变换矩阵；

计算目标点云数据在各网格内的响应概率密度p：

其中，p(Xj)为目标点云数据Xj在网格内的响应概率密度；

S3.3、通过步骤S3.2的目标点云数据在各网格内的响应概率密度计算正态分布变换匹配方法的配准得分：

S3.4、根据牛顿优化算法对目标函数Score(p)进行优化，寻找初始化变换参数使得Score(p)的值最大，完成三维激光雷达点云数据的位姿构建。

4.根据权利要求3所述的一种多源融合定位导航方法，其特征在于：步骤S4中的时间阈值范围为5-10s。

5.根据权利要求4所述的一种多源融合定位导航方法，其特征在于：步骤S4中对步骤S3构建的位姿添加载波相位差分测量仪约束的具体实现方法为通过因子图融合三维激光雷达点云数据、载波相位差分测量仪测量数据；

S4.1、设置三维激光雷达通过点云配准得到位姿的因子为f_L，则有：

f_L＝x_t+1-h(x_t)

其中，x_k+1为t+1时刻的位姿状态值，h(x_k)为t时刻递推的位姿状态值；

载波相位差分测量仪位置的因子为f_r，则有：

f_r＝g_t+1-h_r(g_t)

其中，g_t+1为t+1时刻的载波相位差分测量仪的测量值，h_r(g_t)为t时刻载波相位差分测量仪的测量方程；

S4.2、通过最大后验概率约束得到目标函数X^*：

其中，Xi为因子图中的第i个数据，fi为第i个因子；

S4.3、最后通过GTSAM求解，完成三维激光雷达点云数据、载波相位差分测量仪测量数据的融合，进行多源融合定位导航。

6.根据权利要求5所述的一种多源融合定位导航方法，其特征在于：步骤S5的具体实现方法包括如下步骤：

S5.1、蓝牙扫描模块进行iBeacon设备扫描；

S5.2、扫描结束，判断是否采集到蓝牙数据，判断为是，则将采集到的蓝牙数据根据信号强度值RSSI，蓝牙扫描模块定位3个距离最近的iBeacon设备，分别解析3个距离最近的iBeacon设备的广播数据，根据BLE标准的广播数据格式，解析获取UUID值，为移动设备的位置，用于多源融合定位导航。

7.电子设备，其特征在于，包括存储器和处理器，存储器存储有计算机程序，所述的处理器执行所述计算机程序时实现权利要求1-6任一项所述的一种多源融合定位导航方法的步骤。

8.计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-6任一项所述的一种多源融合定位导航方法。

Images