CN102997913A - 用于确定物体姿态的方法及装置 - Google Patents

Abstract

本发明公开了一种用于确定物体姿态的方法及装置。该用于确定物体姿态的方法包括接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化；基于所述重力加速度信号确定物体坐标系中的重力场表示，基于所述地磁场信号确定物体坐标系中的地磁场表示；基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数；基于所述坐标系转换参数确定所述物体的姿态。

Description

用于确定物体姿态的方法及装置

技术领域

本发明涉及计算机技术领域，更具体地，本发明涉及一种用于确定物体姿态的方法及装置。

背景技术

姿态定向参考系统(Attitude Heading Reference System，AHRS)是一种用于提供物体的姿态以及运动方向等信息的系统。物体姿态的信息可以被用于例如自动控制、导航、虚拟现实等应用中。通常地，姿态定向参考系统包括多个多轴传感器，例如陀螺仪、加速度传感器以及磁强计等。姿态定向参考系统通过这些传感器来采集与物体姿态相关的数据信息(例如，角速度、加速度或磁场强度等)，并采用特定的算法来计算并表示物体的姿态和/或运动方向。

在传统的姿态定向参考系统中，物体相对于物体坐标系(即物体自身所在的局部坐标系)的姿态通常由欧拉角(即，对应于三个旋转轴的章动角、进动角以及自旋角)表示。然而，采用欧拉角表示物体的姿态或物体的旋转时，会出现万向节锁定(Gimbal Lock)问题，即，当物体的旋转使得三个旋转轴中的两个旋转轴相互平行时，绕一个旋转轴的旋转可能会覆盖绕另一个旋转轴的旋转，从而失去一个旋转自由度。

一种避免出现万向节锁定的方法采用四元数(Quaternion)参数来描述物体的姿态。四元数由实数与三个元素i、j、k组成，其中i、j、k满足i²+k²+j²＝ijk＝-1的关系，每个四元数都可以表示为1、i、j、k的线性组合。由于四元数的表达式没有类似于欧拉角的奇点，因此，以四元数来描述的物体姿态不会出现万向节锁定的问题。在传统的姿态定向参考系统中，可以利用陀螺仪测量物体运动的角速度，并进一步地对该角速度进行积分，从而得到以四元数参数表示的物体的姿态。

然而，基于陀螺仪的姿态定向参考系统是一种相对姿势定向参考系统，在每次确定物体姿态时，其需要基于前一次或前一时刻物体姿态的参数。考虑到陀螺仪的各种测量误差，例如实际灵敏度误差、灵敏度的非线性、灵敏度的温度漂移等，这种相对姿态定向参考系统所提供的关于物体姿态的信息会累积测量误差，这使得很难在较长的时间内精确地表示物体姿态。

因此，需要一种能够稳定并且精确地确定物体姿态的方法及装置。

发明内容

为了解决上述问题，根据本发明的一个方面，提供了一种用于确定物体姿态的方法，包括：接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化；基于所述重力加速度信号确定物体坐标系中的重力场表示，并基于所述地磁场信号确定物体坐标系中的地磁场表示；基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数；基于所述坐标系转换参数确定所述物体的姿态。

根据本发明的另一方面，还提供了一种用于确定物体姿态的装置，包括：接收模块，用于接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化；第一确定模块，用于基于所述重力加速度信号确定物体坐标系中的重力场表示，并基于所述地磁场信号确定物体坐标系中的地磁场表示；转换模块，用于基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数；第二确定模块，用于基于所述坐标系转换参数确定所述物体的姿态。

根据本发明的又一方面，还提供了一种姿态定向参考系统，包括前述方面的用于确定物体姿态的装置。

不同于现有技术，在本发明的用于确定物体姿态的方法和装置中，每次确定物体姿态的操作相互独立，这就避免了由于传感器测量误差的累积所引起的偏差，使得能够在长时间内稳定且精确地确定物体姿态；此外，本发明采用四元数的坐标系转换参数来确定物体姿态，由于四元数表达式没有类似于欧拉角的奇点，因此，本发明还避免了万向节锁定的问题。

