CN110207687B - 物体姿态测量方法、测量装置、电子设备及存储介质 - Google Patents

Abstract

本发明提供一种物体姿态测量方法，其包括计算待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；计算待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；通过第二传感器，获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；计算待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；以及使用待测量物体的融合预测状态变量矩阵以及待测量物体的融合预测状态变量协方差矩阵，获取待测量物体的预测位置坐标以及预测姿态。本发明还提供一种物体姿态测量装置，本发明的物体姿态测量方法及物体姿态测量装置提高了获取的被测物体的姿态位置的准确性。

Description

物体姿态测量方法、测量装置、电子设备及存储介质

技术领域

本发明涉及物体姿态测量领域，特别是涉及一种物体姿态测量方法、测量装置、电子设备及存储介质。

背景技术

随着VR(虚拟现实，Virtual Reality)技术以及AR(增强现实，AugmentedReality)技术的广泛使用，为了使得对应的VR或AR产品的体验效果更好，人们对被测物体(如VR头盔等)的实时姿态测量的要求也越来越高。

现有的物体姿态测量一般是通过不同种类的传感器进行姿态数据的融合计算得到的。假设有A、B两种传感器，A传感器设置在被测物体上，用于获取被测物体的六自由度姿态；B传感器也设置在被测物体上，用于获取B传感器自身的加速度以及角速度。这样物体姿态测量装置通过对上述A传感器和B传感器获取的姿态数据进行融合计算，即可获取被测物体的姿态位置。

但是物体姿态测量装置进行融合计算时存在以下问题：

1、融合计算需要对被测物体的坐标系和B传感器的坐标系进行转换，而这个坐标系的转换难度较大；

2、B传感器的测量值的偏移误差会随机变化而不固定；

3、B传感器的重力大小和方向会随着B传感器开机时的姿态变化而发生变化。

因此现有的物体姿态测量中B传感器由于上述融合计算的问题，导致经过融合计算后的数据准确性较差，从而使得获取的被测物体的姿态位置准确性也较差。

发明内容

本发明实施例提供一种可准确获取被测物体的姿态位置的物体姿态测量方法以及测量装置；以解决现有的物体姿态测量方法以及测量装置由于传感器中的测量值误差变化导致获取的被测物体的姿态位置的准确性较低的技术问题。

本发明实施例提供一种物体姿态测量方法，其包括：

获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；

获取所述第一传感器的第二参数；并根据所述第一传感器的第二参数、所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定所述待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；

通过所述第二传感器，获取所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；

根据所述待测量物体的第一预测状态变量矩阵、所述待测量物体的第一预测状态变量协方差矩阵、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定所述待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；以及

基于所述待测量物体的融合预测状态变量矩阵以及所述待测量物体的融合预测状态变量协方差矩阵，获取所述待测量物体的预测位置坐标以及预测姿态。

本发明实施例还提供一种物体姿态测量装置，其包括：

当前状态参数计算模块，用于获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；

第一预测状态参数计算模块，用于获取所述第一传感器的第二参数；并根据所述第一传感器的第二参数、所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定所述待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；

第二预测状态参数获取模块，用于通过所述第二传感器，获取所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；

数据融合模块，用于根据所述待测量物体的第一预测状态变量矩阵、所述待测量物体的第一预测状态变量协方差矩阵、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定所述待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；以及

物体位置姿态获取模块，用于基于所述待测量物体的融合预测状态变量矩阵以及所述待测量物体的融合预测状态变量协方差矩阵，获取所述待测量物体的预测位置坐标以及预测姿态。

本发明实施例还提供一种计算机可读存储介质，其内存储有处理器可执行指令，所述指令由一个或一个以上处理器加载，以执行上述的物体姿态测量方法。

相较于现有技术，本发明的物体姿态测量方法以及测量装置实时根据第一传感器的测量参数以及第二传感器的测量参数，对待测量物体的预测状态变量矩阵以及预测状态变量协方差矩阵进行修正，从而及时补偿了传感器中的测量值误差，提高了获取的被测物体的姿态位置的准确性；解决了现有的物体姿态测量方法以及测量装置由于传感器中的测量值误差变化导致获取的待测量物体的姿态位置的准确性较低的技术问题。

附图说明

图1为本发明的物体姿态测量方法的实施例的流程图；

图2为本发明的物体姿态测量装置的实施例的结构示意图；

图3为本发明的物体姿态测量装置的实施例的当前状态参数计算模块的结构示意图；

图4为本发明的物体姿态测量装置的实施例的第一预测状态参数计算模块的结构示意图；

图5为本发明的物体姿态测量装置的实施例的数据融合模块的结构示意图；

图6A为本发明的物体姿态测量方法及物体姿态测量装置的第一具体实施例的示意图；

图6B为本发明的物体姿态测量方法及物体姿态测量装置的第二具体实施例的示意图；

图7为本发明的物体姿态测量方法及物体姿态测量装置的具体实施例的流程图；

图8为本发明的物体姿态测量装置所在的电子设备的工作环境结构示意图。

具体实施方式

请参照图式，其中相同的组件符号代表相同的组件，本发明的原理是以实施在一适当的运算环境中来举例说明。以下的说明是基于所例示的本发明具体实施例，其不应被视为限制本发明未在此详述的其它具体实施例。

在以下的说明中，本发明的具体实施例将参考由一部或多部计算机所执行之作业的步骤及符号来说明，除非另有述明。因此，其将可了解到这些步骤及操作，其中有数次提到为由计算机执行，包括了由代表了以一结构化型式中的数据之电子信号的计算机处理单元所操纵。此操纵转换该数据或将其维持在该计算机之内存系统中的位置处，其可重新配置或另外以本领域技术人员所熟知的方式来改变该计算机之运作。该数据所维持的数据结构为该内存之实体位置，其具有由该数据格式所定义的特定特性。但是，本发明原理以上述文字来说明，其并不代表为一种限制，本领域技术人员将可了解到以下所述的多种步骤及操作亦可实施在硬件当中。

本发明的物体姿态测量方法及测量装置可设置在任何的电子设备，用于获取第一传感器的加速度以及角速度的测量数值以及第二传感器的待测量物体的六自由度的测量数值，并对上述测量数据进行融合操作，以获取待测量物体的预测位置坐标以及预测姿态。该电子设备包括但不限于可穿戴设备、头戴设备、医疗健康平台、个人计算机、服务器计算机、手持式或膝上型设备、移动设备(比如移动电话、个人数字助理(PDA)、媒体播放器等等)、多处理器系统、消费型电子设备、小型计算机、大型计算机、包括上述任意系统或设备的分布式计算环境，等等。该电子设备优选为物体姿态测量终端或物体姿态测量服务器，该物体姿态测量终端或物体姿态测量服务器同时与第一传感器和第二传感器连接，以便实时采集第一传感器和第二传感器的测量参数，并对传感器中的测量值误差进行补偿，从而可准确的获取待测量物体的姿态位置。

请参照图1，图1为本发明的物体姿态测量方法的实施例的流程图。本实施例的物体姿态测量方法可使用上述的电子设备进行实施，本实施例的物体姿态测量方法包括：

步骤S101，获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；其中第一传感器设置在待测量物体上，用于获取第一传感器自身的加速度以及角速度；

步骤S102，获取所述第一传感器的第二参数；并根据第一传感器的第二参数、待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；

步骤S103，通过第二传感器，获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；其中第二传感器设置在待测量物体外或待测量物体上，用于获取待测量物体的六自由度状态；

