CN113188505B - 姿态角度的测量方法、装置、车辆及智能臂架 - Google Patents

Abstract

本申请提供了一种姿态角度的测量方法、装置、车辆及智能臂架，通过角速度计获取第一姿态角速度、加速度计获取第二姿态角速度，并且加权融合第一姿态角速度和第二姿态角速度，得到最终的姿态角度；利用静止状态时加速度计获取的第二姿态角速度的运动噪音较小、运动状态时角速度计获取的第一姿态角速度的累计误差较小的特性，综合第一姿态角速度和第二姿态角速度的优势，可以得到较为准确的姿态角度信息。

Description

姿态角度的测量方法、装置、车辆及智能臂架

技术领域

本申请涉及姿态测量技术领域，具体涉及一种姿态角度的测量方法、装置、车辆及智能臂架。

背景技术

常见的姿态(例如泵车臂架的姿态、车辆本身的姿态等)表示方法有欧拉角法、方向余弦法等。其中，欧拉角法在求解姿态时存在奇点(万向节死锁)，不能用于全姿态解算，方向余弦可用于全姿态的解算，但计算量大，也无法满足实时性要求。

为准确获取泵车臂架的姿态角度值，对陀螺仪积分或者依据加速度计得到姿态角是一种直观的想法。但是陀螺仪工作一段时间会出现零点漂移，且积分带来的误差随时间将越来越大；加速度计则容易受到噪声或者震动的干扰，因此依据两种方式获取的姿态角都与真实的姿态角相差较大大，从而无法获得准确的姿态角信息。

发明内容

为了解决上述技术问题，提出了本申请。本申请的实施例提供了一种姿态角度的测量方法、装置、车辆及智能臂架，解决上了上述无法获得准确的姿态角信息的问题。

根据本申请的一个方面，提供了一种姿态角度的测量方法，包括：根据角速度计获取第一姿态角速度；根据加速度计获取第二姿态角速度；以及加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度；其中，所述第一姿态角速度的第一权重和所述第二姿态角速度的第二权重根据当前状态确定。

通过角速度计获取第一姿态角速度、加速度计获取第二姿态角速度，并且加权融合第一姿态角速度和第二姿态角速度，得到最终的姿态角度；利用静止状态时加速度计获取的第二姿态角速度的运动噪音较小、运动状态时角速度计获取的第一姿态角速度的累计误差较小的特性，综合第一姿态角速度和第二姿态角速度的优势，可以得到较为准确的姿态角度信息。

在一实施例中，所述第一权重和所述第二权重的确定方式包括：当当前状态为静止状态时，所述第二权重大于所述第一权重；以及当当前状态为运动状态时，所述第二权重小于所述第一权重。

通过设置静止状态时第二权重大于第一权重、运动状态时第二权重小于第一权重，即静止时第二姿态角速度的权重较大，运动时第二姿态角速度的权重较小，从而可以尽量降低静止时第一姿态角速度的误差和运动时第二姿态角速度的运动噪音，以获取较为准确的姿态角度信息。

在一实施例中，所述加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度包括：根据所述第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差；其中所述四元数表征机体坐标系与地理坐标系之间的旋转关系；以及根据所述当前姿态误差和所述第一姿态角速度，得到所述最终姿态角度。

通过上一周期的四元数和上一周期的姿态误差、第二姿态角速度计算当前的姿态误差，并且根据当前的姿态误差和第一姿态角速度，得到最终姿态角度，即综合上一周期的四元数、上一周期的姿态误差和第二姿态角速度，计算得到最终姿态角度，以提高最终姿态角度的准确性，且利用四元数计算量小、无奇点且可以满足运动过程中姿态的实时解算要求的性能，可以降低计算难度且满足各种工况需求。

在一实施例中，所述根据所述第二姿态角速度和上一周期的四元数，计算得到当前姿态误差包括：将所述第二姿态角速度对应的坐标向量和所述上一周期的四元数转换得到的单位向量叉乘，得到叉乘向量；以及根据所述叉乘向量和上一周期的姿态误差，计算得到所述当前姿态误差。

通过当前第二姿态角速度对应的坐标向量和上一周期的四元数转换得到的单位向量叉乘(或外积)得到包括每个坐标方向的坐标值，然后根据叉乘得到的叉乘向量和上一周期的姿态误差，即综合考虑上一周期的姿态误差和当前的第二姿态角速度，得到较为准确的当前姿态误差。

