CN110954103A - 基于mems传感器的车体动态姿态估计的方法及系统 - Google Patents

Abstract

本申请提供一种基于MEMS传感的车体动态姿态估计的方法及系统，获取车载加速度计的测量数据；获取车载陀螺仪的测量数据；获取车载速度计的测量数据；将以上三种测量数据进行加速度数据处理后利用卡尔曼滤波进行处理，根据滤波处理后的数据输出新的姿态角。由此，可以更准确补偿加速度计与陀螺仪的值，有了更准确的加速度计和陀螺仪的值，就可以得到更准确的姿态信息该算法可以获得准确的角度值，并且可以在动态环境中使用，以使动态车辆控制系统能够稳定运行。

Description

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

技术领域

本申请涉及MEMS传感技术领域，尤其涉及基于MEMS传感器的车体动态姿态估计的方法。

背景技术

现有技术中有很多用于动态姿势研究的估计算法。准确地建模惯性传感器非常重要。动态姿态估计算法需要考虑传感器产生的各种误差，例如安装误差，制造误差，非正交误差和零偏移误差。由于倾斜传感器的特性，传统的惯性传感器易于因温度和噪声而产生不同程度的漂移。仅使用陀螺仪和加速度计的手势在角度测量中会有较大的误差，这将对动态姿态估计产生重大误差并产生影响。因此，如何充分利用两个传感器的优点，消除传感器产生的干扰，获得更加实用，准确的数据，已成为许多专家学者研究的关键问题之一。

发明内容

本申请提供一种基于MEMS传感器的车体动态姿态估计的方法，能够解决以上问题的一个或多个。

根据本申请的一个方面，提供一种基于MEMS传感的车体动态姿态估计的方法，获取车载加速度计的测量数据；获取车载陀螺仪的测量数据；获取车载速度计的测量数据；将以上三种测量数据进行加速度数据处理后利用卡尔曼滤波进行处理，根据滤波处理后的数据输出新的姿态角。

在某些实施方式中，卡尔曼滤波处理包括以下步骤：

确定系统模型为：

其中：x_(k)∈Rⁿ是k时刻系统的状态向量，z_(k)∈R^m是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rⁿ和v(k)∈R^m是具有方差的关联过程噪声矢量Q(k)和R(k)。

在某些实施方式中，还包括：

系统的状态向量为：

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)。

在某些实施方式中，状态矩阵A为：

如果a(y)是正数，则

如果a(y)是负数，则

在某些实施方式中，系统的观测向量为：

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)H^T(k)[H(k)P(k，k-1)H^T(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)K^T(k)。

在一些实施方式中，

所述观测矩阵

在一些实施方式中，Q(k)的参数设置为10×10的单位矩阵：

在一些实施方式中，P(k)的参数设置为7×7的单位矩阵：

根据本申请的另一个方面，还提供了应用前述方法的惯性系统，包括加速度计、磁力计、陀螺仪、处理器和滤波器，所述加速度计、磁力计和陀螺仪的数据输出端与所述处理器的输入端相连，所述处理器的数据输出端与所述滤波器的输入端相连。

本申请技术方案与现有技术相比，具有以下有益效果：

动态姿态测量是车体高精度控制系统设计中非常重要的方面，需要非常精确地测量角度变化，由于单个轴向姿态倾斜传感器无法满足要求，因此我们使用多轴传感器以获得更高的精度和精度。最后，透过扩展卡尔曼滤波器的最优自回归，并设定好卡尔曼的相关系数，除了加速度计与陀螺仪之外，特别的是加入了速度量，可以更准确补偿加速度计与陀螺仪的值，有了更准确的加速度计和陀螺仪的值，就可以得到更准确的姿态信息该算法可以获得准确的角度值，并且可以在动态环境中使用，以使动态车辆控制系统能够稳定运行。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本技术方案一实施方式的方法流程图；

图2是本技术方案一实施方式的系统结构示意图；

图3是本申请一实施方式的直线模拟的倾角变化图；

图4是本申请一实施方式的总加速度变化图；

图5是本申请一实施方式的加速、减速模拟的倾角变化图；

图6是本申请一实施方式的总价速度变化图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

提供一种基于MEMS传感器的车体动态姿态估计的方法，包括以下步骤：

获取车载加速度计的测量数据；获取车载陀螺仪的测量数据；获取车载速度计的测量数据；将以上三种测量数据进行加速度数据处理后利用卡尔曼滤波进行处理，根据滤波处理后的数据输出新的姿态角。

在某些实施方式中，卡尔曼滤波处理包括以下步骤：

确定系统模型为：

其中：x_(k)∈Rⁿ是k时刻系统的状态向量，z_(k)∈R^m是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rⁿ和v(k)∈R^m是具有方差的关联过程噪声矢量Q(k)和R(k)。

在某些实施方式中，还包括：

系统的状态向量为：

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)。

