融合定位方法、系统、设备和介质与流程

本发明涉及定位领域，特别涉及一种融合定位方法、系统、设备和介质。

背景技术：

1、随着技术的进步，利用imu(inertial measurement unit，惯性测量单元)来估计物体的位置和运动变化已广泛应用于各个领域。但在使用imu进行定位时，经常面临一个挑战：如何消除imu自身的偏置。为此，业界普遍采用imu与gps(global positioning system，全球定位系统)的融合定位方法。这种方法首先通过动力学方程，仅依赖imu的加速度计或陀螺仪，计算出物体当前的状态，包括位置、速度、方向和imu偏置。接收到gps数据后，再利用gps与当前状态的位置差，构建观测方程以更新状态量。

2、然而，这种传统的imu和gps融合定位的方式存在如下问题：首先，由于缺乏对imu偏置的直接评估，这种方法仅依赖于gps数据作为观测方程来进行偏置修正，导致修正精度较低且偏置估计不够准确。其次，高频率(通常高达100hz)的imu数据与低频率(1hz)的gps数据融合时，使用gps数据对imu数据进行矫正的速度过慢。在两次gps的间隔(一秒钟)时间内，imu累积的误差不容忽视，从而导致使用gps对imu矫正后的数据曲线容易出现明显的锯齿状波动。因此，需要提供一种融合定位方法、系统、设备和介质。

技术实现思路

1、本发明提供一种融合定位方法、系统、设备和介质。以解决现有技术中将gps和imu数据融合时，由于对imu的偏置估计不准确致使最后定位精度不高的问题。

2、本发明提供的一种融合定位方法，包括：获取并保存当前时刻的gps数据和imu数据；对前一时刻的imu数据进行检测，若前一时刻的imu数据满足预设的零速检测条件，则对预先存储的前一时刻的状态量中的速度量进行置零处理；其中，状态量包括位置量、速度量、姿态量、加速度计偏置量和陀螺仪偏置量；根据前一时刻的imu数据和前一时刻的状态量，预测当前时刻的状态量；根据当前时刻的状态量以及预先存储的前一时刻的状态量协方差矩阵，预测当前时刻的状态量协方差矩阵；根据当前时刻的状态量、当前时刻的状态量协方差矩阵和当前时刻的gps数据，获得当前时刻的卡尔曼增益和残差；根据当前时刻的卡尔曼增益和残差，对当前时刻的状态量和状态量协方差矩阵进行更新并保存更新后的结果，将更新后的位置量作为当前时刻的位置。

3、于本发明一实施例中，所述imu数据包括加速度和角速度，所述零速检测条件为：前一时刻的加速度的模长属于预设的加速度阈值区间；且前一时刻角速度的模长小于预设的角速度模长阈值；且前一时刻加速度的方差大于预设的方差阈值。

4、于本发明一实施例中，所述根据当前时刻的状态量、当前时刻的协方差矩阵和当前时刻的gps数据，获得当前时刻的卡尔曼增益和残差，包括：根据当前时刻的状态量、当前时刻的协方差矩阵获得当前时刻的卡尔曼增益其中，ck为当前时刻k的协方差矩阵，hk为当前时刻k的状态量的雅克比矩阵，cε为预设的观测协方差矩阵，t为矩阵的转置，()-1为矩阵的逆运算；将当前时刻的gps数据和当前时刻的状态量中的位置量相减，获得当前时刻的残差。

5、于本发明一实施例中，当前时刻的状态量的更新过程包括：将当前时刻的卡尔曼增益和残差进行加权处理；将加权处理后的值与当前时刻的状态量相加，将相加后的状态量作为当前时刻的状态量。

6、于本发明一实施例中，当前时刻的状态量协方差矩阵更新过程包括：计算当前时刻的状态量的雅克比矩阵；根据当前时刻的卡尔曼增益和当前时刻的状态量的雅克比矩阵，更新当前时刻的状态量协方差矩阵。

7、于本发明一实施例中，当前一时刻为初始时刻时，初始时刻的状态量的获取过程为：获取并保存以预设的时间间隔发送的gps数据和imu数据；基于gps数据和imu数据获取初始的位置量、速度量、姿态量；根据预设时长内累计的gps数据，对预存的加速度计偏置量和陀螺仪偏置量进行初始化处理，获取初始化后的加速度计偏置量和陀螺仪偏置量；将初始化后的加速度计偏置量和陀螺仪偏置量，以及初始的位置量、速度量、姿态量作为初始时刻的状态量；其中，初始时刻为加速度计偏置量和陀螺仪偏置量初始化完成的时刻。