在一实施例中，所述根据所述当前姿态误差和所述第一姿态角速度，得到所述最终姿态角度包括：根据所述当前姿态误差和所述第一姿态角速度，得到修正后的第一姿态角速度；根据所述修正后的第一姿态角速度和所述上一周期的四元数，得到当前四元数；以及根据所述当前四元数，计算得到当前的最终姿态角度。

通过当前姿态误差和第一姿态角速度得到修正后的第一姿态角速度，并利用修正后的第一姿态角速度和上一周期的四元数得到当前四元数，然后将当前四元数转换为欧拉角度，得到最终姿态角度，可以简化计算且保证最终姿态角度的准确性。

在一实施例中，在所述根据角速度计获取第一姿态角速度之后，所述测量方法还包括：对所述第一姿态角速度进行滤波，得到滤波后的第一姿态角速度；其中，所述加权融合所述第一姿态角速度和所述第二姿态角速度包括：加权融合所述滤波后的第一姿态角速度和所述第二姿态角速度。

通过对第一姿态角速度进行滤波，以得到更为准确的第一姿态角速度信息，然后基于该第一姿态角速度信息，可以计算得到更为准确的第一姿态角速度，从而为后续的姿态测量提供了准确的基础数据，以提供测量的准确度。

在一实施例中，在所述根据加速度计获取第二姿态角速度之后，所述测量方法还包括：对所述第二姿态角速度进行滤波，得到滤波后的第二姿态角速度；其中，所述加权融合所述第一姿态角速度和所述第二姿态角速度包括：加权融合所述滤波后的第一姿态角速度和所述滤波后的第二姿态角速度。

通过对第二姿态角速度进行滤波，以得到更为准确的第二姿态角速度信息，然后基于该第二姿态角速度信息，可以计算得到更为准确的第二姿态角速度，从而为后续的姿态测量提供了准确的基础数据，以提供测量的准确度。

根据本申请的一个方面，提供了一种姿态角度的测量装置，包括：第一姿态计算模块，用于根据角速度计获取第一姿态角速度；第二姿态计算模块，用于根据加速度计获取第二姿态角速度；以及加权模块，用于加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度；其中，所述第一姿态角速度的第一权重和所述第二姿态角速度的第二权重根据当前状态确定。

通过第一姿态计算模块根据角速度计获取第一姿态角速度、第二姿态计算模块根据加速度计获取第二姿态角速度，并且加权模块加权融合第一姿态角速度和第二姿态角速度，得到最终的姿态角度；利用静止状态时第二姿态角速度的运动噪音较小、运动状态时第一姿态角速度的累计误差较小的特性，综合第一姿态角速度和第二姿态角速度的优势，可以得到较为准确的姿态角度信息。

根据本申请的一个方面，提供了一种车辆，所述车辆包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于执行上述任一所述的臂架姿态角度的测量方法。

根据本申请的一个方面，提供了一种智能臂架，所述智能臂架包括：处理器；用于存储所述处理器可执行指令的存储器；所述处理器，用于执行上述任一所述的姿态角度的测量方法。

附图说明

通过结合附图对本申请实施例进行更详细的描述，本申请的上述以及其他目的、特征和优势将变得更加明显。附图用来提供对本申请实施例的进一步理解，并且构成说明书的一部分，与本申请实施例一起用于解释本申请，并不构成对本申请的限制。在附图中，相同的参考标号通常代表相同部件或步骤。

图1是本申请一示例性实施例提供的一种姿态角度的测量方法的流程示意图。

图2是本申请一示例性实施例提供的一种姿态角度融合方法的流程示意图。

图3是本申请一示例性实施例提供的一种姿态误差计算方法的流程示意图。

图4是本申请一示例性实施例提供的一种最终姿态角度计算方法的流程示意图。

图5是本申请另一示例性实施例提供的一种姿态角度的测量方法的流程示意图。

图6是本申请一示例性实施例提供的一种卡尔曼滤波方法的流程示意图。

图7是本申请另一示例性实施例提供的一种姿态角度的测量方法的流程示意图。

图8是本申请一示例性实施例提供的一种姿态角度的测量装置的结构示意图。

图9是本申请另一示例性实施例提供的一种姿态角度的测量装置的结构示意图。