步骤S104，根据待测量物体的第一预测状态变量矩阵、待测量物体的第一预测状态变量协方差矩阵、待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；

步骤S105，基于待测量物体的融合预测状态变量矩阵以及待测量物体的融合预测状态变量协方差矩阵，获取待测量物体的预测位置坐标以及预测姿态。

下面详细说明本实施例的物体姿态测量方法的待测量物体的位置姿态的获取过程。

在步骤S101中，物体姿态测量装置(如物体姿态测量终端或物体姿态测量服务器等)获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系。

第一传感器设置在待测量物体上，用于获取第一传感器自身的加速度以及角速度等相关的测量参数。待测量物体坐标系是以待测量物体所在位置为坐标系中心建立的坐标系，如直角坐标系、平面极坐标系、柱面坐标系或球面坐标系等。传感器坐标系是以传感器所在位置为坐标系中心建立的坐标系，如直角坐标系、平面极坐标系、柱面坐标系或球面坐标系等。待测量物体坐标系和传感器坐标系的相对位置关系用于实现待测量物体坐标系中的数据与传感器坐标系中的数据的相互转换。

也可以通过设置待测量物体上第一传感器的传感器坐标系和待测量物体坐标系的方式，获取待测量物体坐标系和传感器坐标系的相对位置关系。

随后物体姿态测量装置根据获取的第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵。

具体的，第一传感器的第一参数可包括第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前姿态(即第一传感器在传感器坐标系中的旋转角度)、第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前角速度偏移、第一传感器测量的当前加速度偏移、以及第一传感器在传感器坐标系中的重力向量。

待测量物体坐标系和传感器坐标系的相对位置关系可包括待测量物体坐标系在传感器坐标系中的当前位置坐标、以及待测量物体坐标系在传感器坐标系中的当前姿态。

待测量物体的当前状态变量矩阵可为：

其中r_wi,k-1为第一传感器在传感器坐标系中的当前位置坐标，q_wi,k-1为第一传感器在传感器坐标系中的当前姿态，v_w,k-1为第一传感器在传感器坐标系中的当前运动速度，b_g,k-1为第一传感器测量的当前角速度偏移，b_a,k-1为第一传感器测量的当前加速度偏移，g_w,k-1为第一传感器在传感器坐标系中的当前重力向量，q_ic,k-1为待测量物体坐标系在传感器坐标系中的当前姿态，t_ic,k-1为待测量物体坐标系在传感器坐标系中的当前位置坐标。

待测量物体的当前状态变量协方差矩阵为待测量物体的当前状态变量矩阵中的各个状态变量之间的协方差。协方差表示的是两个状态变量的总体误差。

在步骤S102中，物体姿态测量装置获取第一传感器的第二参数，该第一传感器的第二参数可包括第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的角速度噪声以及第一传感器的加速度噪声。

随后物体姿态测量装置根据第一传感器的第二参数、待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵。

具体的，物体姿态测量装置可根据测量时间间隔、第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的测量噪声、以及测量物体的当前状态变量矩阵，计算待测量物体的第一预测状态变量矩阵，可通过以下公式计算待测量物体的第一预测状态变量矩阵：

其中r_wi,k为第一传感器在传感器坐标系中的预测位置坐标，q_wi,k为第一传感器在传感器坐标系中的预测姿态，v_w,k为第一传感器在传感器坐标系中的预测运动速度，b_g,k为第一传感器测量的预测角速度偏移，b_a,k为第一传感器测量的预测加速度偏移，g_w,k为第一传感器在传感器坐标系中的预测重力向量，q_ic,k为待测量物体坐标系在传感器坐标系中的预测姿态，t_ic,k为待测量物体坐标系在传感器坐标系中的预测位置坐标；Δt为测量时间间隔，ω_i,k-1为第一传感器测量的当前角速度，a_i,k-1为第一传感器测量的当前加速度，n_ω为第一传感器的角速度噪声，n_a为第一传感器的加速度噪声，R_wi,k-1是q_wi,k-1的旋转矩阵。

即物体姿态测量装置根据第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前运动速度以及测量时间间隔，确定第一传感器在传感器坐标系中的预测位置坐标；根据第一传感器在传感器坐标系中的当前姿态、第一传感器测量的当前角速度、第一传感器的角速度噪声、第一传感器测量的当前角速度偏移以及测量时间间隔，确定第一传感器在传感器坐标系中的预测姿态；根据第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前加速度、第一传感器的加速度噪声、第一传感器测量的当前加速度偏移、第一传感器在传感器坐标系中的当前重力向量以及测量时间间隔，确定第一传感器在传感器坐标系中的预测运动速度。

随后物体姿态测量装置根据待测量物体的当前状态变量矩阵、待测量物体的第一预测状态变量矩阵、待测量物体的当前状态变量协方差矩阵以及第一传感器的测量噪声，计算该待测量物体的第一预测状态变量协方差矩阵。

具体的，物体姿态测量装置根据待测量物体的第一预测状态变量矩阵以及待测量物体的当前状态变量矩阵，确定状态变量转移矩阵A；并使用状态变量转移矩阵A，对当前状态变量协方差矩阵进行状态变量变化运算；

物体姿态测量装置根据待测量物体的第一预测状态变量矩阵以及所述第一传感器的测量噪声，确定所述噪声变量转移矩阵W，并使用噪声变量转移矩阵W，对当前噪声协方差矩阵进行噪声变量变化运算；

物体姿态测量装置根据所述状态变量变化运算的结果以及所述噪声变量变化运算的结果，获取待测量物体的第一预测状态变量协方差矩阵。可通过以下公式计算待测量物体的第一预测状态变量协方差矩阵：

P_k＝AP_k-1A^T+WQW^T；

n_B＝[n_a n_ω]；

其中P_k-1为待测量物体的当前状态变量协方差矩阵，Q为第一传感器的角速度噪声和加速度噪声的协方差矩阵，W为第一传感器的噪声和预测状态变量的转移矩阵。

在步骤S103中，物体姿态测量装置通过第二传感器，获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；其中第二传感器设置在待测量物体外或待测量物体上，用于获取待测量物体的六自由度状态。

第二传感器发送的信号可为光学信号、无线电信号或超声波信号，如通过图像扫描、雷达定位等方式获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。

在步骤S104中，物体姿态测量装置根据步骤S102获取的待测量物体的第一预测状态变量矩阵以及待测量物体的第一预测状态变量协方差矩阵、以及步骤S103获取的待测量物体在待检测物体坐标系中的第二预测位置坐标以及第二预测姿态，计算待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵。

具体的，首先物体姿态测量装置根据待测量物体的第一预测状态变量矩阵，计算待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态，以获取待测量物体的观测矩阵。可通过以下公式计算待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态：

其中(q_ic,k)^*是q_ic,k的共轭四元数，R_ic,k是q_ic,k的旋转矩阵，

是R_ic,k的转置矩阵，R_wi,k是q_wi,k的旋转矩阵，q_oc,k是所述待测量物体坐标系中的第一预测位置坐标，t_oc,k是所述待测量物体坐标系中的第一预测姿态；

该待测量物体的观测矩阵H为：

随后物体姿态测量装置根据待测量物体的观测矩阵、待测量物体的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数。可通过以下公式计算卡尔曼增益系数：

K_k＝P_kH^T(HP_kH^T+R)^-1；

P_k为待测量物体的第一预测状态变量协方差矩阵，R为第二传感器的噪声协方差矩阵，K_k为卡尔曼增益系数。

