CN107543546B - 一种六轴运动传感器的姿态解算方法及装置 - Google Patents

Abstract

本发明实施例公开了一种六轴运动传感器的姿态解算方法及装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：获取所述陀螺仪的第一数据；获取所述加速度计的第二数据；对所述第一数据进行预设的第一算法计算得到实时角速度值；对所述第二数据按照预设的第二算法计算得到角度值；对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。使用上述方法和装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。

Description

一种六轴运动传感器的姿态解算方法及装置

技术领域

本发明涉及传感器领域，特别涉及一种六轴运动传感器的姿态解算方法及装置。

背景技术

近年来随着六轴无人机、自平衡车等新产品的兴起，用于检测空间中姿态角度的运动传感器应时而生。为了降低成本以及传感器系统的空间大小，这种运动传感往往采用微电子机械技术(MEMS)制造，例如智能手机中使用的微机电陀螺仪，相比传统机械式陀螺仪这种传感器成本及尺寸大幅减少，虽然精度有所下降，但采取合适的处理算法仍能达到一般的姿态检测要求。

理论上单一使用加速度计或者陀螺仪即可获得姿态角度，但是加速度计动态特性较差而陀螺仪漂移严重，因此实际应用中大多采用多种传感器补偿的方式来进行姿态检测与解算。随着电子芯片制造技术的发展，目前的微机电式运动传感器一般都能够捕捉空间六轴的运动状态，包括三轴加速度计检测的各轴加速度分量和三轴陀螺仪检测的三轴旋转角速度，使得通过一个六轴微机电运动传感器便能够实现以前多种传感器组合的效果。

对于组合的多种传感器方式可以使用互补滤波的方法来进行姿态解算，设定合理的系数来分别取加速度计低频成分和陀螺仪高频成分相加来作为最终的输出姿态倾角。但是对于六轴运动传感器而言，其加速度计和陀螺仪都是通过硅片上精密蚀刻的悬空结构，通电后在压电效应下高速振荡来实现的，原理上相近。实验显示对于微机电六轴运动传感器单纯通过互补滤波得到的姿态并不准确。

发明内容

有鉴于此，本发明实施例提供了一种一种六轴运动传感器的姿态解算方法及装置。

本发明实施例中提供一种六轴运动传感器的姿态解算方法，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

获取所述陀螺仪的第一数据；

获取所述加速度计的第二数据；

对所述第一数据进行预设的第一算法计算得到实时角速度值；

对所述第二数据按照预设的第二算法计算得到角度值；

对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。

可选地，所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：

对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。

可选地，所述第二数据为加速度分量，检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

可选地，所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：

输入为陀螺仪的角速度值和加速度计的角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：

式中，t＝0，1，2…为离散时间，z(t)为通过加速度计计算得到的实时角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差。

可选地，所述获取所述陀螺仪的第一数据之前，还包括：

判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

本发明实施例中提供一种六轴运动传感器的姿态解算装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

第一获取单元，用于获取所述陀螺仪的第一数据；

第二获取单元，用于获取所述加速度计的第二数据；

第一计算单元，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；

第二计算单元，用于对所述第二数据按照预设的第二算法计算得到角度值；

第三计算单元，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。

可选地，所述第一数据为角速度，所述第一计算单元具体用于：

对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。

可选地，所述第二数据为加速度分量，所述第二计算单元具体用于：

检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

可选地，所述第三计算单元具体用于：

输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：

其中，t＝0，1，2…为离散时间，z(t)为通过所述加速度计计算得到的实时角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差。

可选地，还包括：

中断判断单元，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

从以上技术方案可以看出，本发明实施例具有以下优点：

使用上述方法和装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。

附图说明

图1是本发明实施例中提供一种六轴运动传感器的姿态解算方法的流程图；

图2是本发明实施例中提供一种六轴运动传感器的姿态解算方法中陀螺仪的原理图；

图3是本发明实施例中提供一种六轴运动传感器的姿态解算方法中加速度计的原理图；

图4是本发明实施例中提供一种六轴运动传感器的姿态解算方法中卡尔曼滤波结果示意图；

图5是本发明实施例中提供一种六轴运动传感器的姿态解算装置的结构图。

具体实施方式

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

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

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

结合图1所示，本发明实施例中提供一种六轴运动传感器的姿态解算方法，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

S101、获取所述陀螺仪的第一数据；

S102、获取所述加速度计的第二数据；

S103、对所述第一数据进行预设的第一算法计算得到实时角速度值；

具体地，所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：

陀螺仪动态特性出色，使用陀螺仪求解姿态角度时需要对输出的角速度作积分处理，可以根据陀螺仪的这种特点构建卡尔曼滤波器中的状态方程：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。

S104、对所述第二数据按照预设的第二算法计算得到角度值；

具体地，所述第二数据为加速度分量，加速度计可以检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角便能够得到该方向当前的姿态角度。相比陀螺仪计算角度需要依赖对时间的积分，加速度计能够在任意时刻得到当前姿态角度而不受之前时刻状态的影响，这一特性使其恰好可以作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