图10是本申请一示例性实施例提供的智能臂架的结构图。

图11是本申请一示例性实施例提供的车辆的结构图。

具体实施方式

下面，将参考附图详细地描述根据本申请的示例实施例。显然，所描述的实施例仅仅是本申请的一部分实施例，而不是本申请的全部实施例，应理解，本申请不受这里描述的示例实施例的限制。

申请概述

以安装在臂架为例，可以利用安装在臂架上的陀螺仪，测量臂架倾斜的角速度，由于陀螺仪输出的是臂架的角速度，不会受到臂架振动影响，因此该信号中噪声很小。臂架的角度又可以通过对角速度积分而得，因此臂架控制所需要的角度和角速度可以使用陀螺仪所采集的信号中获取。但是，由于从陀螺仪的角速度获得角度信息，需要经过积分运算，若角速度信号存在微小的偏差，经过积分运算之后，将会形成积累误差，这个误差会随着时间延长逐步增加，最终导致电路饱和，导致角度信息偏差较大。

在臂架上安装加速度计，可以测量臂架的重力加速度，继而计算获得臂架的角速度信息。由于加速度计获得的角速度信息为当前瞬时角度信息，不会存在积累误差，但是，加速度计所产生的角速度信息中会叠加很强的有四轴运动加速度噪声信号。

综上，在使用陀螺仪检测角度时，在静止状态下存在无法测量的缺陷，且受时间积分累计的影响；而加速度计检测角度时，在运动状态下存在加速度噪声大的缺陷。

出于解决上述陀螺仪测量角度和加速度计测量角度时的缺陷，可以综合考虑陀螺仪和加速度计的缺陷和优势，利用优势互补的原则弥补陀螺仪和加速度计单独测量角度信息的缺陷。本申请提出了一种姿态的测量方法和装置，通过同时设置陀螺仪和加速度计测量臂架的角速度信息，并且在静止状态时，增大加速度计测量的数据的权重，当处于运动状态时，增大陀螺仪测量的数据的权重，从而获得更准确的姿态数据。

下面结合附图具体说明本申请实施例提供的姿态角度的测量方法、装置、车辆和智能臂架的具体实现方式。

示例性方法

图1是本申请一示例性实施例提供的一种姿态角度的测量方法的流程示意图。如图1所示，该姿态角度的测量方法的应用范围，包括但不限于智能臂架姿态测量及车辆本体姿态测量。下面以泵车臂架为例进行详细说明，该姿态角度的测量方法具体包括如下步骤：

步骤110：根据角速度计获取第一姿态角速度。

可以通过在臂架端部设置陀螺仪等角速度计实时测量臂架端部的角速度，根据实时测量的角速度计算得到臂架的第一姿态角速度信息。

步骤120：根据加速度计获取第二姿态角速度。

可以通过在臂架端部设置的加速度计等测量仪器实时测量臂架端部的加速度，根据该实时测量的加速度计算得到臂架的第二姿态角速度信息，其中，加速度计可以为重力加速度计。

步骤130：加权融合第一姿态角速度和第二姿态角速度，得到最终姿态角度；其中，第一姿态角速度的第一权重和第二姿态角速度的第二权重根据当前状态确定。

由于臂架端部的当前状态(包括运动状态和静止状态)对陀螺仪和加速度计的测量精度有较大影响，因此，在计算得到了第一姿态角速度和第二姿态角速度后，可以根据臂架端部的当前状态确定第一姿态角速度和第二姿态角速度的权重，并将第一姿态角速度和第二姿态角速度加权融合，以综合第一姿态角速度信息和第二姿态角速度信息，利用优势互补以弥补第一姿态角速度和第二姿态角速度的缺陷，从而提供测量的准确度。

应当理解，本申请实施例中的步骤110和步骤120可以同时执行，也可以先执行步骤110、再执行步骤120，或者可以先执行步骤120、再执行步骤110，只要在执行步骤130之前完成步骤110和步骤120即可。

本申请提供的一种姿态的测量方法，通过角速度计获取第一姿态角速度、加速度计获取第二姿态角速度，并且加权融合第一姿态角速度和第二姿态角速度，得到最终的姿态角度；利用静止状态时加速度计获取的第二姿态角速度的运动噪音较小、运动状态时角速度计获取的第一姿态角速度的累计误差较小的特性，综合第一姿态角速度和第二姿态角速度的优势，可以得到较为准确的姿态角度信息。