最后物体姿态测量装置根据待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、卡尔曼增益系数以及待测量物体的第一预测状态变量矩阵，计算待测量物体的融合预测状态变量矩阵。可通过以下公式计算待测量物体的融合预测状态变量矩阵：

x_k'＝x_k+K_k(z_k'-z_k)；

其中q_oc,k'为待测量物体坐标系中的第二预测位置坐标，t_oc,k'为待测量物体坐标系中的第二预测姿态，x_k'为待测量物体的融合预测状态变量矩阵。

物体姿态测量装置根据待测量物体的观测矩阵、卡尔曼增益系数以及待测量物体的第一预测状态变量协方差矩阵，计算待测量物体的融合预测状态变量协方差矩阵。可通过以下公式计算待测量物体的融合预测状态变量协方差矩阵：

P_k'＝P_k-K_kHP_k；

P_k'为待测量物体的融合预测状态变量协方差矩阵。

在步骤S105中，物体姿态测量装置基于步骤S104获取的待测量物体的融合预测状态变量矩阵以及待测量物体的融合预测状态变量协方差矩阵，获取待测量物体的预测位置坐标以及预测姿态。即物体姿态测量装置根据融合预测状态变量矩阵中第一传感器在传感器坐标系中的预测位置坐标、第一传感器在传感器坐标系中的预测姿态、待测量物体坐标系在传感器坐标系中的预测位置坐标以及待测量物体坐标系在传感器坐标系中的预测姿态，获取待测量物体在待测量物体坐标系中的预测位置坐标以及预测姿态。

同时物体姿态测量装置还可使用待测量物体的融合预测状态变量矩阵对当前待测量物体的第一预测状态变量矩阵进行修正，使用待测量物体的融合预测状态变量协方差矩阵对当前待测量物体的第一预测状态变量协方差矩阵进行修正，以便对下一测量时间间隔计算的待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵更加准确。

这样即完成了本实施例的物体姿态测量方法的待测量物体的物体姿态测量过程。

本实施例的物体姿态测量方法可应用于头戴式VR设备，以便对自身的坐标位置以及姿态进行测量。对应的物体姿态测量装置上可设置有用于获取加速度以及角速度的定位传感器以及用于获取待测量物体的六自由度状态的摄像传感器。其中摄像传感器可设置在对应的物体姿态测量装置上或物体姿态测量装置外。

本实施例的物体姿态测量方法不需要在每次测量前对第一传感器以及待测量物体坐标系进行反复标定，而是根据第一传感器和第二传感器的测量参数对待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵进行实施修正或实时数据融合，从而实现了对第一传感器的测量参数以及待测量物体坐标系的实时标定，简化了物体姿态测量前的标定流程且提高了获取的被测量物体的姿态位置的准确性。

本发明还提供一种物体姿态测量装置，用于通过多传感器来获取待测量物体的位置以及姿态。请参照图2，图2为本发明的物体姿态测量装置的实施例的结构示意图。本实施例的物体姿态测量装置可使用上述的物体姿态测量方法进行实施，该物体姿态测量装置20包括当前状态参数计算模块21、第一预测状态参数计算模块22、第二预测状态参数获取模块23、数据融合模块24以及物体位置姿态获取模块25。

当前状态参数计算模块21用于获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；其中第一传感器设置在所述待测量物体上，用于获取所述第一传感器自身的加速度以及角速度；第一预测状态参数计算模块22用于获取第一传感器的第二参数；并根据第一传感器的第二参数、待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；第二预测状态参数获取模块23用于通过第二传感器，获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；其中第二传感器设置在待测量物体外或所述待测量物体上，用于获取待测量物体的六自由度状态；数据融合模块24用于根据待测量物体的第一预测状态变量矩阵、待测量物体的第一预测状态变量协方差矩阵、待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；物体位置姿态获取模块25用于基于待测量物体的融合预测状态变量矩阵以及待测量物体的融合预测状态变量协方差矩阵，获取待测量物体的预测位置坐标以及预测姿态。

请参照图3，图3为本发明的物体姿态测量装置的实施例的当前状态参数计算模块的结构示意图。该当前状态参数计算模块21包括坐标系设置单元31、当前状态参数获取单元32、当前状态变量矩阵计算单元33以及当前状态变量协方差矩阵计算单元34。

坐标系设置单元31用于设置待测量物体上第一传感器的传感器坐标系以及待测量物体坐标系；当前状态参数获取单元32用于通过第一传感器，获取第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前姿态、第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前速度偏移、第一传感器在传感器坐标系中的重力向量、待测量物体坐标系在传感器坐标系中的当前位置坐标、以及待测量物体坐标系在传感器坐标系中的当前姿态，以确定待测量物体的当前状态变量矩阵；当前状态变量协方差矩阵计算单元33用于根据待测量物体的当前状态变量矩阵中的各个状态变量之间的协方差，确定待测量物体的当前状态变量协方差矩阵。

请参照图4，图4为本发明的物体姿态测量装置的实施例的第一预测状态参数计算模块的结构示意图。该第一预测状态参数计算模块22包括第一预测状态变量矩阵计算单元41以及第一预测状态变量协方差矩阵计算单元42。

第一预测状态变量矩阵计算单元41用于根据测量时间间隔、第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的测量噪声以及测量物体的当前状态变量矩阵，计算待测量物体的第一预测状态变量矩阵；第一预测状态变量协方差矩阵计算单元42用于根据待测量物体的当前状态变量矩阵、待测量物体的第一预测状态变量矩阵、待测量物体的当前状态变量协方差矩阵以及第一传感器的测量噪声，计算待测量物体的第一预测状态变量协方差矩阵。

请参照图5，图5为本发明的物体姿态测量装置的实施例的数据融合模块的结构示意图。该数据融合模块24包括观测矩阵获取单元51、卡尔曼增益系数计算单元52、融合预测状态变量矩阵计算单元53以及融合预测状态变量协方差矩阵计算单元54。

观测矩阵获取单元51用于根据待测量物体的第一预测状态变量矩阵，确定待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态，以获取待测量物体的观测矩阵；卡尔曼增益系数计算单元52用于根据待测量物体的观测矩阵、待测量物体的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数；融合预测状态变量矩阵计算单元53用于根据待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、卡尔曼增益系数以及待测量物体的第一预测状态变量矩阵，确定待测量物体的融合预测状态变量矩阵；融合预测状态变量协方差矩阵计算单元54用于根据待测量物体的观测矩阵、卡尔曼增益系数以及待测量物体的第一预测状态变量协方差矩阵，计算待测量物体的融合预测状态变量协方差矩阵。

本实施例的物体姿态测量装置20使用时，首先当前状态参数计算模块21的当前状态参数获取单元32获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系。

第一传感器设置在待测量物体上，用于获取第一传感器自身的加速度以及角速度等相关的测量参数。待测量物体坐标系是以待测量物体所在位置为坐标系中心建立的坐标系，如直角坐标系、平面极坐标系、柱面坐标系或球面坐标系等。传感器坐标系是以传感器所在位置为坐标系中心建立的坐标系，如直角坐标系、平面极坐标系、柱面坐标系或球面坐标系等。待测量物体坐标系和传感器坐标系的相对位置关系用于实现待测量物体坐标系中的数据与传感器坐标系中的数据的相互转换。

当前状态参数计算模块21的坐标系设置单元31也可以通过设置待测量物体上第一传感器的传感器坐标系和待测量物体坐标系的方式，获取待测量物体坐标系和传感器坐标系的相对位置关系。

