CN117928556A - 一种车辆定位估算方法、系统、存储介质及车辆 - Google Patents

一种车辆定位估算方法、系统、存储介质及车辆 Download PDF

Info

Publication number
CN117928556A
CN117928556A CN202410107368.1A CN202410107368A CN117928556A CN 117928556 A CN117928556 A CN 117928556A CN 202410107368 A CN202410107368 A CN 202410107368A CN 117928556 A CN117928556 A CN 117928556A
Authority
CN
China
Prior art keywords
pose
vehicle
point cloud
data
estimation
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.)
Pending
Application number
CN202410107368.1A
Other languages
English (en)
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.)
Guangzhou Automobile Group Co Ltd
Original Assignee
Guangzhou Automobile Group 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 Guangzhou Automobile Group Co Ltd filed Critical Guangzhou Automobile Group Co Ltd
Priority to CN202410107368.1A priority Critical patent/CN117928556A/zh
Publication of CN117928556A publication Critical patent/CN117928556A/zh
Pending legal-status Critical Current

Links

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/20—Instruments for performing navigational calculations
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data

Landscapes

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

Abstract

本发明公开了一种车辆定位估算方法，其至少包括步骤：基于粒子滤波算法获得满足切换条件的初始位姿；基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。本发明还公开了相应的系统、存储介质及车辆。实施本发明，在进行定位估算时，可以摆脱对车辆初始化位姿第三源输入依赖，提高车辆在复杂非线性场景下的定位精确性和稳定性。

Description