在一实施例中，第一权重和第二权重的确定方式可以包括：当当前状态为静止状态时，第二权重大于第一权重；以及当当前状态为运动状态时，第二权重小于第一权重。

通过设置臂架端部为静止状态时第二权重大于第一权重、臂架端部为运动状态时第二权重小于第一权重，即当臂架端部静止时第二姿态角速度的权重较大以尽量降低静止时第一姿态角速度的误差，当臂架端部运动时第一姿态角速度的权重较大以尽量降低运动时第二姿态角速度的运动噪音，从而获取较为准确的姿态角度信息。

图2是本申请一示例性实施例提供的一种姿态角度融合方法的流程示意图。如图2所示，上述步骤130可以包括：

步骤131：根据第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差；其中四元数表征机体坐标系与地理坐标系之间的旋转关系。

四元数是简单的超复数，由实数加上三个虚数单位i、j、k组成，每个四元数都是1、i、j和k的线性组合，即是四元数一般可表示为q＝a+bi+cj+dk，其中a、b、c、d是实数。对于i、j、k本身的几何意义为一种旋转，其中i旋转代表X轴与Y轴相交平面中X轴正向向Y轴正向的旋转，j旋转代表Z轴与X轴相交平面中Z轴正向向X轴正向的旋转，k旋转代表Y轴与Z轴相交平面中Y轴正向向Z轴正向的旋转。

由于欧拉角直接求解姿态时会存在奇点(万向节死锁)，万向节死锁是指物体的两个旋转轴指向同一个方向，当两个旋转轴平行时，绕一个轴旋转可能会覆盖住另一个轴的旋转，从而失去一个维度的自由度。方向余弦的计算量较大，对于实时测量动态的臂架姿态角度，显然是不利于提高计算效率的。为了避免万向节死锁的同时提高计算效率，本申请通过四元数和实测的第二姿态角速度来计算姿态误差，既能避免万向节死锁，而且能够避免三角函数的复杂计算。并且由于四元数是根据上一周期的四元数和传感器实测并计算得到的姿态角度值以及上一周期的姿态误差迭代得到，即兼顾了以往数据和当前实测数据，可以更为准确的计算得到当前的姿态误差值。

步骤132：根据当前姿态误差和第一姿态角速度，得到最终姿态角度。

在计算得到当前姿态误差后，基于该姿态误差，对第一姿态角速度进行修正，以得到最终姿态角度，即综合以往数据和第二姿态角速度来修正第一姿态角速度，从而得到更为准确的姿态角度。

通过上一周期的四元数和第二姿态角速度计算当前的姿态误差，并且根据当前的姿态误差修正第一姿态角速度，得到最终姿态角度，即综合第二姿态角速度以及以往的累计数据，得到最终姿态角度，以提高最终姿态角度的准确性，且利用四元数计算量小、无奇点且可以满足泵车臂架运动过程中姿态的实时解算要求的性能，可以降低计算难度且满足各种工况需求。

图3是本申请一示例性实施例提供的一种姿态误差计算方法的流程示意图。如图3所示，上述步骤131可以包括：

步骤1311：将第二姿态角速度对应的坐标向量和上一周期的四元数转换得到的单位向量叉乘，得到叉乘向量。

具体的，记第二姿态角速度对应的坐标向量为[ax,ay,az]，记上一周期的四元数为q(n-1)＝q₀(n-1)+q₁(n-1)i+q₂(n-1)j+q₃(n-1)k(本申请中无特殊说明时记第n个周期为当前周期)，记上一周期的四元数转换得到的单位向量为[vx,vy,vz]；其中：

vx＝2*(q₁(n-1)*q₃(n-1)–q₀(n-1)*q₂(n-1))，

vy＝2*(q₀(n-1)*q₁(n-1)+q₂(n-1)*q₃(n-1))，

vz＝q₀(n-1)*q₀(n-1)+q₃(n-1)*q₃(n-1)–q₁(n-1)*q₁(n-1)-q₂(n-1)*q₂(n-1)。

则叉乘向量[ex,ey,ez]＝[ay*vz-az*vy,az*vx-ax*vz,ax*vy-ay*vx]。

步骤1312：根据叉乘向量和上一周期的姿态误差，计算得到当前姿态误差。

