本发明涉及车辆的汽车智驾，特别是涉及一种车辆定位估算方法、系统、存储介质及车辆。
背景技术：
1、随着智驾向不同复杂多变场景下行泊功能的技术深入，对于车辆自身定位的准确性和鲁棒性提出了更要的要求。
2、完成车辆的精准定位根据搭载不同的传感器分为不同的技术路线。其中一种技术路线是采用轮式里程计和激光雷达的传感器融合方案。在现有技术中，该技术路线的融合方案大多只采用贝叶斯滤波框架下的卡尔曼滤波及其衍生(无迹卡尔曼滤波、拓展卡尔曼滤波)作为融合器，同时采用点云迭代最近点算法(iterative closet point，icp)或正态分布变换算法(normal distributions transform，ndt)来实现定位。然而该类型算法存在着诸如对初始化位置需求强、复杂场景下准确性和鲁棒性低的局限性。具体地，车辆的初始位姿(位置和航向角)需要第三源的输入，使得传感器方案不能实现自身闭环。现有方案的精度对传感器信息的丰富性有较强依赖。例如，在仅依靠轮式里程计和激光雷达的方案中，只有在有丰富点云特征的特殊场景下才能维持其精准性，在一般的尤其是诸如平滑长廊隧道等缺乏点云特征的环境下，定位结果的稳定性和准确性会有较大的影响。
3、另外，在现有技术中，对于激光点云的匹配大多只是利用单一特征作为icp算法的输入，其特征所占的权重一般被设置为1，或者设置成只与距离呈简单的线性函数。但在实际应用场景中，特别是高度非线性化的车辆行驶或停泊环境下，当前的技术存在精度较低，鲁棒性弱等不足之处。
技术实现思路
1、本发明所要解决的技术问题在于，提供一种车辆定位估算方法、系统、存储介质及车辆，在进行定位估算时，可以摆脱对车辆初始化位姿第三源输入依赖，提高车辆在复杂非线性场景下的定位精确性和稳定性。
2、为解决上述技术问题，作为本发明的一方面，提供一种车辆定位估算方法，其至少包括如下步骤：
3、基于粒子滤波算法获得满足切换条件的初始位姿；
4、基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
5、计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
6、其中，基于粒子滤波算法获得满足切换条件的初始位姿，包括：
7、持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
8、将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
9、其中，基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据，包括：
10、提取当前激光点云数据中的激光点云密度、点云方向特征；
11、根据匹配范围过滤点云数量；
12、根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图匹配范围中的点进行匹配，形成至少一匹配对，并利用基于m-estimator的权重函数为每个匹配对赋予权重，所述点云地图匹配范围由轮式里程计数据确定；
13、采用预先建立的误差函数，利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
14、检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
15、将所述转换矩阵左乘以当前时刻的车辆初始位姿的向量作为本次位姿状态估算的最新的估算位姿数据，并输出。
16、其中，所述权重函数为：
17、所述误差函数为：
18、其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
19、其中，所述计算本次位姿状态估算的方差，确定本次位姿是否可信，包括：
20、计算本次位姿估算数据的方差；
21、将所述方差与一方差阈值进行比较；
22、如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信。
23、作为本发明的另一方面，还提供一种车辆定位估算系统，至少包括：
24、粒子滤波处理单元，用于基于粒子滤波算法获得满足切换条件的初始位姿；
25、无迹卡尔曼滤波处理单元，用于基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据；
26、滤波切换处理单元，用于计算本次位姿状态估算的方差，确定本次位姿是否可信；如果可信，则将本次位姿更新为本次位姿估算数据，并基于无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；如果不可信，则基于所述粒子滤波算法对车辆下一状态的位姿状态进行估算。
27、其中，所述粒子滤波处理单元包括：
28、持续处理单元，用于持续基于粒子滤波算法获得车辆初始位姿，直至单个粒子的权重大于预定的权重阈值，其中粒子的权重表示该粒子和观测数据的匹配程度；
29、特定初始位姿确定单元，用于将所述权重大于预定的权重阈值的单个粒子的位姿作为满足切换条件的车辆的初始位姿。
30、其中，所述无迹卡尔曼滤波处理单元包括：
31、多维特征提取单元，用于提取当前激光点云数据中的激光点云密度、点云方向特征；
32、过滤处理单元，用于根据匹配范围过滤点云数量；
33、匹配赋权处理单元，用于根据点云的方向、密度和最近点，对激光点云中的探测的点与点云地图匹配范围中的点进行匹配，形成至少一匹配对，并利用基于m-estimator的权重函数为每个匹配对赋予权重，所述点云地图匹配范围由轮式里程计数据确定；
34、迭代求解单元，用于采用预先的建立误差函数,利用梯度下降法，不断迭代求解误差函数的最小值，并获得最小值时对应的转换矩阵；其中，所述误差函数中包含有点云的方向、密度、最近点距离三个特征的偏差；
35、收敛结束检测单元，用于检测所述转换矩阵的收敛程度，当所述转换矩阵的各项值已经收敛至预定的收敛阈值时，结束迭代处理过程；
36、估算单元，用于将所述转换矩阵左乘以当前时刻的车辆初始位姿的向量作为本位姿状态估算的最新的估算位姿数据，并输出。
37、其中，所述权重函数为：
38、所述误差函数为：
39、其中，e为偏差，w为权重，k值为可调工程参数；m为匹配点的个数，n为特征数量；n为3时，所述特征分别代表最近点距离、点云密度和点云方向；对应于三个特征，e分别代表了方向角的偏差、密度的偏差和最近点距离的偏差。
40、其中，所述滤波切换处理单元包括：
41、可信度判断单元，用于计算计算本次位姿估算数据的方差，将所述方差与一方差阈值进行比较，如果比较结果为所述方差小于所述方差阈值，则认为本次位姿估算可信；否则认为本次位姿估算不可信；
42、切换单元，用于在判断结果为可信时，继续采用无迹卡尔曼滤波对车辆下一状态的位姿状态进行估算；在判断结果为不可信时，则切换至所述粒子滤波算法，并更新所述车辆的初始位姿。
43、相应地，本发明的再一方面，还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如前述的方法的步骤。
44、相应地，本发明的再一方面，还提供一种芯片，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如前述方法的步骤。
45、相应地，本发明的再一方面，还提供一种车辆，其部署有如前述的系统。
46、相应地，本发明的再一方面，还提供一种计算机程序产品，包括计算机指令，所述计算机指令指示计算机设备执行如前述方法对应的操作。
47、实施本发明实施例，具有如下有的益效果：
48、本发明提供一种车辆定位估算方法、系统、存储介质及车辆，通过在轮式传感器和激光雷达融合定位方案中，利用粒子滤波(pf)算法和无迹卡尔曼滤波(ukf)算两种滤波方法，摆脱对车辆初始化位姿第三源输入依赖，从而降低了对传感器多样性和复杂性的需求，并提高车辆在复杂非线性场景下的定位精确性和稳定性；
49、在本发明的实施例中，在采用无迹卡尔曼滤波进行定位估计的过程中，车辆出现特殊状态，诸如受第三方不可控外力被移动、环境点云特征极其单一、传感器突发故障一段时间等，可快速切换回粒子滤波处理过程，能够快速重新恢复自身定位；
50、在进行无迹卡尔曼滤波处理时，利用多特征(最近点距离、点云密度、点云方向)匹配，且给每个匹配对赋予基于稳健性回归分析m-estimator的权重值，可以大大抑制离群点、噪声点对位姿估算的影响，进而消除对丰富传感器信息源的需求，提升了激光点云与地图匹配的准确性，减少环境噪声对定位的影响，从而实现仅用轮式传感器和激光雷达就能完成车辆自身定位，且精确高和鲁棒性强。
1.一种车辆定位估算方法，其特征在于，至少包括如下步骤：
2.如权利要求1所述的方法，其特征在于，基于粒子滤波算法获得满足切换条件的初始位姿，包括：
3.如权利要求2所述的方法，其特征在于，基于无迹卡尔曼滤波算法融合满足切换条件的车辆初始位姿、激光点云数据和轮式里程计数据，对车辆位姿进行估算，获得车辆本次位姿估算数据，包括：
4.如权利要求3所述的方法，其特征在于，其中：
5.如权利要求4所述的方法，其特征在于，所述计算本次位姿状态估算的方差，确定本次位姿是否可信，包括：
6.一种车辆定位估算系统，其特征在于，至少包括：
7.如权利要求6所述的系统，其特征在于，所述粒子滤波处理单元包括：
8.如权利要求7所述的系统，其特征在于，所述无迹卡尔曼滤波处理单元包括：
9.如权利要求8所述的系统，其特征在于，其中：
10.如权利要求9所述的系统，其特征在于，所述滤波切换处理单元包括：
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5中任一项所述的方法的步骤。
12.一种芯片，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。
13.一种车辆，其特征在于，其部署有如权利要求6至10任一项所述的系统。
14.一种计算机程序产品，包括计算机指令，所述计算机指令指示计算机设备执行如权利要求1至5任一项所述方法对应的操作。