在某些实施方式中，状态矩阵A为：

如果a(y)是正数，则

如果a(y)是负数，则

在某些实施方式中，系统的观测向量为：

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)H^T(k)[H(k)P(k，k-1)H^T(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)K^T(k)。

在一些实施方式中，

所述观测矩阵

在一些实施方式中，Q(k)的参数设置为10×10的单位矩阵：

在一些实施方式中，P(k)的参数设置为7×7的单位矩阵：

实施例2，

基于MEMS传感的车体动态姿态估计方法的惯性系统，包括加速度计、磁力计、陀螺仪、处理器和滤波器，所述加速度计、磁力计和陀螺仪的数据输出端与所述处理器的输入端相连，所述处理器的数据输出端与所述滤波器的输入端相连。滤波器的处理方法同实施例1的阐述，在此不再赘述。

实施例3

本申请技术方案使用的高精度姿态测量单元(BW VG500)包括三轴陀螺仪，三轴加速度计和高性能STM32f103微处理器，该微处理器安装在运动车体，比如三轮车的后轴上，并使用欧姆龙的1AG3-AG5B。将编码器安装在转向系统水龙头上的值编码器已获得转向系统消息，并已添加到卡尔曼滤波器方程中。另外，使用了步进电机HQS86H并将其安装在后桥上。MEMS的原始数据可以通过微处理器进行测量，将频率设置为50HZ，对传感器进行采样，然后执行数据滤波融合处理。通过MATLAB设计的手势融合算法获得准确的姿态信息。

为了验证本申请中的方法，具体实验结果具体可以参见图3-图6。

(1)自平衡三轮车的运动状态是线性加速度-减速运动。当它加速到一定速度时，它会制动，加速，继续该循环并最终停止，从图3可以看出。

(2)当自平衡三轮车的运动状态以普通车辆开始时，其绕过类似于正方形的路径，最后返回原点并停止。

从图3可以看出，自平衡三轮车沿直线运动，线性加速和减速，简单的EKF算法将使侧倾角变化太大而毛刺过多。从图4可以看出，简单EKF算法的总加速度不等于1。通过本申请的姿态融合VEKF算法，从而获得更精确的姿态，从而解决了这些问题。

从图5可以看出，自平衡三轮车环绕着类似于正方形的路径，最后返回原点，然后停止。我们通过短时陀螺仪积分获得姿态角以比较参考量，并且可以看到简单EKF算法的曲线。因为包括了运动员的速度，所以加速度计的测量角度不准确，并且使用本申请的姿态融合VEKF算法的曲线能与通过陀螺仪积分，陀螺仪获得的姿态角的蓝色曲线几乎相同。由于陀螺仪积分引起的漂移(在可接受的范围内)，积分的曲线略有漂移。

从图6可以看出，简单EKF算法的总加速度不等于1。通过本申请中的姿态融合VEKF算法，通过陀螺仪的积分和参数调整来达到加速度的总量，从而获得更精确的姿态，从而解决了这些问题。

因此，本申请使用多轴传感器以获得更高的精度。最后，透过扩展卡尔曼滤波器的最优自回归，并设定好卡尔曼的相关系数，除了加速度计与陀螺仪之外，特别的是加入了速度量，可以更准确补偿加速度计与陀螺仪的值，有了更准确的加速度计和陀螺仪的值，就可以得到更准确的姿态信息该算法可以获得准确的角度值，并且可以在动态环境中使用，以使动态车辆控制系统能够稳定运行。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (9)

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

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

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

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

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

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

确定系统模型为：

其中：x_(k)∈Rⁿ是k时刻系统的状态向量，z_(k)∈R^m是k时刻的观测向量；f(·)是n维向量函数，h(·)是m维向量函数，f(·)和h(·)是其独立变量的非线性函数；w(k)∈Rⁿ和v(k)∈R^m是具有方差的关联过程噪声矢量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)H^T(k)[H(k)P(k，k-1)H^T(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)K^T(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)的输入端相连。

Images