CN112596089B - 融合定位方法、装置、电子设备及存储介质 - Google Patents

融合定位方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN112596089B
CN112596089B CN202110230387.XA CN202110230387A CN112596089B CN 112596089 B CN112596089 B CN 112596089B CN 202110230387 A CN202110230387 A CN 202110230387A CN 112596089 B CN112596089 B CN 112596089B
Authority
CN
China
Prior art keywords
coordinate system
information
gnss
prior
covariance
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110230387.XA
Other languages
English (en)
Other versions
CN112596089A (zh
Inventor
杨东升
李佰霖
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tencent Technology Shenzhen Co Ltd
Original Assignee
Tencent Technology Shenzhen Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tencent Technology Shenzhen Co Ltd filed Critical Tencent Technology Shenzhen Co Ltd
Priority to CN202110230387.XA priority Critical patent/CN112596089B/zh
Publication of CN112596089A publication Critical patent/CN112596089A/zh
Application granted granted Critical
Publication of CN112596089B publication Critical patent/CN112596089B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本申请实施例提供了一种融合定位方法、装置、电子设备及存储介质，涉及定位技术领域，可应用于地图、智慧交通、智慧出行等应用场景。该方法包括：获取运动对象在当前时刻的全局坐标系下的先验位姿信息；根据全局坐标系下的先验位姿信息获得当前时刻的GNSS预估值；获取运动对象在当前时刻的GNSS测量值，结合当前时刻的GNSS预估值，对全局坐标系下的先验位姿信息在运动对象的运动方向的分量进行更新，获得当前时刻的全局坐标系下的后验位姿信息。本申请实施例能够消除了GNSS误差对于融合定位结果横向定位精度的不利影响，保留GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。

Description

融合定位方法、装置、电子设备及存储介质
技术领域
本申请涉及定位技术领域，具体而言，本申请涉及一种融合定位方法、装置、电子设备及存储介质。
背景技术
高精定位是自动驾驶系统的核心功能。单一传感器不可避免的具有其自身的局限性，通常高精定位系统大都采用多传感器融合技术，将GNSS(Global NavigationSatellite System，全球导航卫星系统)、IMU（Inertial Measurement Unit，惯性传感器）、轮速、高精地图、相机、激光雷达等传感信息融合。
GNSS可以在全球位置提供绝对的定位信息，但是受到电离层、反射、城市峡谷等因素影响，普通GNSS的精度往往较差，且不够稳定。随着时间和位置的变化，定位精度会有不同程度的偏差，有些地方甚至偏差可以到几米甚至十几米。
现有融合定位方案大都直接将低精度GNSS信息与其他传感器采集的状态信息进行融合。但这类方法输出的定位精度往往较低，难以满足自动驾驶的需求。
RTK（Real - time kinematic）技术，通过在地面建立校正基站，可以有效提高GNSS的定位精度，精度可达厘米，甚至毫米级别。但是使用RTK技术进行融合定位，需要布设大量基站，设备成本和维护成本高昂。
发明内容
本发明实施例提供一种克服上述问题或者至少部分地解决上述问题的融合定位方法、装置、电子设备及存储介质。
第一方面，提供了一种融合定位方法，该方法包括：
获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，位姿信息包括运动对象的运动方向；
根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值；
根据GNSS测量值和GNSS预估值，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，包括：
将全局坐标系下位姿信息的先验分布转换至局部坐标系，获得局部坐标系下位姿信息的先验分布；局部坐标系的目标坐标轴指向运动对象的运动方向；
构建表征当前时刻的GNSS信息与局部坐标系下位姿信息间对应关系的第一观测方程，将局部坐标系下的位姿信息的先验分布输入至第一观测方程，获得第一观测方程输出的当前时刻的GNSS预估值。
对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括：
根据第一观测方程以及局部坐标系下位姿信息的先验分布，获得局部坐标系下位姿信息的第一卡尔曼增益；
从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益；
根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
将当前时刻的局部坐标系下位姿信息的后验分布转换至全局坐标系，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
根据第一观测方程以及局部坐标系下位姿信息的先验分布，获得局部坐标系下位姿信息的第一卡尔曼增益，包括：
确定第一观测方程的第一雅克比矩阵以及观测噪声的第一协方差；
根据第一协方差、第一雅克比矩阵以及局部坐标系下位姿信息的先验协方差，确定第一卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布，包括：
根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
根据第二卡尔曼增益以及残差的乘积，获得局部坐标系下位姿信息的先验均值的增量，根据增量对局部坐标系下位姿信息的先验均值进行更新，获得当前时刻的局部坐标系下位姿信息的后验均值。
进一步地，根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布，包括：
根据第一雅克比矩阵、第一协方差以及第二卡尔曼增益，对局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差。
进一步地，根据第一雅克比矩阵、第一协方差以及第二卡尔曼增益，对局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差，包括：
确定第一雅克比矩阵与第二卡尔曼增益的乘积，根据乘积以及局部坐标系下位姿信息的先验协方差，获得局部坐标系下位姿信息的后验协方差的第一分量；
根据第一协方差以及第二卡尔曼增益，获得局部坐标系下位姿信息的后验协方差的第二分量；
根据第一分量和第二分量之和，获得局部坐标系下位姿信息的后验协方差。
进一步地，根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，包括：
构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，将全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值。
进一步地，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括：
根据第二观测方程以及全局坐标系下位姿信息的先验分布，获得全局坐标系下位姿信息的第三卡尔曼增益；
从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第三卡尔曼增益中确定目标子位姿信息的第三卡尔曼增益在运动方向上的分量，作为第四卡尔曼增益；
根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
根据第二观测方程以及全局坐标系下位姿信息的先验分布，获得全局坐标系下位姿信息的第三卡尔曼增益，包括：
确定第二观测方程的第二雅克比矩阵以及观测噪声的第二协方差；
根据第二协方差、第二雅克比矩阵以及全局坐标系下位姿信息的先验协方差，确定第三卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布包括：
根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
根据第四卡尔曼增益、残差以及运动方向的方向向量的乘积，获得全局坐标系下位姿信息的先验均值的增量；
根据增量对全局坐标系下位姿信息的先验均值进行更新，获得当前时刻的全局坐标系下位姿信息的后验均值。
进一步地，根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布包括：
根据第二雅克比矩阵、第二协方差以及第四卡尔曼增益，对全局坐标系下的先验协方差进行更新，获得当前时刻的全局坐标系下位姿信息的后验协方差。
进一步地，根据第二雅克比矩阵、第二协方差以及第四卡尔曼增益，对全局坐标系下的先验协方差进行更新，获得当前时刻的全局坐标系下位姿信息的后验协方差，包括：
确定第二雅克比矩阵、对角矩阵与第四卡尔曼增益的乘积，根据乘积以及全局坐标系下位姿信息的先验协方差，获得全局坐标系下位姿信息的后验协方差的第一分量；
根据第二协方差、对角矩阵以及第四卡尔曼增益，获得全局坐标系下位姿信息的后验协方差的第二分量；
根据第一分量和第二分量之和，获得全局坐标系下位姿信息的后验协方差；
其中，对角矩阵中非0的元素位于与目标运动状态对应行列，非0的元素为运动方向的单位向量与运动方向的单位向量的转置的乘积。
第二方面，提供了一种融合定位装置，包括：
先验数据获取模块，用于获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，位姿信息包括运动对象的运动方向；
GNSS预估模块，用于根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值；
定位更新模块，用于根据GNSS测量值和GNSS预估值，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，GNSS预估模块包括：
先验分布转换子模块，用于将全局坐标系下位姿信息的先验分布转换至局部坐标系，获得局部坐标系下位姿信息的先验分布；局部坐标系的目标坐标轴指向运动对象的运动方向；
第一观测方程构建子模块，用于构建表征当前时刻的GNSS信息与局部坐标系下位姿信息间对应关系的第一观测方程，将局部坐标系下的位姿信息的先验分布输入至第一观测方程，获得第一观测方程输出的当前时刻的GNSS预估值。
进一步地，定位更新模块包括：
局部增益计算子模块，用于根据第一观测方程以及局部坐标系下位姿信息的先验分布，获得局部坐标系下位姿信息的第一卡尔曼增益；
局部增益更新子模块，用于从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益；
局部后验子模块，用于根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
全局后验子模块，用于将当前时刻的局部坐标系下位姿信息的后验分布转换至全局坐标系，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
局部增益计算子模块包括：
第一观测参数确定单元，用于确定第一观测方程的第一雅克比矩阵以及观测噪声的第一协方差；
局部增益确定单元，用于根据第一协方差、第一雅克比矩阵以及局部坐标系下位姿信息的先验协方差，确定第一卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
局部后验子模块包括：
残差确定单元，用于根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
局部后验均值单元，用于根据第二卡尔曼增益以及残差的乘积，获得局部坐标系下位姿信息的先验均值的增量，根据增量对局部坐标系下位姿信息的先验均值进行更新，获得当前时刻的局部坐标系下位姿信息的后验均值。
进一步地，局部后验子模块包括：
局部后验协方差确定单元，用于根据第一雅克比矩阵、第一协方差以及第二卡尔曼增益，对局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差。
进一步地，局部后验协方差确定单元，包括：
第一局部后验协方差分量子单元，用于确定第一雅克比矩阵与第二卡尔曼增益的乘积，根据乘积以及局部坐标系下位姿信息的先验协方差，获得局部坐标系下位姿信息的后验协方差的第一分量；
第二局部后验协方差分量子单元，用于根据第一协方差以及第二卡尔曼增益，获得局部坐标系下位姿信息的后验协方差的第二分量；
局部后验协方差分量求和子单元，用于根据第一分量和第二分量之和，获得局部坐标系下位姿信息的后验协方差。
进一步地，GNSS预估模块具体用于：构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，将全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值。
进一步地，定位更新模块包括：
全局增益计算子模块，用于根据第二观测方程以及全局坐标系下位姿信息的先验分布，获得全局坐标系下位姿信息的第三卡尔曼增益；
全局增益更新子模块，用于从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第三卡尔曼增益中确定目标子位姿信息的第三卡尔曼增益在运动方向上的分量，作为第四卡尔曼增益；
位姿分布更新子模块，用于根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
全局增益计算子模块包括：
第二观测参数确定单元，用于确定第二观测方程的第二雅克比矩阵以及观测噪声的第二协方差；
全局增益确定单元，用于根据第二协方差、第二雅克比矩阵以及全局坐标系下位姿信息的先验协方差，确定第三卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
位姿分布更新子模块包括：
残差计算单元，用于根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
全局后验均值单元，用于根据第四卡尔曼增益、残差以及运动方向的方向向量的乘积，获得全局坐标系下位姿信息的先验均值的增量；根据增量对全局坐标系下位姿信息的先验均值进行更新，获得当前时刻的全局坐标系下位姿信息的后验均值。
进一步地，位姿分布更新子模块具体用于：根据第二雅克比矩阵、第二协方差以及第四卡尔曼增益，对全局坐标系下的先验协方差进行更新，获得当前时刻的全局坐标系下位姿信息的后验协方差。
进一步地，位姿分布更新子模块包括：
第一全局后验协方差分量子单元，用于确定第二雅克比矩阵、对角矩阵与第四卡尔曼增益的乘积，根据乘积以及全局坐标系下位姿信息的先验协方差，获得全局坐标系下位姿信息的后验协方差的第一分量；
第二全局后验协方差分量子单元，用于根据第二协方差、对角矩阵以及第四卡尔曼增益，获得全局坐标系下位姿信息的后验协方差的第二分量；
全局后验协方差分量求和子单元，用于根据第一分量和第二分量之和，获得全局坐标系下位姿信息的后验协方差；
其中，对角矩阵中非0的元素位于与目标运动状态对应行列，非0的元素为运动方向的单位向量与运动方向的单位向量的转置的乘积。
第三方面，本发明实施例提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现如第一方面所提供的方法的步骤。
第四方面，本发明实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第一方面所提供的方法的步骤。
第五方面，本发明实施例提供一种计算机程序，该计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中，当计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行实现如第一方面所提供的方法的步骤。
本发明实施例提供的融合定位方法、装置、电子设备及存储介质，通过根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，在根据GNSS测量值和预估值对全局坐标系下位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对本申请实施例描述中所需要使用的附图作简单地介绍。
图1示例性示出了本申请实施例的应用场景示意图；
图2示例性示出了本申请实施例提供的融合定位方法的流程示意图；
图3示例性示出了本申请实施例提供的全局坐标系和局部坐标系的示意图；
图4示例性示出了本申请另一个实施例提供的融合定位方法的流程示意图；
图5示例性示出了本申请实施例通过Schmidt-EKF算法对局部坐标系下位姿信息的先验分布在目标坐标轴的分量进行更新的流程示意图；
图6示例性示出了本申请再一个实施例的融合定位方法的流程示意图；
图7示例性示出了本申请再一个实施例提供的融合定位方法的流程示意图；
图8示例性示出了本申请实施例的融合定位方法与相关技术的定位轨迹的对比示意图；
图9示例性示出了本申请实施例的融合定位方法与相关技术在横向定位精度上的对比示意图；
图10为本申请实施例提供的一种融合定位装置的结构示意图；
图11为本申请实施例提供的一种电子设备的结构示意图。
具体实施方式
下面详细描述本申请的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能解释为对本发明的限制。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或无线耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的全部或任一单元和全部组合。
为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。
首先对本申请涉及的几个名词进行介绍和解释：
定位：确定物体在空间中位置和姿态的方法，如车辆的经纬度坐标，车头朝向等。
融合定位：联合使用多种传感器采集的信息，实现定位功能的方法。
全局坐标系（Global Coordinate）：全局坐标系是三维空间物体所在的坐标系，模型的顶点坐标就是基于这个坐标系来表达的。在本申请实施例中，全局坐标系可以是WGS（World Geodetic System）84坐标系、GCJ-02坐标系（中国国家测绘局制订的地理信息系统的坐标系统）、BD-09坐标系（百度坐标系）等等。
局部坐标系(Local Coordinate)：该坐标系以物体的中心为坐标原点，并且该坐标系与物体的相对位置至始至终是不变。
坐标转换：坐标转换是空间实体的位置描述，是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现。空间中三维坐标变换一般由三种方式实现，第一种是旋转矩阵和旋转向量；第二种是欧拉角；第三种是四元数。
旋转矩阵（Rotation matrix）：在乘以一个向量的时候有改变向量的方向但不改变大小的效果并保持了手性的矩阵。
雅可比矩阵（jacobi matrix）：在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。
GNSS：(Global Navigation Satellite System)全球导航卫星系统的简称，为星基无线电导航系统。以人造地球卫星作为导航台，为全球海陆空的各类军民载体提供位置、速度和时间信息。常见的系统有GPS（Global Positioning System，全球定位系统）、GLONASS格洛纳斯定位系统、BDS（BeiDou Navigation Satellite System，北斗卫星导航系统）、GALILEO伽利略卫星导航系统等。
Schmidt-EKF：卡尔曼滤波器的一个变种。由Stanley F. Schmidt提出，在进行状态更新时，可以锁住某些状态不被更新，但是仍然更新这些状态与其他状态的协方差。
为了方便介绍本发明实施例所提供的一种融合定位方法，首先介绍本发明实施例所提供的定位方法适用的融合定位系统。该多源融合定位系统可以包括处理设备和多种定位设备，例如，多种定位设备可以包括UWB（Ultra Wideband，超宽带）定位设备、MIMO(multiple input multiple output，多进多出)定位设备、IMU(Inertialmeasurementunit，惯性测量单元)传感器、Wi-Fi定位设备、蓝牙定位设备、GPS定位设备等。处理设备可以与多种定位设备通信连接。其中UWB定位设备可以包括UWB基站及UWB接收机，MIMO定位设备可以包括MIMO天线及MIMO接收机，IMU传感器可以安装在目标对象上。
本发明实施例所提供的一种融合定位方法可以应用于融合定位系统中的处理设备，处理设备可以为处理器、电脑、服务器、手机等电子设备。
处理设备通过UWB定位设备、MIMO定位设备及IMU传感器可以获得目标对象的实际位姿观测信息，目标对象的实际位姿观测信息可以包括目标对象的位置坐标信息、姿态信息及运动状态信息。
其中，处理设备可以获取UWB基站的位置，进而可以基于UWB基站的位置，根据TDOA(Time Difference of Arrival，到达时间差)、TOF(Time of flight，飞行时间测距法)等定位算法确定目标对象的位置坐标信息；处理设备还可以获取MIMO天线的位置，进而可以基于MIMO天线的位置，根据DOA(Direction Of Arrival，波达方向)算法确定目标对象的姿态信息。当目标对象为无人机等飞行器时，目标对象的姿态信息可以包括目标对象的高度角、方向角等。其中，UWB基站的位置及MIMO天线的位置可以通过测量获得。
处理设备还可以获取IMU传感器的坐标系标定信息，根据IMU传感器的传感信号确定目标对象的运动状态信息，其中运动状态信息可以包括目标对象的速度、加速度及角速度，IMU传感器的坐标系标定信息可以用于将IMU坐标系转换为世界坐标系（全局坐标系），坐标系标定信息可以借助基于九轴惯性信息的AHRS(Attitude and heading referencesystem，航姿参考系统)工具获得。这样，处理设备获取的目标对象的实际位姿观测信息可以包括目标对象的位置坐标信息、姿态信息及运动状态信息。
处理设备除了可以获得上述实际位姿观测信息，还可以获得GNSS观测信息（也称之为GNSS测量值），从而根据实际位姿观测信息和GNSS测量值进行融合，从而获得融合后的位姿信息。
对自动驾驶而言，对车辆的横向（沿着垂直于道路的方向或者车身的横向方向）定位精度要求更高，要求在分米甚至厘米级别，而对于纵向（沿着道路行进的方向或者车辆前进方向）精度要求不高，可以容忍米级的误差。如果横向上定位精度较低，会导致自动驾驶车辆跑出车道，严重时引发交通事故。
相关技术通常直接将低精度的GNSS信息进行融合，这就导致将GNSS偏差直接带入了位姿信息中，降低了定位精度。而采用RTK技术的方法，存在设备和维护成本高的问题。
本申请提供的融合定位方法、装置、电子设备和计算机可读存储介质，旨在解决现有技术的如上技术问题。
下面以具体地实施例对本申请的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本申请的实施例进行描述。
在自动驾驶技术中，融合定位的关键因素在于车辆自身的车载传感器采集的状态的精确度和种类，车载传感器采集的状态的精确度越高且种类越多，往往意味这最终的融合定位结果越精确。
一般地，车载传感器可以包括：
操控角传感器，能够检测方向盘与转向器之间的输出轴之间的转矩以及方向盘与水平中心线的夹角；
制动踏板行程传感器，能够检测制动踏板的位移，为确定车速在单位时间的变化值奠定基础；
制动压力传感器，能够检测制动的压力，为确定车速在单位时间的变化值奠定基础；
ECB（Electrically Controlled Brake，电控制动）传动装置；
ECB ECU（Electronic Control Unit，电子控制单元），接收车轮速度传输的信息或信号，并将其进行测量比较、放大分析和判别处理，经过精确算之后，最终得出车轮滑移率和车轮角加速度或制动角加速度的实际数值，再将其指令信号输出，送至制动压力调节器，使其执行制动压力调节。
VGRS（Variable Gear Ratio Steering，可变传动比转向系统）传动装置；
方向盘操控扭矩传感器，能够采集方向盘转动的扭矩，为确定车辆的转向角奠定基础；
电动动力助力传感装置，依靠电机提供辅助扭矩的传感装置；
发动机ECU，用于控制发动机的点火和喷油，可以理解为发动机控制单元，为确定车速在单位时间的变化量奠定基础；
电动动力转向ECU，用于通过电机控制辅助扭矩大小；
偏航率传感器，记录车辆绕垂直轴线的运动量；
加速度传感器，用于采集车辆的加速度，一般分为三轴加速器、六轴加速器（3轴加速度传感器+3轴陀螺仪）等类型；
车轮速度传感器，用于采集车轮转速，车轮转速可以进一步确定车辆的车速。
请参阅图1，其示例性地示出了本申请实施例的应用场景示意图，如图，车辆10通过车载传感器实时采集车辆的实际位姿观测信息，实际位姿观测信息包括运动状态信息，运动状态信息中包括位置、姿态、速度、加速度偏差、角速度偏差中的至少一种；还通过GNSS接收模块实时接收GNSS卫星20发送的GNSS信息；
车辆10将每时刻的车辆的实际位姿观测信息和GNSS信息发送至服务器30，服务器30根据上一时刻至当前时刻车辆的位姿信息变化量与上一时刻的全局坐标系下的位姿信息的后验分布，获得当前时刻的全局坐标系下的位姿信息的先验分布，再通过本申请实施例的融合定位方法根据当前时刻的全局坐标系下的位姿信息的先验分布和当前时刻的GNSS信息确定当前时刻的全局坐标系下位姿信息的后验分布；服务器30将当前时刻的全局坐标系下位姿信息的后验分布发送至车辆的车载终端上进行显示，相关人员可通过车载终端查看车辆的实时定位，同时车辆还可以根据当前时刻的全局坐标系下的位姿信息的后验分布作为自动驾驶的依据，实现自动驾驶。
下面对本申请实施例的执行主体和实施环境进行介绍：
需要说明的一点是，上述服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content DeliveryNetwork，内容分发网络)、以及大数据和人工智能平台等基础云计算服务的云服务器，本申请实施例对此不作限定。
本申请实施例的服务器的执行方法可以以云计算（cloud computing）的形式完成，云计算是一种计算模式，它将计算任务分布在大量计算机构成的资源池上，使各种应用系统能够根据需要获取计算力、存储空间和信息服务。提供资源的网络被称为“云”。“云”中的资源在使用者看来是可以无限扩展的，并且可以随时获取，按需使用，随时扩展，按使用付费。
作为云计算的基础能力提供商，会建立云计算资源池(简称云平台，一般称为IaaS(Infrastructure as a Service，基础设施即服务)平台，在资源池中部署多种类型的虚拟资源，供外部客户选择使用。云计算资源池中主要包括：计算设备(为虚拟化机器，包含操作系统)、存储设备、网络设备。
按照逻辑功能划分,在IaaS（Infrastructure as a Service，基础设施即服务）层上可以部署PaaS(Platform as a Service,平台即服务)层，PaaS层之上再部署SaaS(Software as a Service,软件即服务)层，也可以直接将SaaS部署在IaaS上。PaaS为软件运行的平台，如数据库、web容器等。SaaS为各式各样的业务软件，如web门户网站、短信群发器等。一般来说，SaaS和PaaS相对于IaaS是上层。
本申请实施例提供的融合定位方法，各步骤的执行主体还可以是车辆上的车载终端，该车载终端与车辆上的各车载传感器连接，当车载传感器采集到车辆的运动状态和GNSS信息后，不需要上传至服务器进行融合定位处理，而由本地的车载终端对运动状态以及GNSS信息基于本申请实施例的融合定位方法，获得车辆的位置信息。
可选的，本申请实施例除了上述自动驾驶场景，还可以应用任何具有GNSS接收模块（采集GNSS信息）的电子设备的使用场景。比如手机、无人机、智能手表等电子设备，比如用户携带手机或者智能手表进行骑行的场景，无人机在室外进行编队飞行表演的场景。本申请实施例可以应用卡尔曼滤波或扩展卡尔曼滤波进行融合定位的各种场合。
请参阅图2，其示例性地示出了本申请实施例的融合定位方法的流程示意图，如图2所示，包括：
S101、获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，位姿信息包括运动对象的运动方向。
本申请实施例的位姿信息包括目标对象的位置坐标信息、姿态信息及运动状态信息，其中姿态信息中包括运动对象的运动方向，在自动驾驶领域，运动方向也称之为纵向。运动状态包括但不限于位置、速度、加速度偏差、角速度偏差等等。
位姿信息的先验分布可以理解为位姿信息的概率分布，在获得当前时刻的先验信息后，通过本申请实施例的后续流程处理，即可将全局坐标系下位姿信息的先验分布和GNSS测量值融合为当前时刻最终确定的位姿信息，也即位姿信息的后验分布。
可以理解的是，运动物体的位姿信息是连续变化而不是离散变化的，因此运动物体每一时刻的全局坐标系下的位姿信息的先验分布，都可以根据前一时刻的全局坐标系下的位姿信息的后验分布，结合前一时刻至当前时刻的时间段内的位姿信息变化量获得。
S102、根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值。
具体的，本申请实施例在每一时刻建立GNSS信息对位姿信息的观测方程，该观测方程可以以位姿信息的先验分布为输入，输出GNSS预估值。
S103、根据GNSS测量值和GNSS预估值，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下的位姿信息的后验分布。
本申请实施例根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，进一步根据获得的GNSS测量值和预估值，仅对位姿信息的先验分布在运动方向上的分量进行更新，一方面消除了GNSS误差对于融合定位结果横向方向的影响，另一方面又保留了GNSS信息对于纵向方向的约束，有效降低了融合定位的横向误差。
请参阅图3，其示例性地示出了本申请实施例的全局坐标系和局部坐标系的示意图，如图所示，Gx和Gy分别表示全局坐标系的x轴和y轴，Lx和Ly分别表示局部坐标系的x轴和y轴。
在表达一个对象的运动方向或者位置时，必然需要有坐标系进行参考，在自动驾驶场景，车辆上的传感器采集的状态信息都是以全局坐标系进行表示的，这样在描述车辆的运动方向时需要由Gx和Gy两个坐标轴上的分量进行表示，也即Gx和Gy上的分量在描述车辆的运动状态时是耦合在一起的，进一步在获得GNSS测量值（GNSS measurement）后，如何确定GNSS测量值对Gx和Gy上的分量具体更新的数值是比较困难的，并且准确率也较低。
本申请出于将Gx和Gy解耦的目的，以运动对象的运动方向（纵向，longitudinaldirection）为一个Lx轴建立局部坐标系，将车辆的运动状态在局部坐标系进行表达时，由于Ly轴的分量为0，相当于只需要单独考虑Lx的分量，这样在获得GNSS测量值后，再利用GNSS测量值对局部坐标系下的运动状态信息进行更新就变得比较容易，同时准确率也大大增加。
请参阅图4，其示例性地示出了本申请另一个实施例的融合定位方法的流程示意图，如图所示，包括：
S201、获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布；
S202、将全局坐标系下的位姿信息的先验分布转换至局部坐标系，获得局部坐标系下的位姿信息的先验分布；局部坐标系的目标坐标轴指向运动对象的运动方向；
S203、根据局部坐标系下的位姿信息的先验分布以及预先构建的观测方程，获得当前时刻的GNSS预估值；
S204、根据GNSS测量值和GNSS预估值，对局部坐标系下的位姿信息的先验分布在目标坐标轴的分量进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
S205、将局部坐标系下位姿信息的后验分布转换至全局坐标系，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
本申请实施例的融合定位方法通过将全局坐标系下的位姿信息的先验分布转化至坐标轴指向当前运动方向的局部坐标系，实现了横、纵状态的解耦，进一步利用局部坐标系下的位姿信息的先验分布获得GNSS预估值提升了运算的准确性、降低了复杂度，在根据GNSS测量值和预估值对局部坐标系下的位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
在上述各实施例的基础上，作为一种可选实施例，根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，包括步骤S102a和步骤S102b，具体的：
S102a、将全局坐标系下位姿信息的先验分布转换至局部坐标系，获得局部坐标系下位姿信息的先验分布；局部坐标系的目标坐标轴指向运动对象的运动方向。
由图3所示的实施例可知，为了准确地确定GNSS信息对位姿信息的影响，在每一时刻根据运动对象的运动方向建立局部坐标系，局部坐标系的原点与全局坐标系的原点重合，其中一个坐标轴的方向与运动方向相同。在实际应用时，若使用了高精度地图数据，运动方向可以通过高精度地图中道路的方向确定，若没有使用高精度地图，运动方向即车辆的行驶方向。
本申请实施例可以采用旋转矩阵的方法将位姿信息的先验分布由全局坐标系转 换至局部坐标系。具体地在确定局部坐标系各坐标轴的方向后，可以获得从全局坐标系到 局部坐标系的旋转矩阵
Figure 22235DEST_PATH_IMAGE001
。需要说明的是，该旋转矩阵的确定操作是实时进行的，其可以 是电子设备独立完成，也可以是借助服务器完成，比如电子设备可以实时向服务器上传坐 标轴方向信息和局部坐标系，服务器根据上传信息确定旋转矩阵。通常来说，处于不同地理 位置或者具有不同放置方式的电子设备，该转换矩阵一般不同。
S102b、构建表征当前时刻的GNSS信息与局部坐标系下位姿信息间对应关系的第一观测方程，将局部坐标系下的位姿信息的先验分布输入至第一观测方程，获得第一观测方程输出的当前时刻的GNSS预估值。
在每一时刻建立GNSS信息对局部坐标系下位姿信息的观测方程，该观测方程可以以局部坐标系下的局部坐标系下位姿信息的先验分布为输入，输出GNSS预估值。
观测方程可以表示为：
Figure 782381DEST_PATH_IMAGE002
其中，
Figure 33103DEST_PATH_IMAGE003
为GNSS预估值；
Figure 372948DEST_PATH_IMAGE004
为观测方程；
Figure 25515DEST_PATH_IMAGE005
为局部坐标系下的位姿信息的先 验分布；n为观测噪声，服从零均值的高斯分布。
本申请实施例对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括：
S301、根据第一观测方程以及局部坐标系下位姿信息的先验分布，获得局部坐标系下位姿信息的第一卡尔曼增益。
本申请实施例的位姿信息的第一卡尔曼增益用于表征GNSS测量值和GNSS预估值间的差异，对位姿信息的影响程度，
具体的，本申请实施例的先验分布包括先验均值和先验协方差，后验分布包括后验均值和后验协方差。
姿态信息的先验均值包括姿态信息中各子位姿信息的先验均值，子位姿信息的先验均值是子位姿信息的概率分布的期望。同理，姿态信息的后验均值包括姿态信息中各子位姿信息的后验均值，子位姿信息的验均值可以认为是子位姿信息融合了GNSS信息后的概率分布的期望。
记全局坐标系下的子位姿信息：位置为
Figure 159824DEST_PATH_IMAGE006
，除位置以外的其他子位姿信息为
Figure 897842DEST_PATH_IMAGE007
，则全局坐标系下的姿态信息的先验均值表示为：
Figure 41379DEST_PATH_IMAGE008
相应的，局部坐标系下的姿态信息的先验均值可以通过以下公式表示：
Figure 548452DEST_PATH_IMAGE009
其中，
Figure 384821DEST_PATH_IMAGE010
表示局部坐标系下的位置，
Figure 360868DEST_PATH_IMAGE011
表示局部坐标系下除位置以外的子位 姿信息，
Figure 822942DEST_PATH_IMAGE012
和
Figure 669675DEST_PATH_IMAGE013
构成了局部坐标系下姿态信息的先验均值
Figure 739262DEST_PATH_IMAGE014
，I为单位矩阵，
Figure 932432DEST_PATH_IMAGE015
和单 位矩阵I构成了转换矩阵
Figure 745668DEST_PATH_IMAGE016
。
在统计学中，均值（期望）描述的是样本集合的中间点（平均值），但是均值所提供的信息是有限的，所以还可以用协方差来进一步描述不同样本的联系，协方差就是这样一种用来度量两个随机变量关系的统计量，来度量各个维度偏离其均值的程度，协方差的结果为正值，说明两者是正相关，如果结果为负值，就说明两者是负相关。
姿态信息的先验协方差是指姿态信息中的各子位姿信息间的先验协方差，也就是说，若姿态信息包括n个子位姿信息，那么姿态信息的先验协方差为n*n的矩阵，该矩阵中第i行j列的元素表示第i个子位姿信息与第j个子位姿信息间的协方差。同理，姿态信息的后验协方差是指姿态信息中的各子位姿信息间的先验协方差。
局部坐标系下姿态信息的协方差可以通过以下公式表示：
Figure 446907DEST_PATH_IMAGE017
其中，
Figure 687396DEST_PATH_IMAGE018
表示局部坐标系下姿态信息的协方差，
Figure 621723DEST_PATH_IMAGE019
表示全局坐标系下姿态信 息的协方差，T表示转置运算。
步骤S301可以包括：
S301a、根据观测方程的第一雅克比矩阵以及观测噪声的第一协方差。
可选地，本申请实施例的第一观测方程为：
Figure 973070DEST_PATH_IMAGE020
其中，
Figure 997657DEST_PATH_IMAGE021
表示GNSS预估值，
Figure 923894DEST_PATH_IMAGE022
表示全局坐标系（G）至局部坐标系（L）的旋转 矩阵，L pv 表示局部坐标系下的位置（一种子位姿信息），
Figure 830670DEST_PATH_IMAGE023
为全局坐标系下的姿态信息，
Figure 985708DEST_PATH_IMAGE024
为GNSS天线在车辆坐标系下的位置，该值可以通过标定或者直接在车上测量获取，n 表示观测噪声。在此基础上，第一雅克比矩阵表示为：
Figure 114070DEST_PATH_IMAGE025
其中，符号
Figure 961940DEST_PATH_IMAGE026
表示求反对称矩阵。
S301b、根据第一协方差、第一雅克比矩阵以及局部坐标系下位姿信息的先验协方差，确定第一卡尔曼增益。
第一卡尔曼增益可用以下公式进行表示：
Figure 356012DEST_PATH_IMAGE027
其中，V表示观测噪声的第一协方差。
S302、从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益。
可以理解的是第一卡尔曼增益包含了各子位姿信息的第一卡尔曼增益，例如，以姿态信息中包括5种子位姿信息（姿态信息R、速度v、位置p、姿态角偏差bg、加速度偏差ag）为例，第一卡尔曼增益可以分块为：
Figure 49162DEST_PATH_IMAGE028
其中，
Figure 297610DEST_PATH_IMAGE029
表示姿态信息的第一卡尔曼增益，
Figure 50802DEST_PATH_IMAGE030
表示姿态角偏差的第一卡尔曼 增益，
Figure 932170DEST_PATH_IMAGE031
表示速度的第一卡尔曼增益，
Figure 881541DEST_PATH_IMAGE032
表示加速度偏差的第一卡尔曼增益，
Figure 531965DEST_PATH_IMAGE033
表 示位置的第一卡尔曼增益。
进一步的，每一种子位姿信息的第一卡尔曼增益又进一步包含了该子位姿信息在局部坐标系的不同坐标轴的方向上的卡尔曼增益分量，本申请可以从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益。
例如，若第一卡尔曼增益表示为：
Figure 721638DEST_PATH_IMAGE034
Figure 73990DEST_PATH_IMAGE035
为对应目标子位姿信息的X、Y、Z三个方向的卡尔曼增益，
Figure 312205DEST_PATH_IMAGE036
为对应其他子位姿信息的卡尔曼增益。
那么第二卡尔曼增益
Figure 82715DEST_PATH_IMAGE037
为：
Figure 692557DEST_PATH_IMAGE038
由此可知，第二卡尔曼增益中只保留了目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量。
S303、根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
S304、将当前时刻的局部坐标系下位姿信息的后验分布转换至全局坐标系，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
本申请实施例的融合定位方法通过将全局坐标系下的位姿信息的先验分布转化至坐标轴指向当前运动方向的局部坐标系，实现了横、纵状态的解耦，进一步利用局部坐标系下的位姿信息的先验分布获得GNSS预估值提升了运算的准确性、降低了复杂度，在根据GNSS测量值和预估值对局部坐标系下的位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
在上述各实施例的基础上，作为一种可选实施例，步骤S303包括：
S303a、根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差r；
残差可以认为是测量值扣除其中包含的有用信息后剩余的部分。可以有余(残差为正)，也可以不足(残差为负)。残差是评价测量值的质量好坏的比较直接和客观的标准。测量值质量较好时，残差一定是在零值附近，且抖动较小，反之，测量值则可能较差或者有些偏差或误差项没有被校正完全。
S303b、根据第二卡尔曼增益以及残差的乘积，获得局部坐标系下位姿信息的先验均值的增量，根据增量对局部坐标系下位姿信息的先验均值进行更新，获得当前时刻的局部坐标系下位姿信息的后验均值。
局部坐标系下位姿信息的后验均值
Figure 486200DEST_PATH_IMAGE039
可通过以下公式表示：
Figure 324843DEST_PATH_IMAGE040
其中，
Figure 464706DEST_PATH_IMAGE041
为局部坐标系下位姿信息的先验均值；
Figure 730603DEST_PATH_IMAGE042
为第二卡尔曼增益：r为残 差。
在上述各实施例的基础上，作为一种可选实施例，步骤S303还包括：
根据第一雅克比矩阵、第一协方差以及第二卡尔曼增益，对局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差，具体的：
确定雅克比矩阵与新卡尔曼增益的乘积，根据乘积以及局部坐标系下位姿信息的先验协方差，获得局部坐标系下各运动状态的后验协方差的第一分量；
根据观测噪声的协方差以及新卡尔曼增益获得局部坐标系下位姿信息的后验协方差的第二分量；
根据第一分量和第二分量之和，获得局部坐标系下位姿信息的后验协方差。
局部坐标系下位姿信息的后验协方差可通过以下公式进行表示：
Figure 11542DEST_PATH_IMAGE043
其中，
Figure 903144DEST_PATH_IMAGE044
表示局部坐标系下位姿信息的后验协方差，
Figure 382667DEST_PATH_IMAGE045
表示单位矩阵，
Figure 819464DEST_PATH_IMAGE046
表示 第二卡尔曼增益，
Figure 384438DEST_PATH_IMAGE047
表示第一雅克比矩阵，
Figure 17413DEST_PATH_IMAGE048
表示局部坐标系下位姿信息的先验协方差，V表示观测噪声的协方差，T表示转置运算。
请参阅图5，其示例性地示出了本申请实施例通过Schmidt-EKF算法对局部坐标系下位姿信息的先验分布在目标坐标轴的分量进行更新的流程示意图，如图所示，包括：
构建第一观测方程；
确定第一观测方程的第一雅克比矩阵以及观测噪声的第一协方差；
根据第一协方差、第一雅克比矩阵以及局部坐标系下位姿信息的先验协方差，确定第一卡尔曼增益；
从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益；
根据第二卡尔曼增益对局部坐标系下位姿信息的先验均值进行更新，获得当前时刻的局部坐标系下位姿信息的后验均值；
根据第二卡尔曼增益对局部坐标系下位姿信息的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差。
本申请实施例在获得局部坐标系下位姿信息的后验均值和后验协方差后，通过坐标系转换，即可获得全局坐标系下位姿信息的后验均值和后验协方差。
具体的，全局坐标系下位姿信息的后验均值可以表示为：
Figure 351443DEST_PATH_IMAGE049
其中，
Figure 224721DEST_PATH_IMAGE050
表示全局坐标系下位姿信息的后验均值，
Figure 526258DEST_PATH_IMAGE051
表示局部坐标系下位姿信 息的后验均值。
全局坐标系下位姿信息的后验协方差可以表示为：
Figure 510395DEST_PATH_IMAGE052
其中，
Figure 902193DEST_PATH_IMAGE053
表示全局坐标系下位姿信息的后验协方差，
Figure 930061DEST_PATH_IMAGE054
表示局部坐标系下位姿 信息的后验协方差。
请参阅图6，其示例性地示出了本申请再一个实施例的融合定位方法的流程示意图，如图6所示，包括：
获取运动对象在t-1时刻的全局坐标系G下的位姿信息的后验均值
Figure 735205DEST_PATH_IMAGE055
和后验 协方差值
Figure 178825DEST_PATH_IMAGE056
，获取运动对象由t-1时刻至t时刻的时间差的位姿信息的增量Δx。
将
Figure 956288DEST_PATH_IMAGE057
和增量Δx的均值相加，获得运动对象在t时刻的全局坐标系下的位姿信 息的先验均值
Figure 171369DEST_PATH_IMAGE058
，将
Figure 119602DEST_PATH_IMAGE059
与增量Δx的协方差相加，获得运动对象在t时刻的全局坐标系 下的位姿信息的先验协方差值
Figure 898071DEST_PATH_IMAGE060
；
根据t时刻的运动方向建立的局部坐标系L，局部坐标系L的原点与全局坐标系G的 原点重合，并且局部坐标系L的x轴与纵向方向同向；将位姿信息的先验均值
Figure 530041DEST_PATH_IMAGE061
和先验协方 差值
Figure 911430DEST_PATH_IMAGE062
分别转换至局部坐标系L，获得位姿信息在t时刻的局部坐标系L下位姿信息的先验 均值
Figure 160009DEST_PATH_IMAGE063
和先验协方差值
Figure 227322DEST_PATH_IMAGE064
；
建立t时刻的观测方程h，将位姿信息的先验均值
Figure 759803DEST_PATH_IMAGE065
输入至观测方程h，获得GNSS预 估值
Figure 316687DEST_PATH_IMAGE066
；
获取t时刻的GNSS测量值z，结合GNSS预估值
Figure 521403DEST_PATH_IMAGE066
获得GNSS的残差
Figure 641675DEST_PATH_IMAGE067
，根据观测方程h确定雅克比矩阵H和观测噪声的协方差V，结合局部坐标系 下的位姿信息的先验协方差
Figure 44974DEST_PATH_IMAGE068
，确定位姿信息的卡尔曼增益K；
根据卡尔曼增益K，构建新卡尔曼增益
Figure 507179DEST_PATH_IMAGE069
，新卡尔曼增益
Figure 979618DEST_PATH_IMAGE069
中包括局部坐标系L下 的目标子位姿信息的x轴方向的卡尔曼增益分量；
根据新卡尔曼增益
Figure 185471DEST_PATH_IMAGE070
和残差r对局部坐标系L下的位姿信息的先验均值
Figure 177698DEST_PATH_IMAGE071
进行更 新，根据新卡尔曼增益
Figure 76384DEST_PATH_IMAGE072
、雅克比矩阵H和观测方程的观测噪声的协方差V对先验协方差值
Figure 36119DEST_PATH_IMAGE073
进行更新，获得局部坐标系L下的位姿信息的后验均值
Figure 780084DEST_PATH_IMAGE074
和后验协方差值
Figure 892396DEST_PATH_IMAGE075
；
将局部坐标系L下位姿信息的后验均值
Figure 696404DEST_PATH_IMAGE076
和后验协方差值
Figure 143435DEST_PATH_IMAGE077
转化至全局坐标系 G，获得运动对象在t时刻的全局坐标系G下位姿信息的后验验均值
Figure 894353DEST_PATH_IMAGE078
和后验协方差值
Figure 861172DEST_PATH_IMAGE079
。
本申请实施例还提供一种不需要构建局部坐标系的、仅根据GNSS测量值在运动方向的分量进行融合定位的方法，请参见图7，为本申请再一个实施例提供的融合定位方法的流程示意图，包括：
S401、获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布；
S402、构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，将全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值；
S403、根据第二观测方程以及全局坐标系下位姿信息的先验分布，获得全局坐标系下位姿信息的第三卡尔曼增益；
S404、从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第三卡尔曼增益中确定目标子位姿信息的第三卡尔曼增益在运动方向上的分量，作为第四卡尔曼增益；
S405、根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布。
本申请实施例不需要构建局部坐标系，利用全局坐标系下的观测方差获得GNSS预估值，并且在获得位姿信息的卡尔曼增益后，从卡尔曼增益中获得目标子位姿信息的卡尔曼增益在运动方向上的分量，以对全局坐标系下位姿信息的先验分布进行更新，进一步提升了运算的速度。
在上述实施例的基础上，作为一种可选实施例，根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，包括：
构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程；将全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值。
区别与上述第一种方法需要先构建局部坐标系，然后构建局部坐标系下的观测方程的方法，本申请实施例直接构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，从而可以根据全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值。需要注意的是，第二观测方程输出的GNSS预估值与第一观测方差输出的GNSS预估值的精度几乎一致。
在上述实施例的基础上，作为一种可选实施例，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括上述步骤S403~S405。
步骤S403进一步包括：
确定第二观测方程的第二雅克比矩阵以及观测噪声的第二协方差。
可选地，本申请实施例的第二观测方程可以为
Figure 616507DEST_PATH_IMAGE080
其中，
Figure 35987DEST_PATH_IMAGE081
表示GNSS预估值，G pv 表示全局坐标系下的位置，
Figure 590597DEST_PATH_IMAGE082
为全局坐标系下 的姿态信息，
Figure 661190DEST_PATH_IMAGE083
为GNSS天线在车辆坐标系下的位置，n表示观测噪声。在此基础上，第二 雅克比矩阵
Figure 72580DEST_PATH_IMAGE084
为：
Figure 244935DEST_PATH_IMAGE085
根据第二协方差、第二雅克比矩阵以及全局坐标系下位姿信息的先验协方差，确定第三卡尔曼增益。
第三卡尔曼增益
Figure 134393DEST_PATH_IMAGE086
可用以下公式进行表示：
Figure 325072DEST_PATH_IMAGE087
其中，
Figure 907363DEST_PATH_IMAGE088
表示全局坐标系下位姿信息的先验协方差，J表示第二观测方程中观测 噪声的协方差。
本申请实施例中从第三卡尔曼增益中确定第四卡尔曼增益的方法与上述实施例的步骤S302类似，在此不再赘述。
在上述各实施例的基础上，作为一种可选实施例，根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布包括：
根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
根据第四卡尔曼增益、残差以及运动方向的方向向量的乘积，获得全局坐标系下位姿信息的先验均值的增量；
根据增量对全局坐标系下位姿信息的先验均值进行更新，获得当前时刻的全局坐标系下位姿信息的后验均值。
当前时刻的全局坐标系下位姿信息的后验均值可通过以下公式表示：
Figure 301436DEST_PATH_IMAGE089
其中，
Figure 447115DEST_PATH_IMAGE090
为全局坐标系下位姿信息的后验均值，
Figure 977453DEST_PATH_IMAGE091
为全局坐标系下位姿信息的 先验均值；V为运动方向的方向向量；
Figure 465067DEST_PATH_IMAGE092
为第四卡尔曼增益：r为残差。
在上述各实施例的基础上，作为一种可选实施例，根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布，还包括：
根据第二雅克比矩阵、第二协方差以及第四卡尔曼增益，对全局坐标系下的先验协方差进行更新，获得当前时刻的全局坐标系下位姿信息的后验协方差，具体地：
确定第二雅克比矩阵、对角矩阵与第四卡尔曼增益的乘积，根据乘积以及全局坐标系下位姿信息的先验协方差，获得全局坐标系下位姿信息的后验协方差的第一分量；
根据第二协方差、对角矩阵以及第四卡尔曼增益，获得全局坐标系下位姿信息的后验协方差的第二分量；
根据第一分量和第二分量之和，获得全局坐标系下位姿信息的后验协方差；
其中，对角矩阵中非0的元素位于与目标运动状态对应行列，非0的元素为运动方向的单位向量与运动方向的单位向量的转置的乘积。
全局坐标系下位姿信息的后验协方差可通过以下公式进行表示：
Figure 595702DEST_PATH_IMAGE093
其中，Ω为对角矩阵，
Figure 92543DEST_PATH_IMAGE094
为全局坐标系下位姿信息的后验协方差，
Figure 742967DEST_PATH_IMAGE095
为全局坐 标系下位姿信息的先验协方差,
Figure 401482DEST_PATH_IMAGE096
为第二雅克比矩阵。
请参阅图8，其示例性地示出了本申请实施例的融合定位方法与相关技术的定位轨迹的对比示意图，图中分别示出了GNSS测量值、车辆的实际位置（ground truth）、本申请实施例的融合定位方法（即简洁）以及直接融合GNSS，共4种方法获得的定位轨迹。需要注意的是，由于本申请实施例提供的需要进行坐标系转换和不需要进行坐标系转换共两种融合定位方法的精度几乎一致，因此图8中只示出了一条表示本申请实施例的定位轨迹。其中，GNSS测量值与车辆的实际位置的定位轨迹距离最远，也就意味这定位精度最低，而采用直接融合GNSS的方式形成的定位轨迹仍然与实际位置存在肉眼可见的差距，而根据本申请实施例获得的定位轨迹几乎与实际轨迹重合。经验证，本申请实施例的融合定位方法能够在横向精度上达到厘米级别。
请参阅图9，其示例性地示出了本申请实施例的融合定位方法与相关技术在横向定位精度上的对比示意图，需要注意的是，由于本申请实施例提供的需要进行坐标系转换和不需要进行坐标系转换，共两种融合定位方法的精度几乎一致，因此图9中只示出了一条表示本申请实施例的横向定位精度的曲线。如图所示，GNSS系统直接测量的GNSS信息，即GNSS测量值在横向上的精度是最低的，横向精度的误差在2m左右；采用将GNSS测量值与位姿信息直接融合的方式在精度上要高于GNSS测量值，横向精度的误差在1m左右，而采用本发明的仅利用GNSS测量值对位姿信息在纵向分量进行更新的精度要明显高于上述两种相关技术，横向精度的误差接近0。
本申请实施例提供了一种融合定位装置，如图10所示，该装置可以包括：先验数据获取模块101、GNSS预估模块102以及定位更新模块103，具体地：
先验数据获取模块101，用于获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，位姿信息包括运动对象的运动方向；
GNSS预估模块102，用于根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值；
定位更新模块103，用于根据GNSS测量值和GNSS预估值，对位姿信息的先验分布在运动方向上的分量进行更新，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，GNSS预估模块包括：
先验分布转换子模块，用于将全局坐标系下位姿信息的先验分布转换至局部坐标系，获得局部坐标系下位姿信息的先验分布；局部坐标系的目标坐标轴指向运动对象的运动方向；
第一观测方程构建子模块，用于构建表征当前时刻的GNSS信息与局部坐标系下位姿信息间对应关系的第一观测方程，将局部坐标系下的位姿信息的先验分布输入至第一观测方程，获得第一观测方程输出的当前时刻的GNSS预估值。
进一步地，定位更新模块包括：
局部增益计算子模块，用于根据第一观测方程以及局部坐标系下位姿信息的先验分布，获得局部坐标系下位姿信息的第一卡尔曼增益；
局部增益更新子模块，用于从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第一卡尔曼增益中确定目标子位姿信息的第一卡尔曼增益在目标坐标轴的分量，作为第二卡尔曼增益；
局部后验子模块，用于根据第二卡尔曼增益对局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
全局后验子模块，用于将当前时刻的局部坐标系下位姿信息的后验分布转换至全局坐标系，获得运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
局部增益计算子模块包括：
第一观测参数确定单元，用于确定第一观测方程的第一雅克比矩阵以及观测噪声的第一协方差；
局部增益确定单元，用于根据第一协方差、第一雅克比矩阵以及局部坐标系下位姿信息的先验协方差，确定第一卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
局部后验子模块包括：
残差确定单元，用于根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
局部后验均值单元，用于根据第二卡尔曼增益以及残差的乘积，获得局部坐标系下位姿信息的先验均值的增量，根据增量对局部坐标系下位姿信息的先验均值进行更新，获得当前时刻的局部坐标系下位姿信息的后验均值。
进一步地，局部后验子模块包括：
局部后验协方差确定单元，用于根据第一雅克比矩阵、第一协方差以及第二卡尔曼增益，对局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差。
进一步地，局部后验协方差确定单元，包括：
第一局部后验协方差分量子单元，用于确定第一雅克比矩阵与第二卡尔曼增益的乘积，根据乘积以及局部坐标系下位姿信息的先验协方差，获得局部坐标系下位姿信息的后验协方差的第一分量；
第二局部后验协方差分量子单元，用于根据第一协方差以及第二卡尔曼增益，获得局部坐标系下位姿信息的后验协方差的第二分量；
局部后验协方差分量求和子单元，用于根据第一分量和第二分量之和，获得局部坐标系下位姿信息的后验协方差。
进一步地，GNSS预估模块具体用于：构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，将全局坐标系下先验位姿信息输入至第二观测方程，获得第二观测方程输出的当前时刻的GNSS预估值。
进一步地，定位更新模块包括：
全局增益计算子模块，用于根据第二观测方程以及全局坐标系下位姿信息的先验分布，获得全局坐标系下位姿信息的第三卡尔曼增益；
全局增益更新子模块，用于从位姿信息中确定至少一种目标子位姿信息，从位姿信息的第三卡尔曼增益中确定目标子位姿信息的第三卡尔曼增益在运动方向上的分量，作为第四卡尔曼增益；
位姿分布更新子模块，用于根据第四卡尔曼增益对全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布。
进一步地，先验分布包括先验协方差，后验分布包括后验协方差；
全局增益计算子模块包括：
第二观测参数确定单元，用于确定第二观测方程的第二雅克比矩阵以及观测噪声的第二协方差；
全局增益确定单元，用于根据第二协方差、第二雅克比矩阵以及全局坐标系下位姿信息的先验协方差，确定第三卡尔曼增益。
进一步地，先验分布包括先验均值，后验分布包括后验均值；
位姿分布更新子模块包括：
残差计算单元，用于根据运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
全局后验均值单元，用于根据第四卡尔曼增益、残差以及运动方向的方向向量的乘积，获得全局坐标系下位姿信息的先验均值的增量；根据增量对全局坐标系下位姿信息的先验均值进行更新，获得当前时刻的全局坐标系下位姿信息的后验均值。
进一步地，位姿分布更新子模块具体用于：根据第二雅克比矩阵、第二协方差以及第四卡尔曼增益，对全局坐标系下的先验协方差进行更新，获得当前时刻的全局坐标系下位姿信息的后验协方差。
进一步地，位姿分布更新子模块包括：
第一全局后验协方差分量子单元，用于确定第二雅克比矩阵、对角矩阵与第四卡尔曼增益的乘积，根据乘积以及全局坐标系下位姿信息的先验协方差，获得全局坐标系下位姿信息的后验协方差的第一分量；
第二全局后验协方差分量子单元，用于根据第二协方差、对角矩阵以及第四卡尔曼增益，获得全局坐标系下位姿信息的后验协方差的第二分量；
全局后验协方差分量求和子单元，用于根据第一分量和第二分量之和，获得全局坐标系下位姿信息的后验协方差；
其中，对角矩阵中非0的元素位于与目标运动状态对应行列，非0的元素为运动方向的单位向量与运动方向的单位向量的转置的乘积。
本申请实施例中提供了一种电子设备，该电子设备包括：存储器和处理器；至少一个程序，存储于存储器中，用于被处理器执行时，与现有技术相比可实现：通过根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，在根据GNSS测量值和预估值对全局坐标系下位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
在一个可选实施例中提供了一种电子设备，如图11所示，图11所示的电子设备4000包括：处理器4001和存储器4003。其中，处理器4001和存储器4003相连，如通过总线4002相连。可选地，电子设备4000还可以包括收发器4004。需要说明的是，实际应用中收发器4004不限于一个，该电子设备4000的结构并不构成对本申请实施例的限定。
处理器4001可以是CPU（Central Processing Unit，中央处理器），通用处理器，DSP（Digital Signal Processor，数据信号处理器），ASIC（Application SpecificIntegrated Circuit，专用集成电路），FPGA（Field Programmable Gate Array，现场可编程门阵列）或者其他可编程逻辑器件、晶体管逻辑器件、硬件部件或者其任意组合。其可以实现或执行结合本申请公开内容所描述的各种示例性的逻辑方框，模块和电路。处理器4001也可以是实现计算功能的组合，例如包含一个或多个微处理器组合，DSP和微处理器的组合等。
总线4002可包括一通路，在上述组件之间传送信息。总线4002可以是PCI（Peripheral Component Interconnect，外设部件互连标准）总线或EISA（ExtendedIndustry Standard Architecture，扩展工业标准结构）总线等。总线4002可以分为地址总线、数据总线、控制总线等。为便于表示，图11中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
存储器4003可以是ROM（Read Only Memory，只读存储器）或可存储静态信息和指令的其他类型的静态存储设备，RAM（Random Access Memory，随机存取存储器）或者可存储信息和指令的其他类型的动态存储设备，也可以是EEPROM（Electrically ErasableProgrammable Read Only Memory，电可擦可编程只读存储器）、CD-ROM（Compact DiscRead Only Memory，只读光盘）或其他光盘存储、光碟存储（包括压缩光碟、激光碟、光碟、数字通用光碟、蓝光光碟等）、磁盘存储介质或者其他磁存储设备、或者能够用于携带或存储具有指令或数据结构形式的期望的程序代码并能够由计算机存取的任何其他介质，但不限于此。
存储器4003用于存储执行本申请方案的应用程序代码，并由处理器4001来控制执行。处理器4001用于执行存储器4003中存储的应用程序代码，以实现前述方法实施例所示的内容。
本申请实施例提供了一种计算机可读存储介质，该计算机可读存储介质上存储有计算机程序，当其在计算机上运行时，使得计算机可以执行前述方法实施例中相应内容。与现有技术相比，通过根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，在根据GNSS测量值和预估值对全局坐标系下位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
本申请实施例提供了一种计算机程序，该计算机程序包括计算机指令，该计算机指令存储在计算机可读存储介质中，当计算机设备的处理器从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行如前述方法实施例所示的内容。与现有技术相比，通过根据全局坐标系下位姿信息的先验分布，确定运动对象在当前时刻的GNSS预估值，在根据GNSS测量值和预估值对全局坐标系下位姿信息的先验分布在目标坐标轴（指向运动方向）进行更新，即消除了GNSS误差对于融合定位结果横向定位精度的不利影响，同时保留了GNSS信息对于车辆纵向方向的约束，有效降低了融合GNSS定位的横向误差。
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
以上仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (14)

1.一种融合定位方法，其特征在于，包括：
获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，所述位姿信息包括所述运动对象的运动方向；
根据所述全局坐标系下位姿信息的先验分布，确定所述运动对象在当前时刻的GNSS预估值；
根据所述GNSS测量值和GNSS预估值，对所述位姿信息的先验分布在所述运动方向上的分量进行更新，获得所述运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
2.根据权利要求1所述的融合定位方法，其特征在于，所述根据所述全局坐标系下位姿信息的先验分布，确定所述运动对象在当前时刻的GNSS预估值，包括：
将所述全局坐标系下位姿信息的先验分布转换至局部坐标系，获得局部坐标系下位姿信息的先验分布；所述局部坐标系的目标坐标轴指向所述运动对象的运动方向；
构建表征当前时刻的GNSS信息与局部坐标系下位姿信息间对应关系的第一观测方程，将所述局部坐标系下的位姿信息的先验分布输入至所述第一观测方程，获得所述第一观测方程输出的当前时刻的GNSS预估值。
3.根据权利要求2所述的融合定位方法，其特征在于，所述对所述位姿信息的先验分布在所述运动方向上的分量进行更新，获得所述运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括：
根据所述第一观测方程以及所述局部坐标系下位姿信息的先验分布，获得所述局部坐标系下位姿信息的第一卡尔曼增益；
从所述位姿信息中确定至少一种目标子位姿信息，从所述位姿信息的第一卡尔曼增益中确定所述目标子位姿信息的第一卡尔曼增益在所述目标坐标轴的分量，作为第二卡尔曼增益；
根据所述第二卡尔曼增益对所述局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布；
将所述当前时刻的局部坐标系下位姿信息的后验分布转换至全局坐标系，获得所述运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
4.根据权利要求3所述的融合定位方法，其特征在于，所述先验分布包括先验协方差，所述后验分布包括后验协方差；
所述根据所述第一观测方程以及所述局部坐标系下位姿信息的先验分布，获得所述局部坐标系下位姿信息的第一卡尔曼增益，包括：
确定所述第一观测方程的第一雅克比矩阵以及观测噪声的第一协方差；
根据所述第一协方差、所述第一雅克比矩阵以及所述局部坐标系下位姿信息的先验协方差，确定所述第一卡尔曼增益。
5.根据权利要求3所述的融合定位方法，其特征在于，所述先验分布包括先验均值，所述后验分布包括后验均值；
所述根据所述第二卡尔曼增益对所述局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布，包括：
根据所述运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
根据所述第二卡尔曼增益以及所述残差的乘积，获得所述局部坐标系下位姿信息的先验均值的增量，根据所述增量对所述局部坐标系下位姿信息的先验均值进行更新，获得所述当前时刻的局部坐标系下位姿信息的后验均值。
6.根据权利要求4所述的融合定位方法，其特征在于，所述根据所述第二卡尔曼增益对所述局部坐标系下位姿信息的先验分布进行更新，获得当前时刻的局部坐标系下位姿信息的后验分布，包括：
根据所述第一雅克比矩阵、所述第一协方差以及所述第二卡尔曼增益，对所述局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差。
7.根据权利要求6所述的融合定位方法，其特征在于，所述根据所述第一雅克比矩阵、所述第一协方差以及所述第二卡尔曼增益，对所述局部坐标系下的先验协方差进行更新，获得当前时刻的局部坐标系下位姿信息的后验协方差，包括：
确定所述第一雅克比矩阵与所述第二卡尔曼增益的乘积，根据所述乘积以及所述局部坐标系下位姿信息的先验协方差，获得局部坐标系下位姿信息的后验协方差的第一分量；
根据所述第一协方差以及所述第二卡尔曼增益，获得局部坐标系下位姿信息的后验协方差的第二分量；
根据所述第一分量和所述第二分量之和，获得局部坐标系下位姿信息的后验协方差。
8.根据权利要求1所述的融合定位方法，其特征在于，所述根据所述全局坐标系下位姿信息的先验分布，确定所述运动对象在当前时刻的GNSS预估值，包括：
构建表征当前时刻的GNSS信息与全局坐标系下位姿信息间对应关系的第二观测方程，将所述全局坐标系下先验位姿信息输入至所述第二观测方程，获得所述第二观测方程输出的当前时刻的GNSS预估值。
9.根据权利要求8所述的融合定位方法，其特征在于，所述对所述位姿信息的先验分布在所述运动方向上的分量进行更新，获得所述运动对象在当前时刻的全局坐标系下的位姿信息的后验分布，包括：
根据所述第二观测方程以及所述全局坐标系下位姿信息的先验分布，获得所述全局坐标系下位姿信息的第三卡尔曼增益；
从所述位姿信息中确定至少一种目标子位姿信息，从所述位姿信息的第三卡尔曼增益中确定所述目标子位姿信息的第三卡尔曼增益在运动方向上的分量，作为第四卡尔曼增益；
根据所述第四卡尔曼增益对所述全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布。
10.根据权利要求9所述的融合定位方法，其特征在于，所述先验分布包括先验协方差，所述后验分布包括后验协方差；
所述根据所述第二观测方程以及所述全局坐标系下位姿信息的先验分布，获得所述全局坐标系下位姿信息的第三卡尔曼增益，包括：
确定所述第二观测方程的第二雅克比矩阵以及观测噪声的第二协方差；
根据所述第二协方差、所述第二雅克比矩阵以及所述全局坐标系下位姿信息的先验协方差，确定所述第三卡尔曼增益。
11.根据权利要求9所述的融合定位方法，其特征在于，所述先验分布包括先验均值，所述后验分布包括后验均值；
所述根据所述第四卡尔曼增益对所述全局坐标系下位姿信息的先验分布进行更新，获得当前时刻的全局坐标系下位姿信息的后验分布包括：
根据所述运动对象在当前时刻的GNSS测量值和GNSS预估值获得GNSS信息的残差；
根据所述第四卡尔曼增益、所述残差以及所述运动方向的方向向量的乘积，获得所述全局坐标系下位姿信息的先验均值的增量；
根据所述增量对所述全局坐标系下位姿信息的先验均值进行更新，获得所述当前时刻的全局坐标系下位姿信息的后验均值。
12.一种融合定位装置，其特征在于，包括：
先验数据获取模块，用于获取运动对象在当前时刻的GNSS测量值和全局坐标系下位姿信息的先验分布，所述位姿信息包括所述运动对象的运动方向；
GNSS预估模块，用于根据所述全局坐标系下位姿信息的先验分布，确定所述运动对象在当前时刻的GNSS预估值；
定位更新模块，用于根据所述GNSS测量值和GNSS预估值，对所述位姿信息的先验分布在所述运动方向上的分量进行更新，获得所述运动对象在当前时刻的全局坐标系下位姿信息的后验分布。
13.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至11任一项所述融合定位方法的步骤。
14.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至11中任意一项所述融合定位方法的步骤。
CN202110230387.XA 2021-03-02 2021-03-02 融合定位方法、装置、电子设备及存储介质 Active CN112596089B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110230387.XA CN112596089B (zh) 2021-03-02 2021-03-02 融合定位方法、装置、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110230387.XA CN112596089B (zh) 2021-03-02 2021-03-02 融合定位方法、装置、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN112596089A CN112596089A (zh) 2021-04-02
CN112596089B true CN112596089B (zh) 2021-05-18

Family

ID=75208074

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110230387.XA Active CN112596089B (zh) 2021-03-02 2021-03-02 融合定位方法、装置、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN112596089B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113295159B (zh) * 2021-05-14 2023-03-03 浙江商汤科技开发有限公司 端云融合的定位方法、装置和计算机可读存储介质
CN114199233B (zh) * 2021-11-08 2024-04-05 北京旷视科技有限公司 位姿确定方法及可移动设备
CN117676459A (zh) * 2022-08-31 2024-03-08 华为技术有限公司 一种相对位置的确定方法及装置
CN115453580A (zh) * 2022-09-13 2022-12-09 广东汇天航空航天科技有限公司 Gnss传感器的故障诊断方法、装置、导航系统及交通工具

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6167347A (en) * 1998-11-04 2000-12-26 Lin; Ching-Fang Vehicle positioning method and system thereof
CN104181574A (zh) * 2013-05-25 2014-12-03 成都国星通信有限公司 一种捷联惯导系统/全球导航卫星系统组合导航滤波系统及方法
KR20150122097A (ko) * 2015-10-08 2015-10-30 김인태 항법 알고리즘을 이용한 네비게이션 연동방법
CN110146909A (zh) * 2018-09-06 2019-08-20 腾讯科技（深圳）有限公司 一种定位数据处理方法
CN110221328A (zh) * 2019-07-23 2019-09-10 广州小鹏汽车科技有限公司 一种组合导航方法和装置
CN110411462A (zh) * 2019-07-22 2019-11-05 武汉大学 一种gnss/惯性/车道线约束/里程计多源融合方法
CN110865398A (zh) * 2018-08-27 2020-03-06 腾讯大地通途（北京）科技有限公司 定位数据的处理方法及处理装置、终端设备和存储介质
CN110873888A (zh) * 2018-09-04 2020-03-10 腾讯大地通途（北京）科技有限公司 定位方法、定位装置、定位设备和计算机存储介质
CN111240187A (zh) * 2020-01-16 2020-06-05 南京理工大学 一种基于车辆误差模型的车辆轨迹跟踪控制算法
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质
CN112083725A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向自动驾驶车辆的结构共用多传感器融合定位系统

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6167347A (en) * 1998-11-04 2000-12-26 Lin; Ching-Fang Vehicle positioning method and system thereof
CN104181574A (zh) * 2013-05-25 2014-12-03 成都国星通信有限公司 一种捷联惯导系统/全球导航卫星系统组合导航滤波系统及方法
KR20150122097A (ko) * 2015-10-08 2015-10-30 김인태 항법 알고리즘을 이용한 네비게이션 연동방법
CN110865398A (zh) * 2018-08-27 2020-03-06 腾讯大地通途（北京）科技有限公司 定位数据的处理方法及处理装置、终端设备和存储介质
CN110873888A (zh) * 2018-09-04 2020-03-10 腾讯大地通途（北京）科技有限公司 定位方法、定位装置、定位设备和计算机存储介质
CN110146909A (zh) * 2018-09-06 2019-08-20 腾讯科技（深圳）有限公司 一种定位数据处理方法
CN110411462A (zh) * 2019-07-22 2019-11-05 武汉大学 一种gnss/惯性/车道线约束/里程计多源融合方法
CN110221328A (zh) * 2019-07-23 2019-09-10 广州小鹏汽车科技有限公司 一种组合导航方法和装置
CN111240187A (zh) * 2020-01-16 2020-06-05 南京理工大学 一种基于车辆误差模型的车辆轨迹跟踪控制算法
CN111780756A (zh) * 2020-07-20 2020-10-16 北京百度网讯科技有限公司 车辆航位推算方法、装置、设备以及存储介质
CN112083725A (zh) * 2020-09-04 2020-12-15 湖南大学 一种面向自动驾驶车辆的结构共用多传感器融合定位系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于多传感器信息融合的移动机器人定位方法研究;吴显;《中国优秀硕士学位论文全文数据库 信息科技辑》;20160715(第7期);第I140-211页 *
基于车辆运动约束的里程计误差在线标定方法;李艳等;《中国惯性技术学报》;20160815;第24卷(第04期);第485-489页 *

Also Published As

Publication number Publication date
CN112596089A (zh) 2021-04-02

Similar Documents

Publication Publication Date Title
CN112596089B (zh) 融合定位方法、装置、电子设备及存储介质
CN110412635B (zh) 一种环境信标支持下的gnss/sins/视觉紧组合方法
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
CN113899375B (zh) 车辆定位方法和装置、存储介质及电子设备
CN112255648B (zh) 运动状态检测方法、装置、电子设备及存储介质
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
CN113050142B (zh) 终端设备的定位方法、装置、电子设备及可读存储介质
CN107703527B (zh) 一种基于北斗三频单历元宽巷/超宽巷的组合定位方法
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN116047565A (zh) 一种多传感器数据融合定位系统
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
WO2022046317A2 (en) System and method for providing gnss corrections
CN115326084A (zh) 车辆定位方法、装置、计算机设备、存储介质
CN114942025A (zh) 车辆导航定位方法、装置、电子设备及存储介质
Park et al. Implementation of vehicle navigation system using GNSS, INS, odometer and barometer
CN113822944A (zh) 一种外参标定方法、装置、电子设备及存储介质
Zhou et al. Rao‐Blackwellised particle filtering for low‐cost encoder/INS/GNSS integrated vehicle navigation with wheel slipping
CN114897942A (zh) 点云地图的生成方法、设备及相关存储介质
Kim et al. Kalman–Hatch dual‐filter integrating global navigation satellite system/inertial navigation system/on‐board diagnostics/altimeter for precise positioning in urban canyons
Noureldin et al. a Framework for Multi-Sensor Positioning and Mapping for Autonomous Vehicles
Abdellattif Multi-sensor fusion of automotive radar and onboard motion sensors for seamless land vehicle positioning in challenging environments
CN113029130B (zh) 数据处理方法、装置、计算机设备和存储介质
CN114608564B (zh) 一种基于夜间月光偏振-星光信息融合的组合定位方法
Fei et al. Low-Cost and High-Precision Lane-Level Vehicle Positioning Method in an Urban Area Based on Traversal Optimization for MIMU Calibration Parameters
CN111174786B (zh) 一种ins/sar组合导航的几何精度因子计算方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
REG Reference to a national code

Ref country code: HK

Ref legal event code: DE

Ref document number: 40041565

Country of ref document: HK