CN102192740B - 姿势信息计算装置、姿势信息计算系统及姿势信息计算法 - Google Patents

Abstract

本发明提供姿势信息计算装置、姿势信息计算系统及姿势信息计算法，能够实现简单的结构，并且能够抑制位置的发散而确切地计算虚拟3维空间中的姿势。传感器信息取得部(210)取得由3个角速度传感器分别检测的绕3个轴的角速度信息(GX、GY、GZ)以及由3个加速度传感器分别检测的3个轴方向的加速度信息(AX、AY、AZ)。姿势信息计算部220根据角速度信息(GX、GY、GZ)和加速度信息(AX、AY、AZ)计算虚拟3维空间中的姿势角和位置座标。尤其，姿势信息计算部(220)根据由加速度信息(AX、AY、AZ)得到的惯性座标系加速度矢量(A)，计算固定座标系速度矢量，与该固定座标系速度矢量对应地计算虚拟3维空间中的位置座标。

Description

姿势信息计算装置、姿势信息计算系统及姿势信息计算法

技术领域

本发明涉及姿势信息计算装置、姿势信息计算系统以及姿势信息计算方法。

背景技术

近年来，在各种情况下均利用了使用惯性传感器计算物体姿势的系统。例如，在汽车、直升机等移动体上安装3轴角速度传感器和3轴加速度传感器，根据从传感器获得的信息计算移动体的姿势，由此实现进行移动体姿势的稳定控制的系统等。

一般情况下，物体的姿势可根据姿势角和位置来确定。通过对3轴角速度传感器检测出的角速度矢量进行1阶积分来获得物体的姿势角。不过，由于3轴角速度传感器的输出值包含微小的偏移量，所以，对角速度矢量进行1阶积分将导致偏移量逐渐蓄积。因此，通过计算获得的姿势角相对于正确姿势角的误差与偏移量成比例地逐渐增大，例如，虽然物体是静止的，但却得到了好像正在以微小的角速度进行旋转的结果。作为解决此问题的方法，例如日本特开平9﹣5104号公报及日本特开2007﹣183138号公报所公开的那样，使用卡尔曼滤波器根据传感器输出值来估计姿势角真值的方法是公知的。

另一方面，物体的位置是通过对3轴加速度传感器检测出的加速度矢量进行2阶积分而获得的。因为在3轴加速度传感器的输出值中也包含微小的偏移量，所以偏移量逐渐蓄积，从而虽然物体是静止的，但却得到了好像正在移动这样的结果。而且，因为是对加速度矢量进行2阶积分，所以，通过计算获得的位置相对于正确位置的误差增加，该误差的增大速度与偏移量的平方成正比，因而该增加速度远远大于姿势角误差的增大速度。因此，为了使用卡尔曼滤波器估计位置的真值，必需增大卡尔曼增益。但是，当增大卡尔曼增益时，使物体的计算出的位置回归到正确位置的速度过快，虽然实际上物体在慢慢移动，但却得到了好像处于静止这样的计算结果。尤其，如果传感器的偏移量比较大，则必需将卡尔曼增益设定得更大，在现实中，仅基于加速度传感器的输出值使用卡尔曼滤波器来估计物体的位置是极困难的。

为了解决此问题，在现实中使用了根据加速度传感器的输出值和从外部获得的特定信息来估计物体位置的方法。例如，将红外线发光装置固定在规定位置处，利用固定在物体上的CCD照相机对该红外线发光装置进行摄像，由此能够获得物体与红外线发光装置之间的相对位置关系的信息，所以可使用该信息并利用卡尔曼滤波器来估计物体位置的真值。但是，在这样的方法中，因为需要传感器以外的装置，所以系统变得大型化。

另一方面，例如在使用定点设备来操作虚拟3D模型这样的用途中，要求虚拟3D模型的位置不会因时间的经过而发散，并且要求虚拟3D模型跟随于定点设备的动作而动作，不过，有时不要求计算出虚拟3D模型的精确位置。在不需要计算这样的精确位置的系统中，低成本化的要求变高，不希望系统大型化。

发明内容

本发明正是鉴于以上这样的问题而完成的，根据本发明的几个方式，可提供结构简单且能够抑制位置发散而确切地计算出虚拟3维空间中的姿势的姿势信息计算装置、姿势信息计算系统以及姿势信息计算方法。

(1)本发明提供一种姿势信息计算装置，其根据由3个角速度传感器分别检测的绕相互不平行的3个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置坐标，该姿势信息计算装置包含：传感器信息取得部，其取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及姿势信息计算部，其根据上述传感器信息取得部所取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，上述姿势信息计算部根据由上述3个轴方向的加速度信息得到的惯性坐标系加速度矢量计算固定坐标系速度矢量，与该固定坐标系速度矢量对应地计算上述虚拟3维空间中的上述位置坐标。

在本发明的姿势信息计算装置中，首先，将惯性坐标系(传感器坐标系)的加速度矢量变换为固定坐标系的速度矢量。这里，在根据惯性坐标系的加速度矢量计算固定坐标系的位置(距离)时需要进行2阶积分，但在根据惯性坐标系的加速度矢量计算固定坐标系的速度矢量时，只需进行1阶积分即可。此外，在本发明的姿势信息计算装置中，具有如下特征，即：与该固定坐标系的速度矢量对应地生成虚拟3维空间中的位置坐标。由此，因传感器偏移引起的位置坐标(对加速度矢量进行1阶积分而获得)的漂移量和姿势角(对角速度矢量进行1阶积分而获得)的漂移量为相同程度，以往应用于姿势角的计算的使用了卡尔曼滤波器的方法等也能够应用于本发明中的位置坐标的计算。因此，根据本发明，能够提供结构简单且能够抑制位置的发散而确切地计算虚拟3维空间中的姿势的姿势信息计算装置。

(2)在该姿势信息计算装置中，可以构成为，上述姿势信息计算部包含：姿势信息更新部，其根据绕上述3个轴的角速度信息和3个轴方向的加速度信息，对为了确定上述虚拟3维空间中的上述姿势角和上述位置坐标所需的给定参数进行更新；姿势信息修正部，其按照给定的约束条件来修正由上述姿势信息更新部进行更新后的上述参数；以及姿势信息生成部，其根据由上述姿势信息修正部进行修正后的上述参数，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，上述姿势信息更新部包含速度矢量更新单元，该速度矢量更新单元根据上述惯性坐标系加速度矢量和时刻k﹣1处的上述固定坐标系速度矢量，更新时刻k处的上述固定坐标系速度矢量，上述姿势信息修正部包含速度矢量修正单元，该速度矢量修正单元按照使上述固定坐标系速度矢量的范数成为0的约束条件，修正由上述速度矢量更新单元进行更新后的时刻k处的上述固定坐标系速度矢量，上述姿势信息生成部包含位置坐标生成单元，该位置坐标生成单元与由上述速度矢量修正单元进行修正后的时刻k处的上述固定坐标系速度矢量对应地生成上述虚拟3维空间中的上述位置坐标。

在该姿势信息计算装置中，在进行了固定坐标系速度矢量的更新处理之后，按照使范数成为0(即，使固定坐标系速度矢量成为(0、0、0))的约束条件进行修正处理，与修正后的固定坐标系速度矢量对应地生成虚拟3维空间中的位置坐标。由此，以返回到与固定坐标系速度矢量(0、0、0)对应的规定坐标(例如原点)的方式生成位置坐标，所以能够抑制由偏移漂移引起的位置坐标的发散。

另外，根据该姿势信息计算装置，因为将固定坐标系的速度矢量与虚拟3维空间的位置坐标对应起来(并非将固定坐标系的位置坐标与虚拟3维空间的位置坐标对应起来)，所以能够比较容易地调整返回到规定坐标的速度，使得位置坐标确切地跟随于传感器的输出。

(3)在该姿势信息计算装置中，可以构成为，上述速度矢量更新单元执行以下处理：根据时刻k﹣1处的固定坐标系姿势角矢量使上述惯性坐标系加速度矢量进行旋转而变换为固定坐标系加速度矢量，从该固定坐标系加速度矢量中减去固定坐标系重力加速度矢量，以规定的单位时间对相减结果进行1阶积分而计算单位时间固定坐标系速度矢量，在上述速度矢量修正单元进行修正后的时刻k﹣1处的上述固定坐标系速度矢量中加上上述单位时间固定坐标系速度矢量，由此来更新时刻k处的上述固定坐标系速度矢量。

(4)在该姿势信息计算装置中，可以构成为，上述位置坐标生成单元将由上述速度矢量修正单元进行修正后的时刻k处的上述固定坐标系速度矢量作为时刻k处的上述位置坐标。

由此，不需要进行基于修正后的固定坐标系速度矢量生成位置坐标的计算，所以能够降低计算量。