附图说明

通过对结合附图所示出的实施方式进行详细说明，本发明的上述以及其他特征将更加明显，本发明附图中相同的标号表示相同或相似的元素。在附图中：

图1示出了适于应用本发明的方法及装置的姿态定向参考系统；

图2示出了根据本发明一个方面的用于确定物体姿态的方法的流程；

图3示出了重力场与地磁场在物体坐标系与地球坐标系中的表示；

图4示出了根据本发明一个方面的用于确定物体姿态的装置。

在附图中，相同或对应的标号表示相同或对应的部分。

具体实施方式

下文将参考若干示例性实施方式来描述本发明的原理和精神。应当理解，给出这些实施方式仅仅是为了使本领域技术人员能够更好地理解进而实现本发明，而并非以任何方式限制本发明的范围。

参考图1，示出了适于应用本发明的方法及装置的姿态定向参考系统100。姿态定向参考系统100通常至少部分地装载在可移动的物体10上。在图1中，物体10被表示为移动通信终端，但是可以理解，根据具体应用的不同，物体10亦可为例如工业设备、虚拟现实设备、便携式电子设备或其他机电设备，不应限制其范围。

图1中示出了物体10的物体坐标系(即物体自身的局部坐标系)，其具有相互正交的三个坐标轴：B_x轴、B_y轴以及B_z轴。其中，物体坐标系以物体10的中心为坐标原点，物体10的旋转或平移等操作都是围绕该物体坐标系进行的；当物体10进行旋转或平移等操作时，物体坐标系也执行相应的旋转或平移操作。

可以看出，由于物体坐标系随着物体10姿势的变化而改变，这通常与处于静止状态或者不跟随物体10姿势变化的观测设备的用于观测的基准坐标系不同。因此，可以选取相对静止的地球坐标系作为描述物体10姿态的基准坐标系，并利用该基准坐标系中的四元数参数来表示物体姿势。具体地，在基准坐标系中，物体10的姿势可以表示为绕某一归一化的旋转轴N(x₀，y₀，z₀)旋转θ的角度，这对应于四元数参数：Q＝(cos(θ/2)，x₀sin(θ/2)，y₀sin(θ/2)，z₀sin(θ/2))。

通常地，观测设备的基准坐标系通常选取地球坐标系。地球坐标系包括三个相互正交的坐标轴，这三个坐标轴的位置关系符合右手螺旋法则：例如东北天坐标系，其包括E轴(朝向地平面的正东方向)、N轴(朝向地平面的正北方向)以及U轴(朝向天顶)；再例如北东地坐标系，其包括N轴、E轴以及D轴(朝向地心)。本领域技术人员可以理解，这些地球坐标系易于相互转换，在接下来的实施例中，均以地球坐标系为东北天坐标系为例进行说明，但本发明并不限于此。

姿态定向参考系统100包括装载在物体10上的加速度传感器101以及磁强计103。其中，该加速度传感器101至少具有三个相互正交的测量轴，以沿这三个测量轴测量重力加速度并输出重力加速度信号，而该磁强计103至少具有三个相互正交的测量轴，以沿这三个测量轴测量地磁场并输出地磁场信号。可以理解，当物体10的姿态变化而使得加速度传感器101以及磁强计103的测量轴方向变化时，重力加速度信号以及地磁场信号相应变化，即不同测量轴所测得的重力加速度或地磁场的分量的测量值变化。

优选地，加速度传感器101与磁强计103的测量轴与物体10物体坐标系的坐标轴分别重合，以使得所测得的重力加速度与地磁场分别对应到物体坐标系的B_x轴、B_y轴以及B_z轴。可以理解，加速度传感器101与磁强计103的测量轴亦可与物体坐标系的坐标轴不完全重合，而是偏离一定的角度；相应地，在此情况下，在利用传感器的测量结果确定物体姿态时，还需要计入该角度的偏差。为了便于说明，在下文中，均以加速度传感器101与磁强计103的测量轴与物体坐标系的坐标轴重合为例进行说明，但本发明并不限于此。