一种车辆定位估算方法、系统、存储介质及车辆
技术领域
本发明涉及车辆的汽车智驾技术领域，特别是涉及一种车辆定位估算方法、系统、存储介质及车辆。
背景技术
随着智驾向不同复杂多变场景下行泊功能的技术深入，对于车辆自身定位的准确性和鲁棒性提出了更要的要求。
完成车辆的精准定位根据搭载不同的传感器分为不同的技术路线。其中一种技术路线是采用轮式里程计和激光雷达的传感器融合方案。在现有技术中，该技术路线的融合方案大多只采用贝叶斯滤波框架下的卡尔曼滤波及其衍生(无迹卡尔曼滤波、拓展卡尔曼滤波)作为融合器，同时采用点云迭代最近点算法(Iterative Closet Point，ICP)或正态分布变换算法(Normal Distributions Transform，NDT)来实现定位。然而该类型算法存在着诸如对初始化位置需求强、复杂场景下准确性和鲁棒性低的局限性。具体地，车辆的初始位姿(位置和航向角)需要第三源的输入，使得传感器方案不能实现自身闭环。现有方案的精度对传感器信息的丰富性有较强依赖。例如，在仅依靠轮式里程计和激光雷达的方案中，只有在有丰富点云特征的特殊场景下才能维持其精准性，在一般的尤其是诸如平滑长廊隧道等缺乏点云特征的环境下，定位结果的稳定性和准确性会有较大的影响。
另外，在现有技术中，对于激光点云的匹配大多只是利用单一特征作为ICP算法的输入，其特征所占的权重一般被设置为1，或者设置成只与距离呈简单的线性函数。但在实际应用场景中，特别是高度非线性化的车辆行驶或停泊环境下，当前的技术存在精度较低，鲁棒性弱等不足之处。
发明内容
本发明所要解决的技术问题在于，提供一种车辆定位估算方法、系统、存储介质及车辆，在进行定位估算时，可以摆脱对车辆初始化位姿第三源输入依赖，提高车辆在复杂非线性场景下的定位精确性和稳定性。
为解决上述技术问题，作为本发明的一方面，提供一种车辆定位估算方法，其至少包括如下步骤：
基于粒子滤波算法获得满足切换条件的初始位姿；
基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
其中，基于粒子滤波算法获得满足切换条件的初始位姿，包括：
持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
其中，基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据，包括：
提取当前激光点云数据中的激光点云密度、点云方向特征；
根据匹配范围过滤点云数量；
根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图匹配范围中的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重，所述点云地图匹配范围由轮式里程计数据确定；
采用预先建立的误差函数，利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
将所述转换矩阵左乘以当前时刻的车辆初始位姿的向量作为本次位姿状态估算的最新的估算位姿数据，并输出。
其中，所述权重函数为：
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
其中，所述计算本次位姿状态估算的方差，确定本次位姿是否可信，包括：
计算本次位姿估算数据的方差；
将所述方差与一方差阈值进行比较；
如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信。
作为本发明的另一方面，还提供一种车辆定位估算系统，至少包括：
粒子滤波处理单元，用于基于粒子滤波算法获得满足切换条件的初始位姿；
无迹卡尔曼滤波处理单元，用于基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
滤波切换处理单元，用于计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
其中，所述粒子滤波处理单元包括：
持续处理单元，用于持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
特定初始位姿确定单元，用于将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
其中，所述无迹卡尔曼滤波处理单元包括：
多维特征提取单元，用于提取当前激光点云数据中的激光点云密度、点云方向特征；
过滤处理单元，用于根据匹配范围过滤点云数量；
匹配赋权处理单元，用于根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图匹配范围中的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重，所述点云地图匹配范围由轮式里程计数据确定；
迭代求解单元，用于采用预先的建立误差函数,利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
收敛结束检测单元，用于检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
估算单元，用于将所述转换矩阵左乘以当前时刻的车辆初始位姿的向量作为本位姿状态估算的最新的估算位姿数据，并输出。
其中，所述权重函数为：
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
其中，所述滤波切换处理单元包括：
可信度判断单元，用于计算计算本次位姿估算数据的方差，将所述方差与一方差阈值进行比较，如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信；
切换单元，用于在判断结果为可信时，继续采用无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；在判断结果为不可信时，则切换至所述粒子滤波算法，并更新所述车辆的初始位姿。
相应地，本发明的再一方面，还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如前述的方法的步骤。
相应地，本发明的再一方面，还提供一种芯片，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如前述方法的步骤。
相应地，本发明的再一方面，还提供一种车辆，其部署有如前述的系统。
相应地，本发明的再一方面，还提供一种计算机程序产品，包括计算机指令，所述计算机指令指示计算机设备执行如前述方法对应的操作。
实施本发明实施例，具有如下有的益效果：
本发明提供一种车辆定位估算方法、系统、存储介质及车辆，通过在轮式传感器和激光雷达融合定位方案中，利用粒子滤波(PF)算法和无迹卡尔曼滤波(UKF)算两种滤波方法，摆脱对车辆初始化位姿第三源输入依赖，从而降低了对传感器多样性和复杂性的需求，并提高车辆在复杂非线性场景下的定位精确性和稳定性；
在本发明的实施例中，在采用无迹卡尔曼滤波进行定位估计的过程中，车辆出现特殊状态，诸如受第三方不可控外力被移动、环境点云特征极其单一、传感器突发故障一段时间等，可快速切换回粒子滤波处理过程，能够快速重新恢复自身定位；
在进行无迹卡尔曼滤波处理时，利用多特征(最近点距离、点云密度、点云方向)匹配，且给每个匹配对赋予基于稳健性回归分析M-Estimator的权重值，可以大大抑制离群点、噪声点对位姿估算的影响，进而消除对丰富传感器信息源的需求，提升了激光点云与地图匹配的准确性，减少环境噪声对定位的影响，从而实现仅用轮式传感器和激光雷达就能完成车辆自身定位，且精确高和鲁棒性强。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术佩戴人员来讲，在不付出创造性劳动性的前提下，根据这些附图获得其他的附图仍属于本发明的范畴；
图1为本发明提供的一种车辆定位估算方法的一个实施例的主流程示意图；
图2是图1中步骤S10的更详细流程图；
图3是图1中步骤S11的更详细流程图；
图4是本发明所采用的一种权重函数的示意图；
图5是本发明提供的一种车辆定位估算系统的一个实施例的结构示意图；
图6是图5中粒子滤波处理单元的结构示意图；
图7为图5中无迹卡尔曼滤波处理单元的结构示意图；
图8为图5中滤波切换处理单元的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步地详细描述。
如图1所示，示出了本发明提供的一种车辆定位估算方法的一个实施例的主流程示意图。一并结合图2至图4所示，在本实施例中，所述车辆定位估算方法，其至少包括如下步骤：
步骤S10，根据车辆当前的点云地图，基于粒子滤波算法(PF)获得满足切换条件的初始位姿；
步骤S11，基于无迹卡尔曼滤波算法(UKF)融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
步骤S12，计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
可以理解的是，UKF算法运行所耗时间比PF较优，在环境非线性因素不极端的情况下，采用该滤波算法可以融合来自激光里程计(激光点云匹配地图估计的车辆位姿)和轮式里程计的数据进行车辆的自身位姿估计。
更具体地，如图2所示，在一个具体的例子中，所述步骤S10，基于粒子滤波算法获得满足切换条件的初始位姿，包括：
持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
在具体的例子中，包括如下的步骤：
步骤S100，在点云地图中随机离散地投放预定数量的粒子，每个粒子代表车辆可能的位置和姿态，初始时，所有粒子的权重都设为1；其中，位置是指粒子在坐标系中的空间坐标，而姿态则描述了粒子的朝向或方向，在车辆定位中，位姿估计会提供车辆的具体位置信息，如x、y坐标和航向角等，以及车辆的姿态信息，如横滚角、俯仰角和偏航角等。这些数据可以全面地描述车辆在世界坐标系中的准确位置和朝向。在本发明的实施例中，该粒子就代表车辆的可能位姿；
步骤S101，将激光传感器获取的环境信息与地图中的信息进行匹配，选定一定范围内的地图点云，计算每一粒子与所述地图点云的距离的平均值；
可以理解的是，在地图中选择一定范围时，可以考虑本车的位置，但并不一定需要与本车的位置完全相关。这个范围的选择主要是为了反映地图中与车辆位姿相关的信息，因此只要该范围能够涵盖与车辆位姿估计相关的地图信息。在一些例子中，可以根据车辆当前激光点云数据中的一个特征点(如路牌、红绿灯)来选定该范围；
更具体地，可以根据具体的场景和需求来确定。例如，如果场景中的地图点云分布比较密集，那么可以选择一个相对较小的范围，这样可以确保该范围内的地图点云包含了足够的车辆位姿相关信息。如果地图点云分布比较稀疏，那么可以选择一个相对较大的范围，这样可以确保即使地图信息不足，PF算法也能够得到较为准确的车辆位姿估计结果。
步骤S102，根据所述平均值计算并更新每个粒子的权重，其中，距离越近，权重越高；所述权重用来表示车辆在该位置出现的可能性。如果一个粒子(即车辆的位置)更接近这些特征，那么它的权重就会增加，意味着这个位置更可能是车辆的真实位置。反之，如果一个粒子离这些特征较远，那么它的权重就会降低；
在实际应用中，可以采用很多方式来根据平均值计算并更新每个粒子的权重。例如在一个例子中，可以使用高斯函数来计算粒子和地图点云之间的距离，那么权重计算公式可以是这样的：
权重＝exp(-(距离^2)/(2*σ^2))
其中，σ是高斯函数的宽度参数，它决定了权重衰减的快慢。如果σ比较大，那么权重衰减得比较慢，粒子的权重会比较大；如果σ比较小，那么权重衰减得比较快，粒子的权重会比较小。
步骤S103，根据粒子的权重进行重采样，保留权重较高的粒子，淘汰权重较低的粒子；
步骤S104，根据前述步骤进行递归迭代，当其中一单个粒子的权重大于预定的权重阈值时，将所述单个粒子的位姿作为满足切换条件的车辆的初始位姿。其中，权重阈值预先通过标定获得。
具体地，递归迭代前述步骤S101至步骤S104，收敛粒子数量，不断增加某单个粒子的概率权重。当该单个粒子的权重大于一个预定的权重阈值(W0)时，则认为达到较好的初始位置条件，退出粒子滤波算法的处理，将车辆位姿状态估计的任务交付无迹卡尔曼滤波(UKF)算法处理。
更具体地，如图3所示，在一个具体的例子中，所述步骤S11，基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据，包括：
步骤S110，提取当前激光点云数据中的激光点云密度、点云方向特征；
步骤S111，根据匹配范围过滤点云数量，所述匹配范围结合轮式里程计数据确定；可以理解的是，为了减少计算量并提高效率，本步骤是根据一定的匹配范围来过滤点云的数量，尽量保留与车辆位置相关的点云；例如，在一些例子中，如果根据轮式里程计数据确定当前点，并将匹配范围确定为当前点的10米范围之内，那么只有在这个范围内的点云会被保留，其他远离车辆的点云会被过滤掉。
步骤S112，根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重；
所述权重函数为：
其中，e为偏差，w为权重，k值为可调工程参数；
可以理解的是，本步骤根据点云的方向、密度和最近点的匹配来与地图上的点进行匹配，通过这些匹配可以帮助确定车辆在地图上的位置。例如，假设有一个点云中的点与地图上的一个建筑物非常接近，并且方向也一致，那么这两个点可以被认为是一个匹配对；
可以理解的是，M-Estimator是一种稳健性回归分析的一种模型，对异常值十分敏感的经典最小二乘回归目标函数进行修改，从而降低离群点和强偏离的影响。利用M-Estimator为每个匹配对赋予权重，可以降低噪声和奇异点的影响。上述权重函数仅为举例，还可以有其他的形式，也可以应用于本发明的方法中；
步骤S113，采用预先的建立误差函数,利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三大特征的偏差；
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
可以理解的是，误差函数引入点云的方向、密度、最近点距离三大特征，增加匹配因素和匹配的准确度。误差函数的具体表现可以参照图4所示，当偏差e大的时候，w(e)呈函数级降低，从而大大抑制噪声点对点云匹配的影响。
在本步骤中，利用梯度下降法来优化误差函数，以获得最小的误差值。这个最小值对应的转换矩阵可以用来表示车辆在地图上的位置。
步骤S114，检测所述转换矩阵的收敛程度,当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；其中，收敛阈值通过预先设定而获得。
步骤S115，将所述转换矩阵左乘以当前时刻的车辆位姿的向量作为本位姿状态估算的最新的估算位姿数据，并输出。本步骤用于检查算法是否已经收敛到一定的精度。如果转换矩阵的各项值已经收敛到一定的阈值，那么算法可以认为已经达到精度要求，不需要继续迭代。
在一个例子中，如果当前时刻的车辆位姿的向量为[x,y,θ]，转换矩阵为[0.8,0.2；0.2,0.8]，则新的估算位姿向量就是[0.8x+0.2y,0.2x+0.8y,θ]。
可以理解的是，在步骤S1中，通过结合激光点云匹配和无迹卡尔曼滤波(UKF)算法，提供了一种高效且准确的车辆位姿估计方法。通过提取和利用激光点云的密度、方向等特征，以及利用梯度下降法优化误差函数，该方案能够提高车辆位姿估计的精度和效率。
更具体地，在一个具体的例子中，所述步骤S12，所述计算本次位姿状态估算的方差，确定本次位姿是否可信，包括：
步骤S120，计算本次位姿估算数据的方差；可以理解的是，所述方差是通过预测阶段和更新阶段的协方差矩阵计算得到的，该方差代表了预测位置与真实位置之间的不确定性。
步骤S121，将所述方差与一方差阈值(如0.1米)进行比较；
步骤S122，如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则，如果比较结果为所述方差大于或等于所述方差阈值，说明当前环境的非线性因素可能比较极端，则认为本次位姿估算不可信。其中，方差阈值预先通过标定获得。
在结果可信时，说明无迹卡尔曼滤波(UKF)算法的预测比较可靠，则将本次位姿作为初始位姿，并继续使用无迹卡尔曼滤波(UKF)算法进行下一轮的位姿估计。
在结果不可信时，认为无迹卡尔曼滤波(UKF)算法的预测是不可靠的，则切换到粒子滤波(PF)算法处理，在当前估计位置的周围随机离散地投放权重粒子，然后重新开始粒子滤波(PF)算法的预测和更新过程。并在获得初始位姿后，切换进入无迹卡尔曼滤波(UKF)算法处理。
如图5所示，示出了本发明提供的一种车辆定位估算系统的一个实施例的结构示意图。一并结合图6至图8所示，在本实施例中，所述车辆定位估算系统至少包括：
粒子滤波处理单元10，用于基于粒子滤波算法获得满足切换条件的初始位姿；
无迹卡尔曼滤波处理单元11，用于基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
滤波切换处理单元12，用于计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
更具体地，如图6所示，所述粒子滤波处理单元10包括：
持续处理单元100，用于持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度。
具体地，在持续处理单元100中需要处理如下的工作：
在点云地图中随机离散地投放预定数量的粒子，每个粒子代表车辆可能的位置和姿态，所有粒子的权重都初始化为1；
将激光传感器获取的环境信息与地图中的信息进行匹配，选定一定范围内的地图点云，计算每一粒子与所述地图点云的距离的平均值；
根据所述平均值计算并更新每个粒子的权重，其中，距离越近，权重越高；
根据粒子的权重进行重采样，保留权重较高的粒子，淘汰权重较低的粒子；
根据前述步骤进行递归迭代处理。
特定初始位姿确定单元101，用于将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
更具体地，如图7所示，所述无迹卡尔曼滤波处理单元11包括：
多维特征提取单元110，用于提取当前激光点云数据中的激光点云密度、点云方向特征；
过滤处理单元111，用于根据匹配范围过滤点云数量，所述点云地图匹配范围由轮式里程计数据确定；
匹配赋权处理单元112，用于根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重；
迭代求解单元113，用于采用预先的建立误差函数,利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三大个特征的偏差；
收敛结束检测单元114，用于检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
估算单元115，用于将所述转换矩阵左乘以当前时刻的车辆位姿的位姿向量作为本位姿状态估算的最新的估算位姿数据，并输出。
其中，所述权重函数为：
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
更具体地，如图8所示，所述滤波切换处理单元12包括：
可信度判断单元120，用于计算计算本次位姿估算数据的方差，将所述方差与一方差阈值进行比较，如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信；
切换单元121，用于在判断结果为可信时，继续采用无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；在判断结果为不可信时，则切换至所述粒子滤波算法，并更新所述车辆的初始位姿。
更多细节，可以参考并结合前述对图1至图4的描述，在此不进行赘述。
相应地，本发明的再一方面，还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如前述图1至图4所描述的方法的步骤。更多细节，可以参考并结合前述对图1至图4的描述，在此不进行赘述。
相应地，本发明的再一方面，还提供一种芯片，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如前述图1至图4所描述的方法的步骤。更多细节，可以参考并结合前述对图1至图4的描述，在此不进行赘述。
相应地，本发明的再一方面，还提供一种车辆，其部署有如图5至图8所描述的系统。更多细节，可以参考并结合前述对图5至图8的描述，在此不进行赘述。
相应地，本发明的再一方面，还提供一种计算机程序产品，包括计算机指令，所述计算机指令指示计算机设备执行如前述图1至图4所描述的方法对应的操作。
实施本发明实施例，具有如下有的益效果：
本发明提供一种车辆定位估算方法、系统、存储介质及车辆，通过在轮式传感器和激光雷达融合定位方案中，利用粒子滤波(PF)算法和无迹卡尔曼滤波(UKF)算两种滤波方法，摆脱对车辆初始化位姿第三源输入依赖，从而降低了对传感器多样性和复杂性的需求，并提高车辆在复杂非线性场景下的定位精确性和稳定性；
在本发明的实施例中，在采用无迹卡尔曼滤波进行定位估计的过程中，车辆出现特殊状态，诸如受第三方不可控外力被移动、环境点云特征极其单一、传感器突发故障一段时间等，可快速切换回粒子滤波处理过程，能够快速重新恢复自身定位；
在进行无迹卡尔曼滤波处理时，利用多特征(最近点距离、点云密度、点云方向)匹配，且给每个匹配对赋予基于稳健性回归分析M-Estimator的权重值，可以大大抑制离群点、噪声点对位姿估算的影响，进而消除对丰富传感器信息源的需求，提升了激光点云与地图匹配的准确性，减少环境噪声对定位的影响，从而实现仅用轮式传感器和激光雷达就能完成车辆自身定位，且精确高和鲁棒性强。
本领域内的技术人员应明白，本发明的实施例可提供为方法、装置、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (14)

