基于MEMS传感器的车体动态姿态估计的方法及系统与流程

文档序号:20273111发布日期:2020-04-03 19:17阅读:来源:国知局

技术特征:

1.基于mems传感器的车体动态姿态估计的方法,其特征在于,

获取车载加速度计的测量数据;

获取车载陀螺仪的测量数据;

获取车载速度计的测量数据;

将以上三种测量数据进行加速度数据处理后利用扩展卡尔曼滤波进行处理,根据滤波处理后的数据输出新的姿态角。

2.根据权利要求1所述的基于mems传感器的车体动态姿态估计的方法,其特征在于,所述卡尔曼滤波处理包括以下步骤:

确定系统模型为:

其中:x(k)∈rn是k时刻系统的状态向量,z(k)∈rm是k时刻的观测向量;f(·)是n维向量函数,h(·)是m维向量函数,f(·)和h(·)是其独立变量的非线性函数;w(k)∈rn和v(k)∈rm是具有方差的关联过程噪声矢量q(k)和r(k)。

3.根据权利要求1所述的基于mems传感器的车体动态姿态估计的方法,其特征在于,还包括:

系统的状态向量为:

x(k)=[a(x)a(y)a(z)g(x)g(y)g(z)w(x)w(y)w(z)v];其中,a是三轴加速度,g是三轴角加速度,w是三轴角速度,v是物体的速度;

系统的状态方程为:x(k)=ax(k-1),其中a是状态矩阵;

计算状态向量一步预测误差方差矩阵:

p(k,k-1)=aap(k,k-1)aa+q(k)。

4.根据权利要求3所述的基于mems传感器的车体动态姿态估计的方法,其特征在于,所述状态矩阵a为:

如果a(y)是正数,则

如果a(y)是负数,则

5.根据权利要求3所述的基于mems传感器的车体动态姿态估计的方法,其特征在于,

系统的观测向量为:

z(k)=[e(x)e(y)e(z)w(x)w(y)w(z)v];其中,e(x)e(y)e(z)j为三轴加速度计原始值减去三轴向心加速度,即

e(x)=a(x)-x轴的向心加速度;

e(y)=a(y)-速度的微分;

e(z)=a(z)-z轴的向心加速度;

系统的观测方程为:

z(k)=h(k)+v(k),其中,v(k)是噪声,这边忽略不计,h为观测矩阵;

计算状态增益矩阵:

k(k)=p(k,k-1)ht(k)[h(k)p(k,k-1)ht(k)+r(k)]-1

k时刻的状态向量估计值为:

更新状态误差协方差矩阵为:

p(k)=[i-k(k)h(k)]p(k,k-1)[i-k(k)h(k)]t+k(k)r(k)kt(k)。

6.根据权利要求5所述的基于mems传感的车体动态姿态估计的方法,其特征在于,

所述观测矩阵

7.根据权利要求3所述的基于mems传感的车体动态姿态估计的方法,其特征在于,

q(k)的参数设置为10×10的单位矩阵:

8.根据权利要求3所述的基于mems传感的车体动态姿态估计的方法,其特征在于,

p(k)的参数设置为7×7的单位矩阵:

9.一种应用权利要求1-8所述的基于mems传感的车体动态姿态估计的方法的系统,包括加速度计(1)、速度计(2)、陀螺仪(3)、处理器(4)和滤波器(5),

所述加速度计(1)、速度计(2)和陀螺仪(3)的数据输出端与所述处理器(4)的输入端相连,

所述处理器(4)的数据输出端与所述滤波器(5)的输入端相连。

当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1