S105、对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。

结合图3所示，具体地，所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：

得到输入为陀螺仪的角速度值和加速度计的角度值，输出为姿态融合角度值(卡尔曼最优估计值)与最优估计值偏差的卡尔曼滤波器。为了在数字芯片中能够实现卡尔曼滤波器，将联立后的方程离散化，令T_s为系统采样周期，亦即获取陀螺仪和加速度计数据的时间间隔，同时按照卡尔曼滤波器迭代更新格式将过程噪声与测量噪声提取转化为噪声协方差矩阵可以得到卡尔曼滤波器的系统方程为：

式中，t＝0，1，2…为离散时间，z(t)为通过加速度计计算得到的实时角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差。

图2为微机电陀螺仪的原理示意图，里面没有旋转部件而是一个振动部分高频振动。如果陀螺仪沿某一轴有旋转运动，振动部分会产生科里奥利力，这个力会反作用于震动元件使其产生扭曲，通过检测这个微小的扭曲就能知道沿陀螺仪敏感轴方向的角速度。

图3为加速度计原理，其检测重力加速度在某一轴向上的分量来解算姿态倾角。在处理时，需要将陀螺仪输出的角速度对时间积分，而加速度计则不受时间约束，通过合成空间矢量即可求得姿态角度。这两种特点恰好适用于卡尔曼滤波器的状态方程和测量方程。

为了解算传感器姿态，我们根据卡尔曼滤波器建立了系统方程对陀螺仪和加速度计的检测数据进行融合，通过对方程求解得到准确的姿态倾角。卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

可选地，所述获取所述陀螺仪的第一数据之前，还包括：

判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

本发明实施例中的一种实施例，可以是这样的，选择合适的控制芯片实现姿态解算，首先将控制芯片初始化，配置好时钟、中断等系统并且根据传感器通信特点选择合适的通信协议；之后采用定时中断的方法，每Ts周期读取传感器中陀螺仪和加速度计的值，编程实现卡尔曼滤波器将二者融合解算出实时姿态倾角。目前本方法已应用与两轮自平衡机器人的姿态控制中，效果较好。

使用上述方法进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。

结合图5所示，本发明实施例中提供一种六轴运动传感器的姿态解算装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

第一获取单元501，用于获取所述陀螺仪的第一数据；

第二获取单元502，用于获取所述加速度计的第二数据；

第一计算单元503，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；

第二计算单元504，用于对所述第二数据按照预设的第二算法计算得到角度值；

第三计算单元505，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。

可选地，所述第一数据为角速度，所述第一计算单元503具体用于：

对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。

可选地，所述第二数据为加速度分量，所述第二计算单元504具体用于：

检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

可选地，所述第三计算单元505具体用于：

输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：

其中，t＝0，1，2…为离散时间，z(t)为通过所述加速度计计算得到的实时角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差。

可选地，还包括：

中断判断单元506，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

使用上述装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。

以上对本发明所提供的一种六轴运动传感器的姿态解算方法及装置进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

1.一种六轴运动传感器的姿态解算方法，其特征在于，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

获取所述陀螺仪的第一数据；

获取所述加速度计的第二数据；

对所述第一数据进行预设的第一算法计算得到实时角速度值；

对所述第二数据按照预设的第二算法计算得到角度值；

对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角；

所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：

对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声；

所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：

输入为陀螺仪的角速度值和加速度计的角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：

式中，t＝0,1,2…为离散时间，z(t)为通过加速度计计算得到的角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差，T_s为系统采样周期。

2.根据权利要求1所述的方法，其特征在于，所述第二数据为，所述对所述第二数据按照预设的第二算法计算得到角度值，包括：

检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到当前方向的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

3.根据权利要求2所述的方法，其特征在于，所述获取所述陀螺仪的第一数据之前，还包括：

判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

4.一种六轴运动传感器的姿态解算装置，其特征在于，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：

第一获取单元，用于获取所述陀螺仪的第一数据；

第二获取单元，用于获取所述加速度计的第二数据；

第一计算单元，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；

第二计算单元，用于对所述第二数据按照预设的第二算法计算得到角度值；

第三计算单元，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角；

所述第一数据为角速度，所述第一计算单元具体用于：

对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：

其中

为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；

为最优估计值的偏差，u_gyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声；

所述第三计算单元具体用于：

输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：

其中，t＝0,1,2…为离散时间，z(t)为通过所述加速度计计算得到的角度值，u_gyro(t)为陀螺仪检测到的实时角速度值，x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值，x₂(t+T_s)为最优估计值的偏差，T_s为系统采样周期。

5.根据权利要求4所述的装置，其特征在于，所述第二数据为加速度分量，所述第二计算单元具体用于：

检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到当前方向的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。

6.根据权利要求5所述的装置，其特征在于，还包括：

中断判断单元，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。

Images