(5)在该姿势信息计算装置中，可以构成为，上述姿势信息计算部根据由绕上述3个轴的角速度信息得到的惯性坐标系角速度矢量计算固定坐标系姿势角矢量，根据该固定坐标系姿势角矢量计算上述虚拟3维空间中的上述姿势角，上述姿势信息更新部包含姿势角矢量更新单元，该姿势角矢量更新单元根据上述惯性坐标系角速度矢量和时刻k﹣1处的上述固定坐标系姿势角矢量，更新时刻k处的上述固定坐标系姿势角矢量，上述姿势信息修正部包含姿势角矢量修正单元，该姿势角矢量修正单元按照基于上述惯性坐标系加速度矢量的约束条件，修正由上述姿势角矢量更新单元进行更新后的时刻k处的上述固定坐标系姿势角矢量，上述姿势信息生成部包含姿势角生成单元，该姿势角生成单元根据由上述姿势角矢量修正单元进行修正后的上述固定坐标系姿势角矢量，生成上述虚拟3维空间中的上述姿势角。

在该姿势信息计算装置中，在进行固定坐标系姿势角矢量的更新处理之后，按照基于惯性坐标系加速度矢量的约束条件进行修正处理，与修正后的固定坐标系姿势角矢量对应地生成虚拟3维空间中的姿势角。由此，生成基于当前的惯性坐标系加速度矢量的姿势中的姿势角，所以能够抑制由传感器偏移引起的姿势角的发散。

(6)在该姿势信息计算装置中，可以构成为，上述姿势角矢量更新单元执行以下处理：以规定的单位时间对上述惯性坐标系角速度矢量进行1阶积分而计算单位时间旋转角四元数，使上述单位时间旋转角四元数作用于上述姿势角矢量修正单元进行修正后的时刻k﹣1处的上述固定坐标系姿势角矢量，由此来更新时刻k处的上述固定坐标系姿势角矢量。

(7)在该姿势信息计算装置中，可以构成为，上述姿势角生成单元将由上述姿势角矢量修正单元进行修正后的时刻k处的上述固定坐标系姿势角矢量作为时刻k处的上述姿势角。

由此，不需要进行基于修正后的固定坐标系姿势角矢量生成姿势角的计算，所以能够降低计算量。

(8)在该姿势信息计算装置中，可以构成为，上述传感器信息取得部接受由温度传感器检测的温度信息，上述姿势信息更新部包含：角速度偏移量更新单元，其根据与上述温度信息对应的角速度传感器的偏移量和时刻k﹣1处的上述3个角速度传感器的偏移量，更新时刻k处的上述3个角速度传感器的偏移量；以及加速度偏移量更新单元，其根据与上述温度信息对应的加速度传感器的偏移量和时刻k﹣1处的上述3个加速度传感器的偏移量，更新时刻k处的上述3个加速度传感器的偏移量，上述姿势信息修正部包含：角速度偏移量修正单元，其按照给定的约束条件，修正由上述角速度偏移量更新单元进行更新后的时刻k处的上述3个角速度传感器的偏移量；以及加速度偏移量修正单元，其按照给定的约束条件，修正由上述加速度偏移量更新单元进行更新后的时刻k处的上述3个加速度传感器的偏移量。

由此，能够考虑因温度引起的传感器偏移量的变动来更新传感器偏移量。因此，由传感器偏移引起的漂移量降低，能够更容易地调整位置坐标及姿势角的返回速度。因此，能够使虚拟3维空间中的位置坐标和姿势角更迅速地跟随于传感器的输出。

(9)本发明提供一种姿势信息计算系统，其包含：3个角速度传感器，其分别检测绕相互不平行的3个轴的角速度信息；3个加速度传感器，其分别检测相互不平行的3个轴方向的加速度信息；以及上述任意一个姿势信息计算装置。

(10)本发明提供一种姿势信息计算方法，根据由3个角速度传感器分别检测的绕相互不平行的3个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置坐标，该姿势信息计算方法包含以下步骤：传感器信息取得步骤，取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及姿势信息计算步骤，根据在上述传感器信息取得步骤中取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，在上述姿势信息计算步骤中，根据由上述3个轴方向的加速度信息得到的惯性坐标系加速度矢量计算固定坐标系速度矢量，与该固定坐标系速度矢量对应地计算上述虚拟3维空间中的上述位置坐标。

附图说明

图1是第1实施方式的姿势信息计算系统的概略结构图。

图2是示出传感器信息输出装置的结构例的图。

图3是示出姿势信息计算装置的结构例的图。

图4是示出姿势信息计算装置的处理步骤的流程图。

图5是示出由姿势信息计算部执行的姿势信息计算处理的步骤的流程图。

图6是示出卡尔曼滤波器的更新处理的步骤的一例的流程图。

图7是示出卡尔曼滤波器的修正处理的步骤的一例的流程图。

图8是第2实施方式的姿势信息计算系统的概略结构图。

图9是示出传感器信息输出装置的结构例的图。

图10是示出姿势信息计算装置的结构例的图。

图11是示出卡尔曼滤波器的更新处理的步骤的一例的流程图。

图12是示出应用了姿势信息计算系统的游戏系统的一例的图。

符号说明

1A、1B姿势信息计算系统；10A、10B传感器信息输出装置；20A、20B姿势信息计算装置；100，102传感器模块；110X、110Y、110Z陀螺仪传感器；112X、112Y、112Z加速度传感器；114温度传感器；120X、120Y、120Z、122X、122Y、122Z、124放大器(AMP)；130X、130Y、130Z、132X、132Y、132Z、134低通滤波器(LPF)；140A/D转换器(ADC)；210传感器信息取得部；212ADC接口电路(ADCI/F)；214微型计算机接口电路(微型计算机I/F)；220姿势信息计算部；230姿势信息更新部；232速度矢量更新单元；234姿势角矢量更新单元；236角速度偏移量更新单元；238加速度偏移量更新单元；240姿势信息修正部；242速度矢量修正单元；244姿势角矢量修正单元；246角速度偏移量修正单元；248加速度偏移量修正单元；250姿势信息生成部；252位置坐标生成单元；254姿势角生成单元；300微型计算机；310处理部；320存储部；330通信部；400信息存储介质；500控制器；510，520操作杆；600游戏机主体；610，620通信电缆；700显示器装置；710显示部；720战斗机；1000游戏系统。

具体实施方式

以下，使用附图对本发明的优选实施方式进行详细说明。需要说明的是，以下说明的实施方式并不对权利要求书所记载的本发明的内容作不当的限定。另外，以下说明的全部结构并不一定是本发明的必要结构要件。

1.第1实施方式

1﹣1.姿势信息计算系统的结构

图1是第1实施方式的姿势信息计算系统的概略结构图。如图1所示，第1实施方式的姿势信息计算系统1A包含传感器信息输出装置10A和姿势信息计算装置20A。

传感器信息输出装置10A包含分别检测相互垂直的3个轴(X轴、Y轴、Z轴)方向的角速度信息的3个陀螺仪传感器(角速度传感器)110X、110Y、110Z，并输出该X轴、Y轴、Z轴方向的角速度信息。此外，传感器装置10A包含分别检测相互垂直的3个轴(X轴、Y轴、Z轴)方向的加速度信息的3个加速度传感器112X、112Y、112Z，并输出该X轴、Y轴、Z轴方向的加速度信息。

姿势信息计算装置20A包含传感器信息取得部210和姿势信息计算部220，并进行如下处理：根据传感器信息输出装置10A的检测信息(角速度信息和加速度信息)计算虚拟3维空间中的姿势角和位置坐标。

传感器信息取得部210进行如下处理(传感器信息取得处理)：取得由陀螺仪传感器110X、110Y、110Z分别检测的X轴、Y轴、Z轴方向的角速度信息和由加速度传感器112X、112Y、112Z分别检测的X轴、Y轴、Z轴方向的加速度信息。

姿势信息计算部220进行如下处理(姿势信息计算处理)：根据由传感器信息取得部210取得的X轴、Y轴、Z轴方向的角速度信息和X轴、Y轴、Z轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置坐标。具体地说，姿势信息计算部220根据由X轴、Y轴、Z轴方向的加速度信息获得的惯性坐标系加速度矢量(传感器坐标系(惯性坐标系)中的加速度矢量)，计算固定坐标系速度矢量(实际3维空间(固定坐标系)中的速度矢量)，与该固定坐标系速度矢量对应地计算虚拟3维空间中的位置坐标。另外，姿势信息计算部220根据由X轴、Y轴、Z轴方向的角速度信息获得的惯性坐标系角速度矢量(传感器坐标系(惯性坐标系)中的角速度矢量)，计算固定坐标系中的姿势角矢量(实际3维空间(固定坐标系)中的姿势角矢量)，并根据该固定坐标系姿势角矢量计算虚拟3维空间中的姿势角。