在计算得到叉乘向量之后，即获取了当前周期的第二姿态角速度与上一周期的四元数转换得到的单位向量之间的差值(即叉乘向量)，再根据该值和上一周期的姿态误差，计算得到当前周期的姿态误差。具体的计算方式可以是：

记当前周期的姿态误差为[exInt(n),eyInt(n),ezInt(n)]，则当前周期的姿态误差可以通过以下公式计算得到：

exInt(n)＝exInt(n-1)+ex*Ki；

eyInt(n)＝eyInt(n-1)+ey*Ki；

ezInt(n)＝ezInt(n-1)+ez*Ki；

其中，Ki为比例修正系数，可以根据具体的臂架结构预先设定。

通过当前第二姿态角速度对应的坐标向量和上一周期的四元数转换得到的单位向量叉乘(或外积)得到包括每个坐标方向的坐标值，然后根据叉乘得到的叉乘向量和上一周期的姿态误差，即综合考虑上一周期的姿态误差和当前的第二姿态角速度，得到较为准确的当前姿态误差。

图4是本申请一示例性实施例提供的一种最终姿态角度计算方法的流程示意图。如图4所示，上述步骤132可以包括：

步骤1321：根据当前姿态误差和第一姿态角速度，得到修正后的第一姿态角速度。