在实际应用中，姿态定向参考系统100还包括数据处理设备105，其用于接收由加速度传感器101与磁强计103输出的重力加速度信号与地磁场信号，并对这些测量信号进行数据处理。在图1所示的实施例中，该数据处理设备105与物体10分离，例如其可以为诸如工作站、个人计算机或服务器等远程数据处理设备；该远程数据处理设备可以通过有线或无线连接与加速度传感器101与磁强计103通信地耦接，以接收测量信号并进行数据处理。可以理解，在其他的实施例中，该数据处理设备105也可以装载在物体10上，其例如为与加速度传感器101以及磁强计103相耦接的微处理器(MCU)。

需要说明的是，由于应用本发明的方法及装置的姿态定向参考系统100基于重力场与地磁场来确定物体姿态，因此，在姿态定向参考系统100进行确定物体姿态的操作时，加速度传感器101应在物体10处于基本静止或匀速运动状态下(相对于地球而言)测量重力加速度，即物体10基本没有受到除重力之外的其他力作用形成的加速度而影响其重力加速度的测量。同时，磁强计103应在物体10附近基本不存在除地磁场之外的其他磁场的情况下测量物体10所在位置的环境磁场，以避免其他磁场干扰地磁场的测量。

参考图2，示出了根据本发明一个方面的用于确定物体姿态的方法的流程。应当理解，该方法中记载的各个步骤可以按照不同的顺序执行，和/或并行执行。此外，该方法可以包括附加的步骤和/或省略执行示出的步骤。本发明的范围在此方面不受限制。

如图2所示，在步骤S202中，接收重力加速度信号与地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化。

正如前述，物体的重力加速度信号表示物体所受到的重力加速度的幅度以及方向，其由装载在物体上的加速度传感器测得。例如，对于三轴加速度传感器，其具有三个相互正交的测量轴，在测量物体的所受到的重力加速度时，重力加速度会根据其方向与测量轴的夹角分别地映射到各个测量轴上。当三轴加速度传感器的三个测量轴与物体坐标系的三个坐标轴重合时，所测得的重力加速度信号对应于重力场相对于物体坐标系各个坐标轴的偏移。

类似地，物体的地磁场信号表示物体所在位置的地磁场的幅度以及方向，其由装载在物体上的磁强计测得。例如，对于三轴磁强计，其具有三个相互正交的测量轴，在测量地磁场时，地磁场会根据其方向与测量轴的夹角分别地映射到各个测量轴上。当三轴加速度传感器的三个测量轴与物体坐标系的三个坐标轴重合时，所测得的地磁场信号对应于地磁场相对于物体坐标系各个坐标轴的偏移。

接着，在步骤S204中，基于所述重力加速度信号确定物体坐标系中的重力场表示，并基于所述地磁场信号确定物体坐标系中的地磁场表示。

正如前述，所测得的重力加速度信号对应于重力场相对于物体坐标系各个坐标轴的偏移，并且所测得的地磁场信号对应于地磁场相对于物体坐标系各个坐标轴的偏移，因此，可以基于这些测量信号确定物体坐标系中的重力场表示与地磁场表示。

由于地球坐标系实质上是基于重力场方向以及地磁场相关的方向来定义的，因此，易于确定地球坐标系中的重力场表示以及地磁场表示。进一步地，基于物体坐标系中的重力场表示，即可确定物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系；而基于物体坐标系中的地磁场表示，即可确定物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系。

参考图3，同时示出了重力场与地磁场在物体坐标系与地球坐标系中的表示。

具体地，物体坐标系包括三个相互正交的坐标轴B_x轴、B_y轴以及B_z轴，而重力加速度信号包括映射在这三个坐标轴上的测量值G_x、G_y以及G_z，则物体坐标系中重力场表示即为由原点(0，0，0)指向(G_x，G_y，G_z)的矢量G；而地磁场信号包括在这三个坐标轴上的测量值M_x、M_y以及M_z，则物体坐标系中的地磁场表示即为由原点(0，0，0)指向(M_x，M_y，M_z)的矢量M。进一步地，对于地球坐标系，其通常是针对地平面而言的，例如东北天坐标系中的N轴，其是指地平面(物体所在位置的地球的切平面)上的正北方向。而对于地平面而言，地磁场并非指向正北，而是基于地理纬度的不同而与该切平面存在沿U轴方向的夹角。因此，对于地磁场而言，其在地平面的投影矢量N_h(M_hx，M_hy，M_hz)更易于表示。可以理解，该地磁场的投影矢量(M_hx，M_hy，M_hz)可以由(G_x，G_y，G_z)及(M_x，M_y，M_z)确定，这会在下文中详述。

另一方面，对于地球坐标系，例如东北天坐标系或北东地坐标系，或者其他具有相互正交的三个坐标轴的地球坐标系，重力场以及地磁场亦具有对应的表示。例如，对于东北天坐标系，指向地心的重力场的方向与U轴方向重合，因此，重力场的方向在东北天坐标系中可以表示为(0，0，1)。类似地，地磁场在地平面的投影矢量(M_hx，M_hy，M_hz)的方向与东北天坐标系中的N轴方向重合，那么，地磁场在地平面的投影矢量(M_hx，M_hy，M_hz)的方向在东北天坐标系中可以被表示为(0，1，0)。

此外，对于东北天坐标系的E轴，其在东北天坐标系中被表示为(1，0，0)，而其在物体坐标系中的表示(E_x，E_y，E_z)由表示N轴方向的矢量N_h与表示U轴方向的矢量G的外积确定，即(M_hyG_z-G_yM_hz，M_hzG_x-G_zM_hx，M_hxG_y-G_xM_hy)，或基于(G_x，G_y，G_z)及(M_x，M_y，M_z)的外积确定。

正如前述，在物体坐标系中，地磁场在地平面的投影矢量(M_hx，M_hy，M_hz)可以由(G_x，G_y，G_z)及(M_x，M_y，M_z)确定。具体地，在物体坐标系中，地平面由下述地平面方程表示：

xG_x+yG_y+zG_z＝0              (1)

点(M_hx，M_hy，M_hz)位于该地平面上，即

M_hxG_x+M_hyG_y+M_hzG_z＝0        (2)

而由点(M_hx，M_hy，M_hz)到点(M_x，M_y，M_z)的直线由下述方程表示：

$\frac{x-M_x}{M_{\mathrm{hx}}-M_x}=\frac{y-M_y}{M_{\mathrm{hy}}-M_y}=\frac{z-M_z}{M_{\mathrm{hz}}-M_z}---\left(3\right)$

并且该直线垂直于地平面，即：

$\frac{G_x}{M_{\mathrm{hx}}-M_x}=\frac{G_y}{M_{\mathrm{hy}}-M_y}=\frac{G_z}{M_{\mathrm{hz}}-M_z}=K---\left(4\right)$

由方程(1)、(2)、(3)、(4)，可以得到用(G_x，G_y，G_z)及(M_x，M_y，M_z)表示的(M_hx，M_hy，M_hz)，具体为：

K＝-(G_xM_x+G_yM_y+G_zM_z)/(G_xG_x+G_yG_y+G_zG_z)            (5)

M_hx＝KG_x+M_x                                      (6)

M_hy＝KG_y+M_y                                      (7)

M_hz＝KG_z+M_z                                      (8)

需要说明的是，上述对(M_hx，M_hy，M_hz)的计算是示例性而非限制性的，本领域技术人员可以通过其他适合的方式来确定(M_hx，M_hy，M_hz)的表达式。

仍参考图2，接着，在步骤S206中，基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数。

由于四元数的表达式没有类似于欧拉角的奇点，以四元数来描述的物体姿态不会出现万向节锁定。因此，优选地，采用四元数参数Q(a，b，c，d)(Q优选为单位四元数参数)表示并计算坐标系转换参数。

具体地，对于物体坐标系中任意的矢量V(x，y，z)，其对应于地球坐标系中的矢量V’(x’，y’，z’)，相应地，以四元数形式表示的这两个矢量分别为：

V＝(0，x，y，z)                                  (9)

V’＝(0，x’，y’，z’)                    (10)

这两个矢量满足下述坐标系转换方程：

V＝QV’Q^-1                                 (11)

其中，Q＝(a，b，c，d)                      (12)

Q^-1＝(a，-b，-c，-d)                       (13)

可以对方程(11)进行求解，以得到坐标系转换参数Q。具体地，基于四元数乘法规则，将等式(9)、(10)、(12)、(13)代入等式(3)可得，

a²+b²+c²+d²＝1                             (14)

(a²+b²-c²-d²)x’+2(bc-ad)y’+2(bd+ac)z’＝x(15)

2(bc+ad)x’+(a²-b²+c²-d²)y’+2(cd-ab)z’＝y(16)

2(bd-ac)x’+2(cd+ab)y’+(a²-b²-c²+d²)z’＝z(17)

正如前述，物体坐标系中的矢量表示与地球坐标系中的矢量表示存在对应关系。对于重力场G，在物体坐标系中，其被表示为(G_x，G_y，G_z)，而在地球坐标系中，其被表示为(0，0，1)；对于地磁场在地平面投影矢量M_h，在物体坐标系中，其被表示为(M_hx，M_hy，M_hz)，而在地球坐标系中，其被表示为(0，1，0)；对于重力场与地磁场的外积E，在物体坐标系中，其被表示为(E_x，E_y，E_z)，而在地球坐标系中，其被表示为(1，0，0)。

相应地，利用在步骤S204中获得的对应关系来求解坐标系转换方程。可以将上述3组对应关系代入等式(15)、(16)、(17)，可以得到：

2(bd+ac)＝G_x                               (18)

2(cd-ab)＝G_y                               (19)

(a²-b²-c²+d²)＝G_z                          (20)

2(bc-ad)＝M_hx                              (21)

(a²-b²+c²-d²)＝M_hy                         (22)

2(cd+ab)＝M_hz                              (23)

(a²+b²-c²-d²)＝E_x                          (24)

2(bc+ad)＝E_y                               (25)

2(bd-ac)＝E_z                               (26)

对上述方程求解可得a、b、c、d的表达式，从而进一步得到坐标系转换参数Q(a，b，c，d)。需要说明的是，对于求解坐标系转换参数Q，上述等式(18)-(26)存在冗余等式，该冗余等式用于验证求得的解，并删除不符合初始条件(即3组对应关系)的虚根。因此，在实际应用中，亦可仅选取上述3组对应关系中的两组，例如物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系以及物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系来求解坐标系转换参数Q(a，b，c，d)。

在一个实施例中，坐标系转换参数Q(a，b，c，d)的解为：

a＝(f₂+f₃-f₄)/2；

b＝(-f₂+f₃+f₄)/2

c＝(2f₁-f₂-f₃+f₄)/2

d＝(f₂-f₃+f₄)/2

其中，

$f_1=\±\sqrt{\frac{|1+M_{\mathrm{hy}}+G_x-E_z|}{2}}$ $f_2=\±\sqrt{\frac{|1+G_z+E_y-M_{\mathrm{hx}}|}{2}}$

$f_3=\±\sqrt{\frac{|1+E_x+M_{\mathrm{hz}}-G_y|}{2}}$ $f_4=\±\sqrt{\frac{|1-M_{\mathrm{hy}}+G_x+E_z|}{2}}$

$f_5=\±\sqrt{\frac{|1-G_z+E_y+M_{\mathrm{hx}}|}{2}}$ $f_6=\±\sqrt{\frac{|1-E_x+M_{\mathrm{hz}}+G_y|}{2}}$

其中，a应大于0，并且a、b、c、d应满足使得Q为单位四元数矢量。

接下来，在步骤S208中，基于所述坐标系转换参数确定物体的姿态。

具体地，由于坐标系转换参数Q表示了物体坐标系与地球坐标系之间的坐标系转换关系，而对于地球某一位置的物体而言，地球坐标系中各个坐标轴，即例如东北天坐标系中的N轴、E轴以及U轴的方向是确定的。另一方面，对于物体本身而言，物体坐标系是固定不变的。因此，该坐标系转换参数Q实质上即对应于物体相对于地球坐标系的姿态，不同的坐标系转换参数Q即表示出物体在地球坐标系中的不同姿态。