更具体地说，姿势信息计算部220构成为包含姿势信息更新部230、姿势信息修正部240和姿势信息生成部250，进行计算虚拟3维空间中的姿势角和位置坐标的上述处理。这里，姿势信息更新部230如下处理(姿势信息更新处理)：根据绕X轴、Y轴、Z轴的角速度信息和X轴、Y轴、Z轴方向的加速度信息来更新为了确定虚拟3维空间中的姿势角和位置坐标所需的给定参数。姿势信息修正部240进行如下处理(姿势信息修正处理)：按照给定的约束条件，修正由姿势信息更新部240进行更新后的参数。姿势信息生成部250进行如下处理(姿势信息生成处理)：根据由姿势信息修正部240进行修正后的参数，生成虚拟3维空间中的姿势角和位置坐标。

1﹣2.传感器信息输出装置的结构

图2是示出传感器信息输出装置10A的结构例的图。

在传感器模块100中，固定着图1中说明的3个陀螺仪传感器110X、110Y、110Z和3个加速度传感器112X、112Y、112Z。这里，陀螺仪传感器110X、110Y、110Z被配置为它们的检测轴相互垂直。同样，加速度传感器112X、112Y、112Z被配置为它们的检测轴相互垂直。此外，陀螺仪传感器110X与加速度传感器112X、陀螺仪传感器110Y与加速度传感器112Y、陀螺仪传感器110Z与加速度传感器112Z被配置为彼此的检测轴朝着相同方向(依次定义为X轴方向、Y轴方向、Z轴方向)。

陀螺仪传感器110X、110Y、110Z分别输出与施加给传感器模块100的绕X轴、绕Y轴、绕Z轴的角速度对应的大小的直流电压。另外，加速度传感器112X、112Y、112Z分别输出与施加给传感器模块100的X轴方向、Y轴方向、Z轴方向的加速度对应的大小的直流电压。

陀螺仪传感器110X、110Y、110Z的各输出电压在分别被放大器(AMP)120X、120Y、120Z放大之后，分别经过低通滤波器(LPF)130X、130Y、130Z而去除噪声成分。

同样，加速度传感器112X、112Y、112Z的各输出电压在分别被放大器(AMP)122X、122Y、122Z放大之后，分别经过低通滤波器(LPF)132X、132Y、132Z而去除噪声成分。

A/D转换器(ADC)140以时间分割方式对低通滤波器(LPF)130X、130Y、130Z，132X、132Y、132Z的各输出电压V_GX、V_GY、V_GZ、V_AX、V_AY、V_AZ进行采样，将它们分别转换为N比特的数字值GX、GY、GZ、AX、AY、AZ，并以时间分割的方式输出。

这里，GX、GY、GZ分别是与施加给传感器模块100的绕X轴、绕Y轴、绕Z轴的角速度的方向和大小对应的数字值(角速度信息)，AX、AY、AZ分别是与施加给传感器模块100的X轴方向、Y轴方向、Z轴方向的加速度的方向和大小对应的数字值(加速度信息)。该数字值GX、GY、GZ(角速度信息)和AX、AY、AZ(加速度信息)从传感器信息输出装置10A发送至姿势信息计算装置20A。

1﹣3.姿势信息计算装置的结构

图3是示出姿势信息计算装置20A的结构例的图。

姿势信息计算装置20A包含ADC接口电路(ADC I/F)212、微型计算机接口电路(微型计算机I/F)214、微型计算机300。

ADC接口电路(ADC I/F)212进行如下处理：在恰当的时机，依次进行采样而取得传感器信息输出装置10A以时间分割方式输出的角速度信息(GX、GY、GZ)和加速度信息(AX、AY、AZ)。

微型计算机接口电路(微型计算机I/F)214进行如下处理：根据由ADC接口电路(ADC I/F)212取得的角速度信息(GX、GY、GZ)，生成1组角速度矢量(惯性坐标系角速度矢量)G=(GX、GY、GZ)。另外，微型计算机接口电路(微型计算机I/F)214进行如下处理：根据由ADC接口电路(ADC I/F)212取得的加速度信息(AX、AY、AZ)，生成1组加速度矢量(惯性坐标系加速度矢量)A=(AX、AY、AZ)。微型计算机接口电路(微型计算机I/F)214还按照微型计算机的规格，进行比特扩展、取整处理等各种处理。将这样生成的惯性坐标系角速度矢量G和惯性坐标系加速度矢量A提供给微型计算机300。

此外，ADC接口电路(ADC I/F)212和微型计算机接口电路(微型计算机I/F)214作为图1所示的传感器信息取得部210发挥功能。

微型计算机300构成为包含处理部310、存储部320、通信部330等。另外，微型计算机300构成为可与信息存储介质400连接。

信息存储介质400存储程序及数据等，其功能可由光盘(CD、DVD等)、光磁盘(MO)、磁盘、硬盘、磁带或者存储器(ROM)等硬件来实现。

存储部320为处理部310及通信部330等的工作区，其功能可由RAM等硬件来实现。

处理部310根据存储在信息存储介质400中的程序(姿势信息计算程序)及从信息存储介质400读出的数据等，将存储部320作为工作区进行各种处理。即，在信息存储介质400中，存储有用于使微型计算机300(计算机的一例)作为本实施方式的各个单元发挥功能的程序(用于使计算机执行各个单元的处理的程序)。可通过各种处理器(CPU、DSP等)、ASIC(门阵列等)等硬件、应用程序、OS(例如通用OS等)来实现处理部310的功能。

通信部330进行用于与外部(例如，服务器装置或其它终端机)之间进行通信的各种控制，可通过各种处理器或通信用ASIC等硬件及程序等来实现其功能。

在本实施方式中，处理部310包含速度矢量更新单元232、姿势角矢量更新单元234、角速度偏移量更新单元236、加速度偏移量更新单元238。

速度矢量更新单元232进行如下处理：根据惯性坐标系加速度矢量(A)和时刻k﹣1处的修正后的固定坐标系速度矢量，更新时刻k处的固定坐标系速度矢量。例如，速度矢量更新单元232可以采用以下方式：根据时刻k﹣1处的固定坐标系姿势角矢量使惯性坐标系加速度矢量(A)旋转，变换为固定坐标系加速度矢量，从该固定坐标系加速度矢量中减去固定坐标系重力加速度矢量，以规定的单位时间Δt对相减结果进行1阶积分而计算单位时间固定坐标系速度矢量(单位时间Δt中的速度变化量)，在时刻k﹣1处的修正后的固定坐标系速度矢量中加上单位时间固定坐标系速度矢量，由此更新时刻k处的固定坐标系速度矢量。

姿势角矢量更新单元234进行如下处理：根据惯性坐标系角速度矢量(G)和时刻k﹣1处的修正后的固定坐标系姿势角矢量来更新时刻k处的固定坐标系姿势角矢量。例如，姿势角矢量更新单元234可采用如下方式：以规定的单位时间Δt对惯性坐标系角速度矢量(G)进行1阶积分，计算单位时间旋转角四元数(单位时间Δt中的旋转角)，使单位时间旋转角四元数作用于时刻k﹣1处的修正后的固定坐标系姿势角矢量，由此来更新时刻k处的固定坐标系姿势角矢量。

角速度偏移量更新单元236进行更新时刻k处的陀螺仪传感器110X、110Y、110Z的偏移量的处理。

加速度偏移量更新单元238进行更新时刻k处的加速度传感器112X、112Y、112Z的偏移量的处理。

此外，速度矢量更新单元232、姿势角矢量更新单元234、角速度偏移量更新单元236、加速度偏移量更新单元238作为图1所示的姿势信息更新部230发挥功能。

另外，在本实施方式中，处理部310包含速度矢量修正单元242、姿势角矢量修正单元244、角速度偏移量修正单元246、加速度偏移量修正单元248。

速度矢量修正单元242进行如下处理：按照使固定坐标系速度矢量的范数（norm）成为0的约束条件，修正由速度矢量更新单元232更新后的时刻k处的固定坐标系速度矢量。

姿势角矢量修正单元244进行如下处理：按照基于惯性坐标系加速度矢量(A)的约束条件，修正由姿势角矢量更新单元234更新后的时刻k处的固定坐标系姿势角矢量。

角速度偏移量修正单元246进行如下处理：按照给定的约束条件，修正由角速度偏移量更新单元236更新后的时刻k处的陀螺仪传感器110X、110Y、110Z的偏移量。

加速度偏移量修正单元248进行如下处理：按照给定的约束条件，修正由加速度偏移量更新单元238更新后的时刻k处的加速度传感器112X、112Y、112Z的偏移量。

此外，速度矢量修正单元242、姿势角矢量修正单元244、角速度偏移量修正单元246、加速度偏移量修正单元248作为图1所示的姿势信息修正部240发挥功能。