随后当前状态参数计算模块21根据获取的第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵。

具体的，第一传感器的第一参数可包括第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前姿态(即第一传感器在传感器坐标系中的旋转角度)、第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前角速度偏移、第一传感器测量的当前加速度偏移、以及第一传感器在传感器坐标系中的重力向量。

待测量物体坐标系和传感器坐标系的相对位置关系可包括待测量物体坐标系在传感器坐标系中的当前位置坐标、以及待测量物体坐标系在传感器坐标系中的当前姿态。

这样当前状态参数计算模块21的当前状态变量矩阵计算单元33将待测量物体的当前状态变量矩阵设置为：

其中r_wi,k-1为第一传感器在传感器坐标系中的当前位置坐标，q_wi,k-1为第一传感器在传感器坐标系中的当前姿态，v_w,k-1为第一传感器在传感器坐标系中的当前运动速度，b_g,k-1为第一传感器测量的当前角速度偏移，b_a,k-1为第一传感器测量的当前加速度偏移，g_w,k-1为第一传感器在所述传感器坐标系中的当前重力向量，q_ic,k-1为待测量物体坐标系在所述传感器坐标系中的当前姿态，t_ic,k-1为待测量物体坐标系在所述传感器坐标系中的当前位置坐标。

同时当前状态参数计算模块21的当前状态变量协方差矩阵计算单元34将待测量物体的当前状态变量矩阵中的各个状态变量之间的协方差，设置为待测量物体的当前状态变量协方差矩阵。

随后第一预测状态参数计算模块22获取第一传感器的第二参数，该第一传感器的第二参数可包括第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的角速度噪声以及第一传感器的加速度噪声；

随后第一预测状态参数计算模块22根据第一传感器的第二参数、待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵。

具体的，第一预测状态参数计算模块22的第一预测状态变量矩阵计算单元41可根据测量时间间隔、第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的测量噪声、以及测量物体的当前状态变量矩阵，计算待测量物体的第一预测状态变量矩阵，可通过以下公式计算待测量物体的第一预测状态变量矩阵：

其中r_wi,k为第一传感器在传感器坐标系中的预测位置坐标，q_wi,k为第一传感器在传感器坐标系中的预测姿态，v_w,k为第一传感器在传感器坐标系中的预测运动速度，b_g,k为第一传感器测量的预测角速度偏移，b_a,k为第一传感器测量的预测加速度偏移，g_w,k为第一传感器在传感器坐标系中的预测重力向量，q_ic,k为待测量物体坐标系在传感器坐标系中的预测姿态，t_ic,k为待测量物体坐标系在传感器坐标系中的预测位置坐标；Δt为测量时间间隔，ω_i,k-1为第一传感器测量的当前角速度，a_i,k-1为第一传感器测量的当前加速度，n_ω为第一传感器的角速度噪声，n_a为第一传感器的加速度噪声，R_wi,k-1是q_wi,k-1的旋转矩阵。

即第一预测状态变量矩阵计算单元41根据第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前运动速度以及测量时间间隔，确定第一传感器在传感器坐标系中的预测位置坐标；根据第一传感器在传感器坐标系中的当前姿态、第一传感器测量的当前角速度、第一传感器的角速度噪声、第一传感器测量的当前角速度偏移以及测量时间间隔，确定第一传感器在传感器坐标系中的预测姿态；根据第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前加速度、第一传感器的加速度噪声、第一传感器测量的当前加速度偏移、第一传感器在传感器坐标系中的当前重力向量以及测量时间间隔，确定第一传感器在传感器坐标系中的预测运动速度。

第一预测状态参数计算模块22的第一预测状态变量协方差矩阵计算单元42根据待测量物体的当前状态变量矩阵、待测量物体的第一预测状态变量矩阵、待测量物体的当前状态变量协方差矩阵以及第一传感器的测量噪声，计算该待测量物体的第一预测状态变量协方差矩阵。

具体的，第一预测状态变量协方差矩阵计算单元42根据待测量物体的第一预测状态变量矩阵以及待测量物体的当前状态变量矩阵，确定状态变量转移矩阵A；并使用状态变量转移矩阵A，对当前状态变量协方差矩阵进行状态变量变化运算；

第一预测状态变量协方差矩阵计算单元42根据待测量物体的第一预测状态变量矩阵以及所述第一传感器的测量噪声，确定所述噪声变量转移矩阵W，并使用噪声变量转移矩阵W，对当前噪声协方差矩阵进行噪声变量变化运算；

第一预测状态变量协方差矩阵计算单元42根据所述状态变量变化运算的结果以及所述噪声变量变化运算的结果，获取待测量物体的第一预测状态变量协方差矩阵。可通过以下公式计算待测量物体的第一预测状态变量协方差矩阵：

P_k＝AP_k-1A^T+WQW^T；

n_B＝[n_a n_ω]；

其中P_k-1为待测量物体的当前状态变量协方差矩阵，Q为第一传感器的角速度噪声和加速度噪声的协方差矩阵，W为第一传感器的噪声和预测状态变量的转移矩阵。

然后第二预测状态参数获取模块23通过第二传感器，获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；其中第二传感器设置在待测量物体外或待测量物体上，用于获取待测量物体的六自由度状态。

第二传感器发送的信号可为光学信号、无线电信号或超声波信号，如通过图像扫描、雷达定位等方式获取待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。

随后数据融合模块24根据第一预测状态参数计算模块22获取的待测量物体的第一预测状态变量矩阵以及待测量物体的第一预测状态变量协方差矩阵、以及第二预测状态参数获取模块23获取的待测量物体在待检测物体坐标系中的第二预测位置坐标以及第二预测姿态，计算待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵。

具体的，数据融合模块24的观测矩阵获取单元51根据待测量物体的第一预测状态变量矩阵，计算待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态，以获取待测量物体的观测矩阵。可通过以下公式计算待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态：

其中(q_ic,k)^*是q_ic,k的共轭四元数，R_ic,k是q_ic,k的旋转矩阵，

是R_ic,k的转置矩阵，R_wi,k是q_wi,k的旋转矩阵，q_oc,k是所述待测量物体坐标系中的第一预测位置坐标，t_oc,k是所述待测量物体坐标系中的第一预测姿态；

该待测量物体的观测矩阵H为：

数据融合模块24的卡尔曼增益系数计算单元52根据待测量物体的观测矩阵、待测量物体的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数。可通过以下公式计算卡尔曼增益系数：

K_k＝P_kH^T(HP_kH^T+R)^-1；

P_k为待测量物体的第一预测状态变量协方差矩阵，R为第二传感器的噪声协方差矩阵，K_k为卡尔曼增益系数。

数据融合模块24的融合预测状态变量矩阵计算单元53根据待测量物体在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、待测量物体在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、卡尔曼增益系数以及待测量物体的第一预测状态变量矩阵，计算待测量物体的融合预测状态变量矩阵。可通过以下公式计算待测量物体的融合预测状态变量矩阵：

x_k'＝x_k+K_k(z_k'-z_k)；

其中q_oc,k'为待测量物体坐标系中的第二预测位置坐标，t_oc,k'为待测量物体坐标系中的第二预测姿态，x_k'为待测量物体的融合预测状态变量矩阵。

数据融合模块24的融合预测状态变量协方差矩阵计算单元54根据待测量物体的观测矩阵、卡尔曼增益系数以及待测量物体的第一预测状态变量协方差矩阵，计算待测量物体的融合预测状态变量协方差矩阵。可通过以下公式计算待测量物体的融合预测状态变量协方差矩阵：

