一种基于多传感器融合的高精度定位方法及系统技术方案

本发明专利技术公开了一种基于多传感器融合的高精度定位方法及系统，包括：先将码盘与IMU数据作EKF滤波处理，得到高精度里程计再进入SLAM模块中作融合；分别构建坐标对齐和坐标转换模块，在坐标对齐模块中，使用SLAM输出位置与RTK输出进行非线性优化得到其坐标转换矩阵，再通过坐标转换模块载入该转换矩阵将RTK

【技术实现步骤摘要】
一种基于多传感器融合的高精度定位方法及系统


[0001]本专利技术涉及一种基于多传感器融合的高精度定位方法及系统，用于解决各类移动机器人或智能车辆在巡航过程中如何实现精准定位的问题。

技术介绍

[0002]单一的导航系统都存在着各自的缺陷，例如：GNSS定位在信号覆盖不到的地方，譬如隧道、复杂城市环境等地方定位结果极度偏差；运动估计定位会随着时间累计产生积分误差造成定位不准确；SLAM(同步定位与地图构建)定位主要依靠环境特征，当环境变化明显的时候，定位误差大。基于此，越来越多的多传感器融合定位方案开始应用于移动机器人或车辆的定位系统中。
[0003]然而在目前已有的多传感器融合定位方案中，存在以下技术问题：一方面，IMU和底盘数据直接用于SLAM模块作位姿预测，当IMU或者底盘码盘任一数据出现错误时，都可能导致SLAM系统崩溃从而出现定位不稳定现象。
[0004]另一方面，在高精度定位系统中(定位精度要求1
‑
2cm)，非RTK工作模式的GNSS系统或者未经过精确标定的GNSS到SLAM地图坐标系的转换会导致系统出现较大的误差，甚至会使得定位系统误差大于不融合的定位误差。
[0005]为此，本专利技术提供一种基于多传感器融合的高精度定位方法及系统，一方面使用EKF对IMU以及码盘数据进行初步处理，再传入SLAM模块进行位姿预测，提高SLAM系统稳定性；另一方面通过构建SLAM定位数据与GNSS定位数据的误差模型，求解非线性优化问题，得到高精度的坐标转换矩阵，使最终的融合输出能够实现稳定高精度的定位输出。

技术实现思路