此外，在本实施方式中，处理部310包含位置坐标生成单元252、姿势角生成单元254。

位置坐标生成单元252进行如下处理：与由速度矢量修正单元242进行修正后的时刻k处的固定坐标系速度矢量对应地，生成虚拟3维空间中的位置坐标。例如，位置坐标生成单元252可构成为将时刻k处的修正后的固定坐标系速度矢量作为时刻k处的虚拟3维空间中的位置坐标。

姿势角生成单元254进行根据修正后的固定坐标系姿势角矢量而生成虚拟3维空间中的姿势角的处理。例如，姿势角生成单元254可构成为将时刻k处的修正后的固定坐标系姿势角矢量作为时刻k处的虚拟3维空间中的姿势角。

此外，位置坐标生成单元252和姿势角生成单元254作为图1所示的姿势信息生成部250发挥功能。

另外，处理部310通过作为姿势信息修正部240发挥功能的上述各个单元、作为姿势信息生成部250发挥功能的上述各个单元、作为姿势信息生成部250发挥功能的上述各个单元，从而作为图1所示的姿势信息计算部220发挥功能。

此外，用于使计算机作为本实施方式的各个单元发挥功能的程序(姿势信息计算程序)可从主机装置(服务器)所具有的信息存储介质经由网络以及通信部330分发到信息存储介质400(存储部320)。这样的主机装置(服务器)的信息存储介质的使用也包含于本发明的范围内。

1﹣4.姿势信息计算处理的步骤

1﹣4﹣1.处理步骤的概略

图4(A)是示出姿势信息计算装置20A的处理步骤的流程图。如图4(A)所示，姿势信息计算装置20A在产生结束事件之前(步骤S30中为“是”的情况)，反复进行上述传感器信息取得部210的传感器信息取得处理和姿势信息计算部220的姿势信息计算处理。

图4(B)是示出姿势信息计算部220的姿势信息计算处理(图4(A)的步骤S20的处理)的步骤的流程图。如图4(B)所示，姿势信息计算部220首先利用姿势信息更新部230进行姿势信息更新处理(步骤S22)。具体地说，速度矢量更新单元232、姿势角矢量更新单元234、角速度偏移量更新单元236、加速度偏移量更新单元238进行上述的各个处理。

接着，姿势信息计算部220利用姿势信息修正部240进行姿势信息修正处理(步骤S24)。具体地说，速度矢量修正单元242、姿势角矢量修正单元244、角速度偏移量修正单元246、加速度偏移量修正单元248进行上述的各个处理。

最后，姿势信息计算部220利用姿势信息生成部250进行姿势信息生成处理(步骤S26)。具体地说，位置坐标生成单元252和姿势角生成单元254进行上述的各个处理。

尤其在本实施方式中，姿势信息计算部220使用卡尔曼滤波器进行给定参数(变量)的更新处理和修正处理，估计虚拟3维空间中的姿势角的真值和位置坐标的真值。将规定时间设为单位时间Δt而在时刻0、1、…处分别离散地进行该卡尔曼滤波器的更新处理和修正处理。关于该单位时间Δt，根据惯性坐标系角速度矢量G和惯性坐标系加速度矢量A的更新周期、以及姿势信息计算系统1A中使用的应用程序的规格等来确定恰当的时间。

接着，对本实施方式中的卡尔曼滤波器的结构进行详细说明。

1﹣4﹣2.卡尔曼滤波器

[使用变量]

在本实施方式中，在卡尔曼滤波器的更新处理和修正处理中使用以下所示的变量。

Δt：离散积分用微小步骤时间(sec)

w_ω：角速度传感器中的、在时间上不具有相关关系且平均值为0的噪声(mV)

w_α：加速度传感器中的、在时间上不具有相关关系且平均值为0的噪声(mV)

V_gi：3个角速度传感器所输出的角速度矢量(mV)(惯性坐标系)

V_ai：3个加速度传感器所输出的加速度矢量(mV)(惯性坐标系)

Se_g：角速度传感器灵敏度(mV/dps)

Se_a：加速度传感器灵敏度(mV/(m/sec²))

bω₀：角速度传感器的初始偏移电压(mV)

bα₀：加速度传感器的初始偏移电压(mV)

bω^_k：时刻k处的更新后的角速度传感器偏移电压(mV)

bω_k：时刻k处的修正后的角速度传感器偏移电压(mV)

bα^_k：时刻k处的更新后的加速度传感器偏移电压(mV)

bα_k：时刻k处的修正后的加速度传感器偏移电压(mV)

ΔQt_g：3轴微小步长时间旋转角四元数(度)

Qt^_k：时刻k处的更新后的姿势角矢量(固定坐标系)

Qt_k：时刻k处的修正后的姿势角矢量(固定坐标系)

ΔVe_a：3轴微小步长时间速度变化量(m/sec)

Ve^_k：时刻k处的更新后的速度矢量(固定坐标系)

Ve_k：时刻k处的修正后的速度矢量(固定坐标系)

在本实施方式中，设定以下4种参数作为构成卡尔曼滤波器的更新/修正的对象的参数，所述4种参数是：固定坐标系速度矢量Ve_k(Ve^_k)、固定坐标系姿势角矢量Qt_k(Qt^_k)、陀螺仪传感器(110X、110Y、110Z)的偏移电压bω_k(bω^_k)、加速度传感器(112X、112Y、112Z)的偏移电压bα_k(bα^_k)。

固定坐标系速度矢量Ve_k(Ve^_k)表示实际3维空间中的传感器模块100的速度的方向和大小。固定坐标系姿势角矢量Qt_k(Qt^_k)是利用矢量来表示实际3维空间中的传感器模块100的姿势角，例如可使用法线矢量。陀螺仪传感器的偏移电压bω_k(bω^_k)是在未施加角速度时(例如静止时)由该陀螺仪传感器输出的电压值。加速度传感器的偏移电压bα_k(bα^_k)是在未施加加速度时(例如静止时)由该加速度传感器输出的电压值。

[处理步骤]

图5是示出使用了卡尔曼滤波器的姿势信息计算处理的步骤的流程图。如图5所示，姿势信息计算部220首先将表示当前时刻的变量k初始化为0(步骤S110)，之后进行卡尔曼滤波器的初始化处理(步骤S120)。在该初始化处理(步骤S120)中，进行将作为卡尔曼滤波器的更新/修正对象的参数设定为初始值的处理。这里，可设定预定的固定值作为Ve₀、Qt₀、bω₀、bα₀。例如，可设定范数为0的矢量(0、0、0)作为Ve₀，设定铅直方向单位矢量(0、0、﹣1)作为Qt₀，设定在预先测定的规定温度(25℃等)下的偏移电压值作为bω₀及bα₀。或者，可以在每次启动时进行测定，将其测定值设定为Ve₀、Qt₀、bω₀、bα₀。另外，还可将上次姿势信息计算处理中的最终值(上次启动结束时的值)设定为bω₀、bα₀。

接着，姿势信息计算部220使时刻k加1(步骤S130)，之后进行卡尔曼滤波器的更新处理(步骤S140)。在该更新处理(步骤S140)中，进行如下处理：根据从传感器信息取得部210提供的信息(惯性坐标系角速度矢量(G)和惯性坐标系加速度矢量(A))和时刻k﹣1处的参数值，更新时刻k处的参数值。

接着，姿势信息计算部220进行卡尔曼滤波器的修正处理(步骤S150)。在该修正处理(步骤S150)中进行如下处理：根据时刻k处的更新后的参数值和给定的约束条件来修正时刻k处的参数值。

接着，姿势信息计算部220进行姿势信息生成处理(步骤S160)。在该姿势信息生成处理(步骤S160)中，进行如下处理：根据时刻k处的修正后的参数值，生成时刻k处的姿势信息(虚拟3维空间中的姿势角和位置坐标)。

然后，如果在经过单位时间Δt之前产生了姿势信息计算处理的结束事件(步骤S170中为“是”的情况)，则姿势信息计算部220结束姿势信息计算处理。另一方面，如果即使经过了单位时间Δt也未产生姿势信息计算处理的结束事件(步骤S170中为“否”的情况)，则姿势信息计算部220使时刻k增加1(步骤S130)，之后再次进行卡尔曼滤波器更新处理(步骤S140)、卡尔曼滤波器修正处理(步骤S150)、姿势信息生成处理(步骤S160)。

这样，在产生结束事件之前，按照单位时间Δt反复进行卡尔曼滤波器更新处理(步骤S140)和卡尔曼滤波器修正处理(步骤S150)，生成新的姿势信息。

[卡尔曼滤波器更新处理]

在本实施方式的卡尔曼滤波器的更新处理中，进行如下处理：在时刻k，将在时刻k﹣1通过后述的修正处理进行修正后的4种参数bω_k﹣1、bα_k﹣1、Qt_k﹣1、Ve_k﹣1(或它们的初始值bω₀、bα₀、Qt₀、Ve₀)分别更新为bω^_k、bα^_k、Qt^_k、Ve^_k。