在通过上述实施例所提供的方法计算得到了当前姿态误差后，可以根据当前姿态误差修正角速度积分的漂移，即根据当前姿态误差调整更新第一姿态角速度。具体的，记当前周期实测的第一姿态角速度为[gx,gy,gz]，修正后的第一姿态角速度为[gx',gy',gz']，则gx'＝gx+Kp*ex+exInt(n)；gy'＝gy+Kp*ey+eyInt(n)；gz'＝gz+Kp*ez+ezInt(n)；其中Kp为积分修正系数，可以根据具体的臂架结构预先设定。

步骤1322：根据修正后的第一姿态角速度和上一周期的四元数，得到当前四元数。

为了进一步降低计算难度、提高计算效率，本申请提出直接利用四元数的方法计算得到最终姿态角度，在保证最终姿态角度的准确性的前提下，提高计算效率。本申请可以根据修正后的第一姿态角速度和上一周期的四元数得到当前的四元数，从而可以利用臂架的连续运动特性综合考虑上一周期的四元数和当前姿态角速度，以得到较为准确的当前四元数。具体的，当前四元数的迭代方式可以是：

q₀(n)＝q₀(n-1)+(-q₁(n-1)*gx'-q₂(n-1)*gy'-q₃(n-1)*gz')*T/2；

q₁(n)＝q₁(n-1)+(q₀(n-1)*gx'+q₂(n-1)*gz'-q₃(n-1)*gy')*T/2；

q₂(n)＝q₂(n-1)+(q₀(n-1)*gy'-q₁(n-1)*gz'+q₃(n-1)*gx')*T/2；

q₃(n)＝q₃(n-1)+(q₀(n-1)*gz'+q₁(n-1)*gy'-q₂(n-1)*gx')*T/2；

其中，T为时间周期。

步骤1323：根据当前四元数，计算得到当前的最终姿态角度。

由于机体坐标系与地理坐标系之间的转换矩阵可以由四元数表示，也可以由欧拉角表示，欧拉角是用来确定定点转动位置的3个一组独立角参量，由俯仰角、横滚角、航向角组成。因此，可以建立四元数与欧拉角之间的对应关系，从而可以根据当前四元数计算得到当前的欧拉角度(即最终姿态角度)。具体的，上述转换矩阵由四元数表示为：

上述转换矩阵由欧拉角表示为：

则可以得出欧拉角的俯仰角θ、横滚角γ和航向角ψ分别为：

θ＝arcsin(2q₀q₂-2q₁q₃)*π/3；

其中，

因此，可以根据当前四元数计算得到当前的欧拉角度，从而简单的计算得到当前周期准确的姿态角度。通过当前姿态误差更新第一姿态角速度，并利用修正后的第一姿态角速度和上一周期的四元数得到当前四元数，然后将当前四元数转换为欧拉角度，得到最终姿态角度，可以简化计算且确保最终姿态角度的准确性。

图5是本申请另一示例性实施例提供的一种姿态角度的测量方法的流程示意图。如图5所示，在步骤110之后，上述测量方法还可以包括：

步骤140：对第一姿态角速度进行滤波，得到滤波后的第一姿态角速度。

相应的步骤130调整为：加权融合滤波后的第一姿态角速度和第二姿态角速度；其中，滤波后的第一姿态角速度的第一权重和第二姿态角速度的第二权重根据当前状态确定。通过对臂架端部的第一姿态角速度进行滤波，以得到更为准确的角速度信息，然后基于该角速度信息，可以计算得到更为准确的第一姿态角速度，从而为后续的臂架姿态测量提供了准确的基础数据，以提供测量的准确度。

具体的，可以利用卡尔曼滤波器对实测的第一姿态角速度进行滤波，卡尔曼滤波是一个迭代的过程，如图6所示，其具体的实现方式可以包括如下步骤：

步骤610：根据上一时刻的状态值预测当前时刻的状态估计值。

具体的，当前时刻的状态估计值＝上一时刻的状态值+调整值；其中，上一时刻的状态值为上一时刻的状态最优值，调整值可以为预先设定的定值。

步骤620：根据上一时刻的系统协方差估计当前时刻的系统协方差。

具体的，当前时刻的系统协方差＝上一时刻的系统协方差+系统噪声协方差；其中，系统噪声协方差根据当前系统的噪声确定。

步骤630：根据当前时刻的系统协方差和测量噪声协方差，计算当前时刻的卡尔曼增益。

具体的，当前时刻的卡尔曼增益＝当前时刻的系统协方差/(当前时刻的系统协方差+测量噪声协方差)；其中，测量噪声协方差为当前实际测量得到的噪声协方差。

步骤640：根据当前时刻的卡尔曼增益、当前时刻的状态估计值、当前时刻的测量值，计算得到当前时刻的最优状态值。

具体的，当前时刻的最优状态值＝当前时刻的状态值+当前时刻的卡尔曼增益*(当前时刻的测量值-当前时刻的状态估计值)；其中，当前时刻的测量值为当前时刻实际测量得到的值(例如实测的角速度)。

步骤650：根据上一时刻的系统噪声协方差和当前时刻的卡尔曼增益，计算当前时刻的系统噪声协方差。

具体的，当前时刻的系统噪声协方差＝上一时刻的系统噪声协方差/(1-当前时刻的卡尔曼增益)。

通过循环执行上述步骤，可以迭代获取当前时刻的最优状态值，即较为准确的当前角速度。

图7是本申请另一示例性实施例提供的一种姿态角度的测量方法的流程示意图。如图7所示，在步骤120之后，上述测量方法还可以包括：

步骤150：对第二姿态角速度进行滤波，得到滤波后的第二姿态角速度。

相应的步骤130调整为：加权融合滤波后的第一姿态角速度和滤波后的第二姿态角速度；其中，滤波后的第一姿态角速度的第一权重和滤波后的第二姿态角速度的第二权重根据当前状态确定。通过对臂架端部的加速度计获取的第二姿态角速度进行滤波，以得到更为准确的角速度信息，然后基于该角速度信息，可以计算得到更为准确的第二姿态角速度，从而为后续的臂架姿态测量提供了准确的基础数据，以提供测量的准确度。具体的，可以利用卡尔曼滤波器对实测的重力加速度进行滤波，该滤波的具体过程与上述实施例中对角速度滤波的过程相同，此处不再赘述。

在一实施例中，在实测得到第二姿态角速度之后，还可以对该第二姿态角速度进行归一化处理，即将实测的第二姿态角速度对应向量的元素都除以该向量的模以得到单位向量，从而可以方便后续迭代过程中的单位向量的获取，继而简化计算。应当理解，本申请实施例中的归一化处理可以在卡尔曼滤波之前或之后，只要能够方便后续计算即可。

示例性装置

图8是本申请一示例性实施例提供的一种姿态角度的测量装置的结构示意图。如图8所示，该姿态的测量装置80包括：第一姿态计算模块81，用于根据角速度计获取第一姿态角速度；第二姿态计算模块82，用于根据加速度计获取第二姿态角速度；以及加权模块83，用于加权融合第一姿态角速度和第二姿态角速度，得到最终姿态角度；其中，第一姿态角速度的第一权重和第二姿态角速度的第二权重根据当前状态确定。

通过第一姿态计算模块81根据角速度计获取第一姿态角速度、第二姿态计算模块82根据加速度计获取第二姿态角速度，并且加权模块83加权融合第一姿态角速度和第二姿态角速度，得到最终的姿态角度；利用静止状态时第二姿态角速度的运动噪音较小、运动状态时第一姿态角速度的累计误差较小的特性，综合第一姿态角速度和第二姿态角速度的优势，可以得到较为准确的姿态角度信息。

在一实施例中，加权模块83可以进一步配置为：当当前状态为静止状态时，第二权重大于第一权重；以及当当前状态为运动状态时，第二权重小于第一权重。

图9是本申请另一示例性实施例提供的一种姿态角度的测量装置的结构示意图。如图9所示，上述加权模块83可以包括：姿态误差计算单元831，用于根据第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差；其中四元数表征机体坐标系与地理坐标系之间的旋转关系；最终姿态角度计算单元832，用于根据当前姿态误差和第一姿态角速度，得到最终姿态角度。

在一实施例中，姿态误差计算单元831可以进一步配置为：将第二姿态角速度对应的坐标向量和上一周期的四元数转换得到的单位向量叉乘，得到叉乘向量；根据叉乘向量和上一周期的姿态误差，计算得到当前姿态误差。

在一实施例中，最终姿态角度计算单元832可以进一步配置为：根据当前姿态误差和第一姿态角速度，得到修正后的第一姿态角速度；根据修正后的第一姿态角速度和上一周期的四元数，得到当前四元数；根据当前四元数，计算得到当前的最终姿态角度。

在一实施例中，如图9所示，上述姿态的测量装置80还可以包括：第一滤波模块84，用于对第一姿态角速度进行滤波，得到滤波后的第一姿态角速度。相应的加权模块83配置为：加权融合滤波后的第一姿态角速度和第二姿态角速度。

在一实施例中，如图9所示，上述姿态的测量装置80还可以包括：第二滤波模块85，用于对第二姿态角速度进行滤波，得到滤波后的第二姿态角速度。相应的加权模块83配置为：加权融合第一姿态角速度和滤波后的第二姿态角速度。

在一实施例中，第一滤波模块84可以进一步配置为：根据上一时刻的状态值预测当前时刻的状态估计值；根据上一时刻的系统协方差估计当前时刻的系统协方差；根据当前时刻的系统协方差和测量噪声协方差，计算当前时刻的卡尔曼增益；根据当前时刻的卡尔曼增益、当前时刻的状态估计值、当前时刻的测量值，计算得到当前时刻的最优状态值；根据上一时刻的系统噪声协方差和当前时刻的卡尔曼增益，计算当前时刻的系统噪声协方差。

在一实施例中，第二滤波模块85可以进一步配置为：根据上一时刻的状态值预测当前时刻的状态估计值；根据上一时刻的系统协方差估计当前时刻的系统协方差；根据当前时刻的系统协方差和测量噪声协方差，计算当前时刻的卡尔曼增益；根据当前时刻的卡尔曼增益、当前时刻的状态估计值、当前时刻的测量值，计算得到当前时刻的最优状态值；根据上一时刻的系统噪声协方差和当前时刻的卡尔曼增益，计算当前时刻的系统噪声协方差。

在一实施例中，第二姿态计算模块82可以进一步配置为：在实测得到第二姿态角速度之后，还可以对该第二姿态角速度进行归一化处理。

示例性设备

下面，参考图10和图11来描述根据本申请实施例的智能臂架和车辆的电子结构。该智能臂架和车辆可以包括第一设备和第二设备中的任一个或两者、或与它们独立的单机设备，该单机设备可以与第一设备和第二设备进行通信，以从它们接收所采集到的输入信号。

图10和图11分别图示了根据本申请实施例的智能臂架和车辆的框图。

如图10和图11所示，智能臂架10和车辆20包括一个或多个处理器11和存储器12。

处理器11可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制智能臂架10和车辆20中的其他组件以执行期望的功能。

存储器12可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器11可以运行所述程序指令，以实现上文所述的本申请的各个实施例的臂架姿态角度的测量方法以及/或者其他期望的功能。在所述计算机可读存储介质中还可以存储诸如输入信号、信号分量、噪声分量等各种内容。

在一个示例中，智能臂架10和车辆20还可以包括：输入装置13和输出装置14，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。

例如，在该智能臂架包括第一设备或第二设备时，该输入装置13可以是摄像头，用于捕捉图像的输入信号。在该智能臂架包括单机设备时，该输入装置13可以是通信网络连接器，用于从第一设备和第二设备接收所采集的输入信号。

此外，该输入设备13还可以包括例如键盘、鼠标等等。

该输出装置14可以向外部输出各种信息，包括确定出的距离信息、方向信息等。该输出设备14可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。

当然，为了简化，图10和图11中仅示出了该智能臂架10和车辆20中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，智能臂架10和车辆20还可以包括任何其他适当的组件。

示例性计算机程序产品和计算机可读存储介质

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的臂架姿态角度的测量方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的臂架姿态角度的测量方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims (9)

1.一种姿态角度的测量方法，其特征在于，包括：

根据角速度计获取第一姿态角速度；

根据加速度计获取第二姿态角速度；以及

加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度；

其中，所述第一姿态角速度的第一权重和所述第二姿态角速度的第二权重根据当前状态确定；

其中，所述加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度包括：

根据所述第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差；其中所述四元数表征机体坐标系与地理坐标系之间的旋转关系；以及

根据所述当前姿态误差和所述第一姿态角速度，得到所述最终姿态角度。

2.根据权利要求1所述的姿态角度的测量方法，其特征在于，所述第一权重和所述第二权重的确定方式包括：

当当前状态为静止状态时，所述第二权重大于所述第一权重；以及

当当前状态为运动状态时，所述第二权重小于所述第一权重。

3.根据权利要求1所述的姿态角度的测量方法，其特征在于，所述根据所述第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差包括：

将所述第二姿态角速度对应的坐标向量和所述上一周期的四元数转换得到的单位向量叉乘，得到叉乘向量；以及

根据所述叉乘向量和上一周期的姿态误差，计算得到所述当前姿态误差。

4.根据权利要求3所述的姿态角度的测量方法，其特征在于，所述根据所述当前姿态误差和所述第一姿态角速度，得到所述最终姿态角度包括：

根据所述当前姿态误差和所述第一姿态角速度，得到修正后的第一姿态角速度；

根据所述修正后的第一姿态角速度和所述上一周期的四元数，得到当前四元数；以及

根据所述当前四元数，计算得到当前的最终姿态角度。

5.根据权利要求1所述的姿态角度的测量方法，其特征在于，在所述根据角速度计获取第一姿态角速度之后，还包括：

对所述第一姿态角速度进行滤波，得到滤波后的第一姿态角速度；其中，所述加权融合所述第一姿态角速度和所述第二姿态角速度包括：加权融合所述滤波后的第一姿态角速度和所述第二姿态角速度。

6.根据权利要求5所述的姿态角度的测量方法，其特征在于，在所述根据加速度计获取第二姿态角速度之后，还包括：

对所述第二姿态角速度进行滤波，得到滤波后的第二姿态角速度；

其中，所述加权融合所述第一姿态角速度和所述第二姿态角速度包括：

加权融合所述滤波后的第一姿态角速度和所述滤波后的第二姿态角速度。

7.一种姿态角度的测量装置，其特征在于，包括：

第一姿态计算模块，用于根据角速度计获取第一姿态角速度；

第二姿态计算模块，用于根据加速度计获取第二姿态角速度；以及

加权模块，用于加权融合所述第一姿态角速度和所述第二姿态角速度，得到最终姿态角度；

其中，所述第一姿态角速度的第一权重和所述第二姿态角速度的第二权重根据当前状态确定；

其中，所述加权模块包括：

姿态误差计算单元，用于根据所述第二姿态角速度、上一周期的四元数和上一周期的姿态误差，计算得到当前姿态误差；其中所述四元数表征机体坐标系与地理坐标系之间的旋转关系；

最终姿态角度计算单元，用于根据所述当前姿态误差和所述第一姿态角速度，得到所述最终姿态角度。

8.一种车辆，其特征在于，包括：

处理器；

用于存储所述处理器可执行指令的存储器；

所述处理器用于运行所述可执行指令以实现上述权利要求1-6任一所述的姿态角度的测量方法。

9.一种智能臂架，其特征在于，包括：

处理器；

用于存储所述处理器可执行指令的存储器；

所述处理器用于运行所述可执行指令以实现上述权利要求1-6任一所述的姿态角度的测量方法。

Images