【技术实现步骤摘要】
本专利技术涉及一种陀螺仪结合卡尔曼滤波判定车辆位姿方法、装置及存储介质。
技术介绍
1、陀螺仪传感器在测量物体姿态方面使用比较广泛，现有方案是直接使用陀螺仪的俯仰角数据，但这个原始数据存在噪声，测量会有比较大的误差，导致影响最终结果。在没有用卡尔曼算法过滤前，数据噪声比较大，导致无法准确判断车辆所处的姿态，无法判断上下坡或者平路。
技术实现思路
1、本专利解决的问题是降低了陀螺仪数据的噪声，通过卡尔曼滤波过滤后的数据更接近于实际值。
2、为解决上述问题，本专利技术采用了如下技术方案：
3、一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，包括步骤：
4、s1:获取原始俯仰角角度α和原始角速度p；
5、s2:计算设定周期dt内的预测角度α1，预测角度α1根据周期dt、原始角速度p和后验估计值bias融合计算；
6、s3:计算卡尔曼增益系数，卡尔曼增益系数根据先验估计协方差q和系统测量误差err1融合计算；
7、s4:计算先验估算值err2，先验估算值err2为原始俯仰角角度α与预测角度值α1的差值；
8、s5:计算设定周期dt内俯仰角最优解，俯仰角最优解根据预测角度值α1、卡尔曼增益系数和先验估计值err2融合计算；
9、s6:计算后验估计值bias，后验估计值bias根据卡尔曼增益系数和先验估计协方差q融合计算；
10、s7:返回步骤s2,迭代计算，修正俯仰角最优解。
1
12、α1＝(p-bias)*dt
13、进一步的，卡尔曼增益系数包括角度卡尔曼增益系数k0和角速度卡尔曼增益系数k1；
14、角度卡尔曼增益系数k0计算方法为：
15、k0＝pct0/(err1+c0*pct0)
16、式中，c0为系统经验系数，pct0为角度协方差；
17、角速度卡尔曼增益系数k1计算方法为：
18、k1＝pct1/(err1+c0*pct0)
19、式中，c0为系统经验系数，pct1为角速度协方差。
20、进一步的，俯仰角最优解的计算方法为：
21、＝k0*err2；
22、进一步的，后验估计值bias的计算方法为：
23、bias＝k1*err2/dt；
24、本专利技术还提供了一种车辆俯仰角修正装置，包括处理器、存储器以及存储在存储器中且被配置为由处理器执行的计算机程序，处理器执行计算机程序时实现陀螺仪结合卡尔曼滤波判定车辆位姿方法。
25、本专利技术还提供了一种存储介质，所述存储介质存储有可执行的程序，程序用于实现车辆陀螺仪结合卡尔曼滤波判定车辆位姿方法。
26、本专利技术主要是通过陀螺仪的获取的俯仰角和对应俯仰角的角速度值计算出一个俯仰角的最优解，结合角速度预测，一直迭代最优解，并用此最优解应用于业务逻辑。本专利技术降低了原始俯仰角数据的噪声，计算的最优解更接近于实际场景。通过俯仰角，比较真实的反馈出路面的坡度情况。
本文档来自技高网...【技术保护点】
1.一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于，包括步骤：
2.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于：
3.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于：
4.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于，
5.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于，
6.一种车辆俯仰角修正装置，其特征在于，包括处理器、存储器以及存储在所述存储器中且被配置为由所述处理器执行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述的陀螺仪结合卡尔曼滤波判定车辆位姿方法。
7.一种存储介质，其特征在于，所述存储介质存储有可执行的程序，所述程序用于实现权利要求1-5中任一项所述的陀螺仪结合卡尔曼滤波判定车辆位姿方法。
【技术特征摘要】
1.一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于，包括步骤：
2.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于：
3.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于：
4.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆位姿方法，其特征在于，
5.根据权利要求1所述的一种陀螺仪结合卡尔曼滤波判定车辆...
【专利技术属性】
技术研发人员：马宏，段桂江，杨望明，
申请(专利权)人：易显智能科技有限责任公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。