CN111578928B - 一种基于多源融合定位系统的定位方法及装置 - Google Patents

一种基于多源融合定位系统的定位方法及装置 Download PDF

Info

Publication number
CN111578928B
CN111578928B CN202010376773.5A CN202010376773A CN111578928B CN 111578928 B CN111578928 B CN 111578928B CN 202010376773 A CN202010376773 A CN 202010376773A CN 111578928 B CN111578928 B CN 111578928B
Authority
CN
China
Prior art keywords
information
pose
target object
preset
observation
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
CN202010376773.5A
Other languages
English (en)
Other versions
CN111578928A (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.)
Beijing University of Posts and Telecommunications
Original Assignee
Beijing University of Posts and Telecommunications
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 Beijing University of Posts and Telecommunications filed Critical Beijing University of Posts and Telecommunications
Priority to CN202010376773.5A priority Critical patent/CN111578928B/zh
Publication of CN111578928A publication Critical patent/CN111578928A/zh
Application granted granted Critical
Publication of CN111578928B publication Critical patent/CN111578928B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明实施例提供了一种基于多源融合定位系统的定位方法及装置，所述方法包括：获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。采用本发明实施例，可以提高位姿信息的确定准确度。

Description

一种基于多源融合定位系统的定位方法及装置
技术领域
本发明涉及定位技术领域，特别是涉及一种基于多源融合定位系统的定位方法及装置。
背景技术
多源融合定位系统包括多种定位设备和处理设备。在通过多源融合定位系统确定目标对象的位置和姿态时，处理设备可以获取多种定位设备针对目标对象的位姿观测信息，然后将位姿观测信息及目标对象的初始位姿信息输入EKF(Extended Kalman Filter，扩展卡尔曼滤波)模型，EKF模型可以根据位姿观测信息及目标对象的初始位姿信息确定目标对象开始时刻的位姿信息，然后可以不断迭代目标对象的位姿信息，确定目标对象运动过程中每一时刻的位姿信息。其中，位姿观测信息可以包括UWB(Ultra Wide Band，超宽带)位姿观测信息、Wi-Fi位姿观测信息、GPS(Global Positioning System，全球定位系统)位姿观测信息等。
当目标对象进行复杂运动时，由于信号干扰、传感器故障、通信冲突等因素，多源融合的定位系统获取的目标对象的位姿观测信息可能存在异常观测信息。基于目前的多源融合定位系统确定目标对象的位姿信息时，当位姿观测信息为异常信息时，会导致多源融合的定位系统确定的位姿信息出现偏差，位姿信息的确定准确度低。
发明内容
本发明实施例的目的在于提供一种基于多源融合定位系统的定位方法及装置，以提高位姿信息的确定准确度。具体技术方案如下：
第一方面，本发明实施例提供了一种基于多源融合定位系统的定位方法，所述方法包括：
获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息，其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息；
当确定所述当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度；
将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息；
返回所述获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息的步骤，直到停止确定所述目标对象的位姿信息。
可选的，所述确定预设的扩展卡尔曼滤波模型的卡尔曼增益的步骤，包括：
基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；
基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；
基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；
基于所述目标权值，确定观测误差总协方差；
基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。
可选的，所述基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数的步骤，包括：
根据公式
Figure GDA0003392056630000021
计算阶数动态系数σk；
其中，Qk为所述目标对象的过程误差协方差，Rk为所述目标对象的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
可选的，所述基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值的步骤，包括：
根据公式
Figure GDA0003392056630000031
计算预设自适应代价函数
Figure GDA0003392056630000032
的函数值；
其中，
Figure GDA0003392056630000033
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
可选的，所述基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值的步骤，包括：
根据公式
Figure GDA0003392056630000034
计算观测误差协方差对应的目标权值
Figure GDA0003392056630000035
其中，
Figure GDA0003392056630000036
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
可选的，所述基于所述目标权值，确定观测误差总协方差的步骤，包括：
根据公式
Figure GDA0003392056630000037
计算观测误差总协方差Sk；
其中，
Figure GDA0003392056630000038
为所述目标权值，Rk为所述目标对象的观测误差协方差，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵。
可选的，所述基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益的步骤，包括：
根据公式
Figure GDA0003392056630000039
计算预设的扩展卡尔曼滤波模型的卡尔曼增益Kk；
其中，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵，Sk为观测误差总协方差。
第二方面，本发明实施例提供了一种基于多源融合定位系统的定位装置，所述装置包括：
信息获取模块，用于获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息，其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息；
卡尔曼增益确定模块，用于当确定所述当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度；
位姿信息确定模块，用于将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息，并触发所述信息获取模块，直到停止确定所述目标对象的位姿信息。
可选的，所述卡尔曼增益确定模块包括：
系数确定子模块，用于基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；
代价函数确定子模块，用于基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；
权值确定子模块，用于基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；
协方差确定子模块，用于基于所述目标权值，确定观测误差总协方差；
卡尔曼增益确定子模块，用于基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。
可选的，所述系数确定子模块包括：
系数计算单元，用于根据公式
Figure GDA0003392056630000051
计算阶数动态系数σk；
其中，Qk为所述目标对象的过程误差协方差，Rk为所述目标对象的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
本发明实施例提供的方案中，处理设备可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息，其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿观测信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，卡尔曼增益为基于预设自适应代价函数确定的，且与预设自适应代价函数成正比，预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与异常信息监测参数成反比，异常信息监测参数用于表征实际位姿观测信息的异常程度；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。
当处理设备获取的实际位姿观测信息为异常信息时，由于异常信息监测参数可以表征实际位姿观测信息的异常程度，所以异常信息监测参数的值较高，预设自适应代价函数与异常信息监测参数成反比，此时自适应代价函数的值也就较低，卡尔曼增益与预设自适应代价函数成正比，所以此时卡尔曼增益较低，而卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，所以卡尔曼增益较低时可以降低当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，这样，也就能够降低异常信息对确定目标对象的位姿信息的影响，从而可以提高位姿信息的确定准确度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例所提供的一种基于多元融合定位系统的定位方法的流程图；
图2为图1所示实施例中预设的扩展卡尔曼滤波模型的运行方式的示意图；
图3为图1所示实施例中步骤S102的具体流程图；
图4为本发明实施例所提供的一种基于多元融合定位系统的定位装置的结构示意图；
图5为图4所示实施例中卡尔曼增益确定模块402的具体结构示意图；
图6为本发明实施例所提供的一种处理设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了提高位姿信息的确定准确度，本发明实施例提供了一种基于多源融合定位系统的定位方法、装置、处理设备及计算机可读存储介质。下面首先对本发明实施例提供的一种基于多源融合定位系统的定位方法进行介绍。
为了方便介绍本发明实施例所提供的一种基于多源融合定位系统的定位方法，首先介绍本发明实施例所提供的定位方法适用的多源融合定位系统。该多源融合定位系统可以包括处理设备和多种定位设备，例如，多种定位设备可以包括UWB定位设备、MIMO(multiple input multiple output，多进多出)定位设备、IMU(Inertial measurementunit，惯性测量单元)传感器、Wi-Fi定位设备、蓝牙定位设备、GPS定位设备等。处理设备可以与多种定位设备通信连接。
本发明实施例所提供的一种基于多源融合定位系统的定位方法可以应用于多源融合定位系统中的处理设备，处理设备可以为处理器、电脑、服务器、手机等电子设备。
如图1所示，一种基于多源融合定位系统的定位方法，所述方法包括：
S101，获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息；
其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息。
S102，当确定所述当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益；
其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度。
S103，将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息；
S104，返回所述获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息的步骤，直到停止确定所述目标对象的位姿信息。
本发明实施例提供的方案中，处理设备可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息，其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿观测信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，卡尔曼增益为基于预设自适应代价函数确定的，且与预设自适应代价函数成正比，预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与异常信息监测参数成反比，异常信息监测参数用于表征实际位姿观测信息的异常程度；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。
当处理设备获取的实际位姿观测信息为异常信息时，由于异常信息监测参数可以表征实际位姿观测信息的异常程度，所以异常信息监测参数的值较高，预设自适应代价函数与异常信息监测参数成反比，此时自适应代价函数的值也就较低，卡尔曼增益与预设自适应代价函数成正比，所以此时卡尔曼增益较低，而卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，所以卡尔曼增益较低时可以降低当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，这样，也就能够降低异常信息对确定目标对象的位姿信息的影响，从而可以提高位姿信息的确定准确度。
为了确定目标对象的位置和姿态，在上述步骤S101中，处理设备可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息。其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿信息。目标对象为需要确定其位置和姿态的对象，目标对象可以为移动终端、无人机、车辆、载人飞行器等。前一时刻的位姿信息为处理设备在当前时刻的前一时刻通过预设的扩展卡尔曼滤波模型确定的目标设备的位姿信息。
当目标对象前一时刻的位姿信息不存在时，说明此时为第一次获取目标对象当前时刻的实际位姿观测信息，为了方便进行处理，处理设备可以预先设置目标对象的初始位姿信息，作为第一次获取目标对象当前时刻的实际位姿观测信息所对应的前一时刻的位姿信息。
在一种实施方式中，多种定位设备可以包括UWB定位设备、MIMO定位设备及IMU传感器，其中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 reference system，航姿参考系统)工具获得。这样，处理设备获取的目标对象的实际位姿观测信息可以包括目标对象的位置坐标信息、姿态信息及运动状态信息。
由于信号干扰、传感器故障、通信冲突等因素，处理设备获取的目标对象当前时刻的实际位姿观测信息可能为异常信息。当目标对象当前时刻的实际位姿观测信息为异常信息时，处理设备通过预设的扩展卡尔曼滤波模型确定的目标对象的位姿信息将存在偏差。
为了降低确定的目标对象的位姿信息的偏差，在获取目标对象当前时刻的实际位姿观测信息及前一时刻的位姿信息后，处理设备可以判断当前时刻的实际位姿观测信息是否为异常信息。
当当前时刻的实际位姿观测信息为异常信息时，处理设备可以执行上述步骤S102，根据预设自适应代价函数确定预设的扩展卡尔曼滤波模型的卡尔曼增益。
其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，预设自适应代价函数与卡尔曼增益成正比，预设自适应代价函数是基于预先设置的异常信息监测参数确定的高阶函数，且预设自适应代价函数与异常信息监测参数成反比。当前时刻的先验位姿预测信息为基于目标对象前一时刻的位姿信息确定的，表示目标对象当前时刻的位置及姿态的预测结果。
异常信息监测参数用于表征实际位姿观测信息的异常程度。实际位姿观测信息与目标对象实际的位姿信息差异越大，该实际位姿观测信息的异常程度也就越高，这时异常信息监测参数的数值也就越高。由于预设自适应代价函数与异常信息监测参数成反比，且预设自适应代价函数与卡尔曼增益成正比，所以当异常信息监测参数的数值较高时，预设自适应代价函数的数值较低，根据预设自适应代价函数确定的卡尔曼增益也就较低；当异常信息监测参数的数值较低时，预设自适应代价函数的数值较高，根据预设自适应代价函数确定的卡尔曼增益也就较高。
在一种实施方式中，可以预先设置异常检测阈值，处理设备可以通过比较异常信息监测参数的绝对值与异常检测阈值的大小，确定当前时刻的实际位姿观测信息是否为异常信息。当异常信息监测参数的绝对值不小于异常检测阈值时，可以确定当前时刻的实际位姿观测信息为异常信息；当异常信息监测参数的绝对值小于异常检测阈值时，可以确定当前时刻的实际位姿观测信息非异常信息。
在确定卡尔曼增益之后，为了确定目标对象的位姿信息，处理设备可以执行上述步骤S103，即将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，预设的扩展卡尔曼滤波模型可以基于当前时刻的卡尔曼增益，确定目标对象当前时刻的位姿信息。其中，预设的扩展卡尔曼滤波模型可以包括当前时刻的实际位姿观测信息、前一时刻的位姿信息及当前时刻的位姿信息之间的对应关系。
例如，如图2所示，处理设备可以将目标对象t1时刻的实际位姿观测信息201及目标对象t1时刻的前一时刻的位姿信息202输入预设的扩展卡尔曼滤波模型，预设的扩展卡尔曼滤波模型可以基于t1时刻的卡尔曼增益203，确定目标对象t1时刻的位姿信息205，然后，当需要确定目标对象t2时刻的位姿信息时，处理设备可以将目标对象t2时刻的实际位姿观测信息204及目标对象t1时刻的位姿信息205输入预设的扩展卡尔曼滤波模型，预设的扩展卡尔曼滤波模型可以基于t2时刻的卡尔曼增益206，确定目标对象t2时刻的位姿信息。其中，目标对象t1时刻的位姿信息205也就是目标对象t2时刻的前一时刻的位姿信息。
这样，处理设备可以不断迭代目标对象前一时刻的位姿信息，确定目标对象当前时刻的位姿信息。当处理设备需要确定目标对象tn时刻的位姿信息时，处理设备可以将目标对象tn时刻的实际位姿观测信息207及目标对象tn-1时刻的位姿信息208输入预设的扩展卡尔曼滤波模型，预设的扩展卡尔曼滤波模型可以基于tn时刻的卡尔曼增益209，确定目标对象tn时刻的位姿信息210。
在确定目标对象当前时刻的位姿信息之后，为了继续对目标对象进行定位，处理设备可以返回上述步骤S101，直到停止确定目标对象的位姿信息时，停止返回。例如，当目标对象停止运动时，处理设备可以停止确定目标对象的位姿信息；又例如，当不再需要跟踪目标对象的位姿信息时，处理设备可以停止确定目标对象的位姿信息。这样，处理设备可以通过预设的扩展卡尔曼滤波模型持续跟踪目标对象的位姿信息。
作为本发明实施例的一种实施方式，如图3所示，上述确定预设的扩展卡尔曼滤波模型的卡尔曼增益的步骤，可以包括：
S301，基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；
为了确定当前时刻预设的扩展卡尔曼滤波模型的卡尔曼增益，处理设备可以基于目标对象的观测误差协方差与目标对象的过程误差协方差确定阶数动态系数。其中，目标对象的观测误差协方差可以表示目标对象的实际位姿观测信息与目标对象的先验位姿预测信息之间的差异，目标对象的过程误差协方差可以表示目标对象的实际的位姿信息与目标对象的先验位姿预测信息之间的差异，观测误差协方差可以根据多种定位设备的误差进行设置，过程误差协方差可以根据经验值设置。
S302，基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；
在确定阶数动态系数之后，处理设备可以根据阶数动态系数、预设的阶数调整系数及预设的异常信息监测参数，确定预设自适应代价函数的函数值。其中，预设的阶数调整系数的取值范围可以为[5,10]。
为了当实际位姿观测信息为异常信息时降低卡尔曼增益，阶数动态系数与预设自适应代价函数的阶数可以成反比。当阶数动态系数的数值较高时，预设自适应代价函数的阶数较低，此时预设自适应代价函数的函数值也就较低，由于预设自适应代价函数与卡尔曼增益成正比，因此此时的卡尔曼增益也就较低。
异常信息监测参数可以表示实际位姿观测信息的异常程度，当实际位姿观测信息的异常程度较高时，异常信息检测参数的绝对值也就较高；当实际位姿观测信息的异常程度较低时，异常信息检测参数的绝对值也就较低。
S303，基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；
在确定预设自适应代价函数的函数值之后，处理设备可以根据预设自适应代价函数的函数值确定观测误差协方差对应的目标权值。
其中，目标权值用于表示观测误差协方差对于确定观测误差总协方差的影响程度，观测误差总协方差可以表示基于前一时刻的位姿信息确定当前时刻的先验位姿预测信息后定位系统的整体误差。
为了使预设自适应代价函数与卡尔曼增益成正比，目标权值与预设自适应代价函数及卡尔曼增益可以成反比，这样，当实际位姿观测信息的异常程度较高时，预设自适应代价函数的函数值较低，那么目标权值也就较高，卡尔曼增益也就较低。
S304，基于所述目标权值，确定观测误差总协方差；
在确定当前时刻的观测误差协方差对应的目标权值之后，处理设备可以根据该目标权值和观测误差协方差，确定当前时刻的观测误差总协方差。
当实际位姿信息为异常信息时，实际的过程误差协方差较高，在这种情况下确定当前时刻的观测误差总协方差，需要更多参考观测误差协方差对确定观测误差总协方差的影响程度。
由于观测误差协方差为预设的固定值，因此，可以通过提高目标权值的方式，来提高观测误差协方差对确定观测误差总协方差的影响程度，因此目标权值可以与观测误差总协方差成正比。目标权值越高，观测误差协方差对于确定观测误差总协方差的影响程度也就越高，那么观测误差总协方差也就越高；目标权值越低，观测误差协方差对确定观测误差总协方差的影响程度也就越低，那么观测误差总协方差也就越低。
S305，基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。
在确定当前时刻的观测误差总协方差之后，处理设备可以基于观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。当卡尔曼增益较高时，目标对象的位姿信息更加接近于实际位姿观测信息；当卡尔曼增益较低时，目标对象的位姿信息更加接近于先验位姿预测信息。
由于目标权值可以与观测误差总协方差成正比，且目标权值可以与卡尔曼增益及预设自适应代价函数成反比，因此，为了使预设自适应代价函数与卡尔曼增益成正比，观测误差总协方差与预设的扩展卡尔曼滤波模型的卡尔曼增益可以成反比。
当实际位姿观测信息为异常信息时，预设自适应代价函数的函数值较低，目标权值较高，当前时刻的观测误差总协方差也就较高，那么预设的扩展卡尔曼滤波模型的卡尔曼增益也就较低；当实际位姿观测信息不为异常信息时，预设自适应代价函数的函数值较高，目标权值较低，当前时刻的观测误差总协方差也就较低，那么预设的扩展卡尔曼滤波模型的卡尔曼增益也就较高。
可见，本发明实施例所提供的方案中，处理设备可以基于目标对象的观测误差协方差与目标对象的过程误差协方差，确定阶数动态系数；基于阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；基于预设自适应代价函数的函数值，确定观测误差协方差对应的目标权值；基于目标权值，确定观测误差总协方差；基于观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。这样，当实际位姿观测信息为异常信息时，可以准确地确定当前时刻的卡尔曼增益，降低异常信息对确定位姿信息的影响，提高确定位姿信息的准确度。
作为本发明实施例的一种实施方式，上述基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数的步骤，可以包括：
根据如下公式，计算阶数动态系数σk：
Figure GDA0003392056630000141
其中，Qk为目标对象的过程误差协方差，Rk为目标对象当前时刻的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
假设预设的扩展卡尔曼滤波的集中式融合方程如以下公式所示：
Figure GDA0003392056630000142
其中，F为状态转移矩阵，Zk为目标对象当前时刻的实际位姿观测信息，其维度为M，Zk＝[ZUWB,k T,ZDOA,k T,ZINS,k T]T，ZUWB,k为通过UWB定位设备确定的目标对象当前时刻的位置坐标信息，ZDOA,k为通过MIMO定位设备确定的目标对象当前时刻的姿态信息，ZINS,k为通过INS(Inertial positioning system，惯性定位定向系统)定位设备确定的目标对象当前时刻的运动状态信息；Xk-1为目标对象前一时刻的实际的位姿信息，Xk为目标对象当前时刻的实际的位姿信息，Xk-1与Xk的维度均为N；H(Xk)为当前时刻的实际的位姿信息对应的融合方程，当当前时刻的实际的位姿信息与当前时刻的位姿信息比较接近时，H(Xk)可以视为当前时刻的位姿信息对应的融合方程，Wk-1为前一时刻的过程噪声，Vk为观测噪声。
假设过程噪声与测量噪声不相关，且各传感器测量值互不相关，则过程噪声的协方差Qk可以通过如下所示公式计算：
Figure GDA0003392056630000143
其中，δk,j为如下所示的Kronecker-delta(克罗内克函数)函数：
Figure GDA0003392056630000144
Figure GDA0003392056630000151
为k、j时刻过程噪声的互协方差，也就是说，过程噪声的协方差Qk为过程噪声的自相关函数，观测噪声的协方差为Rk。
为了便于分析，可以上述集中式融合方程进行非线性回归转换，可得如下公式：
Figure GDA0003392056630000152
其中，
Figure GDA0003392056630000153
为目标对象当前时刻的先验位姿预测信息，
Figure GDA0003392056630000154
Figure GDA0003392056630000155
为预测误差信息，表示目标对象当前时刻的先验位姿预测信息与目标对象当前时刻的实际的位姿信息之间的差异。
定义误差噪声信息
Figure GDA0003392056630000156
假设观测噪声和预测噪声不相关，则误差噪声信息
Figure GDA0003392056630000157
的协方差可以根据如下公式进行计算：
Figure GDA0003392056630000158
其中，Pk|k-1为预测误差信息δXk|k-1的协方差。为了方便计算，对上述公式(2)进行归一化处理，即对上述公式(2)中的每一项左乘
Figure GDA0003392056630000159
可得：
Figure GDA00033920566300001510
其中，
Figure GDA00033920566300001511
Yk可以表示经过非线性回归转换和归一化处理后的位姿观测信息；
Figure GDA00033920566300001512
h(Xk)可以表示经过非线性回归转换和归一化处理后的位姿预测信息。
为了确定目标对象当前时刻的实际位姿观测信息是否为异常信息，可以定义异常信息监测参数
Figure GDA00033920566300001513
当
Figure GDA00033920566300001514
的绝对值不小于预设的异常检测阈值η时，可以确定目标对象当前时刻的实际位姿观测信息为异常信息；当
Figure GDA00033920566300001515
的绝对值小于预设的异常检测阈值η时，可以确定目标对象当前时刻的实际位姿观测信息不为异常信息。
当实际位姿观测信息不为异常信息时，异常信息监测参数
Figure GDA00033920566300001516
的期望分布可以表示为如下所示的高斯分布：
E{[Yk-h(Xk)][Yk-h(Xk)]T}＝IN+M
其中，IN+M为N+M维的单位矩阵。
为了减小异常信息对确定目标对象的位姿信息的影响，可以预先设置自适应代价函数
Figure GDA0003392056630000161
当目标对象当前时刻的实际位姿观测信息为异常信息时，自适应代价函数
Figure GDA0003392056630000162
可以为：
Figure GDA0003392056630000163
其中，σk为阶数动态系数，σ0为预设的阶数调整系数，取值范围可以为[5,10]，σk可以通过如下公式进行计算：
Figure GDA0003392056630000164
其中，||a||2为a的L2范数。由于||Qk||2与
Figure GDA0003392056630000165
均大于0，因此，
Figure GDA0003392056630000166
的值也大于0。
可见，本发明实施例所提供的方案中，处理设备可以根据上述公式(1)计算阶数动态系数。这样，处理设备根据上述公式(1)可以准确地计算阶数动态系数。
作为本发明实施例的一种实施方式，上述基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值的步骤，可以包括：
根据如下公式，计算预设自适应代价函数
Figure GDA0003392056630000167
的函数值：
Figure GDA0003392056630000168
其中，
Figure GDA0003392056630000169
为异常信息监测参数，η为预设的异常检测阈值，取值范围可以为[1,1.2]；σk为阶数动态系数，σ0为预设的阶数调整系数。
为了减小异常信息对确定目标对象的位姿信息的影响，可以预先设置如上述公式(3)所示的自适应代价函数
Figure GDA0003392056630000171
当当前时刻的实际位姿观测信息不为异常信息时，确定当前时刻的先验位姿预测信息的协方差无需更多参考观测误差协方差对确定先验位姿预测信息的协方差的影响程度，这时目标权值可以为1。
当前时刻的实际位姿观测信息不为异常信息时，
Figure GDA0003392056630000172
此时，为了使目标权值为1，预设自适应代价函数
Figure GDA0003392056630000173
可以表示为：
Figure GDA0003392056630000174
当前时刻的实际位姿观测信息为异常信息时，
Figure GDA0003392056630000175
此时，为了降低异常信息对确定目标对象的位姿信息的影响，预设自适应代价函数
Figure GDA0003392056630000176
可以表示为：
Figure GDA0003392056630000177
此时，异常信息监测参数的绝对值
Figure GDA0003392056630000178
的阶数为
Figure GDA0003392056630000179
由于
Figure GDA00033920566300001710
的值大于0，因此
Figure GDA00033920566300001711
也就是说，当实际位姿观测信息为异常信息时，异常信息监测参数的绝对值
Figure GDA00033920566300001712
的阶数由2降低至
Figure GDA00033920566300001713
这样预设自适应代价函数
Figure GDA00033920566300001714
的函数值也就会降低。
可见，本发明实施例所提供的方案中，处理设备可以根据上述公式(3)计算预设自适应代价函数的函数值。这样，处理设备可以根据上述公式(3)准确地计算预设自适应代价函数的函数值。
作为本发明实施例的一种实施方式，上述基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值的步骤，包括：
根据如下所示公式，计算当前时刻的观测误差协方差对应的目标权值
Figure GDA00033920566300001715
Figure GDA0003392056630000181
其中，
Figure GDA0003392056630000182
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
基于最小化损失函数准则，可以定义用于表示异常信息对定位系统的影响效果的影响函数
Figure GDA0003392056630000183
Figure GDA0003392056630000184
其中，
Figure GDA0003392056630000185
为预设自适应代价函数。
根据影响函数
Figure GDA0003392056630000186
可以确定观测误差协方差对应的目标权值
Figure GDA0003392056630000187
Figure GDA0003392056630000188
当
Figure GDA0003392056630000189
时，确定当前时刻的先验位姿预测信息的协方差无需更多参考观测误差协方差对确定先验位姿预测信息的协方差的影响程度，这时目标权值
Figure GDA00033920566300001810
当
Figure GDA00033920566300001811
时，确定当前时刻的先验位姿预测信息的协方差需要更多参考观测误差协方差对确定先验位姿预测信息的协方差的影响程度，这时目标权值
Figure GDA00033920566300001812
这样，通过目标权值
Figure GDA00033920566300001813
可以确定抑制后的误差噪声信息
Figure GDA00033920566300001814
对应的协方差矩阵
Figure GDA00033920566300001815
Figure GDA0003392056630000191
其中，Rk为观测噪声的协方差，Pk|k-1为预测误差信息δXk|k-1的协方差。
可见，本发明实施例所提供的方案中，处理设备可以根据上述公式(4)计算观测误差协方差的目标权值。这样，处理设备可以根据上述公式(4)准确地确定观测误差协方差对应的目标权值。
作为本发明实施例的一种实施方式，上述基于所述目标权值，确定位姿观测噪声信息的总协方差的步骤，可以包括：
根据如下公式，计算位姿观测噪声信息的总协方差Sk：
Figure GDA0003392056630000192
其中，
Figure GDA0003392056630000193
为所述目标权值，Rk为所述目标对象的观测误差协方差，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵。
处理设备得到目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息之后，可以根据如下公式，确定当前时刻的先验位姿预测信息
Figure GDA0003392056630000194
Figure GDA0003392056630000195
其中，
Figure GDA0003392056630000196
为目标对象前一时刻的位姿信息，F为状态转移矩阵，表示目标对象前一时刻的位姿信息与目标对象当前时刻的先验位姿预测信息之间的对应关系。
处理设备可以获取前一时刻的后验预测协方差，进而根据前一时刻的后验预测协方差及过程噪声的协方差，确定当前时刻的先验预测协方差Pk|k-1：
Pk|k-1＝FPk-1|k-1FT+Qk
其中，Pk-1|k-1为前一时刻的后验预测协方差，前一时刻的后验预测协方差为基于前一时刻的先验预测协方差及前一时刻的卡尔曼增益确定的；Qk为过程噪声的协方差，当前时刻的先验预测协方差Pk|k-1也就是上述预测误差信息δXk|k-1的协方差。
当前一时刻的后验预测协方差不存在时，说明此时为第一次获取目标对象当前时刻的实际位姿观测信息，为了方便进行处理，处理设备可以预先设置初始预测协方差，作为第一次获取目标对象当前时刻的实际位姿观测信息所对应的前一时刻的后验预测协方差。
在确定当前时刻的先验预测协方差Pk|k-1之后，为了确定当前时刻的卡尔曼增益，处理设备可以根据当前时刻的先验预测协方差Pk|k-1、观测误差协方差Rk，以及目标权值
Figure GDA0003392056630000201
确定观测误差总协方差Sk：
Figure GDA0003392056630000202
可见，本发明实施例所提供的方案中，处理设备可以根据上述公式(5)计算预测观测信息的协方差。这样，处理设备可以根据上述公式(5)，准确地计算预测位置观测信息的协方差。
作为本发明实施例的一种实施方式，上述基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益的步骤，可以包括：
根据如下公式，计算预设的扩展卡尔曼滤波模型的卡尔曼增益Kk：
Figure GDA0003392056630000203
其中，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵，Sk为观测误差总协方差。
在确定当前时刻的先验位姿预测信息
Figure GDA0003392056630000204
之后，处理设备可以确定当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息
Figure GDA0003392056630000205
之间的差异yk：
Figure GDA0003392056630000206
其中，Zk为当前时刻的实际位姿观测信息，
Figure GDA0003392056630000207
为先验位姿预测信息
Figure GDA0003392056630000208
对应的融合观测方程。
当当前时刻的实际位姿观测信息Zk为异常信息时，当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息
Figure GDA0003392056630000209
之间的差异yk较大，为了减少异常信息对于确定当前时刻的位姿信息的影响，处理设备可以根据上述公式(6)，确定预设的扩展卡尔曼滤波模型的卡尔曼增益Kk。
这样，处理设备可以根据先验位姿预测信息
Figure GDA0003392056630000211
卡尔曼增益Kk及当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息
Figure GDA0003392056630000212
之间的差异yk，确定当前时刻的位姿信息
Figure GDA0003392056630000213
Figure GDA0003392056630000214
同时，处理设备还可以根据当前时刻的先验误差协方差Pk|k-1及卡尔曼增益Kl，确定当前时刻的后验误差协方差矩阵Pl|k：
Pk|k＝(I-KkHJ,k)Pk|k-1
其中，I为单位矩阵。
可见，本发明实施例所提供的方案中，处理设备可以根据上述公式(6)计算预设的扩展卡尔曼滤波模型当前时刻的卡尔曼增益。这样，处理设备可以准确地确定卡尔曼增益，进而可以准确地确定目标对象的位姿信息。
相应于上述基于多源融合定位系统的定位方法，本发明实施例还提供了一种基于多源融合定位系统的定位装置。下面对本发明实施例所提供的一种基于多源融合定位系统的定位装置进行介绍。
如图4所示，一种基于多源融合定位系统的定位装置，所述装置包括：
信息获取模块401，用于获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息；
其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息。
卡尔曼增益确定模块402，用于当确定所述当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益；
其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度。
位姿信息确定模块403，用于将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息，并触发所述信息获取模块，直到停止确定所述目标对象的位姿信息。
可见，本发明实施例所提供的方案中，处理设备可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息，其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿观测信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，卡尔曼增益为基于预设自适应代价函数确定的，且与预设自适应代价函数成正比，预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与异常信息监测参数成反比，异常信息监测参数用于表征实际位姿观测信息的异常程度；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。
当处理设备获取的实际位姿观测信息为异常信息时，由于异常信息监测参数可以表征实际位姿观测信息的异常程度，所以异常信息监测参数的值较高，预设自适应代价函数与异常信息监测参数成反比，此时自适应代价函数的值也就较低，卡尔曼增益与预设自适应代价函数成正比，所以此时卡尔曼增益较低，而卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，所以卡尔曼增益较低时可以降低当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，这样，也就能够降低异常信息对确定目标对象的位姿信息的影响，从而可以提高位姿信息的确定准确度。
作为本发明实施例的一种实施方式，如图5所示，上述卡尔曼增益确定模块402可以包括：
系数确定子模块501，用于基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；
代价函数确定子模块502，用于基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；
权值确定子模块503，用于基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；
协方差确定子模块504，用于基于所述目标权值，确定观测误差总协方差；
卡尔曼增益确定子模块505，用于基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益。
作为本发明实施例的一种实施方式，上述系数确定子模块501可以包括：
系数计算单元(图5中未示出)，用于根据公式
Figure GDA0003392056630000231
计算阶数动态系数σk；
其中，Qk为所述目标对象的过程误差协方差，Rk为所述目标对象的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
作为本发明实施例的一种实施方式，上述代价函数确定子模块502可以包括：
代价函数计算单元(图5中未示出)，用于根据公式
Figure GDA0003392056630000232
计算预设自适应代价函数
Figure GDA0003392056630000233
的函数值；
其中，
Figure GDA0003392056630000234
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
作为本发明实施例的一种实施方式，上述权值确定子模块503可以包括：
权值计算单元(图5中未示出)，用于根据公式
Figure GDA0003392056630000241
计算当前时刻的观测误差协方差对应的目标权值
Figure GDA0003392056630000242
其中，
Figure GDA0003392056630000243
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
作为本发明实施例的一种实施方式，上述协方差确定子模块504可以包括：
协方差计算单元(图5中未示出)，用于根据公式
Figure GDA0003392056630000244
Figure GDA0003392056630000245
计算位姿观测噪声信息的总协方差Sk；
其中，
Figure GDA0003392056630000246
为所述目标权值，Rk为所述目标对象的观测误差协方差，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵。
作为本发明实施例的一种实施方式，上述卡尔曼增益确定子模块505可以包括：
卡尔曼增益计算单元(图5中未示出)，用于根据公式
Figure GDA0003392056630000247
计算预设的扩展卡尔曼滤波模型的卡尔曼增益Kk；
其中，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵，Sk为观测误差总协方差。
本发明实施例还提供了一种处理设备，如图6所示，包括处理器601、通信接口602、存储器603和通信总线604，其中，处理器601，通信接口602，存储器603通过通信总线604完成相互间的通信，
存储器603，用于存放计算机程序；
处理器601，用于执行存储器603上所存放的程序时，上述任一实施例所述的基于多源融合定位系统的定位方法步骤。
本发明实施例提供的方案中，处理设备可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息，其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿观测信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，卡尔曼增益为基于预设自适应代价函数确定的，且与预设自适应代价函数成正比，预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与异常信息监测参数成反比，异常信息监测参数用于表征实际位姿观测信息的异常程度；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。
当处理设备获取的实际位姿观测信息为异常信息时，由于异常信息监测参数可以表征实际位姿观测信息的异常程度，所以异常信息监测参数的值较高，预设自适应代价函数与异常信息监测参数成反比，此时自适应代价函数的值也就较低，卡尔曼增益与预设自适应代价函数成正比，所以此时卡尔曼增益较低，而卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，所以卡尔曼增益较低时可以降低当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，这样，也就能够降低异常信息对确定目标对象的位姿信息的影响，从而可以提高位姿信息的确定准确度。
上述处理设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述处理设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一实施例所述的基于多源融合定位系统的定位方法的步骤。
可见，本发明实施例提供的方案中，计算机可读存储介质内存储有计算机程序被处理器执行时，可以获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息，其中，实际位姿观测信息为通过多种定位设备测量的目标对象当前时刻的位姿观测信息；当确定当前时刻的实际位姿观测信息为异常信息时，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，卡尔曼增益为基于预设自适应代价函数确定的，且与预设自适应代价函数成正比，预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与异常信息监测参数成反比，异常信息监测参数用于表征实际位姿观测信息的异常程度；将当前时刻的实际位姿观测信息及前一时刻的位姿信息输入预设的扩展卡尔曼滤波模型，以使预设的扩展卡尔曼滤波模型基于卡尔曼增益确定目标对象当前时刻的位姿信息；返回获取目标对象当前时刻的实际位姿观测信息及目标对象前一时刻的位姿信息的步骤，直到停止确定目标对象的位姿信息。
当处理设备获取的实际位姿观测信息为异常信息时，由于异常信息监测参数可以表征实际位姿观测信息的异常程度，所以异常信息监测参数的值较高，预设自适应代价函数与异常信息监测参数成反比，此时自适应代价函数的值也就较低，卡尔曼增益与预设自适应代价函数成正比，所以此时卡尔曼增益较低，而卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，所以卡尔曼增益较低时可以降低当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异对位姿信息的影响程度，这样，也就能够降低异常信息对确定目标对象的位姿信息的影响，从而可以提高位姿信息的确定准确度。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。
以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (8)