[0006]专利技术目的：为了克服现有技术中存在的不足，本专利技术提供一种基于多传感器融合的高精度定位方法及系统，能够有效提高定位稳定性和定位精度，同时降低部署难度和成本，保证在多种环境下的稳定定位作业，大幅降低安全事故。
[0007]技术方案：为实现上述目的，本专利技术提供一种基于多传感器融合的高精度定位方法及系统，包括以下步骤：
[0008]S1获取激光雷达、GNSS、IMU以及码盘数据，并对激光雷达、IMU、GNSS以及码盘数据进行时间同步；
[0009]S2根据IMU以及码盘数据得到载体的位置状态估计，再进入SLAM模块中作融合进行位姿优化；
[0010]S3初始化阶段，采用坐标对齐模块对SLAM输出位置与GNSS输出位置进行非线性优化，得到其坐标转换矩阵；
[0011]S4待初始化结束后，根据得到的坐标转换矩阵对GNSS数据进行坐标转换，再与SLAM输出位置作融合，得到高精度定位信息。
[0012]进一步地，所述步骤S2中的融合过程具体包括：
[0013]S2.1构建一个EKF融合模块，其输入为IMU以及码盘数据，状态变量为：
[0014]x＝[p,q,v,w,a][0015]其中状态p,q,v,w,a分别为：位置、姿态、速度、角速度和加速度，而后根据底盘形态构建运动模型(模型根据实际运动模型决定)F(x)，于是有预测模型：
[0016]k为时刻数据
[0017]进而将IMU与底盘数据作为观测数据得到：
[0018][0019]其中，y
k
、z
k
分别表示IMU观测数据和底盘码盘观测数据，而后根据EKF原理计算卡尔曼增益K并将预测状态与观测状态融合得到最终输出；
[0020]S2.2将S2.1中得到的位置状态估计作为SLAM预测值，使用激光雷达数据结合局部地图进行位姿优化，并将IMU积分数据用于SLAM后端作为约束。
[0021]进一步地，所述步骤S3中的初始化过程具体包括：
[0022]S3.1设定待计算量：GNSS转到的ENU坐标系到SLAM地图坐标系的转换矩阵和GNSS天线到载体的外参选定一个固定窗口，其大小为n(可配置)；
[0023]S3.2在窗口大小范围内，从SLAM模块采集载体在SLAM地图坐标系中的位置以及姿态k为时刻数据；
[0024]S3.3同步从GNSS系统采集GNSS天线的经纬度信息，并转换为ENU坐标系位置
[0025]S3.4在任意k时刻，由SLAM输出位置和姿态可以得到GNSS天线在SLAM地图坐标系的位置如下：
[0026][0027]由GNSS测量值得到GNSS天线在SLAM地图坐标系的位置如下：
[0028][0029]于是存在线性模型：
[0030][0031]其中n
k
为k时刻的噪声，进而构建并求解最小二乘问题：
[0032][0033]S3.5重复步骤S3.2到S3.4，直到待优化量连续两次变化值和小于阈值ε则认为收敛。
[0034]进一步地，所述步骤S4具体包括：
[0035]S4.1待初始化结束后，根据得到的转换矩阵和外参将i时刻GNSS系统输出的经纬度转换到ENU坐标系得到的转换为SLAM地图坐标系下的坐标：
[0036][0037]S4.2将上述GNSS数据的转换结果与SLAM输出位姿作为输入，进行融合运算得到最终定位结果。
[0038]还有一种基于多传感器融合的高精度定位系统，包括安装于载体上的激光雷达、GNSS天线、GNSS模块、底盘码盘、惯导测量单元(IMU)和处理器，所述GNSS天线通过GNSS模块与处理器通信连接，所述处理器用于接收激光雷达、GNSS模块、底盘码盘、惯导测量单元的传输数据，并根据上述基于多传感器融合的高精度定位方法，处理得到高精度定位信息。
[0039]有益效果：本专利技术提供的一种基于多传感器融合的高精度定位方法及系统，相对于现有技术，具有以下优点：
[0040](1)使用EKF对底盘码盘、IMU数据进行预处理，再传入SLAM模块进行位姿预测，有效提高SLAM系统稳定性，并大幅提高SLAM定位精度；
[0041](2)通过构建SLAM定位数据与GNSS定位数据的误差模型，求解非线性优化问题，得到高精度的坐标转换矩阵，使最终的融合输出能够实现稳定高精度的定位输出；
[0042](3)该标定模块仅需要接收单天线RTK
‑
GNSS系统定位数据，相比较于部分必须使用RTK双天线获取朝向角的坐标对齐方法，有效降低了系统成本，提高了定位精度，同时增强了硬件部署的便利性。
附图说明
[0043]下面结合附图对本专利技术作进一步描写和阐述。
[0044]图1为本专利技术优选实施例的硬件连接示意图；
[0045]图2为本专利技术优选实施例的融合定位原理图。
具体实施方式
[0046]下面将结合附图对本专利技术的优选实施方式进行描述，更加清楚、完整地阐述本专利技术的技术方案。
[0047]实施例
[0048]如图1所示为一种基于多传感器融合的高精度定位系统的优选实施方式，可用于巡检机器人上。在硬件安装上，需要在机器人内部安装GNSS模块，在机器人最高位置加装RTK
‑
GNS本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多传感器融合的高精度定位方法，其特征在于，包括以下步骤：S1获取激光雷达、GNSS、IMU以及码盘数据，并对激光雷达、IMU、GNSS以及码盘数据进行时间同步；S2根据IMU以及码盘数据得到载体的位置状态估计，再进入SLAM模块中作融合进行位姿优化；S3初始化阶段，采用坐标对齐模块对SLAM输出位置与GNSS输出位置进行非线性优化，得到其坐标转换矩阵；S4待初始化结束后，根据得到的坐标转换矩阵对GNSS数据进行坐标转换，再与SLAM输出位置作融合，得到高精度定位信息。2.根据权利要求1所述的一种基于多传感器融合的高精度定位方法，其特征在于，所述步骤S2中的融合过程具体包括：S2.1构建一个EKF融合模块，其输入为IMU以及码盘数据，状态变量为：x＝[p,q,v,w,a]其中状态p,q,v,w,a分别为：位置、姿态、速度、角速度和加速度，而后根据底盘形态构建运动模型F(x)，于是有预测模型：k为时刻数据进而将IMU与底盘数据作为观测数据得到：其中，y
k
、z
k
分别表示IMU观测数据和底盘码盘观测数据，之后根据EKF原理计算卡尔曼增益K并将预测状态与观测状态融合得到最终输出；S2.2将S2.1中得到的位置状态估计作为SLAM预测值，使用激光雷达数据结合局部地图进行位姿优化，并将IMU积分数据用于SLAM后端作为约束。3.根据权利要求2所述的一种基于多传感器融合的高精度定位方法，其特征在于，所述步骤S3中的初始化过程具体包括：S3.1设定待计算量：GNSS转...

【专利技术属性】
技术研发人员：程敏，刘文博，罗作煌，闫宗涛，李勇兵，徐伟，李奇，朱超，
申请(专利权)人：深圳亿嘉和科技研发有限公司，
类型：发明
国别省市：