在扩展卡尔曼滤波器中所使用的更新式如下式(1)所示。

【式1】

${\hat{X}}_k=f(X_{k-1},u,w_{k-1})\·\·\·\left(1\right)$

X^_k=(bω^_k、bα^_k、Qt^_k、Ve^_k)^t，X_k﹣1=(bω_k﹣1、bα_k﹣1、Qt_k﹣1、Ve_k﹣1)^t，u=(V_gi、V_ai)，w_k﹣1=(w_ω、w_α)。式(1)表示这样的情况：当对时刻k﹣1处的修正后的参数X_k﹣1、传感器输出值u和传感器噪声w_k﹣1应用了函数f时，可获得时刻k处的更新后的参数X^_k。因为该函数f是非常复杂的函数，所以当精确计算式(1)时需要非常大的计算处理量，因而，希望实时计算姿势信息是不现实的。因此，在本实施方式中，定义了函数f的雅可比行列式F_k﹣1而使用了下面的更新式(2)。

【式2】

${\hat{X}}_k=F_{k-1}\×X_{k-1}\·\·\·\left(2\right)$

根据更新式(2)，通过计算F_k﹣1和X_k﹣1的矩阵积能够获得X^_k，所以适合于实时地计算姿势信息。这里，如上所述，因为X^_k=(bω^_k、bα^_k、Qt^_k、Ve^_k)^t、X_k﹣1=(bω_{k ﹣1}、bα_k﹣1、Qt_k﹣1、Ve_k﹣1)^t，所以定义用于求出bω^_k、bα^_k、Qt^_k、Ve^_k的更新式，以满足这些更新式的关系的方式实施各种矩阵变形，由此能够求出F_k﹣1。

在本实施方式中，分别如下式(3)、(4)那样定义用于求出bω^_k、bα^_k的更新式。

【式3】

${\widehat{\mathrm{b\ω}}}_k:={\mathrm{b\ω}}_{k-1}\·\·\·\left(3\right)$

【式4】

${\widehat{\mathrm{b\α}}}_k:={\mathrm{b\α}}_{k-1}\·\·\·\left(4\right)$

另外，在本实施方式中，如下式(5)那样定义用于求出Qt^_k的更新式。

【式5】

${\widehat{\mathrm{Qt}}}_k:={\mathrm{\ΔQt}}_g^{*}\×{\mathrm{Qt}}_{k-1}\×{\mathrm{\ΔQt}}_g\·\·\·\left(5\right)$

式(5)中的ΔQt_g可利用下式(6)求出。

【式6】

ΔQt_g＝ω·Δt…(6)

式(6)中的ω可利用下式(7)求出。

【式7】

ω＝φ((ar_x，ar_y，ar_z))＝1+i·ar_x/2+j·ar_y/2+k·ar_z/2…(7)

函数是将角速度矢量变换为四元数的函数，i、j、k是3个四元数的基元。另外，(ar_x、ar_y、ar_z)是惯性坐标系中的角速度矢量(惯性坐标系角速度矢量)，ar_x、ar_y、ar_z是分别绕X轴、绕Y轴、绕Z轴的角速度。(ar_x、ar_y、ar_z)可利用下式(8)求出。

【式8】

(ar_x，ar_y，ar_z)＝(V_gi-bω_k-1)/Se_g…(8)

另外，在本实施方式中，如下式(9)那样定义用于求出Ve^_k的更新式。

【式9】

${\widehat{\mathrm{Ve}}}_k:={\mathrm{Ve}}_{k-1}+{\mathrm{\ΔVe}}_a\·\·\·\left(9\right)$

可利用下式(10)来求出ΔVe_a。

【式10】

ΔVe_a＝((ρ(Qt_k-1))^*(gr_x，gr_y，gr_z)×(ρ(Qt_k-1))-G)×Δt…(10)

函数ρ是将Qt_k﹣1变换为从初始姿势角矢量Qt₀向Qt_k﹣1的旋转四元数的函数。(ρ(Qt_k﹣1))^*是(ρ(Qt_k﹣1))的共轭四元数。G是重力加速度矢量。另外，(gr_x、gr_y、gr_z)是惯性坐标系中的加速度矢量(惯性坐标系加速度矢量)，gr_x、gr_y、gr_z分别是X轴方向、Y轴方向、Z轴方向的加速度。(gr_x、gr_y、gr_z)可利用下式(11)来求出。

【式11】

(gr_x，gr_y，gr_z)＝(V_ai-bα_k-1)/Se_a…(11)

以满足这样定义的4个更新式(3)、(4)、(5)、(9)的方式实施各种矩阵变形，由此能够求出F_k﹣1。

此外，在本实施方式的卡尔曼滤波器的更新处理中，还进行以下处理：在时刻k，使用F_k﹣1，将在时刻k﹣1利用后述的修正处理进行修正后的X_k﹣1与真值的残差的协方差矩阵P_k﹣1(：E[ε_k﹣1，ε_k﹣1 ^t])更新为P^_k。如下式(11)那样地定义用于求出协方差矩阵P^_k的更新式。

【式12】

${\hat{P}}_k:=F_{k-1}{P}_{k-1}{\left(F_{k-1}\right)}^t+Q_{k-1}\·\·\·\left(12\right)$

Q_k﹣1(：E[w_k﹣1，w_k﹣1 ^t])表示w_k﹣1的协方差矩阵。

接着，对卡尔曼滤波器的更新处理步骤的一例进行说明。图6是示出本实施方式中的卡尔曼滤波器的更新处理的步骤的一例的流程图。

首先，使用式(8)，根据3个陀螺仪传感器110X、110Y、110Z的输出电压V_gi计算惯性坐标系角速度矢量(ar_x、ar_y、ar_z)(步骤S141)。

接着，使用式(6)和式(7)，根据在步骤S141中计算出的惯性坐标系角速度矢量(ar_x、ar_y、ar_z)计算3轴微小步长时间旋转角四元数ΔQt_g(步骤S142)。

接着，使用式(11)，根据3个加速度传感器112X、112Y、112Z的输出电压V_ai计算惯性坐标系加速度矢量(gr_x、gr_y、gr_z)(步骤S143)。

接着，使用式(10)，根据在步骤S143中计算出的惯性坐标系加速度矢量计算3轴微小步长时间速度变化量ΔVe_a(步骤S144)。

接着，使用在步骤S142中计算出的3轴微小步长时间旋转角四元数ΔQt_g和在步骤S144中计算出的3轴微小步长时间速度变化量ΔVe_a，以满足式(3)、(4)、(5)、(9)的方式实施各种矩阵变形而生成F_k﹣1(步骤S145)。

接着，使用更新式(2)计算F_k﹣1与X_k﹣1的矩阵积，将X_k﹣1更新为X^_k(步骤S146)。

最后，使用更新式(12)将P_k﹣1更新为P^_k(步骤S147)。

[卡尔曼滤波器修正处理]

在本实施方式的卡尔曼滤波器的更新处理中，进行如下处理：在时刻k，将通过更新处理得到的时刻k处的更新后的4种参数bω^_k、bα^_k、Qt^_k、Ve^_k分别修正为bω_k、bα_k、Qt_k、Ve_k。

在卡尔曼滤波器的修正处理中，定义如下式(13)所示的观测方程式。

【式13】

y=Z-HX…(13)

在式(13)中，Z是观测值矢量，X是待求出的值，y是观测参数，H是观测方程式斜率分量矩阵。

在卡尔曼滤波器修正处理中，进行如下处理：求出使得观测方程式(13)中的观测残差y成为最小、即、使得由下式(14)定义的评价函数J_WLS成为最小的X。

【式14】

J_WLS＝(Z-HX)^tW(Z-HX)…(14)

W是表示权重的对象矩阵。

在本实施方式中，所求出的值X对应于X_k=(bω_k、bα_k、Qt_k、Ve_k)^t，观测值矢量Z对应于时刻k处的观测值矢量Z_k，H对应于时刻k﹣1处的观测方程式斜率分量矩阵H_k﹣1。其中，H_k﹣1既可以在各个时刻每次重新进行计算，也可以是固定值。

本实施方式中使用的修正式如下式(15)那样进行定义。

【式15】

$X_k:={\hat{X}}_k+K_k(Z_k-H_{k-1}{\hat{X}}_k)\·\·\·\left(15\right)$

K_k是卡尔曼增益，如下式(16)那样进行定义。

【式16】

$K_k:={\hat{P}}_k{H_{k-1}}^t{\left(S_k\right)}^{-1}\·\·\·\left(16\right)$

S_k如下式(17)那样进行定义。

【式17】