1.一种基于多源融合定位系统的定位方法，其特征在于，所述方法包括：
获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息，其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息；
当确定所述当前时刻的实际位姿观测信息为异常信息时，基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；基于所述目标权值，确定观测误差总协方差；基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度；
将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息；
返回所述获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息的步骤，直到停止确定所述目标对象的位姿信息。
2.根据权利要求1所述的方法，其特征在于，所述基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数的步骤，包括：
根据公式
Figure FDA0003392056620000011
计算阶数动态系数σk；
其中，Qk为所述目标对象的过程误差协方差，Rk为所述目标对象的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
3.根据权利要求1所述的方法，其特征在于，所述基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值的步骤，包括：
根据公式
Figure FDA0003392056620000021
计算预设自适应代价函数
Figure FDA0003392056620000022
的函数值；
其中，
Figure FDA0003392056620000023
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
4.根据权利要求1所述的方法，其特征在于，所述基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值的步骤，包括：
根据公式
Figure FDA0003392056620000024
计算观测误差协方差对应的目标权值
Figure FDA0003392056620000025
其中，
Figure FDA0003392056620000026
为异常信息监测参数，η为预设的异常检测阈值，σk为阶数动态系数，σ0为预设的阶数调整系数。
5.根据权利要求1所述的方法，其特征在于，所述基于所述目标权值，确定观测误差总协方差的步骤，包括：
根据公式
Figure FDA0003392056620000027
计算观测误差总协方差Sk；
其中，
Figure FDA0003392056620000028
为所述目标权值，Rk为所述目标对象的观测误差协方差，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵。
6.根据权利要求1所述的方法，其特征在于，所述基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益的步骤，包括：
根据公式
Figure FDA0003392056620000031
计算预设的扩展卡尔曼滤波模型的卡尔曼增益Kk；
其中，Pk|k-1为先验误差协方差，HJ,k为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵，Sk为观测误差总协方差。
7.一种基于多源融合定位系统的定位装置，其特征在于，所述装置包括：
信息获取模块，用于获取目标对象当前时刻的实际位姿观测信息及所述目标对象前一时刻的位姿信息，其中，所述实际位姿观测信息为通过多种定位设备测量的所述目标对象当前时刻的位姿观测信息；
卡尔曼增益确定模块，包括：系数确定子模块，用于当确定所述当前时刻的实际位姿观测信息为异常信息时，基于所述目标对象的观测误差协方差与所述目标对象的过程误差协方差，确定阶数动态系数；代价函数确定子模块，用于基于所述阶数动态系数、预设的阶数调整系数及异常信息监测参数，确定预设自适应代价函数的函数值；权值确定子模块，用于基于所述预设自适应代价函数的函数值，确定所述观测误差协方差对应的目标权值；协方差确定子模块，用于基于所述目标权值，确定观测误差总协方差；卡尔曼增益确定子模块，用于基于所述观测误差总协方差，确定预设的扩展卡尔曼滤波模型的卡尔曼增益，其中，所述卡尔曼增益用于表征当前时刻的实际位姿观测信息与当前时刻的先验位姿预测信息之间的差异，对位姿信息的影响程度，所述卡尔曼增益为基于预设自适应代价函数确定的，且与所述预设自适应代价函数成正比，所述预设自适应代价函数为基于异常信息监测参数确定的高阶函数，且与所述异常信息监测参数成反比，所述异常信息监测参数用于表征实际位姿观测信息的异常程度；
位姿信息确定模块，用于将所述当前时刻的实际位姿观测信息及所述前一时刻的位姿信息输入所述预设的扩展卡尔曼滤波模型，以使所述预设的扩展卡尔曼滤波模型基于所述卡尔曼增益确定所述目标对象当前时刻的位姿信息，并触发所述信息获取模块，直到停止确定所述目标对象的位姿信息。
8.根据权利要求7所述的装置，其特征在于，所述系数确定子模块包括：
系数计算单元，用于根据公式
Figure FDA0003392056620000041
计算阶数动态系数σk；
其中，Qk为所述目标对象的过程误差协方差，Rk为所述目标对象的观测误差协方差，SJ为当前时刻的位姿信息对应的融合方程H(Xk)的雅各比矩阵的逆矩阵。
CN202010376773.5A 2020-05-07 2020-05-07 一种基于多源融合定位系统的定位方法及装置 Active CN111578928B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010376773.5A CN111578928B (zh) 2020-05-07 2020-05-07 一种基于多源融合定位系统的定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010376773.5A CN111578928B (zh) 2020-05-07 2020-05-07 一种基于多源融合定位系统的定位方法及装置