P_k'＝P_k-K_kHP_k；

P_k'为待测量物体的融合预测状态变量协方差矩阵。

最后物体位置姿态获取模块25基于数据融合模块24获取的待测量物体的融合预测状态变量矩阵以及待测量物体的融合预测状态变量协方差矩阵，获取待测量物体的预测位置坐标以及预测姿态。即物体位置姿态获取模块25根据融合预测状态变量矩阵中第一传感器在传感器坐标系中的预测位置坐标、第一传感器在传感器坐标系中的预测姿态、待测量物体坐标系在传感器坐标系中的预测位置坐标以及待测量物体坐标系在传感器坐标系中的预测姿态，获取待测量物体在待测量物体坐标系中的预测位置坐标以及预测姿态。

同时物体位置姿态获取模块25还可使用待测量物体的融合预测状态变量矩阵对当前待测量物体的第一预测状态变量矩阵进行修正，使用待测量物体的融合预测状态变量协方差矩阵对当前待测量物体的第一预测状态变量协方差矩阵进行修正，以便对下一测量时间间隔计算的待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵更加准确。

这样即完成了本实施例的物体姿态测量装置20的待测量物体的物体姿态测量过程。

本实施例的物体姿态测量装置可应用于头戴式VR设备，以便对自身的坐标位置以及姿态进行测量。该物体姿态测量装置上可设置有用于获取加速度以及角速度的定位传感器以及用于获取待测量物体的六自由度状态的摄像传感器。其中摄像传感器可设置在对应的物体姿态测量装置上或物体姿态测量装置外。

本实施例的物体姿态测量装置不需要在每次测量前对第一传感器以及待测量物体坐标系进行反复标定，而是根据第一传感器和第二传感器的测量参数对待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵进行实施修正或实时数据融合，从而实现了对第一传感器的测量参数以及待测量物体坐标系的实时标定，简化了物体姿态测量前的标定流程且提高了获取的被测量物体的姿态位置的准确性。

下面通过一具体实施例说明本发明的物体姿态测量方法及物体姿态测量装置。请参照图6A和图7，图6A为本发明的物体姿态测量方法及物体姿态测量装置的第一具体实施例的示意图，图7为本发明的物体姿态测量方法及物体姿态测量装置的具体实施例的流程图。在本具体实施例中，物体姿态测量装置61为头戴式VR设备，该物体姿态测量装置61同时可用于测量自身的坐标位置以及姿态。物体姿态测量装置61上设置有用于获取加速度以及角速度的定位传感器62以及用于获取待测量物体的六自由度状态的摄像传感器63。该摄像传感器63通过拍摄背景画面的运动状态来获取物体姿态测量装置61的六自由度状态。该定位传感器62和摄像传感器63分别与物体姿态测量装置61的控制模块连接。

本具体实施例的物体姿态测量装置61获取物体姿态测量装置61的姿态位置的步骤如下：

步骤S701，物体姿态测量装置61设置定位传感器62的传感器坐标系以及物体姿态测量装置61的待测量物体坐标系，从而可获取待测量物体坐标系和传感器坐标系的相对位置关系。随后物体姿态测量装置对定位传感器62的测量参数以及摄像传感器63的测量参数进行初始化操作。

步骤S702，物体姿态测量装置61获取定位传感器62在传感器坐标系中的当前位置坐标、在传感器坐标系中的当前姿态、定位传感器62在传感器坐标系中的当前运动速度、定位传感器62测量的当前角速度偏移、定位传感器62测量的当前加速度偏移、定位传感器62在传感器坐标系中的重力向量、待测量物体坐标系在传感器坐标系中的当前位置坐标、以及待测量物体坐标系在传感器坐标系中的当前姿态，作为物体姿态测量装置61的当前状态变量。

这样物体姿态测量装置61可创建物体姿态测量装置61的当前状态变量矩阵：

其中r_wi,k-1为定位传感器62在传感器坐标系中的当前位置坐标，可为3行1列的矩阵；q_wi,k-1为定位传感器62在传感器坐标系中的当前姿态，可为4行1列的矩阵，用四元数来表示定位传感器62在传感器坐标系中的旋转；v_w,k-1为定位传感器62在传感器坐标系中的当前运动速度，可为3行1列的矩阵；b_g,k-1为定位传感器62测量的当前角速度偏移，可为3行1列的矩阵；b_a,k-1为定位传感器62测量的当前加速度偏移，可为3行1列的矩阵；g_w,k-1为定位传感器62在传感器坐标系中的当前重力向量；可为3行1列的矩阵；q_ic,k-1为待测量物体坐标系在传感器坐标系中的当前姿态，可为4行1列的矩阵，用四元数来表示在传感器坐标系中待测量物体坐标系的旋转；t_ic,k-1为待测量物体坐标系在传感器坐标系中的当前位置坐标，可为3行1列的矩阵，表示传感器坐标系中待测量物体坐标系的位置。这样当前状态变量矩阵为一26行1列的矩阵。

随后物体姿态测量装置61计算物体姿态测量装置61的当前状态变量矩阵中各个状态变量之间的协方差，从而获取对应的物体姿态测量装置61的当前状态变量协方差矩阵。该当前状态变量协方差矩阵为一26行26列的矩阵，用于表示各个当前状态变量之间的协方差。

步骤S703，物体姿态测量装置61获取检测时间间隔、定位传感器62测量的当前角速度、定位传感器62测量的当前加速度、定位传感器62的测量噪声、以及物体姿态测量装置61的当前状态变量矩阵，计算物体姿态测量装置61的第一预测状态变量矩阵。

具体可通过以下公式计算物体姿态测量装置61的第一预测状态变量矩阵：

其中r_wi,k为定位传感器62在传感器坐标系中的预测位置坐标，q_wi,k为定位传感器62在传感器坐标系中的预测姿态，v_w,k为定位传感器62在传感器坐标系中的预测运动速度，b_g,k为定位传感器62测量的预测角速度偏移，b_a,k为定位传感器62测量的预测加速度偏移，g_w,k为定位传感器62在传感器坐标系中的预测重力向量，q_ic,k为待测量物体坐标系在传感器坐标系中的预测姿态，t_ic,k为待测量物体坐标系在传感器坐标系中的预测位置坐标；Δt为测量时间间隔，即表示k-1时刻到k时刻的时间间隔，ω_i,k-1为定位传感器62测量的当前角速度，a_i,k-1为定位传感器62测量的当前加速度，n_ω为定位传感器62测量的角速度噪声，n_a为定位传感器62测量的加速度噪声，

表示四元数乘法，*表示普通的矩阵乘法，R_wi,k-1是q_wi,k-1的旋转矩阵。

该第一预测状态变量矩阵也为一26行1列的矩阵。

随后物体姿态测量装置61根据物体姿态测量装置61的当前状态变量矩阵、物体姿态测量装置61的第一预测状态变量矩阵、物体姿态测量装置61的当前状态变量协方差矩阵以及定位传感器62的测量噪声，计算物体姿态测量装置61的第一预测状态变量协方差矩阵。

具体可通过以下公式计算物体姿态测量装置61的第一预测状态变量协方差矩阵：

P_k＝AP_k-1A^T+WQW^T；

n_B＝[n_a n_ω]；

其中A是一个26行26列的矩阵，是f(x_k-1)关于x_k-1的偏导；P_k-1为物体姿态测量装置61的当前状态变量协方差矩阵，Q是一个6行6列的矩阵，表示定位传感器62的角速度噪声和加速度噪声的协方差矩阵，W是一个26行6列的矩阵，表示定位传感器62的噪声和预测状态变量的转移矩阵。该第一预测状态变量协方差矩阵为一26行26列的矩阵。

