本发明涉及车辆状态状态估计领域，尤其是涉及一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法。
背景技术：
1、汽车的电动化、智能化正在加速下一代汽车产业变革的到来，智驾汽车已成为全球汽车产业发展的重要战略方向。自动驾驶的基本前提是实现车辆的高精度定位，通过全球定位系统(gps)卫星数量众多且信号覆盖全球的优势，结合地面差分站进行实时动态差分(rtk)，在室外的空旷无遮挡环境下能够达到厘米级定位，满足自动驾驶汽车对定位精度的基本要求。但考虑到车辆作为移动的实体，会经历不同的应用场景，在城市环境密集高楼区、隧道、高架桥、地下停车场等gps信号薄弱或拒止的场景，定位的连续性会受到强烈的干扰，单纯采用一种定位技术并不能满足自动驾驶业务的定位需求。
2、针对gps信号薄弱或拒止的室内定位场景，惯性测量单元(imu)及轮速里程计(wo)的应用成为相关从业人员关注的重点，imu的缺点是误差会随着时间不断累积，无法提供长时间的高精度定位，并且在车辆停滞状态下，imu会持续进行积分计算，进一步增大与实际车辆位置的偏移。为了解决这一缺点，通过wo来引入车辆速度的观测值，约束imu计算出的位移量，进行更准确的航迹推算。
3、除此之外，在实际车辆行驶过程中，室内定位系统的工作条件相对复杂多变，不同的行驶路径和环境对应不同的运动模型。ekf算法基于线性kf进行扩展，因此在线性问题上也有较好的表现，但对于非线性问题，ekf算法只能通过线性化来近似描述非线性函数，滤波结果存在较大误差。所以，单一的滤波算法无法适用车辆运动模型的变换，难以保证较高的定位精度。
技术实现思路
1、本发明的目的就是为了克服上述现有技术存在的针对车辆运动模型变换造成单一滤波算法不完全适用的问题，本发明提供一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，该方法基于零速修正的自适应滤波切换数据融合算法，通过综合零速观测和零速判断策略，为实时的车辆状态提供可靠的判定标准，在零速修正的基础上引入自适应滤波切换，针对不同的运动模型进行相应滤波算法的切换与融合。
2、本发明的目的可以通过以下技术方案来实现：
3、一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，包括以下步骤：
4、s1、根据惯性测量单元和轮速里程计的数据，引入零速判定策略，通过所述零速判定策略对车辆状态进行判定，根据所述车辆状态构建量测方程；
5、s2、实时计算自适应动态切换判断方程，判断所述自适应动态切换判断方程是否满足切换条件，将实时车辆运动模型进行自适应动态切换；
6、s3、在不同的实时车辆运动模型下，通过卡尔曼滤波方法对所述车辆状态和量测方程进行更新，实现对车辆的非线性变量的准确估计。
7、所述零速判定策略包括：
8、首先判断组合速度vcomb与静态阈值tsta的大小关系，若满足vcomb<tsta，则车辆处于静态零速状态；若不满足，则接着判断组合速度vcomb与动态阈值tdy的大小关系；
9、若vcomb不小于tdy，则车辆处于正常运动状态；若vcomb<tdy，则接下来判断前向速度是否小于动态零速前向阈值；
10、若前向速度不小于动态零速前向阈值则车辆处于正常运动状态；若前向速度不小于动态零速前向阈值则车辆处于动态零速状态。
11、所述静态阈值tsta通过车辆启动前的角速度平均值加速度平均值以及轮速平均值得到，采用如下公式表示：
12、
13、所述组合速度通过车辆启动后的角速度平均值加速度平均值轮速里平均值以及速度平均值得到，采用如下公式表示：
14、
15、通过设置一个缓存滑动窗口，实时计算滑动窗口内的角速度平均值加速度平均值轮速平均值以及ins解算的速度平均值
16、所述动态阈值由轮速平均值和轮速的标准差的和得到，采用如下公式表示：
17、
18、其中，τ是一个调整参数。
19、所述自适应动态切换判断方程包括第一自适应动态切换判断方程：
20、
21、和第二自适应动态切换判断方程：
22、
23、其中，η为过程噪声和观测噪声的综合评价参数；σα表示imu中加速度计计算加速度的噪声；σβ表示imu中陀螺仪计算角速度的噪声；σγ表示wo计算速度的噪声；σimu表示imu的观测噪声；σwo表示wo的观测噪声；μ为速度、加速度和角速度变化率的综合评价参数，aimu表示imu中加速度计的输出值，βimu表示表示imu中陀螺仪的输出值，γ表示wo的输出值，f为行驶环境地面的摩擦系数。
24、所述切换条件包括：
25、
26、其中，ε为系统噪声估计标准差；∈为传感器性能要求容忍的最大误差。
27、所述实时车辆运动模型包括线性运动模型、非线性运动模型以及运动模式，当所述第一自适应动态切换判断方程满足切换条件时，采用ekf算法对所述线性运动模型估计；
28、当所述第二自适应动态切换判断方程满足切换条件时，采用ukf算法对所述非线性运动模型估计；
29、当所述第一自适应动态切换判断方程和第二自适应动态切换判断方程均不满足切换条件时，采用ekf+ukf联合估计算法对所述运动模式估计。
30、采用ekf+ukf联合估计算法对所述运动模式估计具体包括以下步骤：
31、利用ekf算法计算状态估计和状态协方差估计；
32、利用ukf算法计算状态估计和状态协方差估计；
33、结合ekf与ukf算法的估计结果，计算状态估计和协方差估计。
34、与现有技术相比，本发明具有以下有益效果：
35、本发明提供了一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，引入零速判断策略，为实时的车辆状态提供了可靠的判定标准，并将ekf和ukf算法与实时运动模型结合，借助自适应动态切换阈值判断融合ekf算法与ukf算法；最后，根据实时车辆运动模型的切换，对状态转移和量测方程进行更新，实现对车辆位置、速度、姿态及相关误差等非线性变量的准确估计。本发明灵活地处理了车辆运动过程中的非线性轨迹，提高了对车辆运动状态的准确估计，抑制了传统单一算法在车辆运动模型变化时产生的误差干扰。
1.一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述零速判定策略包括：
3.根据权利要求2所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述静态阈值tsta通过车辆启动前的角速度平均值加速度平均值以及轮速平均值得到，采用如下公式表示：
4.根据权利要求2所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述组合速度通过车辆启动后的角速度平均值加速度平均值轮速里平均值以及速度平均值得到，采用如下公式表示：
5.根据权利要求4所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，通过设置一个缓存滑动窗口，实时计算滑动窗口内的角速度平均值加速度平均值轮速平均值以及ins解算的速度平均值
6.根据权利要求2所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述动态阈值由轮速平均值和轮速的标准差的和得到，采用如下公式表示：
7.根据权利要求1所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述动态阈值由轮速平均值和轮速的标准差的和得到，采用如下公式表示：
8.根据权利要求7所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述切换条件包括：
9.根据权利要求8所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，所述实时车辆运动模型包括线性运动模型、非线性运动模型以及运动模式，当所述第一自适应动态切换判断方程满足切换条件时，采用ekf算法对所述线性运动模型估计；
10.根据权利要求9所述的一种基于自适应滤波切换数据融合算法的车辆运动状态估计方法，其特征在于，采用ekf+ukf联合估计算法对所述运动模式估计具体包括以下步骤：