1.一种车辆定位估算方法，其特征在于，至少包括如下步骤：
基于粒子滤波算法获得满足切换条件的初始位姿；
基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
2.如权利要求1所述的方法，其特征在于，基于粒子滤波算法获得满足切换条件的初始位姿，包括：
持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
3.如权利要求2所述的方法，其特征在于，基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据，包括：
提取当前激光点云数据中的激光点云密度、点云方向特征；
根据匹配范围过滤点云数量，所述匹配范围结合轮式里程计数据确定；
根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重；
采用预先建立的误差函数，利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
将所述转换矩阵左乘以当前时刻的车辆位姿的向量作为本次位姿状态估算的最新的估算位姿数据，并输出。
4.如权利要求3所述的方法，其特征在于，其中：
所述权重函数为：
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
5.如权利要求4所述的方法，其特征在于，所述计算本次位姿状态估算的方差，确定本次位姿是否可信，包括：
计算本次位姿估算数据的方差；
将所述方差与一方差阈值进行比较；
如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信。
6.一种车辆定位估算系统，其特征在于，至少包括：
粒子滤波处理单元，用于基于粒子滤波算法获得满足切换条件的初始位姿；
无迹卡尔曼滤波处理单元，用于基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
滤波切换处理单元，用于计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
7.如权利要求6所述的系统，其特征在于，所述粒子滤波处理单元包括：
持续处理单元，用于持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
特定初始位姿确定单元，用于将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
8.如权利要求7所述的系统，其特征在于，所述无迹卡尔曼滤波处理单元包括：
多维特征提取单元，用于提取当前激光点云数据中的激光点云密度、点云方向特征；
过滤处理单元，用于根据匹配范围过滤点云数量，所述点云地图匹配范围由轮式里程计数据确定；
匹配赋权处理单元，用于根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图的点进行匹配，形成至少一匹配对，并利用基于M-Estimator的权重函数为每个匹配对赋予权重；
迭代求解单元，用于采用预先的建立误差函数,利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
收敛结束检测单元，用于检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
估算单元，用于将所述转换矩阵左乘以当前时刻的车辆位姿的向量作为本位姿状态估算的最新的估算位姿数据，并输出。
9.如权利要求8所述的系统，其特征在于，其中：
所述权重函数为：
所述误差函数为：
其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
10.如权利要求9所述的系统，其特征在于，所述滤波切换处理单元包括：
可信度判断单元，用于计算计算本次位姿估算数据的方差，将所述方差与一方差阈值进行比较，如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信；
切换单元，用于在判断结果为可信时，继续采用无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；在判断结果为不可信时，则切换至所述粒子滤波算法，并更新所述车辆的初始位姿。
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5中任一项所述的方法的步骤。
12.一种芯片，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。
13.一种车辆，其特征在于，其部署有如权利要求6至10任一项所述的系统。
14.一种计算机程序产品，包括计算机指令，所述计算机指令指示计算机设备执行如权利要求1至5任一项所述方法对应的操作。
CN202410107368.1A 2024-01-25 2024-01-25 一种车辆定位估算方法、系统、存储介质及车辆 Pending CN117928556A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410107368.1A CN117928556A (zh) 2024-01-25 2024-01-25 一种车辆定位估算方法、系统、存储介质及车辆

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410107368.1A CN117928556A (zh) 2024-01-25 2024-01-25 一种车辆定位估算方法、系统、存储介质及车辆