步骤S704，物体姿态测量装置61通过摄像传感器63，获取物体姿态测量装置61在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。由于摄像传感器63设置在物体姿态测量装置61上，因此摄像传感器63可拍摄周围背景画面的运动状态来获取物体姿态测量装置61的六自由度状态，即物体姿态测量装置61在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。

步骤S705，物体姿态测量装置61计算物体姿态测量装置61在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态：

其中(q_ic,k)^*是q_ic,k的共轭四元数，R_ic,k是q_ic,k的旋转矩阵，

是R_ic,k的转置矩阵，R_wi,k是q_wi,k的旋转矩阵，q_oc,k是待测量物体坐标系中的第一预测位置坐标，t_oc,k是待测量物体坐标系中的第一预测姿态。

步骤S705，物体姿态测量装置61根据物体姿态测量装置61的观测矩阵、物体姿态测量装置61的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数。可通过以下公式计算卡尔曼增益系数：

K_k＝P_kH^T(HP_kH^T+R)^-1；

P_k为物体姿态测量装置61的第一预测状态变量协方差矩阵，R为摄像传感器63的噪声协方差矩阵，K_k为卡尔曼增益系数，H为物体姿态测量装置61的观测矩阵。

步骤S706，物体姿态测量装置61根据物体姿态测量装置61在待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、物体姿态测量装置61在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、卡尔曼增益系数以及物体姿态测量装置61的第一预测状态变量矩阵，计算物体姿态测量装置61的融合预测状态变量矩阵。可通过以下公式计算物体姿态测量装置61的融合预测状态变量矩阵：

x_k'＝x_k+K_k(z_k'-z_k)；

其中q_oc,k'为待测量物体坐标系中的第二预测位置坐标，t_oc,k'为待测量物体坐标系中的第二预测姿态，x_k'为物体姿态测量装置61的融合预测状态变量矩阵。

物体姿态测量装置61根据物体姿态测量装置61的观测矩阵、卡尔曼增益系数以及物体姿态测量装置61的第一预测状态变量协方差矩阵，计算物体姿态测量装置61的融合预测状态变量协方差矩阵。可通过以下公式计算物体姿态测量装置61的融合预测状态变量协方差矩阵：

P_k'＝P_k-K_kHP_k；

P_k'为待测量物体的融合预测状态变量协方差矩阵。

步骤S707，物体姿态测量装置61使用物体姿态测量装置61的融合预测状态变量矩阵以及物体姿态测量装置61的融合预测状态变量协方差矩阵，获取物体姿态测量装置61的预测位置坐标以及预测姿态。同时物体姿态测量装置61还可使用物体姿态测量装置61的融合预测状态变量矩阵对物体姿态测量装置61的第一预测状态变量矩阵进行修正，使用物体姿态测量装置61的融合预测状态变量协方差矩阵对物体姿态测量装置61的第一预测状态变量协方差矩阵进行修正。

这样即完成了本具体实施例的物体姿态测量方法及物体姿态测量装置的物体姿态测量装置61的物体姿态的实时测量过程。

请参照图6B，图6B为本发明的物体姿态测量方法及物体姿态测量装置的第二具体实施例的示意图。在本具体实施例中，物体姿态装置71为头戴式VR设备，该物体姿态测量装置71同时可用于测量自身的坐标位置以及姿态。物体姿态测量装置71上设置有用于获取加速度以及角速度的定位传感器72，物体姿态测量装置71外设置有用于获取待测量物体的六自由度状态的摄像传感器73。该摄像传感器73通过直接拍摄物体姿态测量装置71的运动状态来获取物体姿态测量装置71的六自由度状态。该定位传感器72与物体姿态测量装置71的控制模块直接连接，该摄像传感器73与物体姿态测量装置71的控制模块无线连接。

在物体姿态测量方法及物体姿态测量装置的第一具体实施例中的物体姿态测量装置61的物体姿态的实时测量过程的基础上，本具体实施例的步骤S704中，物体姿态测量装置71通过摄像传感器73，获取物体姿态测量装置71在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。由于摄像传感器73设置在物体姿态测量装置71外，因此摄像传感器73可直接拍摄物体姿态测量装置71的运动状态来获取物体姿态测量装置71的六自由度状态，即物体姿态测量装置61在待测量物体坐标系中的第二预测位置坐标以及第二预测姿态。

本发明的物体姿态测量方法以及测量装置实时根据第一传感器的测量参数以及第二传感器的测量参数，对待测量物体的预测状态变量矩阵以及预测状态变量协方差矩阵进行修正，从而及时补偿了传感器中的测量值误差，提高了获取的被测物体的姿态位置的准确性；解决了现有的物体姿态测量方法以及测量装置由于传感器中的测量值误差变化导致获取的待测量物体的姿态位置的准确性较低的技术问题。

如本申请所使用的术语“组件”、“模块”、“系统”、“接口”、“进程”等等一般地旨在指计算机相关实体：硬件、硬件和软件的组合、软件或执行中的软件。例如，组件可以是但不限于是运行在处理器上的进程、处理器、对象、可执行应用、执行的线程、程序和/或计算机。通过图示，运行在控制器上的应用和该控制器二者都可以是组件。一个或多个组件可以有在于执行的进程和/或线程内，并且组件可以位于一个计算机上和/或分布在两个或更多计算机之间。

图8和随后的讨论提供了对实现本发明所述的物体姿态测量装置所在的电子设备的工作环境的简短、概括的描述。图8的工作环境仅仅是适当的工作环境的一个实例并且不旨在建议关于工作环境的用途或功能的范围的任何限制。实例电子设备812包括但不限于可穿戴设备、头戴设备、医疗健康平台、个人计算机、服务器计算机、手持式或膝上型设备、移动设备(比如移动电话、个人数字助理(PDA)、媒体播放器等等)、多处理器系统、消费型电子设备、小型计算机、大型计算机、包括上述任意系统或设备的分布式计算环境，等等。

尽管没有要求，但是在“计算机可读指令”被一个或多个电子设备执行的通用背景下描述实施例。计算机可读指令可以经由计算机可读介质来分布(下文讨论)。计算机可读指令可以实现为程序模块，比如执行特定任务或实现特定抽象数据类型的功能、对象、应用编程接口(API)、数据结构等等。典型地，该计算机可读指令的功能可以在各种环境中随意组合或分布。

图8图示了包括本发明的物体姿态测量装置中的一个或多个实施例的电子设备812的实例。在一种配置中，电子设备812包括至少一个处理单元816和存储器818。根据电子设备的确切配置和类型，存储器818可以是易失性的(比如RAM)、非易失性的(比如ROM、闪存等)或二者的某种组合。该配置在图8中由虚线814图示。

在其他实施例中，电子设备812可以包括附加特征和/或功能。例如，设备812还可以包括附加的存储装置(例如可移除和/或不可移除的)，其包括但不限于磁存储装置、光存储装置等等。这种附加存储装置在图8中由存储装置820图示。在一个实施例中，用于实现本文所提供的一个或多个实施例的计算机可读指令可以在存储装置820中。存储装置820还可以存储用于实现操作系统、应用程序等的其他计算机可读指令。计算机可读指令可以载入存储器818中由例如处理单元816执行。