$S_k:=H_{k-1}{\hat{P}}_k{H_{k-1}}^t+R_{k-1}\·\·\·\left(17\right)$

R_k﹣1如下式(18)那样进行定义。

【式18】

R_k-1:＝W^-1…(18)

这里，关于修正式(15)中对X^_k进行修正以使评价函数J_WLS成为最小的处理，就是进行使Z_k﹣H_k﹣1X_k成为最小、换言之使H_k﹣1X_k接近于Z_k的修正处理。因此，为了提高X_k的真值的估计精度，希望观测值矢量Z_k为最似然值。

因此，在本实施方式中，采用了使用式(11)根据加速度传感器112X、112Y、112Z的输出电压计算的加速度矢量(gr_x、gr_y、gr_z)(=V_ai﹣bα_k﹣1/Se_a)，作为用于对时刻k处的更新后的姿势角矢量Qt^_k进行修正的观测值。当传感器模块100为静止状态时，对加速度传感器仅施加了重力加速度，所以，可基于根据加速度传感器112X、112Y、112Z的输出电压计算的重力方向，来修正姿势角矢量Qt^_k。由此，能够校正由陀螺仪传感器110X、110Y、110Z的偏移误差产生的水平面漂移。

另外，在本实施方式中，采用(0、0、0)作为用于对时刻k处的更新后的速度矢量Ve^_k进行修正的观测值。即，时刻k处的修正后的速度矢量Ve_k接近于(0、0、0)。并且，在本实施方式中，将速度矢量Ve_k定义为虚拟3维空间中的位置坐标，所以在传感器模块100为静止状态时，虚拟3维空间中的位置坐标返回到原点(0、0、0)。由此，能够校正由加速度传感器112X、112Y、112Z的偏移误差产生的虚拟3维空间中的位置坐标的漂移。

此外，在本实施方式的卡尔曼滤波器的修正处理中，还进行如下处理：在时刻k，使用K_k和H_k﹣1，将在时刻k处通过更新处理进行更新后的协方差矩阵P^_k修正为P_k。协方差矩阵P^_k的修正式如下式(19)那样进行定义。

【式19】

$P_k:=(I-K_k{H}_{k-1}){\hat{P}}_k\·\·\·\left(19\right)$

接着，对卡尔曼滤波器的修正处理的步骤的一例进行说明。图7是示出本实施方式中的卡尔曼滤波器的修正处理的步骤的一例的流程图。

首先，使用式(16)、式(17)、式(18)计算卡尔曼增益K_k(步骤S151)。

接着，计算观测值矢量Zk(步骤S152)。如前所述，尤其在本实施方式中，计算(gr_x、gr_y、gr_z)作为用于修正Qt^_k的观测值，而用于修正Ve^_k的观测值为(0、0、0)。此外，用于修正bω^_k的观测值和用于修正bα^_k的观测值可根据通常已知的式子进行计算。

接着，使用修正式(15)将X^_k修正为X_k(步骤S153)。

最后，使用修正式(19)将P^_k修正为P_k(步骤S154)。

1﹣5.效果

如以上说明的那样，在本实施方式中，对于将惯性坐标系(传感器坐标系)的加速度矢量变换为固定坐标系加速度矢量之后进行1阶积分而获得的固定坐标系速度矢量，使得该固定坐标系速度矢量与虚拟3维空间中的位置坐标1对1地对应。由此，能够将固定坐标系速度矢量以与范数和卡尔曼增益的积成正比的速度引回到原点，从而即使在系统启动之后经过了一定程度的时间，也能够稳定地生成位置坐标。更具体地说，在本实施方式中，在进行固定坐标系速度矢量的更新处理之后，按照将固定坐标系速度矢量设为(0、0、0)的约束条件进行修正处理，与修正后的固定坐标系速度矢量对应地生成虚拟3维空间中的位置坐标。由此，以返回原点的方式生成位置坐标，所以能够有效地抑制由传感器偏移所引起的位置坐标的发散。此外，对加速度矢量进行1阶积分而获得速度矢量，因此，通过将速度矢量与位置坐标对应起来，使得位置坐标的漂移量与传感器偏移量成正比(不像现有技术那样，与传感器偏移量的平方成正比)，抑制了急剧的增加，所以能够使得卡尔曼增益较小。

另外，在本实施方式中，对于将惯性坐标系(传感器坐标系)的角速度矢量变换为固定坐标系角速度矢量之后进行1阶积分而获得的固定坐标系姿势角矢量，使得该固定坐标系姿势角矢量与虚拟3维空间中的姿势角1对1地对应。由此，能够抑制因传感器偏移引起的姿势角漂移量的急剧增加，能够使得卡尔曼增益较小。

另外，在本实施方式中，将修正后的固定坐标系速度矢量以及修正后的固定坐标系姿势角矢量各自直接作为虚拟3维空间中的位置坐标以及姿势角，所以不需要进行预先生成位置坐标的计算，能够降低计算量。

此外，根据本实施方式，当在用户应用程序中需要使用姿势时，可使用惯性传感器计算位置，所以，不需要在外部准备传感器来向位于安装有惯性传感器的系统内的用户应用程序或位于系统外部的用户应用程序发送加速度数据而进行处理。

另外，计算位置的系统可利用惯性传感器所位于的系统单独地构成系统，所以不需要使用多个系统彼此之间的通信接口，也不需要规模较大的硬件及系统资源。

另外，在系统的GUI中使用3D窗口作为窗口、且使用传感器模块作为定点设备的情况下，将模块惯性力信息(角速度信息、加速度信息)以物理方式反映在3D窗口内的系统自然就作为UI，此时，可提供用于计算也包含位置(3D窗口)在内的姿势的算法。

这样，根据本实施方式，能够实现简单的结构，并且能够抑制位置坐标与姿势角的发散而确切地计算虚拟3维空间中的姿势信息。

2.第2实施方式

2﹣1.姿势信息计算系统的结构

图8是第2实施方式的姿势信息计算系统的概略结构图。如图8所示，在第2实施方式的姿势信息计算系统1B中，与第1实施方式的姿势信息计算系统1A的不同点是，传感器信息输出装置10B构成为包含温度传感器114。即，传感器信息输出装置10B除了输出相互垂直的3轴(X轴、Y轴、Z轴)方向的角速度信息和加速度信息之外，还输出温度信息。

姿势信息计算装置20B进行如下处理：根据传感器信息输出装置10B的检测信息(加速度信息、角速度信息、温度信息)，计算虚拟3维空间中的姿势角和位置坐标。

2﹣2.传感器信息输出装置的结构

图9是示出传感器信息输出装置10B的结构例的图。在传感器模块102中，与第1实施方式中的传感器模块100同样，固定着3个陀螺仪传感器110X、110Y、110Z和3个加速度传感器112X、112Y、112Z。此外，传感器模块102在陀螺仪传感器110X、110Y、110Z、加速度传感器112X、112Y、112Z的附近固定着温度传感器114。温度传感器114输出与周边温度对应的大小的直流电压。

温度传感器114的输出电压在经由放大器(AMP)124放大之后，经过低通滤波器(LPF)134而去除噪声成分。

A/D转换器(ADC)140以时间分割方式对低通滤波器(LPF)130X、130Y、130Z、132X、132Y、132Z、134的各输出电压V_GX、V_GY、V_GZ、V_AX、V_AY、V_AZ、V_T进行采样，分别变换为N比特的数字值GX、GY、GZ、AX、AY、AZ、T(T为与温度传感器114的周边温度对应的数字值(温度信息))，并以时间分割方式进行输出。该数字值GX、GY、GZ(角速度信息)、AX、AY、AZ(加速度信息)、T(温度信息)从传感器信息输出装置10B向姿势信息计算装置20B发送。

传感器信息输出装置10B的其它结构与传感器信息输出装置10A相同，所以省略其说明。

2﹣3.姿势信息计算装置的结构

图10是示出姿势信息计算装置20B的结构例的图。

在姿势信息计算装置20B中，ADC接口电路(ADC I/F)212进行如下处理：在恰当的时机依次进行采样而取得传感器信息输出装置10B以时间分割方式输出的角速度信息(GX、GY、GZ)、加速度信息(AX、AY、AZ)、温度信息(T)。

微型计算机接口电路(微型计算机I/F)214与第1实施方式中的姿势信息计算装置20A同样，进行生成惯性坐标系角速度矢量(G)及惯性坐标系加速度矢量(A)的处理和其它各种处理，此外，还将温度信息T与惯性坐标系角速度矢量G及惯性坐标系加速度矢量A一起提供给微型计算机300。

微型计算机300的结构与姿势信息计算装置20A是相同的，但角速度偏移量更新单元236和加速度偏移量更新单元238的功能与第1实施方式不同。