Publications (2)

Publication Number Publication Date
CN111578928A CN111578928A (zh) 2020-08-25
CN111578928B true CN111578928B (zh) 2022-04-05

Family

ID=72118789

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010376773.5A Active CN111578928B (zh) 2020-05-07 2020-05-07 一种基于多源融合定位系统的定位方法及装置

Country Status (1)

Country Link
CN (1) CN111578928B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112665593B (zh) * 2020-12-17 2024-01-26 北京经纬恒润科技股份有限公司 一种车辆定位方法及装置
CN112862161B (zh) * 2021-01-18 2024-09-06 湖南睿邮信息技术有限公司 货物分拣管理方法、装置、电子设备和存储介质
CN112799421B (zh) * 2021-03-30 2021-07-02 北京三快在线科技有限公司 无人机的控制方法、装置、存储介质和电子设备
CN114563045B (zh) * 2022-02-28 2023-06-27 南京天溯自动化控制系统有限公司 一种医院后勤系统的检测方法、装置、电子设备及介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101793522A (zh) * 2010-03-31 2010-08-04 上海交通大学 基于抗差估计的稳健滤波方法
CN104880722A (zh) * 2015-03-25 2015-09-02 清华大学 无人机的gps速度及位置观测异常值检测方法
CN106153045A (zh) * 2016-07-05 2016-11-23 东南大学 一种抑制gnss信息异常的滤波增益动态调整方法
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101793522A (zh) * 2010-03-31 2010-08-04 上海交通大学 基于抗差估计的稳健滤波方法
CN104880722A (zh) * 2015-03-25 2015-09-02 清华大学 无人机的gps速度及位置观测异常值检测方法
CN106153045A (zh) * 2016-07-05 2016-11-23 东南大学 一种抑制gnss信息异常的滤波增益动态调整方法
CN109959381A (zh) * 2017-12-22 2019-07-02 深圳市优必选科技有限公司 一种定位方法、装置、机器人及计算机可读存储介质
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Research on Multi-sensor Information Fusion Algorithm with Sensor Fault Diagnosis;Chun Xiao 等;《2016 International Conference on Industrial Informatics - Computing Technology, Intelligent Technology, Industrial Information Integration (ICIICII)》;20170119;第132-135页 *
Robust Unscented Kalman Filter for;Zhao Junbo 等;《IEEE Transactions on Smart Grid》;20191031;第1215-1224页 *
基于多源传感器信息融合的移动机器人协同导航研究;王晨琳;《中国优秀硕士学位论文全文数据库》;20190315;第1-87页 *
水下航行器组合导航定位技术研究;迟凤阳;《中国优秀硕士学位论文全文数据库》;20150615;第1-138页 *