本文所使用的术语“计算机可读介质”包括计算机存储介质。计算机存储介质包括以用于存储诸如计算机可读指令或其他数据之类的信息的任何方法或技术实现的易失性和非易失性、可移除和不可移除介质。存储器818和存储装置820是计算机存储介质的实例。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存储器技术、CD-ROM、数字通用盘(DVD)或其他光存储装置、盒式磁带、磁带、磁盘存储装置或其他磁存储设备、或可以用于存储期望信息并可以被电子设备812访问的任何其他介质。任意这样的计算机存储介质可以是电子设备812的一部分。

电子设备812还可以包括允许电子设备812与其他设备通信的通信连接826。通信连接826可以包括但不限于调制解调器、网络接口卡(NIC)、集成网络接口、射频发射器/接收器、红外端口、USB连接或用于将电子设备812连接到其他电子设备的其他接口。通信连接826可以包括有线连接或无线连接。通信连接826可以发射和/或接收通信媒体。

术语“计算机可读介质”可以包括通信介质。通信介质典型地包含计算机可读指令或诸如载波或其他传输机构之类的“己调制数据信号”中的其他数据，并且包括任何信息递送介质。术语“己调制数据信号”可以包括这样的信号：该信号特性中的一个或多个按照将信息编码到信号中的方式来设置或改变。

电子设备812可以包括输入设备824，比如手柄等控制器、遥控器、键盘、鼠标、笔、语音输入设备、触摸输入设备、红外相机、视频输入设备和/或任何其他输入设备。设备812中也可以包括输出设备822，比如一个或多个显示器、扬声器、打印机和/或任意其他输出设备。输入设备824和输出设备822可以经由有线连接、无线连接或其任意组合连接到电子设备812。在一个实施例中，来自另一个电子设备的输入设备或输出设备可以被用作电子设备812的输入设备824或输出设备822。

电子设备812的组件可以通过各种互连(比如总线)连接。这样的互连可以包括外围组件互连(PCI)(比如快速PCI)、通用串行总线(USB)、火线(IEEE1394)、光学总线结构等等。在另一个实施例中，电子设备812的组件可以通过网络互连。例如，存储器818可以由位于不同物理位置中的、通过网络互连的多个物理存储器单元构成。

本领域技术人员将认识到，用于存储计算机可读指令的存储设备可以跨越网络分布。例如，可经由网络828访问的电子设备830可以存储用于实现本发明所提供的一个或多个实施例的计算机可读指令。电子设备812可以访问电子设备830并且下载计算机可读指令的一部分或所有以供执行。可替代地，电子设备812可以按需要下载多条计算机可读指令，或者一些指令可以在电子设备812处执行并且一些指令可以在电子设备830处执行。

本文提供了实施例的各种操作。在一个实施例中，所述的一个或多个操作可以构成一个或多个计算机可读介质上存储的计算机可读指令，其在被电子设备执行时将使得计算设备执行所述操作。描述一些或所有操作的顺序不应当被解释为暗示这些操作必需是顺序相关的。本领域技术人员将理解具有本说明书的益处的可替代的排序。而且，应当理解，不是所有操作必需在本文所提供的每个实施例中存在。

而且，尽管已经相对于一个或多个实现方式示出并描述了本公开，但是本领域技术人员基于对本说明书和附图的阅读和理解将会想到等价变型和修改。本公开包括所有这样的修改和变型，并且仅由所附权利要求的范围限制。特别地关于由上述组件(例如元件、资源等)执行的各种功能，用于描述这样的组件的术语旨在对应于执行所述组件的指定功能(例如其在功能上是等价的)的任意组件(除非另外指示)，即使在结构上与执行本文所示的本公开的示范性实现方式中的功能的公开结构不等同。此外，尽管本公开的特定特征已经相对于若干实现方式中的仅一个被公开，但是这种特征可以与如可以对给定或特定应用而言是期望和有利的其他实现方式的一个或多个其他特征组合。而且，就术语“包括”、“具有”、“含有”或其变形被用在具体实施方式或权利要求中而言，这样的术语旨在以与术语“包含”相似的方式包括。

本发明实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。上述提到的存储介质可以是只读存储器，磁盘或光盘等。上述的各装置或系统，可以执行相应方法实施例中的方法。

综上所述，虽然本发明已以实施例揭露如上，实施例前的序号仅为描述方便而使用，对本发明各实施例的顺序不造成限制。并且，上述实施例并非用以限制本发明，本领域的普通技术人员，在不脱离本发明的精神和范围内，均可作各种更动与润饰，因此本发明的保护范围以权利要求界定的范围为准。

Claims (12)

1.一种物体姿态测量方法，其特征在于，包括：

获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；其中所述第一传感器的第一参数包括第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前姿态、第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前角速度偏移、第一传感器测量的当前加速度偏移、以及第一传感器在传感器坐标系中的重力向量；

获取所述第一传感器的第二参数；并根据所述第一传感器的第二参数、所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定所述待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；其中所述第一传感器的第二参数包括第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的角速度噪声以及第一传感器的加速度噪声；

通过第二传感器，获取所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；

根据所述待测量物体的第一预测状态变量矩阵、所述待测量物体的第一预测状态变量协方差矩阵、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定所述待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；以及

基于所述待测量物体的融合预测状态变量矩阵以及所述待测量物体的融合预测状态变量协方差矩阵，获取所述待测量物体的预测位置坐标以及预测姿态；

其中所述根据所述待测量物体的第一预测状态变量矩阵、所述待测量物体的第一预测状态变量协方差矩阵、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定所述待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵的步骤包括：

根据所述待测量物体的第一预测状态变量矩阵，确定所述待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态，以获取所述待测量物体的观测矩阵；

根据所述待测量物体的观测矩阵、所述待测量物体的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数；

根据所述待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、所述卡尔曼增益系数以及所述待测量物体的第一预测状态变量矩阵，确定所述待测量物体的融合预测状态变量矩阵；

根据所述待测量物体的观测矩阵、所述卡尔曼增益系数以及所述待测量物体的第一预测状态变量协方差矩阵，计算所述待测量物体的融合预测状态变量协方差矩阵。

2.根据权利要求1所述的物体姿态测量方法，其特征在于，所述根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵的步骤包括：

设置待测量物体上第一传感器的传感器坐标系以及待测量物体坐标系；

通过所述第一传感器，获取所述第一传感器在所述传感器坐标系中的当前位置坐标、所述第一传感器在所述传感器坐标系中的当前姿态、所述第一传感器在所述传感器坐标系中的当前运动速度、所述第一传感器测量的当前速度偏移、所述第一传感器在所述传感器坐标系中的重力向量、待测量物体坐标系在所述传感器坐标系中的当前位置坐标、以及待测量物体坐标系在所述传感器坐标系中的当前姿态，以确定所述待测量物体的当前状态变量矩阵；

根据所述待测量物体的当前状态变量矩阵中的各个状态变量之间的协方差，确定所述待测量物体的当前状态变量协方差矩阵。

3.根据权利要求1所述的物体姿态测量方法，其特征在于，所述第一传感器设置在所述待测量物体上，用于获取所述第一传感器自身的当前加速度以及当前角速度；所述第二传感器设置在所述待测量物体外或所述待测量物体上，用于获取所述待测量物体的六自由度状态。

4.根据权利要求1所述的物体姿态测量方法，其特征在于，所述获取所述第一传感器的第二参数；并根据所述第一传感器的第二参数、所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定所述待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵，包括：

