CN107543546A - 一种六轴运动传感器的姿态解算方法及装置 - Google Patents
一种六轴运动传感器的姿态解算方法及装置 Download PDFInfo
- Publication number
- CN107543546A CN107543546A CN201610482778.XA CN201610482778A CN107543546A CN 107543546 A CN107543546 A CN 107543546A CN 201610482778 A CN201610482778 A CN 201610482778A CN 107543546 A CN107543546 A CN 107543546A
- Authority
- CN
- China
- Prior art keywords
- mtd
- mtr
- msub
- mtable
- mrow
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Gyroscopes (AREA)
Abstract
本发明实施例公开了一种六轴运动传感器的姿态解算方法及装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：获取所述陀螺仪的第一数据；获取所述加速度计的第二数据；对所述第一数据进行预设的第一算法计算得到实时角速度值；对所述第二数据按照预设的第二算法计算得到角度值；对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。使用上述方法和装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。
Description
技术领域
本发明涉及传感器领域，特别涉及一种六轴运动传感器的姿态解算方法及装置。
背景技术
近年来随着六轴无人机、自平衡车等新产品的兴起，用于检测空间中姿态角度的运动传感器应时而生。为了降低成本以及传感器系统的空间大小，这种运动传感往往采用微电子机械技术(MEMS)制造，例如智能手机中使用的微机电陀螺仪，相比传统机械式陀螺仪这种传感器成本及尺寸大幅减少，虽然精度有所下降，但采取合适的处理算法仍能达到一般的姿态检测要求。
理论上单一使用加速度计或者陀螺仪即可获得姿态角度，但是加速度计动态特性较差而陀螺仪漂移严重，因此实际应用中大多采用多种传感器补偿的方式来进行姿态检测与解算。随着电子芯片制造技术的发展，目前的微机电式运动传感器一般都能够捕捉空间六轴的运动状态，包括三轴加速度计检测的各轴加速度分量和三轴陀螺仪检测的三轴旋转角速度，使得通过一个六轴微机电运动传感器便能够实现以前多种传感器组合的效果。
对于组合的多种传感器方式可以使用互补滤波的方法来进行姿态解算，设定合理的系数来分别取加速度计低频成分和陀螺仪高频成分相加来作为最终的输出姿态倾角。但是对于六轴运动传感器而言，其加速度计和陀螺仪都是通过硅片上精密蚀刻的悬空结构，通电后在压电效应下高速振荡来实现的，原理上相近。实验显示对于微机电六轴运动传感器单纯通过互补滤波得到的姿态并不准确。
发明内容
有鉴于此，本发明实施例提供了一种一种六轴运动传感器的姿态解算方法及装置。
本发明实施例中提供一种六轴运动传感器的姿态解算方法，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
获取所述陀螺仪的第一数据；
获取所述加速度计的第二数据；
对所述第一数据进行预设的第一算法计算得到实时角速度值；
对所述第二数据按照预设的第二算法计算得到角度值；
对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
可选地，所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：
对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
可选地，所述第二数据为加速度分量，检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
可选地，所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：
输入为陀螺仪的角速度值和加速度计的角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：
式中，t＝0，1，2…为离散时间，z(t)为通过加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
可选地，所述获取所述陀螺仪的第一数据之前，还包括：
判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
本发明实施例中提供一种六轴运动传感器的姿态解算装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
第一获取单元，用于获取所述陀螺仪的第一数据；
第二获取单元，用于获取所述加速度计的第二数据；
第一计算单元，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；
第二计算单元，用于对所述第二数据按照预设的第二算法计算得到角度值；
第三计算单元，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
可选地，所述第一数据为角速度，所述第一计算单元具体用于：
对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
可选地，所述第二数据为加速度分量，所述第二计算单元具体用于：
检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
可选地，所述第三计算单元具体用于：
输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：
其中，t＝0，1，2…为离散时间，z(t)为通过所述加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
可选地，还包括：
中断判断单元，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
从以上技术方案可以看出，本发明实施例具有以下优点：
使用上述方法和装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。
附图说明
图1是本发明实施例中提供一种六轴运动传感器的姿态解算方法的流程图；
图2是本发明实施例中提供一种六轴运动传感器的姿态解算方法中陀螺仪的原理图；
图3是本发明实施例中提供一种六轴运动传感器的姿态解算方法中加速度计的原理图；
图4是本发明实施例中提供一种六轴运动传感器的姿态解算方法中卡尔曼滤波结果示意图；
图5是本发明实施例中提供一种六轴运动传感器的姿态解算装置的结构图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
结合图1所示，本发明实施例中提供一种六轴运动传感器的姿态解算方法，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
S101、获取所述陀螺仪的第一数据；
S102、获取所述加速度计的第二数据；
S103、对所述第一数据进行预设的第一算法计算得到实时角速度值；
具体地，所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：
陀螺仪动态特性出色，使用陀螺仪求解姿态角度时需要对输出的角速度作积分处理，可以根据陀螺仪的这种特点构建卡尔曼滤波器中的状态方程：
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
S104、对所述第二数据按照预设的第二算法计算得到角度值；
具体地，所述第二数据为加速度分量，加速度计可以检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角便能够得到该方向当前的姿态角度。相比陀螺仪计算角度需要依赖对时间的积分，加速度计能够在任意时刻得到当前姿态角度而不受之前时刻状态的影响，这一特性使其恰好可以作为卡尔曼滤波器的测量方程：
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
S105、对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
结合图3所示，具体地，所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：
得到输入为陀螺仪的角速度值和加速度计的角度值，输出为姿态融合角度值(卡尔曼最优估计值)与最优估计值偏差的卡尔曼滤波器。为了在数字芯片中能够实现卡尔曼滤波器，将联立后的方程离散化，令Ts为系统采样周期，亦即获取陀螺仪和加速度计数据的时间间隔，同时按照卡尔曼滤波器迭代更新格式将过程噪声与测量噪声提取转化为噪声协方差矩阵可以得到卡尔曼滤波器的系统方程为：
式中，t＝0，1，2…为离散时间，z(t)为通过加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
图2为微机电陀螺仪的原理示意图，里面没有旋转部件而是一个振动部分高频振动。如果陀螺仪沿某一轴有旋转运动，振动部分会产生科里奥利力，这个力会反作用于震动元件使其产生扭曲，通过检测这个微小的扭曲就能知道沿陀螺仪敏感轴方向的角速度。
图3为加速度计原理，其检测重力加速度在某一轴向上的分量来解算姿态倾角。在处理时，需要将陀螺仪输出的角速度对时间积分，而加速度计则不受时间约束，通过合成空间矢量即可求得姿态角度。这两种特点恰好适用于卡尔曼滤波器的状态方程和测量方程。
为了解算传感器姿态，我们根据卡尔曼滤波器建立了系统方程对陀螺仪和加速度计的检测数据进行融合，通过对方程求解得到准确的姿态倾角。卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
可选地，所述获取所述陀螺仪的第一数据之前，还包括：
判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
本发明实施例中的一种实施例，可以是这样的，选择合适的控制芯片实现姿态解算，首先将控制芯片初始化，配置好时钟、中断等系统并且根据传感器通信特点选择合适的通信协议；之后采用定时中断的方法，每Ts周期读取传感器中陀螺仪和加速度计的值，编程实现卡尔曼滤波器将二者融合解算出实时姿态倾角。目前本方法已应用与两轮自平衡机器人的姿态控制中，效果较好。
使用上述方法进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。
结合图5所示，本发明实施例中提供一种六轴运动传感器的姿态解算装置，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
第一获取单元501，用于获取所述陀螺仪的第一数据；
第二获取单元502，用于获取所述加速度计的第二数据；
第一计算单元503，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；
第二计算单元504，用于对所述第二数据按照预设的第二算法计算得到角度值；
第三计算单元505，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
可选地，所述第一数据为角速度，所述第一计算单元503具体用于：
对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
可选地，所述第二数据为加速度分量，所述第二计算单元504具体用于：
检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
可选地，所述第三计算单元505具体用于：
输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：
其中，t＝0，1，2…为离散时间，z(t)为通过所述加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
可选地，还包括：
中断判断单元506，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
使用上述装置进行姿态检测能够充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确从微机电六轴运动传感器中解算出实时姿态。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。
以上对本发明所提供的一种六轴运动传感器的姿态解算方法及装置进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。
Claims (10)
1.一种六轴运动传感器的姿态解算方法，其特征在于，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
获取所述陀螺仪的第一数据；
获取所述加速度计的第二数据；
对所述第一数据进行预设的第一算法计算得到实时角速度值；
对所述第二数据按照预设的第二算法计算得到角度值；
对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
2.根据权利要求1所述的方法，其特征在于，所述第一数据为角速度，所述对所述第一数据进行预设的第一算法计算得到实时角速度值，包括：
对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msub>
<mi>u</mi>
<mrow>
<mi>g</mi>
<mi>y</mi>
<mi>r</mi>
<mi>o</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>w</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
3.根据权利要求2所述的方法，其特征在于，所述第二数据为，所述对所述第二数据按照预设的第二算法计算得到角度值，包括：
检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：
<mrow>
<mi>z</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>v</mi>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
4.根据权利要求3所述的方法，其特征在于，所述对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角，包括：
输入为陀螺仪的角速度值和加速度计的角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：
<mfenced open = "{" close = "}">
<mtable>
<mtr>
<mtd>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msub>
<mi>u</mi>
<mrow>
<mi>g</mi>
<mi>y</mi>
<mi>r</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>z</mi>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
</mtd>
</mtr>
</mtable>
</mfenced>
式中，t＝0，1，2…为离散时间，z(t)为通过加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
5.根据权利要求3所述的方法，其特征在于，所述获取所述陀螺仪的第一数据之前，还包括：
判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
6.一种六轴运动传感器的姿态解算装置，其特征在于，所述传感器包括陀螺仪和加速度计，在预设的周期内，包括：
第一获取单元，用于获取所述陀螺仪的第一数据；
第二获取单元，用于获取所述加速度计的第二数据；
第一计算单元，用于对所述第一数据进行预设的第一算法计算得到实时角速度值；
第二计算单元，用于对所述第二数据按照预设的第二算法计算得到角度值；
第三计算单元，用于对所述角度值和所述实时角速度值进行卡曼滤波计算得到姿态倾角。
7.根据权利要求6所述的装置，其特征在于，所述第一数据为角速度，所述第一计算单元具体用于：
对输出的角速度作积分处理，构建卡尔曼滤波器中的状态方程以得到实时角速度值：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msub>
<mi>u</mi>
<mrow>
<mi>g</mi>
<mi>y</mi>
<mi>r</mi>
<mi>o</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>w</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
其中为所述姿态倾角的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，ugyro(t)为陀螺仪检测到的实时角速度值，w(t)为过程噪声。
8.根据权利要求7所述的装置，其特征在于，所述第二数据为加速度分量，所述第二计算单元具体用于：
检测到运动时空间直角坐标系各个轴上的加速度分量，即可以得到当前运动的空间矢量，通过计算任意一轴与空间矢量的夹角得到该方向当前的姿态角度，根据在任意时刻得到当前姿态角度作为卡尔曼滤波器的测量方程：
<mrow>
<mi>z</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>v</mi>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mrow>
其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。
9.根据权利要求8所述的装置，其特征在于，所述第三计算单元具体用于：
输入为陀螺仪的所述实时角速度值和加速度计的所述角度值，输出所述卡尔曼滤波器，得到卡尔曼滤波器的系统方程为：
<mfenced open = "{" close = "}">
<mtable>
<mtr>
<mtd>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msub>
<mi>u</mi>
<mrow>
<mi>g</mi>
<mi>y</mi>
<mi>r</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>z</mi>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>t</mi>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
</mtd>
</mtr>
</mtable>
</mfenced>
其中，t＝0，1，2…为离散时间，z(t)为通过所述加速度计计算得到的实时角度值，ugyro(t)为陀螺仪检测到的实时角速度值，x1(t+Ts)为卡尔曼滤波器对传感器数据融合后的输出值，x2(t+Ts)为最优估计值的偏差。
10.根据权利要求9所述的装置，其特征在于，还包括：
中断判断单元，用于判断预设的中断是否产生，若是，则执行后续获取所述陀螺仪的第一数据的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610482778.XA CN107543546B (zh) | 2016-06-28 | 2016-06-28 | 一种六轴运动传感器的姿态解算方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610482778.XA CN107543546B (zh) | 2016-06-28 | 2016-06-28 | 一种六轴运动传感器的姿态解算方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107543546A true CN107543546A (zh) | 2018-01-05 |
| CN107543546B CN107543546B (zh) | 2021-03-05 |
Family
ID=60961524
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610482778.XA Active CN107543546B (zh) | 2016-06-28 | 2016-06-28 | 一种六轴运动传感器的姿态解算方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107543546B (zh) |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731676A (zh) * | 2018-05-04 | 2018-11-02 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN109029459A (zh) * | 2018-07-24 | 2018-12-18 | 南京信息工程大学 | 一种运动目标轨迹追踪系统及基于该系统的计算方法 |
| CN109506678A (zh) * | 2018-12-29 | 2019-03-22 | 中国电子科技集团公司第二十六研究所 | 基于微机电系统的惯性测量组合中陀螺仪动态自检方法 |
| CN110823181A (zh) * | 2018-07-23 | 2020-02-21 | 郑州宇通重工有限公司 | 环卫车及其车身姿态检测方法、车身稳定方法及对应系统 |
| CN111121705A (zh) * | 2019-05-07 | 2020-05-08 | 浙江利尔达客思智能科技有限公司 | 一种基于六轴检测的开门状态检测系统及方法 |
| CN112874315A (zh) * | 2021-01-13 | 2021-06-01 | 深圳市奇虎智能科技有限公司 | 平衡车的运动控制方法、装置、设备及存储介质 |
| CN113108767A (zh) * | 2021-04-07 | 2021-07-13 | 王陶然 | 一种无人机载雷达水文信息实时监测方法 |
| CN114395953A (zh) * | 2021-12-31 | 2022-04-26 | 武汉大学 | 一种便携式高压水射流钢轨打磨入射角标定方法及系统 |
| CN114440926A (zh) * | 2022-01-18 | 2022-05-06 | 武汉元生创新科技有限公司 | 一种陀螺仪检测量程扩展方法及系统 |
| CN115193009A (zh) * | 2021-04-09 | 2022-10-18 | 广东小天才科技有限公司 | 一种仰卧起坐次数计算方法及可穿戴设备 |
| CN115420249A (zh) * | 2022-08-19 | 2022-12-02 | 智己汽车科技有限公司 | 一种车载四轮定位实时监测系统、监测方法及制动角总成 |
| WO2023087832A1 (zh) * | 2021-11-19 | 2023-05-25 | 荣耀终端有限公司 | 合页角度的检测方法及相关设备 |
| CN116578816A (zh) * | 2023-05-11 | 2023-08-11 | 常州浩万新能源科技有限公司 | 基于图像识别的两轮平衡车驾驶辅助方法及系统 |
| CN116972832A (zh) * | 2022-04-22 | 2023-10-31 | 腾讯科技（深圳）有限公司 | 一种姿态确定方法及相关装置 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1851406A (zh) * | 2006-05-26 | 2006-10-25 | 南京航空航天大学 | 基于捷联惯性导航系统的姿态估计和融合的方法 |
| CN102009653A (zh) * | 2010-11-10 | 2011-04-13 | 清华大学 | 融合卡尔曼滤波和加速度积分的车轮质心侧偏角观测方法 |
| FR2975180A1 (fr) * | 2011-05-09 | 2012-11-16 | Astrium Sas | Dispositif et procede de determination d'attitude d'un satellite et satellite embarquant un tel dispositif. |
| US20140122015A1 (en) * | 2012-10-30 | 2014-05-01 | Yamaha Corporation | Attitude estimation method and apparatus |
| CN104316080A (zh) * | 2014-10-13 | 2015-01-28 | 许志修 | 一种姿态陀螺仪倾斜补偿四元数解算装置 |
| CN104713556A (zh) * | 2015-03-17 | 2015-06-17 | 中国海洋石油总公司 | 一种球磨机内介质运动状态的分析系统及方法 |
| US20160033278A1 (en) * | 2014-07-29 | 2016-02-04 | Chung Hua University | Composite navigation system |
| EP3018447A1 (fr) * | 2014-11-07 | 2016-05-11 | Thales | Procédé de détermination d'un rayon de protection associé à un paramètre de navigation d'un système hybride de navigation inertielle, et système associé |
-
2016
- 2016-06-28 CN CN201610482778.XA patent/CN107543546B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1851406A (zh) * | 2006-05-26 | 2006-10-25 | 南京航空航天大学 | 基于捷联惯性导航系统的姿态估计和融合的方法 |
| CN102009653A (zh) * | 2010-11-10 | 2011-04-13 | 清华大学 | 融合卡尔曼滤波和加速度积分的车轮质心侧偏角观测方法 |
| FR2975180A1 (fr) * | 2011-05-09 | 2012-11-16 | Astrium Sas | Dispositif et procede de determination d'attitude d'un satellite et satellite embarquant un tel dispositif. |
| US20140122015A1 (en) * | 2012-10-30 | 2014-05-01 | Yamaha Corporation | Attitude estimation method and apparatus |
| US20160033278A1 (en) * | 2014-07-29 | 2016-02-04 | Chung Hua University | Composite navigation system |
| CN104316080A (zh) * | 2014-10-13 | 2015-01-28 | 许志修 | 一种姿态陀螺仪倾斜补偿四元数解算装置 |
| EP3018447A1 (fr) * | 2014-11-07 | 2016-05-11 | Thales | Procédé de détermination d'un rayon de protection associé à un paramètre de navigation d'un système hybride de navigation inertielle, et système associé |
| CN104713556A (zh) * | 2015-03-17 | 2015-06-17 | 中国海洋石油总公司 | 一种球磨机内介质运动状态的分析系统及方法 |
Cited By (18)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731676B (zh) * | 2018-05-04 | 2021-06-15 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN108731676A (zh) * | 2018-05-04 | 2018-11-02 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN110823181A (zh) * | 2018-07-23 | 2020-02-21 | 郑州宇通重工有限公司 | 环卫车及其车身姿态检测方法、车身稳定方法及对应系统 |
| CN109029459B (zh) * | 2018-07-24 | 2023-07-21 | 南京信息工程大学 | 一种运动目标轨迹追踪系统及基于该系统的计算方法 |
| CN109029459A (zh) * | 2018-07-24 | 2018-12-18 | 南京信息工程大学 | 一种运动目标轨迹追踪系统及基于该系统的计算方法 |
| CN109506678A (zh) * | 2018-12-29 | 2019-03-22 | 中国电子科技集团公司第二十六研究所 | 基于微机电系统的惯性测量组合中陀螺仪动态自检方法 |
| CN109506678B (zh) * | 2018-12-29 | 2020-09-15 | 中国电子科技集团公司第二十六研究所 | 基于微机电系统的惯性测量组合中陀螺仪动态自检方法 |
| CN111121705A (zh) * | 2019-05-07 | 2020-05-08 | 浙江利尔达客思智能科技有限公司 | 一种基于六轴检测的开门状态检测系统及方法 |
| CN112874315A (zh) * | 2021-01-13 | 2021-06-01 | 深圳市奇虎智能科技有限公司 | 平衡车的运动控制方法、装置、设备及存储介质 |
| CN113108767A (zh) * | 2021-04-07 | 2021-07-13 | 王陶然 | 一种无人机载雷达水文信息实时监测方法 |
| CN115193009A (zh) * | 2021-04-09 | 2022-10-18 | 广东小天才科技有限公司 | 一种仰卧起坐次数计算方法及可穿戴设备 |
| WO2023087832A1 (zh) * | 2021-11-19 | 2023-05-25 | 荣耀终端有限公司 | 合页角度的检测方法及相关设备 |
| WO2023124577A1 (zh) * | 2021-12-31 | 2023-07-06 | 武汉大学 | 一种便携式高压水射流钢轨打磨入射角标定方法及系统 |
| CN114395953A (zh) * | 2021-12-31 | 2022-04-26 | 武汉大学 | 一种便携式高压水射流钢轨打磨入射角标定方法及系统 |
| CN114440926A (zh) * | 2022-01-18 | 2022-05-06 | 武汉元生创新科技有限公司 | 一种陀螺仪检测量程扩展方法及系统 |
| CN116972832A (zh) * | 2022-04-22 | 2023-10-31 | 腾讯科技（深圳）有限公司 | 一种姿态确定方法及相关装置 |
| CN115420249A (zh) * | 2022-08-19 | 2022-12-02 | 智己汽车科技有限公司 | 一种车载四轮定位实时监测系统、监测方法及制动角总成 |
| CN116578816A (zh) * | 2023-05-11 | 2023-08-11 | 常州浩万新能源科技有限公司 | 基于图像识别的两轮平衡车驾驶辅助方法及系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107543546B (zh) | 2021-03-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107543546A (zh) | 一种六轴运动传感器的姿态解算方法及装置 | |
| CN103791905B (zh) | 姿态估计方法和装置 | |
| CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
| Ariffin et al. | Low cost MEMS gyroscope and accelerometer implementation without Kalman Filter for angle estimation | |
| CN109724602A (zh) | 一种基于硬件fpu的姿态解算系统及其解算方法 | |
| US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
| CN105890593A (zh) | 一种mems惯性导航系统及基于该系统的轨迹重构方法 | |
| CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
| US20190025056A1 (en) | Electrostatic offset correction | |
| CN106137209A (zh) | 一种运动姿态检测方法及可穿戴式设备 | |
| Zhilenkov et al. | Based on MEMS sensors man-machine interface for mechatronic objects control | |
| CN108225370A (zh) | 一种运动姿态传感器的数据融合与解算方法 | |
| CN109764870B (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
| CN108369096A (zh) | 对线加速度和角加速度鲁棒的偏航率陀螺仪 | |
| CN108426573A (zh) | 一种终端设备的行人步态检测方法及终端设备 | |
| CN106885566A (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
| Ngo et al. | Experimental comparison of Complementary filter and Kalman filter design for low-cost sensor in quadcopter | |
| CN107402007A (zh) | 一种提高微型ahrs模块精度的方法和微型ahrs模块 | |
| CN109000633A (zh) | 基于异构数据融合的人体姿态动作捕捉算法设计 | |
| CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| Cirillo et al. | A comparison of multisensor attitude estimation algorithms | |
| CN106108909A (zh) | 一种人体姿态检测穿戴设备、系统及控制方法 | |
| CN103557866A (zh) | 一种基于地磁技术的虚拟陀螺仪及算法 | |
| CN108592902A (zh) | 一种基于多传感器的定位设备及定位方法、系统和机械臂 | |
| CN106708088A (zh) | 坐标计算方法及装置、飞行控制方法及系统、无人机 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |