专利名称：姿势信息计算装置、姿势信息计算系统及姿势信息计算法的利记博彩app
技术领域：
本发明涉及姿势信息计算装置、姿势信息计算系统以及姿势信息计算方法。
背景技术：
近年来，在各种情况下均利用了使用惯性传感器计算物体姿势的系统。例如，在汽车、直升机等移动体上安装3轴角速度传感器和3轴加速度传感器，根据从传感器获得的信息计算移动体的姿势，由此实现进行移动体姿势的稳定控制的系统等。一般情况下，物体的姿势可根据姿势角和位置来确定。通过对3轴角速度传感器检测出的角速度矢量进行1阶积分来获得物体的姿势角。不过，由于3轴角速度传感器的输出值包含微小的偏移量，所以，对角速度矢量进行1阶积分将导致偏移量逐渐蓄积。因此，通过计算获得的姿势角相对于正确姿势角的误差与偏移量成比例地逐渐增大，例如，虽然物体是静止的，但却得到了好像正在以微小的角速度进行旋转的结果。作为解决此问题的方法，例如日本特开平9-5104号公报及日本特开2007-183138号公报所公开的那样，使用卡尔曼滤波器根据传感器输出值来估计姿势角真值的方法是公知的。另一方面，物体的位置是通过对3轴加速度传感器检测出的加速度矢量进行2阶积分而获得的。因为在3轴加速度传感器的输出值中也包含微小的偏移量，所以偏移量逐渐蓄积，从而虽然物体是静止的，但却得到了好像正在移动这样的结果。而且，因为是对加速度矢量进行2阶积分，所以，通过计算获得的位置相对于正确位置的误差增加，该误差的增大速度与偏移量的平方成正比，因而该增加速度远远大于姿势角误差的增大速度。因此， 为了使用卡尔曼滤波器估计位置的真值，必需增大卡尔曼增益。但是，当增大卡尔曼增益时，使物体的计算出的位置回归到正确位置的速度过快，虽然实际上物体在慢慢移动，但却得到了好像处于静止这样的计算结果。尤其，如果传感器的偏移量比较大，则必需将卡尔曼增益设定得更大，在现实中，仅基于加速度传感器的输出值使用卡尔曼滤波器来估计物体的位置是极困难的。为了解决此问题，在现实中使用了根据加速度传感器的输出值和从外部获得的特定信息来估计物体位置的方法。例如，将红外线发光装置固定在规定位置处，利用固定在物体上的CXD照相机对该红外线发光装置进行摄像，由此能够获得物体与红外线发光装置之间的相对位置关系的信息，所以可使用该信息并利用卡尔曼滤波器来估计物体位置的真值。但是，在这样的方法中，因为需要传感器以外的装置，所以系统变得大型化。另一方面，例如在使用定点设备来操作虚拟3D模型这样的用途中，要求虚拟3D模型的位置不会因时间的经过而发散，并且要求虚拟3D模型跟随于定点设备的动作而动作， 不过，有时不要求计算出虚拟3D模型的精确位置。在不需要计算这样的精确位置的系统中，低成本化的要求变高，不希望系统大型化。
发明内容
本发明正是鉴于以上这样的问题而完成的，根据本发明的几个方式，可提供结构
5简单且能够抑制位置发散而确切地计算出虚拟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-Ι处的上述固定座标系速度矢量，更新时刻k处的上述固定座标系速度矢量，上述姿势信息修正部包含速度矢量修正单元，该速度矢量修正单元按照使上述固定座标系速度矢量的范数成为0的约束条件，修正由上述速度矢量更新单元进行更新后的时刻k处的上述固定座标系速度矢量，上述姿势信息生成部包含位置座标生成单元，该位置座标生成单元与由上述速度矢量修正单元进行修正后的时刻k处的上述固定座标系速度矢量对应地生成上述虚拟3维空间中的上述位置座标。在该姿势信息计算装置中，在进行了固定座标系速度矢量的更新处理之后，按照使范数成为0( S卩，使固定座标系速度矢量成为(0、0、0))的约束条件进行修正处理，与修正后的固定座标系速度矢量对应地生成虚拟3维空间中的位置座标。由此，以返回到与固定座标系速度矢量(0、0、0)对应的规定座标(例如原点)的方式生成位置座标，所以能够抑制由偏移漂移引起的位置座标的发散。另外，根据该姿势信息计算装置，因为将固定座标系的速度矢量与虚拟3维空间的位置座标对应起来(并非将固定座标系的位置座标与虚拟3维空间的位置座标对应起来)，所以能够比较容易地调整返回到规定座标的速度，使得位置座标确切地跟随于传感器的输出。(3)在该姿势信息计算装置中，可以构成为，上述速度矢量更新单元执行以下处理根据时刻k-Ι处的固定座标系姿势角矢量使上述惯性座标系加速度矢量进行旋转而变换为固定座标系加速度矢量，从该固定座标系加速度矢量中减去固定座标系重力加速度矢量，以规定的单位时间对相减结果进行1阶积分而计算单位时间固定座标系速度矢量，在上述速度矢量修正单元进行修正后的时刻k-Ι处的上述固定座标系速度矢量中加上上述单位时间固定座标系速度矢量，由此来更新时刻k处的上述固定座标系速度矢量。(4)在该姿势信息计算装置中，可以构成为，上述位置座标生成单元将由上述速度矢量修正单元进行修正后的时刻k处的上述固定座标系速度矢量作为时刻k处的上述位置座标。由此，不需要进行基于修正后的固定座标系速度矢量生成位置座标的计算，所以能够降低计算量。(5)在该姿势信息计算装置中，可以构成为，上述姿势信息计算部根据由绕上述3 个轴的角速度信息得到的惯性座标系角速度矢量计算固定座标系姿势角矢量，根据该固定座标系姿势角矢量计算上述虚拟3维空间中的上述姿势角，上述姿势信息更新部包含姿势角矢量更新单元，该姿势角矢量更新单元根据上述惯性座标系角速度矢量和时刻k-Ι处的上述固定座标系姿势角矢量，更新时刻k处的上述固定座标系姿势角矢量，上述姿势信息修正部包含姿势角矢量修正单元，该姿势角矢量修正单元按照基于上述惯性座标系加速度矢量的约束条件，修正由上述姿势角矢量更新单元进行更新后的时刻k处的上述固定座标系姿势角矢量，上述姿势信息生成部包含姿势角生成单元，该姿势角生成单元根据由上述姿势角矢量修正单元进行修正后的上述固定座标系姿势角矢量，生成上述虚拟3维空间中的上述姿势角。在该姿势信息计算装置中，在进行固定座标系姿势角矢量的更新处理之后，按照基于惯性座标系加速度矢量的约束条件进行修正处理，与修正后的固定座标系姿势角矢量对应地生成虚拟3维空间中的姿势角。由此，生成基于当前的惯性座标系加速度矢量的姿势中的姿势角，所以能够抑制由传感器偏移引起的姿势角的发散。(6)在该姿势信息计算装置中，可以构成为，上述姿势角矢量更新单元执行以下处理以规定的单位时间对上述惯性座标系角速度矢量进行1阶积分而计算单位时间旋转角四元数，使上述单位时间旋转角四元数作用于上述姿势角矢量修正单元进行修正后的时刻 k-Ι处的上述固定座标系姿势角矢量，由此来更新时刻k处的上述固定座标系姿势角矢量。(7)在该姿势信息计算装置中，可以构成为，上述姿势角生成单元将由上述姿势角矢量修正单元进行修正后的时刻k处的上述固定座标系姿势角矢量作为时刻k处的上述姿势角。由此，不需要进行基于修正后的固定座标系姿势角矢量生成姿势角的计算，所以能够降低计算量。 (8)在该姿势信息计算装置中，可以构成为，上述传感器信息取得部接受由温度传感器检测的温度信息，上述姿势信息更新部包含角速度偏移量更新单元，其根据与上述温度信息对应的角速度传感器的偏移量和时刻k-Ι处的上述3个角速度传感器的偏移量，更新时刻k处的上述3个角速度传感器的偏移量；以及加速度偏移量更新单元，其根据与上述温度信息对应的加速度传感器的偏移量和时刻k-Ι处的上述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姿势信息计算系统；10AU0B传感器信息输出装置；20A、20B姿势信息计算装置；100，102传感器模块；110XU 10Y、1 IOZ陀螺仪传感器；112XU12YU12Z加速度传感器；114 温度传感器；120X、120Y、120Z、122X、122Y、122Z、1M 放大器(AMP) ；130X、130Y、 130Z、132X、132Y、132Z、i;34低通滤波器(LPF) ；140A/D转换器(ADC) ；210传感器信息取得部；212ADC接口电路(ADCI/F) ；214微型计算机接口电路(微型计算机I/F) ；220姿势信息计算部；230姿势信息更新部；232速度矢量更新单元；234姿势角矢量更新单元；236角速度偏移量更新单元；238加速度偏移量更新单元；240姿势信息修正部；242速度矢量修正单元；244姿势角矢量修正单元；246角速度偏移量修正单元；248加速度偏移量修正单元； 250姿势信息生成部；252位置座标生成单元；2M姿势角生成单元；300微型计算机；310处理部；320存储部；330通信部；400信息存储介质；500控制器；510，520操作杆；600游戏机主体；610，620通信电缆；700显示器装置；710显示部；720战斗机；1000游戏系统。
具体实施例方式以下，使用附图对本发明的优选实施方式进行详细说明。需要说明的是，以下说明的实施方式并不对权利要求书所记载的本发明的内容作不当的限定。另外，以下说明的全部结构并不一定是本发明的必要结构要件。1.第1实施方式1-1.姿势信息计算系统的结构图1是第1实施方式的姿势信息计算系统的概略结构图。如图1所示，第1实施方式的姿势信息计算系统IA包含传感器信息输出装置IOA和姿势信息计算装置20A。传感器信息输出装置IOA包含分别检测相互垂直的3个轴(X轴、Y轴、Z轴)方向的角速度信息的3个陀螺仪传感器(角速度传感器)110X、110Y、110Z，并输出该X轴、Y 轴、Z轴方向的角速度信息。此外，传感器装置IOA包含分别检测相互垂直的3个轴(X轴、 Y轴、Z轴)方向的加速度信息的3个加速度传感器112Χ、112Υ、112Ζ，并输出该X轴、Y轴、 Z轴方向的加速度信息。姿势信息计算装置20Α包含传感器信息取得部210和姿势信息计算部220，并进行如下处理根据传感器信息输出装置IOA的检测信息(角速度信息和加速度信息)计算虚拟3维空间中的姿势角和位置座标。传感器信息取得部210进行如下处理(传感器信息取得处理)取得由陀螺仪传感器110Χ、110Υ、110Ζ分别检测的X轴、Y轴、Z轴方向的角速度信息和由加速度传感器 112Χ、112YU12Z分别检测的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是示出传感器信息输出装置IOA的结构例的图。在传感器模块100中，固定着图1中说明的3个陀螺仪传感器110X、110Y、110Z和 3个加速度传感器112乂、112￥、1122。这里，陀螺仪传感器110Χ、110Υ、IlOZ被配置为它们的检测轴相互垂直。同样，加速度传感器112Χ、112Υ、112Ζ被配置为它们的检测轴相互垂直。 此外，陀螺仪传感器IlOX与加速度传感器112Χ、陀螺仪传感器IlOY与加速度传感器112Υ、 陀螺仪传感器IlOZ与加速度传感器112Ζ被配置为彼此的检测轴朝着相同方向(依次定义为X轴方向、Y轴方向、Z轴方向)。陀螺仪传感器110Χ、110Υ、110Ζ分别输出与施加给传感器模块100的绕X轴、绕Y 轴、绕Z轴的角速度对应的大小的直流电压。另外，加速度传感器112Χ、112Υ、112Ζ分别输出与施加给传感器模块100的X轴方向、Y轴方向、Z轴方向的加速度对应的大小的直流电压。陀螺仪传感器110XU 10Y、1 IOZ的各输出电压在分别被放大器(AMP) 120Χ、120Υ、 120Ζ放大之后，分别经过低通滤波器(LPF) 130Χ、130Υ、130Ζ而去除噪声成分。同样，加速度传感器112Χ、112Υ、112Ζ的各输出电压在分别被放大器(AMP) 122Χ、 122Υ、122Ζ放大之后，分别经过低通滤波器(LPF) 132Χ、132Υ、132Ζ而去除噪声成分。A/D转换器(ADC) 140以时间分割方式对低通滤波器(LPF) 130Χ、130Υ、130Ζ，132Χ、 132YU32Z的各输出电压Vex、VGY, Vgz, Vax, Vay, Vaz进行采样，将它们分别转换为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 (加速度信息)从传感器信息输出装置IOA 发送至姿势信息计算装置20A。1-3.姿势信息计算装置的结构图3是示出姿势信息计算装置20A的结构例的图。姿势信息计算装置20A包含ADC接口电路(ADC I/F) 212、微型计算机接口电路 (微型计算机I/F) 214、微型计算机300。ADC接口电路(ADC I/F) 212进行如下处理在恰当的时机，依次进行采样而取得传感器信息输出装置IOA以时间分割方式输出的角速度信息(GX、GY、GZ)和加速度信息
10(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存储程序及数据等，其功能可由光盘(⑶、DVD等)、光磁盘 (M0)、磁盘、硬盘、磁带或者存储器(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-Ι处的修正后的固定座标系速度矢量，更新时刻k处的固定座标系速度矢量。例如，速度矢量更新单元232可以采用以下方式根据时刻k-Ι处的固定座标系姿势角矢量使惯性座标系加速度矢量(A)旋转，变换为固定座标系加速度矢量，从该固定座标系加速度矢量中减去固定座标系重力加速度矢量，以规定的单位时间At对相减结果进行1阶积分而计算单位时间固定座标系速度矢量(单位时间At中的速度变化量)，在时刻k-Ι处的修正后的固定座标系速度矢量中加上单位时间固定座标系速度矢量，由此更新时刻k处的固定座标系速度矢量。姿势角矢量更新单元234进行如下处理根据惯性座标系角速度矢量(G)和时刻 k-Ι处的修正后的固定座标系姿势角矢量来更新时刻k处的固定座标系姿势角矢量。例如， 姿势角矢量更新单元234可采用如下方式以规定的单位时间At对惯性座标系角速度矢量(G)进行1阶积分，计算单位时间旋转角四元数(单位时间At中的旋转角)，使单位时间旋转角四元数作用于时刻k-Ι处的修正后的固定座标系姿势角矢量，由此来更新时刻k 处的固定座标系姿势角矢量。
角速度偏移量更新单元236进行更新时刻k处的陀螺仪传感器110XU 10Y、1 IOZ 的偏移量的处理。加速度偏移量更新单元238进行更新时刻k处的加速度传感器112X、112Y、112Z 的偏移量的处理。此外，速度矢量更新单元232、姿势角矢量更新单元234、角速度偏移量更新单元 236、加速度偏移量更新单元238作为图1所示的姿势信息更新部230发挥功能。另外，在本实施方式中，处理部310包含速度矢量修正单元对2、姿势角矢量修正单元对4、角速度偏移量修正单元Μ6、加速度偏移量修正单元Μ8。速度矢量修正单元242进行如下处理按照使固定座标系速度矢量的范数(norm) 成为0的约束条件，修正由速度矢量更新单元232更新后的时刻k处的固定座标系速度矢量。姿势角矢量修正单元244进行如下处理按照基于惯性座标系加速度矢量(A)的约束条件，修正由姿势角矢量更新单元234更新后的时刻k处的固定座标系姿势角矢量。角速度偏移量修正单元246进行如下处理按照给定的约束条件，修正由角速度偏移量更新单元236更新后的时刻k处的陀螺仪传感器110X、110Y、110Z的偏移量。加速度偏移量修正单元248进行如下处理按照给定的约束条件，修正由加速度偏移量更新单元238更新后的时刻k处的加速度传感器112X、112Y、112Z的偏移量。此外，速度矢量修正单元Μ2、姿势角矢量修正单元Μ4、角速度偏移量修正单元对6、加速度偏移量修正单元248作为图1所示的姿势信息修正部240发挥功能。此外，在本实施方式中，处理部310包含位置座标生成单元252、姿势角生成单元 254。位置座标生成单元252进行如下处理与由速度矢量修正单元242进行修正后的时刻k处的固定座标系速度矢量对应地，生成虚拟3维空间中的位置座标。例如，位置座标生成单元252可构成为将时刻k处的修正后的固定座标系速度矢量作为时刻k处的虚拟3 维空间中的位置座标。 姿势角生成单元254进行根据修正后的固定座标系姿势角矢量而生成虚拟3维空间中的姿势角的处理。例如，姿势角生成单元2M可构成为将时刻k处的修正后的固定座标系姿势角矢量作为时刻k处的虚拟3维空间中的姿势角。此外，位置座标生成单元252和姿势角生成单元2M作为图1所示的姿势信息生成部250发挥功能。另外，处理部310通过作为姿势信息修正部240发挥功能的上述各个单元、作为姿势信息生成部250发挥功能的上述各个单元、作为姿势信息生成部250发挥功能的上述各个单元，从而作为图1所示的姿势信息计算部220发挥功能。此外，用于使计算机作为本实施方式的各个单元发挥功能的程序(姿势信息计算程序)可从主机装置(服务器)所具有的信息存储介质经由网络以及通信部330分发到信息存储介质400(存储部320)。这样的主机装置(服务器)的信息存储介质的使用也包含于本发明的范围内。1-4.姿势信息计算处理的步骤1-4-1.处理步骤的概略
图4㈧是示出姿势信息计算装置20A的处理步骤的流程图。如图4㈧所示，姿势信息计算装置20A在产生结束事件之前(步骤S30中为“是”的情况)，反复进行上述传感器信息取得部210的传感器信息取得处理和姿势信息计算部220的姿势信息计算处理。图4 (B)是示出姿势信息计算部220的姿势信息计算处理(图4 (A)的步骤S20的处理)的步骤的流程图。如图4(B)所示，姿势信息计算部220首先利用姿势信息更新部 230进行姿势信息更新处理(步骤S2》。具体地说，速度矢量更新单元232、姿势角矢量更新单元234、角速度偏移量更新单元236、加速度偏移量更新单元238进行上述的各个处理。接着，姿势信息计算部220利用姿势信息修正部240进行姿势信息修正处理(步骤S24)。具体地说，速度矢量修正单元对2、姿势角矢量修正单元M4、角速度偏移量修正单元对6、加速度偏移量修正单元248进行上述的各个处理。最后，姿势信息计算部220利用姿势信息生成部250进行姿势信息生成处理(步骤S26)。具体地说，位置座标生成单元252和姿势角生成单元2M进行上述的各个处理。尤其在本实施方式中，姿势信息计算部220使用卡尔曼滤波器进行给定参数(变量)的更新处理和修正处理，估计虚拟3维空间中的姿势角的真值和位置座标的真值。将规定时间设为单位时间At而在时刻0、1、…处分别离散地进行该卡尔曼滤波器的更新处理和修正处理。关于该单位时间At，根据惯性座标系角速度矢量G和惯性座标系加速度矢量A的更新周期、以及姿势信息计算系统IA中使用的应用程序的规格等来确定恰当的时间。接着，对本实施方式中的卡尔曼滤波器的结构进行详细说明。1-4-2.卡尔曼滤波器[使用变量]在本实施方式中，在卡尔曼滤波器的更新处理和修正处理中使用以下所示的变量。At 离散积分用微小步骤时间(sec)Ww 角速度传感器中的、在时间上不具有相关关系且平均值为0的噪声(mV)wa 加速度传感器中的、在时间上不具有相关关系且平均值为0的噪声(mV)Vgi 3个角速度传感器所输出的角速度矢量(mV)(惯性座标系)Vai 3个加速度传感器所输出的加速度矢量(mV)(惯性座标系)角速度传感器灵敏度(mV/dps)加速度传感器灵敏度(mV/(m/SeC2))b ω Q:角速度传感器的初始偏移电压(mV)b α Q 加速度传感器的初始偏移电压(mV)b "k 时刻k处的更新后的角速度传感器偏移电压(mV)bcok 时刻k处的修正后的角速度传感器偏移电压(mV)ba "k 时刻k处的更新后的加速度传感器偏移电压(mV)b a k 时刻k处的修正后的加速度传感器偏移电压(mV)Δ Qtg :3轴微小步长时间旋转角四元数(度)Qt~k:时刻k处的更新后的姿势角矢量(固定座标系)Qtk:时刻k处的修正后的姿势角矢量(固定座标系)
13
AVea 3轴微小步长时间速度变化量(m/sec)Ve~k:时刻k处的更新后的速度矢量(固定座标系)Vek 时刻k处的修正后的速度矢量(固定座标系)在本实施方式中，设定以下4种参数作为构成卡尔曼滤波器的更新/修正的对象的参数，所述4种参数是固定座标系速度矢量Vek(Ve~k)、固定座标系姿势角矢量Qtk(Qt"k)、陀螺仪传感器(110X、110Y、110Z)的偏移电压b ω k (b ω ~k)、加速度传感器 (112X、112Y、112Z)的偏移电压 bak(ba ~k)。固定座标系速度矢量Vek(Ve~k)表示实际3维空间中的传感器模块100的速度的方向和大小。固定座标系姿势角矢量Qtk(Qt~k)是利用矢量来表示实际3维空间中的传感器模块100的姿势角，例如可使用法线矢量。陀螺仪传感器的偏移电压b k(b ~k)是在未施加角速度时(例如静止时)由该陀螺仪传感器输出的电压值。加速度传感器的偏移电压 bak(ba"k)是在未施加加速度时(例如静止时)由该加速度传感器输出的电压值。[处理步骤]图5是示出使用了卡尔曼滤波器的姿势信息计算处理的步骤的流程图。如图5所示，姿势信息计算部220首先将表示当前时刻的变量k初始化为0 (步骤Sl 10)，之后进行卡尔曼滤波器的初始化处理(步骤S120)。在该初始化处理(步骤S120)中，进行将作为卡尔曼滤波器的更新/修正对象的参数设定为初始值的处理。这里，可设定预定的固定值作为 Ve0, Qt0,b 0>ba0O例如，可设定范数为0的矢量(0、0、0)作为Vetl,设定铅直方向单位矢量(0、0、-1)作为Qtci,设定在预先测定的规定温度(25°C等)下的偏移电压值作为b Q及 ba0O或者，可以在每次启动时进行测定，将其测定值设定为VepQtpbcOpbc^。另外，还可将上次姿势信息计算处理中的最终值(上次启动结束时的值)设定为bcopba。。接着，姿势信息计算部220使时刻k加1 (步骤S130)，之后进行卡尔曼滤波器的更新处理(步骤S140)。在该更新处理(步骤S140)中，进行如下处理根据从传感器信息取得部210提供的信息(惯性座标系角速度矢量(G)和惯性座标系加速度矢量(A))和时刻 k-Ι处的参数值，更新时刻k处的参数值。接着，姿势信息计算部220进行卡尔曼滤波器的修正处理(步骤S150)。在该修正处理(步骤S150)中进行如下处理根据时刻k处的更新后的参数值和给定的约束条件来修正时刻k处的参数值。接着，姿势信息计算部220进行姿势信息生成处理(步骤S160)。在该姿势信息生成处理(步骤S160)中，进行如下处理根据时刻k处的修正后的参数值，生成时刻k处的姿势信息(虚拟3维空间中的姿势角和位置座标)。然后，如果在经过单位时间At之前产生了姿势信息计算处理的结束事件(步骤 S170中为“是”的情况)，则姿势信息计算部220结束姿势信息计算处理。另一方面，如果即使经过了单位时间Δ t也未产生姿势信息计算处理的结束事件(步骤S170中为“否”的情况)，则姿势信息计算部220使时刻k增加1 (步骤S130)，之后再次进行卡尔曼滤波器更新处理(步骤S140)、卡尔曼滤波器修正处理(步骤S150)、姿势信息生成处理(步骤S160)。这样，在产生结束事件之前，按照单位时间At反复进行卡尔曼滤波器更新处理 (步骤S140)和卡尔曼滤波器修正处理(步骤S150)，生成新的姿势信息。[卡尔曼滤波器更新处理]
14
在本实施方式的卡尔曼滤波器的更新处理中，进行如下处理在时刻k，将在时刻 k-Ι通过后述的修正处理进行修正后的4种参数b kfb α kfQtH、Velri (或它们的初始值 bQ0>ba0>Qt0,Ve0)分别更新为 bco"k、ba "k、Qt~k、Ve~k。在扩展卡尔曼滤波器中所使用的更新式如下式(1)所示。式1Xk^ f(X ^1,u, W^1) ...(1)X"k = (b ω "k> b a "k> Qt"k, Ve"k) % Xk^1 = (b ω b a k_” Q、—” Velrl) \ u = (Vgi, Vai) ,Wk^1 = (na)。式(1)表示这样的情况当对时刻k-Ι处的修正后的参数Xlri、传感器输出值u和传感器噪声Wlri应用了函数f时，可获得时刻k处的更新后的参数X~k。因为该函数f是非常复杂的函数，所以当精确计算式(1)时需要非常大的计算处理量，因而，希望实时计算姿势信息是不现实的。因此，在本实施方式中，定义了函数f的雅可比行列式Flri 而使用了下面的更新式O)。式2L=Fl^xXw …(2)根据更新式⑵，通过计算Flri和Xlri的矩阵积能够获得X~k，所以适合于实时地计算姿势信息。这里，如上所述，因为x~k== (bOkfbciH、 Qtk_i、Vek^1) \所以定义用于求出b ω ~k、b a ~k、Qt~k、Ve"k的更新式，以满足这些更新式的关系的方式实施各种矩阵变形，由此能够求出Fk_lt)在本实施方式中，分别如下式(3)、(4)那样定义用于求出b ~k、ba~k的更新式。式3bak bm^ ---(3)式4bak :=bak_, ---(4)另外，在本实施方式中，如下式( 那样定义用于求出Qt~k的更新式。式5Qtk I=AQtlxQt^xAQtg …⑶式(5)中的Δ Qtg可利用下式(6)求出。式6AQtg = ω · At... (6)式(6)中的ω可利用下式(7)求出。式7ω = φ ((arx, arv, arz)) = 1+i · arx/2+j · ary/2+k · arz/2." (7)函数φ是将角速度矢量变换为四元数的函数，i、j、k是3个四元数的基元。另外， (arx,ary,arz)是惯性座标系中的角速度矢量(惯性座标系角速度矢量)，arx、ary、arz是分别绕X轴、绕Y轴、绕Z轴的角速度。(arx、ary、arz)可利用下式(8)求出。式8
(arx, ary, arz) = (Vgi-b ω ^1)/Seg- (8)另外，在本实施方式中，如下式(9)那样定义用于求出Ve~k的更新式。式9Vek := Vek_x +AVea …(9)可利用下式(10)来求出AVe3O式10AVea = ((P (Qtlri))*X (grx, gry, grz) X (P (QtkJ)-G) XAt- (10)函数ρ是将Qtlrl变换为从初始姿势角矢量(^、向射㈣的旋转四元数的函数。 (P (QV1))*是(P (QV1))的共轭四元数。G是重力加速度矢量。另外，(grx、gry、grz)是惯性座标系中的加速度矢量(惯性座标系加速度矢量)，grx、gry、grz分别是X轴方向、Y轴方向、Z轴方向的加速度。(grx、gry、grz)可利用下式(11)来求出。式11(grx, gry, grz) = (Vai_b α ^1)/Sea- (11)以满足这样定义的4个更新式(3)、(4)、(5)、(9)的方式实施各种矩阵变形，由此能够求出Fk_lt)此外，在本实施方式的卡尔曼滤波器的更新处理中，还进行以下处理在时刻k， 使用Flri，将在时刻k-Ι利用后述的修正处理进行修正后的Xlri与真值的残差的协方差矩阵 Pk-!( =Etek^1, ekV])更新为P~k。如下式(11)那样地定义用于求出协方差矩阵P~k的更新式。式12
_2] Pk -.= Fk^Pk-AFkJ+Qk-, -(12)Qh ( :E [Wlrl，Wk^1tJ)表示 Wk^1 的协方差矩阵。接着，对卡尔曼滤波器的更新处理步骤的一例进行说明。图6是示出本实施方式中的卡尔曼滤波器的更新处理的步骤的一例的流程图。首先，使用式(8)，根据3个陀螺仪传感器110XU 10Y、1 IOZ的输出电压Vgi计算惯性座标系角速度矢量(arx、ary, arz)(步骤S141)。接着，使用式(6)和式(7)，根据在步骤S141中计算出的惯性座标系角速度矢量 (arx,ary,arz)计算3轴微小步长时间旋转角四元数Δ Qtg (步骤S142)。接着，使用式(11)，根据3个加速度传感器112Χ、112Υ、112Ζ的输出电压Vai计算惯性座标系加速度矢量(grx、gry、grz)(步骤S143)。接着，使用式(10)，根据在步骤S143中计算出的惯性座标系加速度矢量计算3轴微小步长时间速度变化量Δ Vea (步骤S144)。接着，使用在步骤S142中计算出的3轴微小步长时间旋转角四元数Δ Qtg和在步骤S144中计算出的3轴微小步长时间速度变化量Δ Vea，以满足式(3)、(4)、(5)、(9)的方式实施各种矩阵变形而生成Flri (步骤S145)。接着，使用更新式(2)计算？㈣与乂㈣的矩阵积，将Xlri更新为X~k(步骤S146)。最后，使用更新式(12)将Ph更新为P~k(步骤S147)。[卡尔曼滤波器修正处理]
16
在本实施方式的卡尔曼滤波器的更新处理中，进行如下处理在时刻k，将通过更 新处理得到的时刻k处的更新后的4种參数b CO ~k、b a ^,.Qt^,.Ve^,分别修正为b ,.b a ,. Qtk、Ve,.在卡尔曼滤波器的修正处理中，定义如下式(1 所示的观测方程式。式13y = Z-HX (13)在式(1 中，Z是观测值矢量，X是待求出的值，y是观测參数，H是观测方程式斜 率分量矩阵。在卡尔曼滤波器修正处理中，进行如下处理求出使得观测方程式(1 中的观测 残差7成为最小、即、使得由下式(14)定义的评价函数《^3成为最小的乂。式14J^^s=(Z-HX)tW(Z-HX)...(14)
W是表示权重的对象矩阵。在本实施方式中，所求出的值乂对应于ん=03 ぉ、ヒ0ぉ、0セぉ、乂6グ，观测值矢量2 对应于时刻k处的观测值矢量 ，H对应于时刻k-l处的观测方程式斜率分量矩阵11,_1。其 中，!1,_1既可以在各个时刻锋次重新进行计算，也可以是固定值。本实施方式中使用的修正式如下式(15)那样进行定义。式I5
权利要求
1.一种姿势信息计算装置，其根据由3个角速度传感器分别检测的绕相互不平行的3 个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置座标，该姿势信息计算装置包含传感器信息取得部，其取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及姿势信息计算部，其根据由上述传感器信息取得部取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置座标，其中，上述姿势信息计算部根据由上述3个轴方向的加速度信息得到的惯性座标系加速度矢量计算固定座标系速度矢量，与该固定座标系速度矢量对应地计算上述虚拟3维空间中的上述位置座标。
2.根据权利要求1所述的姿势信息计算装置，其中，上述姿势信息计算部包含姿势信息更新部，其根据绕上述3个轴的角速度信息和3个轴方向的加速度信息，更新为了确定上述虚拟3维空间中的上述姿势角和上述位置座标所需的给定参数；姿势信息修正部，其按照给定的约束条件，修正由上述姿势信息更新部进行更新后的上述参数；以及姿势信息生成部，其根据由上述姿势信息修正部进行修正后的上述参数，计算上述虚拟3维空间中的上述姿势角和上述位置座标，上述姿势信息更新部包含速度矢量更新单元，该速度矢量更新单元根据上述惯性座标系加速度矢量和时刻k-Ι处的上述固定座标系速度矢量，更新时刻k处的上述固定座标系速度矢量，上述姿势信息修正部包含速度矢量修正单元，该速度矢量修正单元按照使上述固定座标系速度矢量的范数成为0的约束条件，修正由上述速度矢量更新单元进行更新后的时刻 k处的上述固定座标系速度矢量，上述姿势信息生成部包含位置座标生成单元，该位置座标生成单元与由上述速度矢量修正单元进行修正后的时刻k处的上述固定座标系速度矢量对应地生成上述虚拟3维空间中的上述位置座标。
3.根据权利要求2所述的姿势信息计算装置，其中，上述速度矢量更新单元执行以下处理根据时刻k-Ι处的固定座标系姿势角矢量使上述惯性座标系加速度矢量进行旋转而变换为固定座标系加速度矢量，从该固定座标系加速度矢量中减去固定座标系重力加速度矢量，以规定的单位时间对相减结果进行1阶积分而计算单位时间固定座标系速度矢量， 在上述速度矢量修正单元进行修正后的时刻k-Ι处的上述固定座标系速度矢量中加上上述单位时间固定座标系速度矢量，由此来更新时刻k处的上述固定座标系速度矢量。
4.根据权利要求2所述的姿势信息计算装置，其中，上述位置座标生成单元将由上述速度矢量修正单元进行修正后的时刻k处的上述固定座标系速度矢量作为时刻k处的上述位置座标。
5.根据权利要求2所述的姿势信息计算装置，其中，上述姿势信息计算部根据由绕上述3个轴的角速度信息得到的惯性座标系角速度矢量计算固定座标系姿势角矢量，根据该固定座标系姿势角矢量计算上述虚拟3维空间中的上述姿势角，上述姿势信息更新部包含姿势角矢量更新单元，该姿势角矢量更新单元根据上述惯性座标系角速度矢量和时刻k-Ι处的上述固定座标系姿势角矢量，更新时刻k处的上述固定座标系姿势角矢量，上述姿势信息修正部包含姿势角矢量修正单元，该姿势角矢量修正单元按照基于上述惯性座标系加速度矢量的约束条件，修正由上述姿势角矢量更新单元进行更新后的时刻k 处的上述固定座标系姿势角矢量，上述姿势信息生成部包含姿势角生成单元，该姿势角生成单元根据由上述姿势角矢量修正单元进行修正后的上述固定座标系姿势角矢量，生成上述虚拟3维空间中的上述姿势
6.根据权利要求5所述的姿势信息计算装置，其中， 上述姿势角矢量更新单元执行以下处理以规定的单位时间对上述惯性座标系角速度矢量进行1阶积分而计算单位时间旋转角四元数，使上述单位时间旋转角四元数作用于由上述姿势角矢量修正单元进行修正后的时刻k-Ι处的上述固定座标系姿势角矢量，由此来更新时刻k处的上述固定座标系姿势角矢量。
7.根据权利要求5所述的姿势信息计算装置，其中，上述姿势角生成单元将由上述姿势角矢量修正单元进行修正后的时刻k处的上述固定座标系姿势角矢量作为时刻k处的上述姿势角。
8.根据权利要求2 7中任意一项所述的姿势信息计算装置，其中， 上述传感器信息取得部接受由温度传感器检测的温度信息，上述姿势信息更新部包含角速度偏移量更新单元，其根据与上述温度信息对应的角速度传感器的偏移量和时刻 k-Ι处的上述3个角速度传感器的偏移量，更新时刻k处的上述3个角速度传感器的偏移量；以及加速度偏移量更新单元，其根据与上述温度信息对应的加速度传感器的偏移量和时刻 k-Ι处的上述3个加速度传感器的偏移量，更新时刻k处的上述3个加速度传感器的偏移量，上述姿势信息修正部包含角速度偏移量修正单元，其按照给定的约束条件，修正由上述角速度偏移量更新单元进行更新后的时刻k处的上述3个角速度传感器的偏移量；以及加速度偏移量修正单元，其按照给定的约束条件，修正由上述加速度偏移量更新单元进行更新后的时刻k处的上述3个加速度传感器的偏移量。
9.一种姿势信息计算系统，该姿势信息计算系统包含3个角速度传感器，其分别检测绕相互不平行的3个轴的角速度信息； 3个加速度传感器，其分别检测相互不平行的3个轴方向的加速度信息；以及权利要求1至7中任意一项所述的姿势信息计算装置。
10.一种姿势信息计算系统，该姿势信息计算系统包含3个角速度传感器，其分别检测绕相互不平行的3个轴的角速度信息； 3个加速度传感器，其分别检测相互不平行的3个轴方向的加速度信息；以及权利要求8所述的姿势信息计算装置。
11. 一种姿势信息计算方法，根据由3个角速度传感器分别检测的绕相互不平行的3 个轴的角速度信息和由3个加速度传感器分别检测的相互不平行的3个轴方向的加速度信息，计算虚拟3维空间中的姿势角和位置座标，该姿势信息计算方法包含以下步骤传感器信息取得步骤，取得绕上述3个轴的角速度信息和上述3个轴方向的加速度信息；以及姿势信息计算步骤，根据在上述传感器信息取得步骤中取得的绕上述3个轴的角速度信息和上述3个轴方向的加速度信息，计算上述虚拟3维空间中的上述姿势角和上述位置座标，在上述姿势信息计算步骤中，根据由上述3个轴方向的加速度信息得到的惯性座标系加速度矢量计算固定座标系速度矢量，与该固定座标系速度矢量对应地计算上述虚拟3维空间中的上述位置座标。
全文摘要
本发明提供姿势信息计算装置、姿势信息计算系统及姿势信息计算法，能够实现简单的结构，并且能够抑制位置的发散而确切地计算虚拟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维空间中的位置座标。
文档编号G01C21/20GK102192740SQ20111005328
公开日2011年9月21日 申请日期2011年3月4日 优先权日2010年3月5日
发明者中冈康, 宇田川裕文, 小林祥宏 申请人:精工爱普生株式会社