在一个可选的实施例中，在步骤S208中，还可以基于所述坐标系转换参数显示所述物体。例如，物体被显示在观测设备的显示界面中，该显示界面具有例如为地球坐标系的基准坐标系。然后，利用该坐标系转换参数Q对物体的姿态进行调整。

例如，物体对应的三维图形模型被通过图形应用程序接口OpenGL建模并显示。在确定物体姿态时，可以利用OpenGL函数库中的函数glRotatef(θ，x₀，y₀，z₀)来对物体的三维图像模型进行旋转，以显示物体的姿态。其中，该函数的四个参数可以由坐标系转换参数Q的参数a、b、c、d确定，即：

θ＝2arccos(a)

$x_0=\frac{b}{\sin (\mathrm{\θ}/2)}$

$y_0=\frac{c}{\sin (\mathrm{\θ}/2)}$

$z_0=\frac{d}{\sin (\mathrm{\θ}/2)}$

以这样的方式，物体的姿态即可确定并显示在图形界面中。需要说明的是，上述关于显示物体姿态的说明是示例性而非限制性的，在实际应用中，也可以采用例如DirectX或其他图形应用程序接口来显示物体的姿态。

参考图4，示出了根据本发明一个方面的用于确定物体姿态的装置，包括：

接收模块401，用于接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化。

第一确定模块403，用于基于所述重力加速度信号确定物体坐标系中的重力场表示，并基于所述地磁场信号确定物体坐标系中的地磁场表示。

转换模块405，用于基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数。

第二确定模块407，用于基于所述坐标系转换参数确定所述物体的姿态。

接下来，对本发明的用于确定物体姿态的装置进行详述。

首先，接收模块401接收来自加速度传感器411的重力加速度信号，并接收来自磁强计413的地磁场信号。其中，加速度传感器411与磁强计413分别装载在物体上。当物体姿态变化时，加速度传感器411以及磁强计413的位置相应变化，而由于地磁场与重力场保持不变，因此，重力场以及地磁场相对于传感器的各个测量轴的测量值改变。

在一个实施例中，加速度传感器411至少具有三个相互正交的测量轴，以使得其所测得的重力加速度信号至少包括沿这三个测量轴测量的重力加速度测量值。磁强计413也至少具有三个相互正交的测量轴，以使得其所测得的地磁场信号至少包括沿这三个测量轴测量的地磁场测量值。优选地，加速度传感器411、磁强计413的测量轴与物体坐标系中的坐标轴分别重合。

接着，接收模块401将重力加速度信号以及地磁场信号提供给第一确定模块403。

具体地，由于所测得的重力加速度信号对应于重力场相对于物体坐标系各个坐标轴的偏移，并且所测得地磁场信号对应于地磁场相对于物体坐标系各个坐标轴的偏移，因此，基于这些测量信号的各个测量轴上的测量值分量，即可确定重力场以及地磁场在物体坐标系中的表示。例如，重力加速度信号包括映射在这三个坐标轴上的测量值G_x、G_y以及G_z，则物体坐标系中重力场表示即为由原点(0，0，0)指向(G_x，G_y，G_z)的矢量G；而地磁场信号包括在这三个坐标轴上的测量值M_x、M_y以及M_z，则物体坐标系中的地磁场表示即为由原点(0，0，0)指向(M_x，M_y，M_z)的矢量M。

另一方面，由于地球坐标系实质上是由重力场以及地磁场在地平面的投影分量所定义的，因此，重力场以及地磁场易于由地球坐标系表示。相应地，基于物体坐标系中的重力场表示，即可确定物体坐标系中重力场表示与地球坐标系中的重力场表示的对应关系；并且，基于物体坐标系中的地磁场表示，即可确定物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系。在一个实施例中，地球坐标系是东北天坐标系，则重力场表示为所述东北天坐标系中的U轴，而所述地磁场在地平面的投影矢量表示为所述东北天坐标系中的N轴。