角速度偏移量更新单元236进行如下处理：根据温度信息(T)和时刻k﹣1处的修正后的陀螺仪传感器110X、110Y、110Z的偏移量来更新时刻k处的陀螺仪传感器110X、110Y、110Z的偏移量。例如，角速度偏移量更新单元236可采用以下方式：在时刻k﹣1处的修正后的陀螺仪传感器110X、110Y、110Z的偏移量中，加上偏移量校正值，由此来更新时刻k处的陀螺仪传感器110X、110Y、110Z的偏移量，其中，所述偏移量校正值是参照预先定义的、温度信息(T)与陀螺仪传感器110X、110Y、110Z的偏移量校正值之间的对应关系而获得的。

加速度偏移量更新单元238进行如下处理：根据温度信息(T)和时刻k﹣1处的修正后的加速度传感器112X、112Y、112Z的偏移量来更新时刻k处的加速度传感器112X、112Y、112Z的偏移量。例如，加速度偏移量更新单元238可采用如下方式：在时刻k﹣1处的修正后的加速度传感器112X、112Y、112Z的偏移量中，加上偏移量校正值相加，由此来更新时刻k处的加速度传感器112X、112Y、112Z的偏移量，其中，所述偏移量校正值是参照预先定义的、温度信息(T)与加速度传感器112X、112Y、112Z的偏移量校正值之间的对应关系而获得的。

姿势信息计算装置20B的其它结构与姿势信息计算装置20A相同，所以省略其说明。

2﹣4.姿势信息计算处理的步骤

2﹣4﹣1.处理步骤的概略

第2实施方式中的姿势信息计算装置20B的全部处理步骤与图4(A)以及图4(B)所说明的处理步骤相同，所以省略其说明。

接着，对第2实施方式中的卡尔曼滤波器的结构进行详细说明。

2﹣4﹣2.卡尔曼滤波器

[使用变量]

在第2实施方式中的卡尔曼滤波器的更新处理和修正处理中，除了第1实施方式中说明的变量之外，还使用以下所示的变量。

Se_t：温度灵敏度(mV/℃)

V_t：温度传感器的输出值(mV)

与第1实施方式相同，在第2实施方式中，这些变量中构成卡尔曼滤波器的更新/修正对象的变量(参数)也是bω_k(bω^_k)、bα_k(bα^_k)、Qt_k(Qt^_k)、Ve_k(Ve^_k)4种。

[处理步骤]

使用了第2实施方式中的卡尔曼滤波器的姿势信息计算处理的步骤与图5说明的处理步骤相同，所以省略其说明。但是，在第2实施方式中，与第1实施方式的不同点是，在卡尔曼滤波器的更新处理(步骤S140)中，进行如下处理：根据惯性坐标系角速度矢量(G)、惯性坐标系加速度矢量(A)、温度信息(T)和时刻k﹣1处的参数值来更新时刻k处的参数值。

[卡尔曼滤波器更新处理]

在第2实施方式中，由第1实施方式说明的4个更新式(3)、(4)、(5)、(9)中的更新式(5)和(9)的形式没有变化，但更新式(3)和(4)的形式不同。

在第2实施方式中，如下式(20)那样地定义用于求出bω^_k的更新式。

【式20】

${\widehat{\mathrm{b\ω}}}_k:={\mathrm{b\ω}}_{k-1}+\mathrm{b\ω}\left(T\right)\·\·\·\left(20\right)$

函数bω是将温度T变换为角速度传感器偏移量校正用电压值的函数。温度T可由下式(21)求出。

【式21】

T=V_t/Se_t…(21)

另外，在第2实施方式中，如下式(22)那样地定义用于求出bα^_k的更新式。

【式22】

${\widehat{\mathrm{b\α}}}_k:={\mathrm{b\α}}_{k-1}+\mathrm{b\α}\left(T\right)\·\·\·\left(22\right)$

函数bα是将温度T变换为加速度传感器偏移量校正用电压值的函数。温度T可由上式(21)求出。

第2实施方式中的卡尔曼滤波器更新处理的其它式子与第1实施方式相同，所以省略其说明。

接着，对卡尔曼滤波器的更新处理的步骤的一例进行说明。图11是示出第2实施方式中的卡尔曼滤波器的更新处理的步骤的一例的流程图。

首先，使用式(21)，根据温度传感器114的输出电压V_t计算温度T(步骤S240)。

接着，使用式(20)，根据在步骤S240中计算出的温度值T计算角速度传感器偏移量校正用电压值bω(T)(步骤S241)。

接着，使用式(22)，根据在步骤S240中计算出的温度值T计算加速度传感器偏移量校正用电压值bα(T)(步骤S242)。

接着，进行步骤S243～S249的处理而结束更新处理，步骤S243～S249的处理与图11所示的步骤S141～S147的处理相同，所以省略其说明。

[卡尔曼滤波器修正处理]

第2实施方式中的卡尔曼滤波器修正处理的内容以及步骤与第1实施方式的相同，所以省略其说明。

2﹣5.效果

根据以上说明的第2实施方式，除了与第1实施方式同样的效果之外，还实现了以下效果。

即，在第2实施方式中，在求出bω^_k的更新式(20)和求出bα^_k的更新式(21)中都追加了温度校正项，在修正式(15)中，还对bω^_k追加了基于温度校正项的修正。这样，考虑了由温度引起的传感器偏移量的变动而对传感器偏移量bω^_k、bα^_k进行更新，由此能够降低由传感器偏移引起的漂移量，所以进一步减小了卡尔曼增益。因此，能够使得虚拟3维空间中的位置坐标和姿势角更迅速地跟随于传感器的输出。

3.应用例

本实施方式的姿势信息计算系统1A(1B)例如可应用于使用了作为定点设备的3D鼠标、虚拟实境（virtual reality）等中用到的头盔显示器、检测头部姿势的跟踪器、3D游戏手柄等的各种系统。作为一例，在图12中示出应用了本实施方式的姿势信息计算系统的游戏系统。

图12所示的游戏系统1000由控制器500、游戏机主体600、显示装置700构成，控制器500与游戏机主体600、游戏机主体600与显示装置700分别通过通信电缆610和620进行连接，能够进行信息通信。

在该游戏系统1000中，玩家可通过控制器500来操作配置在虚拟3维空间内的战斗机对象(3D模型)，通过游戏机主体600的游戏运算处理，使该战斗机对象作为战斗机720以二维方式显示到显示装置700的显示部710上。

在该游戏系统1000中，战斗机720的姿势与控制器的姿势联动。例如，当使游戏控制器500在水平面内沿左右方向(图中的A方向)移动时，战斗机720与此对应地沿左右方向(图中的a方向)移动。另外，当使游戏控制器500在水平面内沿前后方向(图中的B方向)移动时，战斗机720与此对应地沿上下方向(图中的b方向)移动。此外，当使游戏控制器500在水平面内旋转时(图中C方向的旋转)，战斗机720与此对应地进行旋转(图中c方向的旋转)。

为了能够实现该操作，控制器500在其内部设置有被配置为检测轴相互垂直的3个角速度传感器和被配置为检测轴相互垂直的3个加速度传感器，这些传感器的输出值被发送至游戏机主体600，进行虚拟3维空间中的战斗机对象的姿势角和位置坐标的计算处理。

通过将控制器500和显示装置700分别构成为本实施方式的传感器信息输出装置10A(10B)和姿势信息计算装置20A(20B)，能够实现该游戏系统1000。由此，能够使得控制器500的姿势角和移动速度分别与战斗机对象的姿势角和位置坐标相对应，每当停止控制器500的移动时，对由传感器偏移引起的漂移进行校正，能够使战斗机720返回到显示部710的中心(原点)。另外，通过将控制器500的移动速度与战斗机对象的位置坐标对应起来，能够使得卡尔曼增益较小，所以，也容易使战斗机对象的姿势实时跟随于用户的操作。

此外，在控制器500中设置有操作杆510和520，玩家可通过对操作杆510、520进行操作，来操纵战斗机对象所具备的导弹等各种装备。即，在该游戏系统1000中，通过改变控制器500的姿势而与其对应地改变战斗机对象的姿势，并且，通过操作杆510、520的操作，可与该姿势操作同时地操纵战斗机对象的装备，所以能够提高玩家的操作性及游戏的趣味性。

需要说明的是，本发明不限于本实施方式，可在本发明主旨的范围内实施各种变形。

例如，以上说明了传感器信息输出装置10A中包含的3个陀螺仪传感器110X、110Y、110Z检测相互垂直的3个轴(X轴、Y轴、Z轴)方向的角速度的例子，但本发明的范围不限于此，陀螺仪传感器110X、110Y、110Z检测相互不平行的任意3个轴方向的角速度的情况也包含于本发明的范围内。在此情况下，姿势信息计算部220只要利用对准校正将相互不平行的任意3个轴中的角速度矢量变换为垂直坐标系中的角速度矢量，且之后进行本实施方式中所说明的处理即可。

