多传感器融合定位方法、装置、系统及存储介质制造方法及图纸

本申请涉及定位技术领域，尤其涉及一种多传感器融合定位方法、系统及相关装置。本方法根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，通过获取全局定位数据和轮速计的数据，并基于惯性测量单元测量数据、全局定位数据和轮速计的数据，构建观测残差和雅可比矩阵。基于观测残差和所述雅可比矩阵采用误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，得到下一时刻状态值，最终输出位姿信息。本方法实现在不损失定位精度的情况下，同时降低计算资源的占用和计算耗时，提高机体定位的准确性和实时性。提高机体定位的准确性和实时性。提高机体定位的准确性和实时性。

【技术实现步骤摘要】
多传感器融合定位方法、装置、系统及存储介质


[0001]本申请涉及定位
，尤其涉及一种多传感器融合定位方法、系统及相关装置。

技术介绍

[0002]随着机器人、无人机和无人驾驶技术的快速发展，为准确确定机体在环境中的位置和方向，对定位技术的要求越来越高。目前，常见定位技术包括全局定位系统(GPS)、惯性导航系统(INS)、视觉识别等。其中，GPS技术是一种广泛使用的全球卫星导航系统，具有高精度、高稳定性和全球覆盖的优势。但是，GPS技术在室内或密闭空间中信号受干扰而失效，而且无法提供高精度的位置测量。INS技术是一种基于惯性测量单元(IMU)和运动学模型的定位技术，具有无需外部信号、高精度和高可靠性的优点。但是，INS技术的传感器的误差和模型误差会随着时间的推移而积累，导致定位精度随时间的增加而降低。视觉识别技术是一种主动或被动式传感器，基于机器视觉和图像处理技术进行环境感知和目标识别，具有定位精度高、适用范围广的优势。但是，视觉识别技术对处理器和算法要求高，处理复杂场景时计算量过大导致实时性弱。
[0003]综上所述，现有定位技术可能存在精度不够高，或者，计算量过大导致实时性不强等缺点。

技术实现思路

[0004]本申请实施例提供一种多传感器融合定位方法、装置、系统及存储介质，实现在不损失定位精度的情况下，同时降低计算资源的占用和计算耗时，提高机体定位的准确性和实时性。
[0005]第一方面，本申请实施例提供一种多传感器融合定位方法、装置、系统及存储介质，该方法包括：
[0006]根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
[0007]获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据、所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
[0008]根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
[0009]根据下一时刻状态值，输出位姿信息。
[0010]在一些实施例中，所述状态值包括位置值、姿态值和速度值。
[0011]所述根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪的步骤，包括：
[0012]获取所述惯性测量单元输出的加速度计测量值和陀螺仪测量值；
[0013]获取所述惯性测量单元的加速度偏置和陀螺仪偏置；
[0014]根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置、所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值。
[0015]在一些实施例中，所述根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置和所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值，包括：
[0016]基于以下预测方程得到所述下一时刻状态预测值：
[0017][0018][0019][0020]biasa
k+1
＝bias_a
k
；
[0021]bias_g
k+1
＝bias_g
k
；
[0022]Scale
k+1
＝Scale
k
；
[0023]其中，P为位置值，V为速度值，R为姿态值，bias_a为加速度偏置，bias_g为陀螺仪偏置，Scale为速度系数，k和k+1分别代表上一时刻和下一时刻，G为重力向量，Δt为上一时刻和下一时刻之间的时间增量，acc为加速度计测量值，ω为陀螺仪测量值，M为全局地图坐标系，I为惯性测量单元坐标系。
[0024]在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
[0025]位置的观测残差为：
[0026][0027]其中，res_P为所述全局定位传感器的观测残差，measurement_P为所述全局定位传感器实际的观测值，为上一时刻状态的位置值；
[0028]位置的雅可比矩阵为：
[0029][0030]位置观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
[0031][0032]在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
[0033]姿态的观测残差为：
[0034][0035]其中，res_θ为全局姿态传感器的观测残差。measurement_θ为全局姿态传感器实际的观测值，为上一时刻状态的姿态值；
[0036]姿态的雅可比矩阵为：
[0037][0038]姿态观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
[0039][0040]在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
[0041]速度的观测残差为：
[0042][0043]其中，res_vel为所述速度的观测残差，new_measurement为所述校正后的轮速计的测量数据，为上一时刻状态值中的姿态数据，Scale为速度系数，为上一时刻状态值中的速度数据；
[0044]速度的雅可比矩阵为：
[0045][0046]轮速计观测对姿态的雅可比矩阵为：
[0047][0048]轮速计观测对速度系数的雅可比矩阵为：
[0049][0050]轮速计观测对位置、加速度偏置、陀螺仪偏置的雅可比矩阵为：
[0051][0052]在一些实施例中，所述方法还包括：
[0053]重复执行预测和更新，得到多个实时定位信息；
[0054]基于所述多个实时定位信息获得运动轨迹，输出所述运动轨迹。
[0055]第二方面，本申请提供一种多传感器融合定位装置，该装置包括：
[0056]预测模块,用于根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
[0057]构建模块，用于获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
[0058]更新模块，用于根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
[0059]输出模块，用于根据下一时刻状态值，输出位姿信息。
[0060]第三方面，本申请提供一种多传感器融合定位系统，该系统包括：惯性测本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种多传感器融合定位方法，其特征在于，所述方法包括：根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据、所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；根据下一时刻状态值，输出位姿信息。2.根据权利要求1所述的方法，其特征在于，所述状态值包括位置值、姿态值和速度值。所述根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪的步骤，包括：获取所述惯性测量单元输出的加速度计测量值和陀螺仪测量值；获取所述惯性测量单元的加速度偏置和陀螺仪偏置；根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置、所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值。3.根据权利要求2所述的方法，其特征在于，所述根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置和所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值，包括：基于以下预测方程得到所述下一时刻状态预测值：基于以下预测方程得到所述下一时刻状态预测值：基于以下预测方程得到所述下一时刻状态预测值：bias_a
k+1
＝ias_a
k
；bias_g
k+1
＝ias_g
k
；Scale
k+1
＝cale
k
；其中，P为位置值，V为速度值，R为姿态值，bias_a为加速度偏置，bias_g为陀螺仪偏置，Scale为速度系数，k和k+1分别代表上一时刻和下一时刻，G为重力向量，Δt为上一时刻和下一时刻之间的时间增量，acc为加速度计测量值，ω为陀螺仪测量值，M为全局地图坐标系，I为惯性测量单元坐标系。4.根据权利要求1
‑
3任一项所述的方法，其特征在于，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：位置的观测残差为：其中，res_P为所述位置的观测残差，measurement_P为所述加速度计的测量数据，为
所述全局位置数据；位置的雅可比矩阵为：1 0 00 1 00 0 1；位置观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：0 0 00 0 00 0 0。5.根据权利要求4所述的方法，其特征在于...

【专利技术属性】
技术研发人员：王鲁佳，杜俊文，刘明，
申请(专利权)人：深圳一清创新科技有限公司，
类型：发明
国别省市：