之后，第一确定模块403将所确定的物体坐标系中的重力场表示与地磁场表示提供给转换模块405，以使得转换模块405确定物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系，以及物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系。利用这些对应关系，转换模块405可以计算物体坐标系与地球坐标系之间的坐标系转换参数。在一个实施例中，转换模块405采用四元数参数计算所述坐标系转换参数。

具体地，转换模块405可以包括方程求解器，其用于对坐标系转换方程V’＝QVQ^-1进行求解以得到坐标系转换参数，其中，Q代表四元数形式的所述坐标系转换参数，V代表任一矢量在物体坐标系中的表示，而V’代表所述任一矢量在地球坐标系中的表示。相应地，该方程求解器将前述的对应关系代入坐标系转换方程来求解坐标系转换参数。具体的求解方式可以参考前述结合图2的说明，在此不再赘述。

然后，第二确定模块407接收由转换模块405提供的该坐标系转换参数，并基于所述坐标系转换参数确定物体的姿态。例如，第二确定模块407可以选取地球坐标系作为基准坐标系，相应地，四元数形式的坐标系转换参数即可作为该基准坐标系中物体姿态的四元数参数。

具体地，由于坐标系转换参数Q表示了物体坐标系与地球坐标系之间的坐标系转换关系，而对于地球某一位置的物体而言，地球坐标系中各个坐标轴，即例如东北天坐标系中的N轴、E轴以及U轴的方向是确定的。另一方面，对于物体本身而言，物体坐标系是固定不变的。因此，该坐标系转换参数Q实质上即对应于物体相对于地球坐标系的姿态，不同的坐标系转换参数Q即表示出物体在地球坐标系中的不同姿态。

在图4所示的实施例中，该用于确定物体姿态的装置还包括显示模块410，用于基于所述坐标系转换参数显示所述物体。例如，该呈现模块410是例如显示屏的显示界面，其具有例如为地球坐标系的基准坐标系；然后，利用该坐标系转换参数Q对物体的姿态进行调整。

可以看出，在本发明的用于确定物体姿态的方法和装置中，每次确定物体姿态的操作相互独立，即不需要前一时刻或前一次获得测量信号来确定物体的姿态，这就避免了由于传感器测量误差的累积所引起的偏差，使得能够在长时间内稳定且精确地确定物体姿态；此外，由于采用四元数形式的坐标系转换参数来确定物体姿态，本发明还避免了万向节锁定的问题。

在本发明中，为示范目的，装置实施例的运作参照方法实施例描述。然而，应该理解本发明中装置的运作和方法的实现互相独立。也就是说，所公开的装置实施例可以依照其他方法运作，所公开的方法实施例可以通过其他装置实现。

本领域技术人员还将容易地理解的是，材料和方法可以变化，同时仍然处于本发明的范围之内。还应理解的是，除了用来示出实施方式的具体上下文之外，本发明提供了多种可应用的创造性构思。因此，所附权利要求意在将这些过程、机器、制品、组合物、装置、方法或者步骤包括在其范围之内。

Claims (19)

1.一种用于确定物体姿态的方法，包括：

接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化；

基于所述重力加速度信号确定物体坐标系中的重力场表示，基于所述地磁场信号确定物体坐标系中的地磁场表示；

基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数；

基于所述坐标系转换参数确定所述物体的姿态。

2.根据权利要求1所述的方法，其特征在于，通过装载在物体上的加速度传感器测得所述重力加速度信号，以及通过装载在物体上的磁强计测得所述地磁场信号。

3.根据权利要求1所述的方法，其特征在于，所述重力加速度信号至少包括沿相互正交的三个测量轴测量的重力加速度测量值，所述地磁场信号至少包括沿相互正交的三个测量轴测量的地磁场测量值。

4.根据权利要求1所述的方法，其特征在于，所述确定重力场表示与地磁场表示的步骤进一步包括：

基于所述物体坐标系中的重力场表示确定所述物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系；

基于所述物体坐标系中的地磁场表示确定所述物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系。

