CN113008245B - 一种定位信息融合方法及其装置、计算机服务器 - Google Patents

一种定位信息融合方法及其装置、计算机服务器 Download PDF

Info

Publication number
CN113008245B
CN113008245B CN201911326703.2A CN201911326703A CN113008245B CN 113008245 B CN113008245 B CN 113008245B CN 201911326703 A CN201911326703 A CN 201911326703A CN 113008245 B CN113008245 B CN 113008245B
Authority
CN
China
Prior art keywords
pose
particle
reliability
particles
value
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
CN201911326703.2A
Other languages
English (en)
Other versions
CN113008245A (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 Tusimple Technology Co Ltd
Original Assignee
Beijing Tusimple Technology 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 Beijing Tusimple Technology Co Ltd filed Critical Beijing Tusimple Technology Co Ltd
Priority to CN201911326703.2A priority Critical patent/CN113008245B/zh
Publication of CN113008245A publication Critical patent/CN113008245A/zh
Application granted granted Critical
Publication of CN113008245B publication Critical patent/CN113008245B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments

Landscapes

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

Abstract

本发明公开一种定位信息融合方法及其装置、计算机服务器，以解决现有定位融合技术中当其中一个或多个定位模块发生异常时导致融合后的定位精度较低的技术问题。方法包括：构建概率模型，其中，所述概率模型包括定位模块的位姿观测模型、定位模块的可靠性迁移模型和可靠性先验模型、位姿迁移模型，且所述位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性取值；基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值。

Description

一种定位信息融合方法及其装置、计算机服务器
技术领域
本发明涉及定位技术，特别涉及一种定位信息融合方法、一种定位信息融合装置、一种计算机服务器。
背景技术
目前，使用GNSS(Global Navigation Satellite System，全球导航卫星系统)是常见的室外定位技术，由于存在信号遮挡、多路径效应等问题，GNSS在有些环境中的定位误差较大甚至定位失败。为了提高定位准确性和鲁棒性，通常在同一目标对象上设置多个定位模块，再对该多个定位模块的定位信息进行融合得到目标对象的定位信息，对不同定位模块取长补短。常见的定位传感器包括GNSS、IMU(Inertial Measurement Unit，惯性测量单元)、摄像头、激光雷达等。基于这些定位传感器又可以发展出多种定位技术，例如，将GNSS和IMU结合可以发展出组合导航系统；将摄像头和IMU结合可以发展出视觉-IMU里程计；基于摄像头和高精度地图可以开发出视觉定位系统等。
目前，在自动驾驶车辆和机器人等应用场景通常使用多种定位模块，融合多种定位模块的定位信息来提高对自动驾驶车辆和机器人等进行定位的精确度和鲁棒性。例如，使用卡尔曼滤波器融合GNSS、编码器、雷达等传感器的信息。
定位融合方法目前主流的有两种，一种是基于滤波方法，可以包括卡尔曼滤波、粒子滤波等，另一种是基于优化的方法。
当所有定位模块均正常工作时，通过卡尔曼滤波或粒子滤波的方法对多个定位模块的定位信息进行融合可以得到较为精准的定位结果，但是当其中一个或多个定位模块失效时，卡尔曼滤波或粒子滤波的结果会被失效的定位模块干扰，使得融合后的定位结果严重偏离真实位置。
发明内容
鉴于上述问题，本发明提供一种定位信息融合方法及其装置、计算机服务器，以解决现有定位融合技术中当其中一个或多个定位模块发生异常时导致融合后的定位精度较低的技术问题。
本发明实施例，第一方面，提供一种定位信息融合方法，包含多个对同一目标对象进行定位得到所述目标对象的位姿观测值的定位模块，方法包括：
构建概率模型，其中，所述概率模型包括定位模块的位姿观测模型、定位模块的可靠性迁移模型和可靠性先验模型、位姿迁移模型，且所述位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性取值；
基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值。
本发明实施例，第二方面，提供一种定位信息融合装置，所述装置与多个对同一目标对象进行定位得到所述目标对象的位姿观测值的定位模块通信连接，所述装置包括：
概率模型构建单元，用于构建概率模型，其中，所述概率模型包括定位模块的位姿观测模型、定位模块的可靠性迁移模型和可靠性先验模型、位姿迁移模型，且所述位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性取值；
定位融合单元，用于基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和从各定位模块接收到的本次位姿观测值预估所述目标对象的本次位姿的真实值。
本发明实施例，第三方面，提供一种计算机服务器，所述计算机服务器包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现第一方面提供的定位信息融合方法。
本发明技术方案，首先，在构建概率模型时不仅仅构建有位姿观测模型、位姿迁移模型，还构建有定位模块的可靠性迁移模型和可靠性先验模型，且在位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性值；其次，基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值。采用本发明技术方案，能够自动检测各个定位模块的可靠性，在定位信息融合时综合考虑各定位模块的可靠性，从而可以降低不可靠的定位模块的影响，能够有效的提高定位融合的准确性和鲁棒性。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。
图1为本发明实施例中定位信息融合方法的流程图；
图2为本发明实施例中概率图模型的结构示意图；
图3为本发明实施例中将位姿真实值用粒子表示的示意图；
图4为本发明实施例中步骤102的具体实现的流程图；
图5为本发明实施例中将位姿真实值和可靠性取值分别用粒子表示的示意图；
图6为本发明实施例中对前次位姿粒子进行抽样后得到本次位姿粒子的示意图；
图7为本发明实施例中定位信息融合装置的结构示意图之一；
图8为本发明实施例中定位信息融合装置的结构示意图之二；
图9为本发明实施例中计算机服务器的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
本发明实施例提供的定位信息融合方法可适用于任何通过多个定位模块对同一目标对象进行定位的应用场景，例如可以是通过多个定位模块对车辆、机器人等进行定位的场景，本申请对应用场景不做严格限定。
本发明实施例中，目标对象的定位结果用“位姿”表示，“位姿”可以包括目标对象的位置和朝向，位置包括经纬度坐标、高度等信息，朝向可以包括俯仰角(pitch)、翻滚角(roll)、偏航角(yaw)等自由度。
实施例一
参见图1，为本发明实施例提供的一种定位信息融合方法的流程图，包含多个对同一目标对象进行定位得到所述目标对象的位姿观测值的定位模块，该方法包括：
步骤101、构建概率模型；
步骤102、基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值。
本发明实施例中，所述概率模型可包括以下几种模型：定位模块的位姿观测模型、定位模块的可靠性迁移模型和可靠性先验模型、位姿迁移模型，且所述位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性取值。
本发明实施例中，可以将概率模型中的各个变量表示如下：
z：表示位姿观测值；
x：表示位姿真实值；
r：表示定位模块的可靠性的指标，取值范围介于0和正无穷大之间，取值越小代表可靠性越高；
t：表示t时刻；
a：表示用户设置的可靠性先验值。
假设定位模块的个数为L个，采样时刻记为0、1、2、…、t，在每一个采样时刻，每个定位模块输出一个位姿观测值。在采样时刻t，位姿观测向量zt、可靠性向量rt、可靠性先验值a均包含L个分量，即表示为：
Figure BDA0002328573050000041
Figure BDA0002328573050000042
a＝{a1、…，aL}
优选地，本发明实施例中，为了更加直观、高效的表达变量之间的关系，前述步骤101中构建的概率模型可以为如图2所示的概率图模型，在该概率图模型中，方框内的参数为已知取值的参数，圆形框内的参数为待估计参数，即x1…,xt和可靠性r1,…,rt为隐变量，该概率图模型对应的联合概率分布如下式(1)：
Figure BDA0002328573050000051
其中，p(r|a)代表给定a的前提下r的概率密度函数；函数p(·)的具体形式由随机变量a服从的分布决定，当图2所示的概率图模型给定后函数p(·)的具体形式即可确定，Π为连乘符号。
优选地，在一个具体实施例中，所述定位模块的位姿观测模型为：给定xt和
Figure BDA0002328573050000052
时
Figure BDA0002328573050000053
出现的概率，将位姿观测模型定义为如下式(2)：
Figure BDA0002328573050000054
距离函数dist表征了两个向量之间的差异，作为一个实施例，dist函数可以定义为如下式(3)：
dist(z，x)＝[(zx-xx)2+(zy-xy)2+λ(zyaw-xyaw)2]1/2 式(3)
式(3)中，下标x、y、yaw分别代表向量在x方向、y方向、航向角的分量；
Figure BDA0002328573050000055
代表正态分布；参数λ控制了位置误差和角度误差的相对权重。
本发明实施例中，假设不同的定位模块之间的位姿观测模型是独立的，用以下式(4)表示为：
Figure BDA0002328573050000056
本发明实施例中，位姿迁移模型可以理解为在给定xt-1(前次位姿真实值)时xt(本次位姿真实值)出现的概率。本发明实施例可提供两种位姿的状态转移模型：一种状态转移模型为，如果从t-1时刻到t时刻的位移观测值Δxt已知，则位姿迁移模型可以定义为如下式(5)；另一种状态转移模型为，如果从t-1时刻到t时刻的位移观测值Δxt未知，则位姿迁移模型可以定义为如下式(6)，在该实施例中xt的取值符合正态分布。当然，本发明实施例中xt的取值也可以不符合正态分布，可以是符合类似于正态分布的分布，例如拉普拉斯分布等，本申请不作严格限定。本发明技术方案以xt的取值符合正态分布为例进行描述。
Figure BDA0002328573050000057
式(5)中，
Figure BDA0002328573050000061
为正态分布；xt-1+Δxt为正态分布的均值，∑为正态分布的方差。
Figure BDA0002328573050000062
式(6)中，
Figure BDA0002328573050000069
为正态分布；xt-1为正态分布的均值，∑为正态分布的方差。
在一个示例中，可以根据位姿中的x方向、y方向和偏航角三个自由度确定方差∑，即∑是3×3的矩阵，例如将∑设置为对角阵，形式如下式(7)：
Figure BDA0002328573050000063
式(7)中，
Figure BDA0002328573050000064
为用户设置的取值。
优选地，本发明实施例中，假设各定位模块的可靠性独立，则可靠性迁移模型表示给定rt-1时出现rt的概率，可以定义为如下式(8)：
Figure BDA0002328573050000065
式(8)中，各定位模块的可靠性取值可服从伽马分布或者类似于伽马分布的其他分布(例如卡方分布、对数正态分布等)，以下以各定位模块的可靠性取值服从伽马分布为例，如下式(9)所示：
Figure BDA0002328573050000066
式(9)中，k为用户设置的参数。
本发明实施例中，假定各定位模块的可靠性先验独立，用下式(10)表示为：
Figure BDA0002328573050000067
对于定位模块的可靠性先验值可以服从指数分布或者类似于指数分布的其他分布(例如Weibull分布)，以下以定位模块的可靠性先验值服从指数分布为例进行描述，用下式(11)表示：
Figure BDA0002328573050000068
优选地，本发明实施例为便于计算，可以基于粒子滤波器的框架采用N个带权重的位姿粒子表示位姿真实值，每个位姿粒子的权重依赖于与该位姿粒子对应的L个可靠性取值，该L个可靠性取值为L个定位模块的可靠性取值，如图3所示，假设t-1时刻的位姿真实值用7个带有权重的位姿粒子表示，每个位姿粒子对应L个可靠性取值。前述步骤102具体可通过但不仅限于以下步骤102A～步骤102C实现，如图4所示，其中：
步骤102A、根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，得到N个本次位姿粒子；
步骤102B、针对每个本次位姿粒子，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值的权重进行调整，得到与所述本次位姿粒子对应的L个可靠性取值；
步骤102C、针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重。
优选地，在一个示例1中，可靠性取值为一个带有可靠性数值和权重的参数，在该示例1中，所述可靠性迁移模型可以理解为：以前次可靠性取值为基准给予一个第一噪声，得到与该前次可靠性取值对应的新的可靠性取值。在所述示例1中，前述步骤102B中，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值(将前次位姿粒子对应的可靠性取值称为前次可靠性取值)进行调整，具体实现可如下：根据所述可靠性迁移模型分别对相应的前次位姿粒子的L个可靠性取值进行处理，得到L个对应的新的可靠性取值，将该L个新的可靠性取值确定为本次位姿粒子对应的L个可靠性取值。前述步骤102C中，具体实现如下：针对每个本次位姿粒子，将所述本次位姿粒子对应的L个可靠性取值的权重的乘积作为本次位姿粒子的权重。
优选地，在另一个示例2中，每个可靠性取值采用M个带权重的可靠性粒子表示，即每个位姿粒子对应L组可靠性粒子，如图5所示，t-1时刻的位姿真实值用7个带权重的位姿粒子表示，每个位姿粒子对应L组可靠性粒子，每组可靠性粒子包含M个带权重的可靠性粒子。在示例2中，前述步骤102B具体可通过步骤102B1～步骤102B2实现，其中：步骤102B1、针对每个本次位姿粒子，根据所述可靠性迁移模型分别对与所述本次位姿粒子对应的前次位姿粒子的L组可靠性粒子进行重采样，得到所述本次位姿粒子对应的L组可靠性粒子；步骤102B2、针对每个本次位姿粒子对应每组可靠性粒子中的每个可靠性粒子，根据所述可靠性粒子的先验概率与给定该可靠性粒子取值时的相应定位模块的位姿观测值的最大似然概率的乘积作为所述可靠性粒子的权重，如下式(12)：
Figure BDA0002328573050000081
式(12)中，
Figure BDA0002328573050000082
为t时刻第l个定位模块中与第n个位姿粒子对应的一组可靠性粒子中的第m个可靠性粒子，xt，(n)为t时刻第n个位姿粒子，
Figure BDA0002328573050000083
为t时刻第l个定位模块输出的位姿观测值，
Figure BDA0002328573050000084
为
Figure BDA0002328573050000085
的权重，
Figure BDA0002328573050000086
为给定
Figure BDA0002328573050000087
和xt，(n)时位姿观测值为
Figure BDA0002328573050000088
出现的最大似然概率，
Figure BDA0002328573050000089
为
Figure BDA00023285730500000810
的可靠性先验值。
前述步骤102C具体实现可如下：将本次位姿粒子对应的每组可靠性粒子的权重的和值作为一个组权重，将L组权重的乘积作为本次位姿粒子的权重，如下式(13)：
Figure BDA00023285730500000811
式(13)中，ωt，(n)为t时刻第n个位姿粒子的权重，
Figure BDA00023285730500000812
为第l个定位模块中与第n个位姿粒子对应的一组可靠性粒子的组权重。
在前述示例2中，所述可靠性迁移模型可以理解为：以抽样得到的可靠性粒子的取值为基准给予一个第一噪声，得到与该可靠性粒子对应的新的可靠性粒子；
前述步骤102B1中根据所述可靠性迁移模型分别对所述前次位姿粒子对应的L组可靠性粒子进行重采样，具体包括：
针对所述前次位姿粒子对应的每组可靠性粒子，执行以下步骤：
步骤a1、分别对该组可靠性粒子进行M次抽样，可靠性粒子权重越大被抽中的概率越高；
步骤a2、对每次抽样得到的可靠性粒子，根据所述可靠性迁移模型对所述可靠性粒子进行处理得到与该可靠性粒子对应的新的可靠性粒子，将M个新的可靠性粒子作为与前次位姿粒子对应的本次位姿粒子的一组可靠性粒子。本发明实施例中，可以为该M个新的可靠性粒子赋予相同的初始值，例如均为1或者均为1/M，还可以不为该M个新的可靠性粒子赋予权重。
优选地，本发明实施例中，可靠性粒子的第一噪声的取值符合伽马分布，当然可靠性粒子的第一噪声的取值还可以是符合类似于伽马分布的其他分布，例如卡方分布、对数正态分布等，本申请不作严格限定。
优选地，在前述各个实施例中，本发明实施例中，位姿迁移模型可以具体为：以前次位姿粒子的位姿为基准按照本次位移观测值进行移动得到新位姿，并给该新位姿一个第二噪声得到与该前次位姿粒子对应的本次位姿粒子。前述步骤102A中，根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，具体包括：分别对前次N个位姿粒子进行N次抽样，位姿粒子权重越大被抽中的概率越高；对每次抽样得到的前次位姿粒子，根据所述位姿迁移模型对所述前次位姿粒子进行处理得到与该前次位姿粒子对应的本次位姿粒子。如图6所示为对前述图3所示的前次N个位姿粒子进行抽样后得到的本次位姿粒子，该本次位姿粒子的初始权重相同。
优选地，本发明实施例中，第二噪声的取值服从正态分布，所述正态分布的均值为前次位姿粒子的位姿与本次位移观测值之和，所述正态分布的方差为预置的取值。
实施例二
基于前述实施例一提供的一种定位信息融合方法的相同发明构思，本发明实施例还提供一种定位信息融合装置，所述装置与多个对同一目标对象进行定位得到所述目标对象的位姿观测值的定位模块通信连接，该装置的结构如图7所示，包括：
概率模型构建单元1，用于构建概率模型，其中，所述概率模型包括定位模块的位姿观测模型、定位模块的可靠性迁移模型和可靠性先验模型、位姿迁移模型，且所述位姿观测模型中各定位模块的本次位姿观测值依赖于所述目标对象的本次位姿真实值和各定位模块的本次可靠性取值；
定位融合单元2，用于基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位姿位移观测值和从各定位模块接收到的本次位姿观测值预估所述目标对象的本次位姿的真实值。
优选地，本发明实施例中，可以基于粒子滤波器的框架采用N个带权重的位姿粒子表示位姿真实值，每个位姿粒子的权重依赖于与该位姿粒子对应的L个可靠性取值，该L个可靠性取值为L个定位模块的可靠性取值；所述定位融合单元2，具体包括可包括位姿粒子采样模块21、可靠性取值确定模块22和位姿粒子权重确定模块23，如图8所示，其中：
位姿粒子采样模块21，用于根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，得到N个本次位姿粒子；
可靠性取值确定模块22，用于针对每个本次位姿粒子，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值的权重进行调整，得到与所述本次位姿粒子对应的L个可靠性取值；
位姿粒子权重确定模块23，用于针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重。
在一个示例1中，所述位姿粒子权重确定模块23，具体用于：针对每个本次位姿粒子，将所述本次位姿粒子对应的L个可靠性取值的权重的乘积作为本次位姿粒子的权重。
在一个示例2中，每个可靠性取值采用M个带权重的可靠性粒子表示，每个位姿粒子对应L组可靠性粒子；所述可靠性取值确定模块22具体用于：
针对每个本次位姿粒子，根据所述可靠性迁移模型分别对与所述本次位姿粒子对应的前次位姿粒子的L组可靠性粒子进行重采样，得到所述本次位姿粒子对应的L组可靠性粒子；以及，针对每个本次位姿粒子对应每组可靠性粒子中的每个可靠性粒子，将所述可靠性粒子的先验概率与给定该可靠性粒子取值时的相应定位模块的位姿观测值的最大似然概率的乘积作为所述可靠性粒子的权重；所述位姿粒子权重确定模块23，具体用于：将本次位姿粒子对应的每组可靠性粒子的权重的和值作为一个组权重，将L组权重的乘积作为本次位姿粒子的权重。具体可参见实施例一中的示例2，在此不再赘述。
优选地，在示例2中，所述可靠性迁移模型为：以抽样得到的可靠性粒子的取值为基准给予一个第一噪声，得到与该可靠性粒子对应的新的可靠性粒子；
可靠性取值确定模块22根据所述可靠性迁移模型分别对所述前次位姿粒子对应的L组可靠性粒子进行重采样，具体包括：针对所述前次位姿粒子对应的每组可靠性粒子，执行以下步骤：分别对该组可靠性粒子进行M次抽样，可靠性粒子权重越大被抽中的概率越高；对每次抽样得到的可靠性粒子，根据所述可靠性迁移模型对所述可靠性粒子进行处理得到与该可靠性粒子对应的新的可靠性粒子，将M个新的可靠性粒子作为与前次位姿粒子对应的本次位姿粒子的一组可靠性粒子。
优选地，可靠性粒子的第一噪声的取值符合伽马分布，或者，可靠性粒子的第一噪声的取值符合类似于伽马分布的其他分布，例如卡方分布、对数正态分布。
优选地，在前述实施例中，所述位姿迁移模型为：以前次位姿粒子的位姿为基准按照本次位移观测值进行移动得到新位姿，并给该新位姿一个第二噪声得到与该前次位姿粒子对应的本次位姿粒子；
所述位姿粒子采样模块21具体用于：分别对前次N个位姿粒子进行N次抽样，位姿粒子权重越大被抽中的概率越高；对每次抽样得到的前次位姿粒子，根据所述位姿迁移模型对所述前次位姿粒子进行处理得到与该前次位姿粒子对应的本次位姿粒子。
优选地，所述第二噪声的取值服从正态分布，所述正态分布的均值为前次位姿粒子的位姿与本次位移观测值之和，所述正态分布的方差为预置的取值。
优选地，本发明实施例中，所述概率模型为概率图模型。
实施例三
本发明实施例三提供一种计算机服务器，该计算机服务器的结构如图9所示，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现实施例一中任意一种实施例的定位信息融合方法。
以上结合具体实施例描述了本发明的基本原理，但是，需要指出的是，对本领域普通技术人员而言，能够理解本发明的方法和装置的全部或者任何步骤或者部件可以在任何计算装置(包括处理器、存储介质等)或者计算装置的网络中，以硬件固件、软件或者他们的组合加以实现，这是本领域普通技术人员在阅读了本发明的说明的情况下运用它们的基本编程技能就能实现的。
本领域普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本发明的上述实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括上述实施例以及落入本发明范围的所有变更和修改。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (19)

1.一种定位信息融合方法，其特征在于，多个定位模块分别对同一目标对象进行定位得到所述目标对象的位姿观测值，所述方法包括：
构建概率模型，其中所述概率模型包括位姿观测模型、位姿迁移模型、可靠性迁移模型及可靠性先验模型，且各定位模块的本次位姿观测值来自于所述目标对象的本次位姿真实值和所述定位模块的本次可靠性取值；
基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值。
2.根据权利要求1所述的方法，其特征在于，基于粒子滤波器的框架，采用N个带权重的位姿粒子表示位姿真实值，每个位姿粒子的权重来自于与该位姿粒子对应的L个可靠性取值，该L个可靠性取值为L个定位模块的可靠性取值；
基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位移观测值和各定位模块输出的本次位姿观测值预估所述目标对象的本次位姿的真实值，具体包括：
根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，得到N个本次位姿粒子；
针对每个本次位姿粒子，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值进行调整，得到与所述本次位姿粒子对应的L个可靠性取值；
针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重。
3.根据权利要求2所述的方法，其特征在于，针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重，具体包括：
针对每个本次位姿粒子，将所述本次位姿粒子对应的L个可靠性取值的权重的乘积作为本次位姿粒子的权重。
4.根据权利要求2所述的方法，其特征在于，每个可靠性取值采用M个带权重的可靠性粒子表示，每个位姿粒子对应L组可靠性粒子；
针对每个本次位姿粒子，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值进行调整，得到与所述本次位姿粒子对应的L个可靠性取值，具体包括：针对每个本次位姿粒子，根据所述可靠性迁移模型分别对与所述本次位姿粒子对应的前次位姿粒子的L组可靠性粒子进行重采样，得到所述本次位姿粒子对应的L组可靠性粒子；以及，针对每个本次位姿粒子对应的每组可靠性粒子中的每个可靠性粒子，将所述可靠性粒子的先验概率与给定该可靠性粒子取值时的相应定位模块的位姿观测值的最大似然概率的乘积作为所述可靠性粒子的权重；
针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重，具体包括：将本次位姿粒子对应的每组可靠性粒子的权重的和值作为一个组权重，将L组权重的乘积作为本次位姿粒子的权重。
5.根据权利要求4所述的方法，其特征在于，所述可靠性迁移模型为：以抽样得到的可靠性粒子的取值为基准给予一个第一噪声，得到与该可靠性粒子对应的新的可靠性粒子；
根据所述可靠性迁移模型分别对所述前次位姿粒子对应的L组可靠性粒子进行重采样，具体包括：
针对所述前次位姿粒子对应的每组可靠性粒子，执行以下步骤：
分别对该组可靠性粒子进行M次抽样，可靠性粒子权重越大被抽中的概率越高；
对每次抽样得到的可靠性粒子，根据所述可靠性迁移模型对所述可靠性粒子进行处理得到与该可靠性粒子对应的新的可靠性粒子，将M个新的可靠性粒子作为与前次位姿粒子对应的本次位姿粒子的一组可靠性粒子。
6.根据权利要求5所述的方法，其特征在于，可靠性粒子的第一噪声的取值符合伽马分布。
7.根据权利要求2所述的方法，其特征在于，所述位姿迁移模型为：以前次位姿粒子的位姿为基准按照本次位移观测值进行移动得到新位姿，并给该新位姿一个第二噪声得到与该前次位姿粒子对应的本次位姿粒子；
根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，具体包括：
分别对前次N个位姿粒子进行N次抽样，位姿粒子权重越大被抽中的概率越高；
对每次抽样得到的前次位姿粒子，根据所述位姿迁移模型对所述前次位姿粒子进行处理得到与该前次位姿粒子对应的本次位姿粒子。
8.根据权利要求7所述的方法，其特征在于，所述第二噪声的取值服从正态分布，所述正态分布的均值为前次位姿粒子的位姿与本次位移观测值之和，所述正态分布的方差为预置的取值。
9.根据权利要求1所述的方法，其特征在于，所述概率模型为概率图模型。
10.一种定位信息融合装置，其特征在于，所述装置与多个定位模块通信连接，所述多个定位模块对同一目标对象进行定位得到所述目标对象的位姿观测值，所述装置包括：
概率模型构建单元，用于构建概率模型，其中所述概率模型包括位姿观测模型、位姿迁移模型、可靠性迁移模型及可靠性先验模型，且各定位模块的本次位姿观测值来自于所述目标对象的本次位姿真实值和所述定位模块的本次可靠性取值；
定位融合单元，用于基于粒子滤波器的递归贝叶斯方法，根据所述概率模型、本次位移观测值和从各定位模块接收到的本次位姿观测值预估所述目标对象的本次位姿的真实值。
11.根据权利要求10所述的装置，其特征在于，基于粒子滤波器的框架，采用N个带权重的位姿粒子表示位姿真实值，每个位姿粒子的权重来自于与该位姿粒子对应的L个可靠性取值，该L个可靠性取值为L个定位模块的可靠性取值；
所述定位融合单元，具体包括：
位姿粒子采样模块，用于根据本次位移观测值、所述位姿迁移模型对前次N个位姿粒子进行重采样，得到N个本次位姿粒子；
可靠性取值确定模块，用于针对每个本次位姿粒子，根据所述可靠性迁移模型对与所述本次位姿粒子对应的前次位姿粒子的L个可靠性取值的权重进行调整，得到与所述本次位姿粒子对应的L个可靠性取值；
位姿粒子权重确定模块，用于针对每个本次位姿粒子，根据所述本次位姿粒子对应的L个可靠性取值的权重得到本次位姿粒子的权重。
12.根据权利要求11所述的装置，其特征在于，所述位姿粒子权重确定模块，具体用于：
针对每个本次位姿粒子，将所述本次位姿粒子对应的L个可靠性取值的权重的乘积作为本次位姿粒子的权重。
13.根据权利要求11所述的装置，其特征在于，每个可靠性取值采用M个带权重的可靠性粒子表示，每个位姿粒子对应L组可靠性粒子；所述可靠性取值确定模块具体用于：
针对每个本次位姿粒子，根据所述可靠性迁移模型分别对与所述本次位姿粒子对应的前次位姿粒子的L组可靠性粒子进行重采样，得到所述本次位姿粒子对应的L组可靠性粒子；以及，针对每个本次位姿粒子对应的每组可靠性粒子中的每个可靠性粒子，将所述可靠性粒子的先验概率与给定该可靠性粒子取值时的相应定位模块的位姿观测值的最大似然概率的乘积作为所述可靠性粒子的权重；
所述位姿粒子权重确定模块，具体用于：将本次位姿粒子对应的每组可靠性粒子的权重的和值作为一个组权重，将L组权重的乘积作为本次位姿粒子的权重。
14.根据权利要求13所述的装置，其特征在于，所述可靠性迁移模型为：以抽样得到的可靠性粒子的取值为基准给予一个第一噪声，得到与该可靠性粒子对应的新的可靠性粒子；
可靠性取值确定模块根据所述可靠性迁移模型分别对所述前次位姿粒子对应的L组可靠性粒子进行重采样，具体包括：
针对所述前次位姿粒子对应的每组可靠性粒子，执行以下步骤：
分别对该组可靠性粒子进行M次抽样，可靠性粒子权重越大被抽中的概率越高；
对每次抽样得到的可靠性粒子，根据所述可靠性迁移模型对所述可靠性粒子进行处理得到与该可靠性粒子对应的新的可靠性粒子，将M个新的可靠性粒子作为与前次位姿粒子对应的本次位姿粒子的一组可靠性粒子。
15.根据权利要求14所述的装置，其特征在于，可靠性粒子的第一噪声的取值符合伽马分布。
16.根据权利要求11所述的装置，其特征在于，所述位姿迁移模型为：以前次位姿粒子的位姿为基准按照本次位移观测值进行移动得到新位姿，并给该新位姿一个第二噪声得到与该前次位姿粒子对应的本次位姿粒子；
所述位姿粒子采样模块具体用于：
分别对前次N个位姿粒子进行N次抽样，位姿粒子权重越大被抽中的概率越高；
对每次抽样得到的前次位姿粒子，根据所述位姿迁移模型对所述前次位姿粒子进行处理得到与该前次位姿粒子对应的本次位姿粒子。
17.根据权利要求16所述的装置，其特征在于，所述第二噪声的取值服从正态分布，所述正态分布的均值为前次位姿粒子的位姿与本次位移观测值之和，所述正态分布的方差为预置的取值。
18.根据权利要求10所述的装置，其特征在于，所述概率模型为概率图模型。
19.一种计算机服务器，其特征在于，包括存储器，以及与所述存储器通信连接的一个或多个处理器；
所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如权利要求1～9中任一项所述的定位信息融合方法。
CN201911326703.2A 2019-12-20 2019-12-20 一种定位信息融合方法及其装置、计算机服务器 Active CN113008245B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911326703.2A CN113008245B (zh) 2019-12-20 2019-12-20 一种定位信息融合方法及其装置、计算机服务器

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911326703.2A CN113008245B (zh) 2019-12-20 2019-12-20 一种定位信息融合方法及其装置、计算机服务器

Publications (2)

Publication Number Publication Date
CN113008245A CN113008245A (zh) 2021-06-22
CN113008245B true CN113008245B (zh) 2022-12-27

Family

ID=76382790

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911326703.2A Active CN113008245B (zh) 2019-12-20 2019-12-20 一种定位信息融合方法及其装置、计算机服务器

Country Status (1)

Country Link
CN (1) CN113008245B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616021A (zh) * 2013-12-04 2014-03-05 苏州大学张家港工业技术研究院 一种全局定位方法及装置
CN105509755A (zh) * 2015-11-27 2016-04-20 重庆邮电大学 一种基于高斯分布的移动机器人同步定位与地图构建方法
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109186625A (zh) * 2018-10-24 2019-01-11 北京奥特贝睿科技有限公司 智能车辆利用混合采样滤波进行精确定位的方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10029577B2 (en) * 2016-06-15 2018-07-24 Qualcomm Incorporated Methods and apparatus for positioning a vehicle

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103616021A (zh) * 2013-12-04 2014-03-05 苏州大学张家港工业技术研究院 一种全局定位方法及装置
CN105509755A (zh) * 2015-11-27 2016-04-20 重庆邮电大学 一种基于高斯分布的移动机器人同步定位与地图构建方法
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109186625A (zh) * 2018-10-24 2019-01-11 北京奥特贝睿科技有限公司 智能车辆利用混合采样滤波进行精确定位的方法及系统

Also Published As

Publication number Publication date
CN113008245A (zh) 2021-06-22

Similar Documents

Publication Publication Date Title
KR102463176B1 (ko) 위치 추정 장치 및 방법
CN109373999B (zh) 基于故障容错卡尔曼滤波的组合导航方法
CN111102978B (zh) 一种车辆运动状态确定的方法、装置及电子设备
CN112964276B (zh) 一种基于激光和视觉融合的在线标定方法
US20190187297A1 (en) System and method for locating a moving object
CN111552757B (zh) 生成电子地图的方法、装置、设备及存储介质
CN112113582A (zh) 时间同步处理方法、电子设备及存储介质
CN109059907A (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN111338383A (zh) 基于gaas的自主飞行方法及系统、存储介质
CN113899363A (zh) 车辆的定位方法、装置及自动驾驶车辆
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN113551666A (zh) 自动驾驶多传感器融合定位方法和装置、设备及介质
JP2022513511A (ja) 完全性範囲を特定するための方法
WO2023142353A1 (zh) 一种位姿预测方法及装置
CN115164936A (zh) 高精地图制作中用于点云拼接的全局位姿修正方法及设备
CN114943952A (zh) 多相机重叠视域下障碍物融合方法、系统、设备和介质
CN114264301A (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN113135302B (zh) 一种与机动非合作目标交会对接的方法
CN110832274A (zh) 地面坡度计算方法、装置、设备及存储介质
CN110794434B (zh) 一种位姿的确定方法、装置、设备及存储介质
CN113008245B (zh) 一种定位信息融合方法及其装置、计算机服务器
CN116958452A (zh) 三维重建方法和系统
CN115561731B (zh) 位姿优化方法、点云地图建立方法、计算机设备及介质
CN113503883B (zh) 采集用于构建地图的数据的方法、存储介质及电子设备

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