Publications (1)

Publication Number Publication Date
CN117928556A true CN117928556A (zh) 2024-04-26

Family

ID=90755346

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410107368.1A Pending CN117928556A (zh) 2024-01-25 2024-01-25 一种车辆定位估算方法、系统、存储介质及车辆

Country Status (1)

Country Link
CN (1) CN117928556A (zh)

Similar Documents

Publication Publication Date Title
CN112639502B (zh) 机器人位姿估计
KR102581263B1 (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
US11594011B2 (en) Deep learning-based feature extraction for LiDAR localization of autonomous driving vehicles
EP3714290B1 (en) Lidar localization using 3d cnn network for solution inference in autonomous driving vehicles
US11364931B2 (en) Lidar localization using RNN and LSTM for temporal smoothness in autonomous driving vehicles
WO2017060947A1 (ja) 推定装置、制御方法、プログラム及び記憶媒体
CN112880694B (zh) 确定车辆的位置的方法
AU2020356082B2 (en) Vehicle and method for generating map corresponding to three-dimensional space
CN114111774B (zh) 车辆的定位方法、系统、设备及计算机可读存储介质
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN113551666A (zh) 自动驾驶多传感器融合定位方法和装置、设备及介质
JP2017072423A (ja) 推定装置、制御方法、プログラム及び記憶媒体
CN114264301A (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN112767545A (zh) 一种点云地图构建方法、装置、设备及计算机存储介质
KR102103651B1 (ko) 지도의 차로 개수를 활용한 파티클 필터링 퇴화 경감 방법 및 시스템
KR102489865B1 (ko) 다중 필터 및 센서 퓨전 기반의 차량의 위치 추정 방법
CN112965076B (zh) 一种用于机器人的多雷达定位系统及方法
Chen et al. An optimal selection of sensors in multi-sensor fusion navigation with factor graph
CN105180955A (zh) 机动车实时精准定位方法及装置
CN110794434B (zh) 一种位姿的确定方法、装置、设备及存储介质
Dong et al. Indoor tracking using crowdsourced maps
CN117928556A (zh) 一种车辆定位估算方法、系统、存储介质及车辆
CN114817765A (zh) 基于地图的目标航向消歧
Zhu et al. Terrain‐inclination‐based Three‐dimensional Localization for Mobile Robots in Outdoor Environments
JP2020165945A (ja) 自己位置推定方法及び自己位置推定装置

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