Also Published As

Publication number Publication date
CN111578928A (zh) 2020-08-25

Similar Documents

Publication Publication Date Title
CN111578928B (zh) 一种基于多源融合定位系统的定位方法及装置
CN107659893B (zh) 一种误差补偿方法、装置、电子设备及可读存储介质
CN108535720B (zh) 用于改进的卡尔曼滤波目标跟踪的自适应过程噪声描述
US20210165077A1 (en) Radar data compensation method for mobile robot, device, and storage medium
CN111208492A (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
CN109085564B (zh) 一种定位方法及装置
Hashlamon A new adaptive extended Kalman filter for a class of nonlinear systems
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN110186456A (zh) 一种目标定位的方法、系统、无人机及存储介质
EP2869026B1 (en) Systems and methods for off-line and on-line sensor calibration
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与系统
CN117785898A (zh) 结合gnss的状态量更新方法、装置、设备和介质
CN112720450B (zh) 机器人关节角度检验方法、装置、设备及介质
CN109945864B (zh) 室内行车定位融合方法、装置、存储介质及终端设备
CN110220513B (zh) 一种目标定位的方法、系统、无人机及存储介质
CN115388914B (zh) 传感器的参数标定方法、装置、存储介质和电子装置
CN116972834A (zh) 多传感器融合定位方法、装置、系统及存储介质
CN111123323B (zh) 一种提高便携设备定位精度的方法
US20240134033A1 (en) Method for determining a movement state of a rigid body
Dubois et al. Performance evaluation of a moving horizon estimator for multi-rate sensor fusion with time-delayed measurements
CN114199236A (zh) 定位数据处理的方法、装置、电子设备和自动驾驶车辆
CN114234970A (zh) 一种运动载体姿态实时测量方法、装置
CN114035154A (zh) 一种运动参数辅助的单站射频信号定位方法
CN108332751B (zh) 一种多源融合定位方法、装置、电子设备及存储介质
CN111343573A (zh) 根据环境差异标校在线rssi值的指纹定位方法及装置

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