5.根据权利要求4所述的方法，其特征在于，所述地球坐标系是东北天坐标系，所述重力场表示为所述东北天坐标系中的U轴，而所述地磁场在地平面的投影矢量表示为所述东北天坐标系中的N轴。

6.根据权利要求1所述的方法，其特征在于，所述计算坐标系转换参数的步骤进一步包括：

采用四元数参数计算所述坐标系转换参数。

7.根据权利要求6所述的方法，其特征在于，所述计算坐标系转换参数的步骤进一步包括：

对坐标系转换方程V’＝QVQ^-1进行求解以得到所述坐标系转换参数，其中，Q代表四元数形式的所述坐标系转换参数，V代表任一矢量在物体坐标系中的表示，而V’代表所述任一矢量在地球坐标系中的表示。

8.根据权利要求7所述的方法，其特征在于，所述求解坐标系转换方程进一步包括：

基于所述物体坐标系中的重力场表示确定所述物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系；

基于所述物体坐标系中的地磁场表示确定所述物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系；以及

将所述对应关系代入所述坐标系转换方程以求解所述坐标系转换参数。

9.根据权利要求1所述的方法，其特征在于，所述基于所述坐标系转换参数确定物体的姿态的步骤还包括：

基于所述坐标系转换参数显示所述物体。

10.一种用于确定物体姿态的装置，包括：

接收模块，用于接收重力加速度信号以及地磁场信号，所述重力加速度信号与所述地磁场信号响应于物体的姿态的变化而变化；

第一确定模块，用于基于所述重力加速度信号确定物体坐标系中的重力场表示，基于所述地磁场信号确定物体坐标系中的地磁场表示；

转换模块，用于基于所述物体坐标系中的所述重力场表示与所述地磁场表示计算所述物体坐标系与地球坐标系之间的坐标系转换参数；

第二确定模块，用于基于所述坐标系转换参数确定所述物体的姿态。

11.根据权利要求10所述的装置，其特征在于，还包括：

加速度传感器，其装载在所述物体上，用于测量所述重力加速度并生成所述重力加速度信号；

磁强计，其装载在所述物体上，用于测量地磁场并生成所述地磁场信号。

12.根据权利要求10所述的装置，其特征在于，所述重力加速度信号至少包括沿相互正交的三个测量轴测量的重力加速度测量值，所述地磁场信号至少包括沿相互正交的三个测量轴测量的地磁场测量值。

13.根据权利要求10所述的装置，其特征在于，所述第一确定模块进一步用于：

基于所述物体坐标系中的重力场表示确定所述物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系；

基于所述物体坐标系中的地磁场表示确定所述物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系。

14.根据权利要求13所述的装置，其特征在于，所述地球坐标系是东北天坐标系，所述重力场表示为所述东北天坐标系中的U轴，而所述地磁场在地平面的投影矢量表示为所述东北天坐标系中的N轴。

15.根据权利要求10所述的装置，其特征在于，所述转换模块进一步用于：

采用四元数参数计算所述坐标系转换参数。

16.根据权利要求15所述的装置，其特征在于，所述转换模块进一步包括方程求解器：

所述方程求解器用于对坐标系转换方程V’＝QVQ^-1进行求解以得到所述坐标系转换参数，其中，Q代表四元数形式的所述坐标系转换参数，V代表任一矢量在物体坐标系中的表示，而V’代表所述任一矢量在地球坐标系中的表示。

17.根据权利要求16所述的装置，其特征在于，所述第一确定模块进一步用于：

基于所述物体坐标系中的重力场表示确定所述物体坐标系中的重力场表示与地球坐标系中的重力场表示的对应关系；

基于所述物体坐标系中的地磁场表示确定所述物体坐标系中的地磁场表示与地球坐标系中的地磁场表示的对应关系；

以及所述方程求解器进一步用于接收所述对应关系并将所述对应关系代入所述坐标系转换方程以求解所述坐标系转换参数。

18.根据权利要求10所述的装置，其特征在于，还包括：

显示模块，用于基于所述坐标系转换参数显示所述物体。

19.一种姿态定向参考系统，包括权利要求10-18中任一项所述的装置。

Images