同样，以上说明了传感器信息输出装置10A中包含的3个加速度传感器112X、112Y、112Z检测相互垂直的3个轴(X轴、Y轴、Z轴)方向的加速度的例子，但本发明的范围不限于此，加速度传感器112X、112Y、112Z检测相互不平行的任意3个轴方向的加速度的情况也包含在本发明的范围内。在此情况下，姿势信息计算部220只要利用对准校正将相互不平行的任意3个轴中的加速度矢量变换为垂直坐标系中的加速度矢量，且之后进行本实施方式中所说明的处理即可。

本发明包含与实施方式中说明的结构实质相同的结构(例如，功能、方法以及结果相同的结构、或者目的以及效果相同的结构)。另外，本发明包含对实施方式中说明的结构的非本质部分进行置换后的结构。另外，本发明包含能够起到与实施方式中说明的结构相同作用效果的结构或能够实现同一目的的结构。另外，本发明包含在实施方式所说明的结构中添加了公知技术后的结构。

Claims (10)

1.一种姿势信息计算装置，其根据由3个角速度传感器分别检测的绕相互不平行的3个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置坐标，该姿势信息计算装置包含：

传感器信息取得部，其取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及

姿势信息计算部，其根据由上述传感器信息取得部取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，

其中，上述姿势信息计算部根据由上述3个轴方向的加速度信息得到的惯性坐标系加速度矢量计算固定坐标系速度矢量，与该固定坐标系速度矢量的终点的坐标对应地计算上述虚拟3维空间中的上述位置坐标，

上述姿势信息计算部包含：

姿势信息更新部，其根据绕上述3个轴的角速度信息和3个轴方向的加速度信息，更新为了确定上述虚拟3维空间中的上述姿势角和上述位置坐标所需的给定参数；

姿势信息修正部，其按照给定的约束条件，修正由上述姿势信息更新部进行更新后的上述参数；以及

姿势信息生成部，其根据由上述姿势信息修正部进行修正后的上述参数，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，

上述姿势信息更新部包含速度矢量更新单元，该速度矢量更新单元根据上述惯性坐标系加速度矢量和时刻k﹣1处的上述固定坐标系速度矢量，更新时刻k处的上述固定坐标系速度矢量，

上述姿势信息修正部包含速度矢量修正单元，该速度矢量修正单元按照使上述固定坐标系速度矢量的范数成为0的约束条件，修正由上述速度矢量更新单元进行更新后的时刻k处的上述固定坐标系速度矢量，

上述姿势信息生成部包含位置坐标生成单元，该位置坐标生成单元与由上述速度矢量修正单元进行修正后的时刻k处的上述固定坐标系速度矢量对应地生成上述虚拟3维空间中的上述位置坐标。

2.根据权利要求1所述的姿势信息计算装置，其中，

上述速度矢量更新单元执行以下处理：

根据时刻k﹣1处的固定坐标系姿势角矢量使上述惯性坐标系加速度矢量进行旋转而变换为固定坐标系加速度矢量，从该固定坐标系加速度矢量中减去固定坐标系重力加速度矢量，以规定的单位时间对相减结果进行1阶积分而计算单位时间固定坐标系速度矢量，在上述速度矢量修正单元进行修正后的时刻k﹣1处的上述固定坐标系速度矢量中加上上述单位时间固定坐标系速度矢量，由此来更新时刻k处的上述固定坐标系速度矢量。

3.根据权利要求1所述的姿势信息计算装置，其中，

上述位置坐标生成单元将由上述速度矢量修正单元进行修正后的时刻k处的上述固定坐标系速度矢量的终点的坐标作为时刻k处的上述位置坐标。

4.根据权利要求1所述的姿势信息计算装置，其中，

上述姿势信息计算部根据由绕上述3个轴的角速度信息得到的惯性坐标系角速度矢量计算固定坐标系姿势角矢量，根据该固定坐标系姿势角矢量计算上述虚拟3维空间中的上述姿势角，

上述姿势信息更新部包含姿势角矢量更新单元，该姿势角矢量更新单元根据上述惯性坐标系角速度矢量和时刻k﹣1处的上述固定坐标系姿势角矢量，更新时刻k处的上述固定坐标系姿势角矢量，

上述姿势信息修正部包含姿势角矢量修正单元，该姿势角矢量修正单元按照基于上述惯性坐标系加速度矢量的约束条件，修正由上述姿势角矢量更新单元进行更新后的时刻k处的上述固定坐标系姿势角矢量，

上述姿势信息生成部包含姿势角生成单元，该姿势角生成单元根据由上述姿势角矢量修正单元进行修正后的上述固定坐标系姿势角矢量，生成上述虚拟3维空间中的上述姿势角。

5.根据权利要求4所述的姿势信息计算装置，其中，

上述姿势角矢量更新单元执行以下处理：

以规定的单位时间对上述惯性坐标系角速度矢量进行1阶积分而计算单位时间旋转角四元数，使上述单位时间旋转角四元数作用于由上述姿势角矢量修正单元进行修正后的时刻k﹣1处的上述固定坐标系姿势角矢量，由此来更新时刻k处的上述固定坐标系姿势角矢量。

6.根据权利要求4所述的姿势信息计算装置，其中，

上述姿势角生成单元将由上述姿势角矢量修正单元进行修正后的时刻k处的上述固定坐标系姿势角矢量作为时刻k处的上述姿势角。

7.根据权利要求1～6中任意一项所述的姿势信息计算装置，其中，

上述传感器信息取得部接受由温度传感器检测的温度信息，

上述姿势信息更新部包含：

角速度偏移量更新单元，其根据与上述温度信息对应的角速度传感器的偏移量和时刻k﹣1处的上述3个角速度传感器的偏移量，更新时刻k处的上述3个角速度传感器的偏移量；以及

加速度偏移量更新单元，其根据与上述温度信息对应的加速度传感器的偏移量和时刻k﹣1处的上述3个加速度传感器的偏移量，更新时刻k处的上述3个加速度传感器的偏移量，

上述姿势信息修正部包含：

角速度偏移量修正单元，其按照给定的约束条件，修正由上述角速度偏移量更新单元进行更新后的时刻k处的上述3个角速度传感器的偏移量；以及

加速度偏移量修正单元，其按照给定的约束条件，修正由上述加速度偏移量更新单元进行更新后的时刻k处的上述3个加速度传感器的偏移量。

8.一种姿势信息计算系统，该姿势信息计算系统包含：

3个角速度传感器，其分别检测绕相互不平行的3个轴的角速度信息；

3个加速度传感器，其分别检测相互不平行的3个轴方向的加速度信息；以及

权利要求1至6中任意一项所述的姿势信息计算装置。

9.一种姿势信息计算系统，该姿势信息计算系统包含：

3个角速度传感器，其分别检测绕相互不平行的3个轴的角速度信息；

3个加速度传感器，其分别检测相互不平行的3个轴方向的加速度信息；以及

权利要求7所述的姿势信息计算装置。

10.一种姿势信息计算方法，根据由3个角速度传感器分别检测的绕相互不平行的3个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置坐标，该姿势信息计算方法包含以下步骤：

传感器信息取得步骤，取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及

姿势信息计算步骤，根据在上述传感器信息取得步骤中取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，

在上述姿势信息计算步骤中，根据由上述3个轴方向的加速度信息得到的惯性坐标系加速度矢量计算固定坐标系速度矢量，与该固定坐标系速度矢量的终点的坐标对应地计算上述虚拟3维空间中的上述位置坐标，

上述姿势信息计算步骤包含：

姿势信息更新步骤，根据绕上述3个轴的角速度信息和3个轴方向的加速度信息，更新为了确定上述虚拟3维空间中的上述姿势角和上述位置坐标所需的给定参数；

姿势信息修正步骤，按照给定的约束条件，修正在上述姿势信息更新步骤中进行更新后的上述参数；以及

姿势信息生成步骤，根据在上述姿势信息修正步骤中进行修正后的上述参数，计算上述虚拟3维空间中的上述姿势角和上述位置坐标，

上述姿势信息更新步骤包含速度矢量更新步骤，在该速度矢量更新步骤中，根据上述惯性坐标系加速度矢量和时刻k﹣1处的上述固定坐标系速度矢量，更新时刻k处的上述固定坐标系速度矢量，

上述姿势信息修正步骤包含速度矢量修正步骤，在该速度矢量修正步骤中，按照使上述固定坐标系速度矢量的范数成为0的约束条件，修正在上述速度矢量更新步骤中进行更新后的时刻k处的上述固定坐标系速度矢量，

上述姿势信息生成步骤包含位置坐标生成步骤，在该位置坐标生成步骤中，与在上述速度矢量修正步骤中进行修正后的时刻k处的上述固定坐标系速度矢量对应地生成上述虚拟3维空间中的上述位置坐标。