8、于本发明一实施例中，所述获取初始的位置量、速度量、姿态量的同时，对预存的加速度计偏置量和陀螺仪偏置量进行初始化处理。

9、在本发明的另一方面，还提供了一种融合定位系统，所述系统包括：数据获取模块，用于获取并保存当前时刻的gps数据和imu数据；零速检测模块，用于对前一时刻的imu数据进行检测，若前一时刻的imu数据满足预设的零速检测条件，则对预先存储的前一时刻的状态量中的速度量进行置零处理；其中，状态量包括位置量、速度量、姿态量、加速度计偏置量和陀螺仪偏置量；状态量预测模块，用于根据前一时刻的imu数据和前一时刻的状态量，预测当前时刻的状态量；协方差矩阵预测模块，用于根据当前时刻的状态量以及预先存储的前一时刻的状态量协方差矩阵，预测当前时刻的状态量协方差矩阵；增益残差获取模块，用于根据当前时刻的状态量、当前时刻的状态量协方差矩阵和当前时刻的gps数据，获得当前时刻的卡尔曼增益和残差；定位模块，用于根据当前时刻的卡尔曼增益和残差，对当前时刻的状态量和状态量协方差矩阵进行更新并保存更新后的结果，将更新后的位置量作为当前时刻的位置。

10、于本发明一实施例中，还提供一种电子设备，电子设备包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述电子设备实现上述任一项所述融合定位方法。

11、于本发明一实施例中，还提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序被计算机的处理器执行时，使计算机执行上述任一项所述融合定位方法。

12、本发明提出的一种融合定位方法，通过对imu数据进行零速检测，当检测到设备处于静止状态时，可以重新校准速度，抵消了部分imu的偏置影响，提升了对imu偏置估计的准确度，从而减少误差的累积。通过分析前一时刻的imu数据和状态量，可以预测当前时刻的状态量，在处于快速变化的动态环境时，这种方式有利于提升定位的准确度。结合当前时刻的状态量、协方差矩阵和gps数据，可以优化状态估计，能够有效地融合多源数据，减少误差，提升了位置估计的准确度。

技术特征：

1.一种融合定位方法，其特征在于，所述方法包括：

2.根据权利要求1所述的融合定位方法，其特征在于，imu数据包括加速度和角速度，所述零速检测条件为：前一时刻的加速度的模长属于预设的加速度阈值区间；且前一时刻的角速度的模长小于预设的角速度模长阈值；且前一时刻的加速度的方差大于预设的方差阈值。

3.根据权利要求1所述的融合定位方法，其特征在于，所述根据所述当前时刻的状态量、所述当前时刻的状态量协方差矩阵和所述当前时刻的gps数据，获得当前时刻的卡尔曼增益和残差，包括：

4.根据权利要求1所述的融合定位方法，其特征在于，所述当前时刻的状态量的更新过程包括：

5.根据权利要求1所述的融合定位方法，其特征在于，所述当前时刻的状态量协方差矩阵更新过程包括：

6.根据权利要求1所述的融合定位方法，其特征在于，当前一时刻为初始时刻时，初始时刻的状态量的获取过程为：

7.根据权利要求6所述的融合定位方法，其特征在于，所述获取初始的位置量、速度量、姿态量的同时，对预存的加速度计偏置量和陀螺仪偏置量进行初始化处理。

8.一种融合定位系统，其特征在于，所述系统包括：

9.一种电子设备，其特征在于：所述电子设备包括：

10.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，当所述计算机程序被计算机的处理器执行时，使计算机执行权利要求1至7中任一项所述融合定位方法。

技术总结
本发明涉及提供一种融合定位方法、系统、设备和介质。方法包括：获取并保存当前时刻的GPS数据和IMU数据；对前一时刻的IMU数据进行检测，若前一时刻的IMU数据满足预设的零速检测条件，对前一时刻的状态量中的速度量进行置零处理；根据前一时刻的IMU数据和前一时刻的状态量，预测当前时刻的状态量；根据当前时刻的状态量及前一时刻的状态量协方差矩阵，预测当前时刻的状态量协方差矩阵；根据当前时刻的状态量、状态量协方差矩阵和GPS数据，获得当前时刻的卡尔曼增益和残差；根据当前时刻的卡尔曼增益和残差，对当前时刻的状态量和状态量协方差矩阵进行更新并保存更新后的结果，将更新后的状态量中的位置量作为当前时刻的位置。提升了定位精度。

技术研发人员：杨清
受保护的技术使用者：恒玄科技（上海）股份有限公司
技术研发日：
技术公布日：2024/3/24