根据测量时间间隔、所述第一传感器测量的当前角速度、所述第一传感器测量的当前加速度、所述第一传感器的测量噪声以及所述测量物体的当前状态变量矩阵，确定所述待测量物体的第一预测状态变量矩阵；以及

根据所述待测量物体的当前状态变量矩阵、所述待测量物体的第一预测状态变量矩阵、所述待测量物体的当前状态变量协方差矩阵以及所述第一传感器的测量噪声，确定所述待测量物体的第一预测状态变量协方差矩阵。

5.根据权利要求4所述的物体姿态测量方法，其特征在于，所述根据测量时间间隔、所述第一传感器测量的当前角速度、所述第一传感器测量的当前加速度、所述第一传感器的测量噪声以及所述测量物体的当前状态变量矩阵，确定所述待测量物体的第一预测状态变量矩阵的步骤包括：

根据所述第一传感器在所述传感器坐标系中的当前位置坐标、所述第一传感器在所述传感器坐标系中的当前运动速度以及所述测量时间间隔，确定所述第一传感器在所述传感器坐标系中的预测位置坐标；

根据所述第一传感器在所述传感器坐标系中的当前姿态、所述第一传感器测量的当前角速度、所述第一传感器的角速度噪声、所述第一传感器测量的当前角速度偏移以及所述测量时间间隔，确定所述第一传感器在所述传感器坐标系中的预测姿态；

根据所述第一传感器在所述传感器坐标系中的当前运动速度、所述第一传感器测量的当前加速度、所述第一传感器的加速度噪声、所述第一传感器测量的当前加速度偏移、所述第一传感器在所述传感器坐标系中的当前重力向量以及所述测量时间间隔，确定所述第一传感器在所述传感器坐标系中的预测运动速度。

6.根据权利要求4所述的物体姿态测量方法，其特征在于，所述确定所述待测量物体的第一预测状态变量协方差矩阵的步骤包括：

根据所述待测量物体的第一预测状态变量矩阵以及所述待测量物体的当前状态变量矩阵，确定所述状态变量转移矩阵；并使用所述状态变量转移矩阵，对所述当前状态变量协方差矩阵进行状态变量变化运算；

根据所述待测量物体的第一预测状态变量矩阵以及所述第一传感器的测量噪声，确定所述噪声变量转移矩阵，并使用所述噪声变量转移矩阵，对当前噪声协方差矩阵进行噪声变量变化运算；

根据所述状态变量变化运算的结果以及所述噪声变量变化运算的结果，获取所述待测量物体的第一预测状态变量协方差矩阵。

7.一种物体姿态测量装置，其特征在于，包括：

当前状态参数计算模块，用于获取第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系；并根据所述第一传感器的第一参数、以及待测量物体坐标系和传感器坐标系的相对位置关系，确定所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵；其中所述第一传感器的第一参数包括第一传感器在传感器坐标系中的当前位置坐标、第一传感器在传感器坐标系中的当前姿态、第一传感器在传感器坐标系中的当前运动速度、第一传感器测量的当前角速度偏移、第一传感器测量的当前加速度偏移、以及第一传感器在传感器坐标系中的重力向量；

第一预测状态参数计算模块，用于获取所述第一传感器的第二参数；并根据所述第一传感器的第二参数、所述待测量物体的当前状态变量矩阵以及当前状态变量协方差矩阵，确定所述待测量物体的第一预测状态变量矩阵以及第一预测状态变量协方差矩阵；其中所述第一传感器的第二参数包括第一传感器测量的当前角速度、第一传感器测量的当前加速度、第一传感器的角速度噪声以及第一传感器的加速度噪声；

第二预测状态参数获取模块，用于通过第二传感器，获取所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态；

数据融合模块，用于根据所述待测量物体的第一预测状态变量矩阵、所述待测量物体的第一预测状态变量协方差矩阵、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态，确定所述待测量物体的融合预测状态变量矩阵以及融合预测状态变量协方差矩阵；以及

物体位置姿态获取模块，用于基于所述待测量物体的融合预测状态变量矩阵以及所述待测量物体的融合预测状态变量协方差矩阵，获取所述待测量物体的预测位置坐标以及预测姿态；

所述数据融合模块包括：

观测矩阵获取单元，用于根据所述待测量物体的第一预测状态变量矩阵，确定所述待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态，以获取所述待测量物体的观测矩阵；

卡尔曼增益系数计算单元，用于根据所述待测量物体的观测矩阵、所述待测量物体的第一预测状态变量协方差矩阵以及第二传感器的测量噪声，计算卡尔曼增益系数；

融合预测状态变量矩阵计算单元，用于根据所述待测量物体在所述待测量物体坐标系中的第一预测位置坐标以及第一预测姿态、所述待测量物体在所述待测量物体坐标系中的第二预测位置坐标以及第二预测姿态、所述卡尔曼增益系数以及所述待测量物体的第一预测状态变量矩阵，确定所述待测量物体的融合预测状态变量矩阵；以及

融合预测状态变量协方差矩阵计算单元，用于根据所述待测量物体的观测矩阵、所述卡尔曼增益系数以及所述待测量物体的第一预测状态变量协方差矩阵，计算所述待测量物体的融合预测状态变量协方差矩阵。

8.根据权利要求7所述的物体姿态测量装置，其特征在于，所述当前状态参数计算模块包括：

坐标系设置单元，用于设置待测量物体上第一传感器的传感器坐标系以及待测量物体坐标系；以及

当前状态参数获取单元，用于通过所述第一传感器，获取所述第一传感器在所述传感器坐标系中的当前位置坐标、所述第一传感器在所述传感器坐标系中的当前姿态、所述第一传感器在所述传感器坐标系中的当前运动速度、所述第一传感器测量的当前速度偏移、所述第一传感器在所述传感器坐标系中的重力向量、待测量物体坐标系在所述传感器坐标系中的当前位置坐标、以及待测量物体坐标系在所述传感器坐标系中的当前姿态，以确定所述待测量物体的当前状态变量矩阵；以及

当前状态变量协方差矩阵计算单元，用于根据所述待测量物体的当前状态变量矩阵中的各个状态变量之间的协方差，确定所述待测量物体的当前状态变量协方差矩阵。

9.根据权利要求7所述的物体姿态测量装置，其特征在于，所述第一预测状态参数计算模块包括：

第一预测状态变量矩阵计算单元，用于根据测量时间间隔、所述第一传感器测量的当前角速度、所述第一传感器测量的当前加速度、所述第一传感器的测量噪声以及所述测量物体的当前状态变量矩阵，计算所述待测量物体的第一预测状态变量矩阵；

第一预测状态变量协方差矩阵计算单元，用于根据所述待测量物体的当前状态变量矩阵、所述待测量物体的第一预测状态变量矩阵、所述待测量物体的当前状态变量协方差矩阵以及所述第一传感器的测量噪声，计算所述待测量物体的第一预测状态变量协方差矩阵。

10.根据权利要求7所述的物体姿态测量装置，其特征在于，所述第一传感器设置在所述待测量物体上，用于获取所述第一传感器自身的当前加速度以及当前角速度；所述第二传感器设置在所述待测量物体外或所述待测量物体上，用于获取所述待测量物体的六自由度状态。

11.一种存储介质，其内存储有处理器可执行指令，所述指令由一个或一个以上处理器加载，以执行如权利要求1-6中任一的物体姿态测量方法。

12.一种电子设备，包括处理器和存储器，所述存储器有计算机程序，其特征在于，所述处理器通过调用所述计算机程序，用于执行如权利要求1至6任一项所述的物体姿态测量方法。

Images