CN102297693B - 测量物体位置和方位的方法 - Google Patents

Abstract

本发明公开了一种测量物体位置和方位的方法，物体的方位首先由三轴陀螺仪的测量结果计算出第一方位。三轴加速度传感器仅在物体处于静止状态时计算第二方位。三轴地磁传感器在物体处于静止状态时、或者在物体持续运动一定时间内计算第三方位。第二方位、第三方位是对第一方位的修正。传统的测量物体方位的方法仅使用三轴陀螺仪，这会产生累计误差而导致长时间测量的结果不准确。本发明在使用三轴陀螺仪的基础上，又采用三轴加速度传感器和三轴地磁传感器对三轴陀螺仪的方位测量结果进行修正，从而可以获得较为准确的物体方位。

Description

测量物体位置和方位的方法

技术领域

本发明涉及一种测量物体方位的方法，所述物体在人体控制下运动，例如游戏手柄、遥控器、鼠标等。

背景技术

物体的位置，是指物体在特定的坐标系中相对于给定参考点(通常是坐标系的原点)的线性位移。在数学上，描述位置就等价于描述线性位移。在概念上，位置是指单一状态，而线性位移是指两个位置间的差别。

物体的方位(orientation)，也称为物体的姿态(attitude)，描述的是物体的朝向。物体的方位是通过与已知方位(通常称为源方位)的旋转来描述的，旋转的量称作角位移。在数学上，描述方位就等价于描述角位移。在概念上，方位是指单一状态，而角位移是指两个状态间的差别。

描述物体的方位或角位移具有多种表示方式，通常用欧拉角(Eulerangles)的形式来表示方位，用四元数(quaternion)或旋转矩阵(rotationmatrix)的形式来表示角位移。这几种表示方式之间都是可以互相转换的。更详细的信息可参考《3D数学基础：图形与游戏开发》(Fletcher Dunn、Ian Parberry著，清华大学出版社，2005年8月出版)

欧拉角具有多种定义方式，本发明中示例性地采用泰特-布莱恩角(Tait-Bryan angles)，又称为导航角(navigation angles)、卡丹角(Cardan angles)，这是一种最常见的欧拉角形式。泰特-布莱恩角包括偏航角(yaw)、俯仰角(pitch)和滚动角(roll)。其中，偏航角以ψ表示，取值范围是-180～180度；俯仰角以f表示，取值范围是-90～90度；滚动角以θ表示，取值范围是-180～180度。

随着微机电系统(Micro Electromechanical System，MEMS)技术的不断发展，基于这种技术的传感器在消费电子产品中也得到了广泛地应用。例如，陀螺仪应用于数码相机中做防抖动处理；加速度传感器集成于笔记本电脑内部实现防跌落硬盘保护；磁传感器应用于手机中实现指南针的功能等等。

陀螺仪(gyroscope)是一种常用的方位测量装置，陀螺仪输出角速度，是瞬时量。通常将角速度与时间做积分运算得到角度变化量(角位移)，将该角位移与初始方位相加，得到目标方位。其中积分时间越小，计算的角位移越准确。但陀螺仪的测量基准是自身，并没有系统外的绝对参照物，加上积分时间不可能无限小，因此积分运算的累积误差会随着时间流逝迅速增加，最终导致输出角度与实际角度不符。陀螺仪具有自校准功能，当陀螺仪在使用过程中处于静止状态并维持一段时间后，其会对中心点进行自校准。

加速度传感器(accelerometer)是一种常用的位置测量装置，加速度传感器输出加速度，也是瞬时量。通常将加速度与时间做两次积分运算得到位置变化量(线性位移)，将该线性位移与初始位置相加，得到目标位置。加速度传感器测量得到的加速度信号是重力加速度分量、向心加速度分量和线性加速度分量的总和。

地磁传感器(geomagnetic sensor)是一种常用的方向测量装置。地磁传感器在使用过程中只有在确保自身方位正确的前提下，才能测量出准确的地磁信号。地磁传感器也具有自校准功能，当地磁传感器在使用过程中采集到足够多的特征点(运行角度较多)，其将会进行自校准。特征点数量是否足够多，由地磁传感器内置的自校准算法进行判断。

发明内容

本发明所要解决的技术问题是提供一种测量物体位置和方位的方法，该方法可以减小使用陀螺仪测量物体方位时的累计误差问题。

为解决上述技术问题，本发明测量物体位置和方位的方法，所述物体上包括三轴陀螺仪、三轴加速度传感器和三轴地磁传感器，所述方法为：

其中，测量物体位置的方法是：三轴加速度传感器测量物体的加速度并输出给处理器，处理器将该加速度中剔除掉重力加速度分量和向心加速度分量，得到线性加速度分量；处理器将该线性加速度分量与时间进行两次积分运算得到物体在该时间内的线性位移；处理器将物体的初始位置与该线性位移相加，得到物体的当前位置；

其中，测量物体方位的方法是：三轴陀螺仪测量物体的角速度并输出给处理器，处理器将该角速度与时间进行积分运算得到物体在该时间内的角位移，处理器将物体的初始方位与该角位移相加称为第一方位；处理器至少给出四元数形式和欧拉角形式表示的第一方位；

当物体处于静止状态时，三轴加速度传感器测量得到的物体的加速度就是该物体的重力加速度，处理器根据此时的物体的加速度值计算物体的欧拉角形式的俯仰角和滚动角；当计算出的俯仰角的绝对值小于或等于80度时，将计算出的俯仰角、滚动角以及上述第一方位的偏航角作为第二方位；当计算出的俯仰角的绝对值大于80度时，将计算出的俯仰角、上述第一方位的滚动角和偏航角作为第二方位；处理器给出四元数形式表示的第二方位；

当物体处于静止状态时、或者当物体处于运动状态且持续运动的时间在预设值之内时，三轴地磁传感器测量地磁信号并输出给处理器，处理器根据该地磁信号计算物体的欧拉角形式的偏航角，并将俯仰角和滚动角分别设为欧拉角形式的上述第一方位的俯仰角和滚动角，作为第三方位；处理器给出四元数形式表示的第三方位；

当物体处于静止状态时，处理器计算第二方位与第一方位的差值作为补偿量一，还计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量一、补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间在预设值之内，处理器计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间超出预设值，处理器将第一方位作为物体的方位。

传统的测量物体方位的方法仅使用三轴陀螺仪，这会产生累计误差而导致长时间测量的结果不准确。本发明在使用三轴陀螺仪的基础上，又采用三轴加速度传感器和三轴地磁传感器对三轴陀螺仪的方位测量结果进行修正，从而可以获得较为准确的物体方位。

附图说明

图1是物体向心运动的示意图；

图2是物体线性运动的示意图；

图3是本发明所述测量物体位置和方位的方法的示意图；

图4是静止坐标系和运动坐标系的对比示意图；

图5是本发明所述测量物体位置和方位的方法的一个实施例的流程图。

具体实施方式

物体在三维空间中的任何运动，都可以分解为向心运动(即转动)和线性运动(即移动)的组合。其中向心运动如图1所示，向心运动改变物体的朝向，不改变物体的位置。线性运动如图2所示，线性运动改变物体的位置，不改变物体的朝向。

本发明测量物体位置和方位的方法，首先要求运动物体上具有三轴陀螺仪、三轴加速度传感器和三轴地磁传感器。

对于物体位置的测量，本发明所述方法为：无论物体处于静止还是运动状态，物体上的三轴加速度传感器都会测量物体的加速度并输出给处理器。处理器将该加速度中剔除掉重力加速度分量和向心加速度分量，得到线性加速度分量。处理器将该线性加速度分量与时间进行两次积分运算得到物体在该时间内的线性位移；处理器将物体的初始位置与该线性位移进行矢量相加，得到物体的当前位置。

对于物体方位的测量，本发明所述方法为：三轴陀螺仪测量物体的角速度并输出给处理器，处理器将该角速度与时间进行积分运算得到物体在该时间内的角位移，处理器将物体的初始方位与该角位移相加称为第一方位；处理器至少给出四元数形式和欧拉角形式表示的第一方位；

当物体处于静止状态时，三轴加速度传感器测量得到的物体的加速度就是该物体的重力加速度，处理器根据此时的物体的加速度值计算物体的欧拉角形式的俯仰角和滚动角；当计算出的俯仰角的绝对值小于或等于80度(即-80度≤俯仰角≤80度)时，将计算出的俯仰角、滚动角以及上述第一方位的偏航角三者构成第二方位；当计算出的俯仰角的绝对值大于80度(即-180度≤俯仰角＜-80度或80度＜俯仰角≤180度)时，将计算出的俯仰角、上述第一方位的滚动角和偏航角三者构成第二方位；处理器给出四元数形式表示的第二方位；

当物体处于静止状态时、或者当物体处于运动状态且持续运动的时间在预设值之内时，三轴地磁传感器测量地磁信号并输出给处理器，处理器根据该地磁信号计算物体的欧拉角形式的偏航角，并将俯仰角和滚动角分别设为欧拉角形式的上述第一方位的俯仰角和滚动角，作为第三方位；处理器给出四元数形式表示的第三方位；

当物体处于静止状态时，处理器计算第二方位与第一方位的差值作为补偿量一，还计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量一、补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间在预设值之内，处理器计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间超出预设值，处理器将第一方位作为物体的方位。

上述方位的加减运算均以四元数的形式进行，具体而言：上述第二方位与第一方位的差值作为补偿量一以四元数相减的形式进行，第三方位与第一方位的差值作为补偿量二以四元数相减的形式进行，第一方位与补偿量一、补偿量二相加以四元数相加的形式进行，第一方位与补偿量二相加以四元数相加的形式进行。以四元数的形式进行方位的加减运算可以避免以欧拉角形式进行方位的加减运算可能出现的“死角”问题。

请参阅图3，在任意情况下，物体的方位首先由陀螺仪的测量结果初步确定，即第一方位。加速度传感器仅在物体处于静止状态时测量计算得到第二方位。三轴地磁传感器在物体处于静止状态时、或者在物体持续运动一定时间内测量计算得到第三方位。当第二方位、第三方位存在时，则将第一方位通过补偿的方式向第二方位、第三方位靠拢。其中，第一方位中的俯仰角、滚动角向第二方位的俯仰角、滚动角靠拢，第一方位中的偏航角向第三方位的偏航角靠拢。

如图3所示，加速度传感器、地磁传感器并非在所有情况下都可对陀螺仪的方位测量结果进行补偿。例如，加速度传感器在物体处于运动状态时，其无法根据重力加速度分量计算出物体当前的欧拉角形式的俯仰角和滚动角，因而无法得到第二方位。又如，地磁传感器只有在确定自身方位的情况下，才能测量得到准确的地磁信号。当物体处于静止状态时，地磁传感器的自身方位为第二方位。当物体处于运动状态且持续运动的时间在预设值之内，地磁传感器的自身方位为第一方位。当物体处于运动状态且持续运动的时间超出预设值，则很难确定地磁传感器自身方位，因而无法得到第三方位。所述持续运动时间的预设值，根据三轴地磁传感器的精度、数据传输稳定性、用户对偏差的可接受程度等有关，在通常的游戏系统中，建议值一般在10～20秒，优选为15秒。

本申请中所述处理器可以是微控制器(MCU)、计算机的CPU等。三轴陀螺仪、三轴加速度传感器、三轴地磁传感器与所述处理器之间可以有线或无线的通讯方式进行数据的传输。

常用的三轴陀螺仪根据输出信号为模拟信号还是数字信号分为两种。

一种三轴陀螺仪向所述处理器输出模拟信号的角速度，此时该模拟信号的角速度经过模数转换模块转换为数字信号后输出给处理器，(该模数转换模块也可能在处理器内部)，此时该模数转换模块将采样时间传输给所述处理器，所述处理器计算第一方位时的积分时间即来自于该模数转换模块所输出的采样时间。

另一种三轴陀螺仪向所述处理器直接输出数字信号的角速度，此时该三轴陀螺仪内部必然具有模数转换模块，那么三轴陀螺仪还向所述处理器输出该模数转换模块的采样时间，此时所述处理器计算第一方位时的积分时间即来自于该模数转换模块所输出的采样时间。

所述处理器在收到三轴陀螺仪、三轴加速度传感器、三轴地磁传感器输出的信号后，通常会进行相应的信号处理，比如滤波。滤波的实现方法很多，例如可以采用卡尔曼滤波算法(The Kalman Filter Algorithm)。

经过滤波处理的数据根据具体的功能进行分类：三轴陀螺仪输入的角速度数据通过积分运算获得角位移，该角位移与初始方位相加得到第一方位。由于初始方位通常为零，该角位移通常就是第一方位。在物体处于静态时三轴加速度传感器输入的加速度数据通过坐标转换获得第二方位。在物体处于运动时三轴加速度传感器输入的加速度数据的线性运动分量通过二次积分运算获得线性位移。该线性位移与物体的初始位置相加得到物体的当前位置。由于初始位置通常为零，该线性位移通常就是物体的位置。当物体持续运动的时间小于预设值时三轴地磁传感器输入的地磁信号用来计算第三方位。

其中将角速度与时间进行积分运算获得第一方位可以采用许多计算方法，例如可以采用经典四阶龙格-库塔法(classical fourth-orderRunge-Kutta method)计算出四元数形式表示的第一方位。在计算过程中如果四元数的平方和不等于1，则对四元数进行归一化处理，这些均为现有技术，在此不作赘述。

四元数形式表示的第一方位可以由处理器转换为欧拉角形式的方位、和/或旋转矩阵形式的方位，这三种表示方式之间可以自由转换，且均为现有技术。

请参阅图4，点M是物体上的一个点，初始状态下，点M在静止坐标系A中的坐标为(x，y，z)。一段时间后，物体的位置和方位都可能发生了变化。当物体的方位变化，实际上是物体的朝向在三维空间中相对于静止坐标系A发生了变化。换一种角度思考，也可以认为是物体的朝向在三维空间中保持不变，而参考坐标系由静止坐标系A变为了运动坐标系A’，如图3所示。为简化起见，图3中假设物体的位置不变，即假设运动坐标系A’和静止坐标系A的原点相同。对于点M，只要知道其在运动坐标系A’下的坐标(x’，y’，z’)，就可以描述物体的方位变化。(x，y，z)与(x’，y’，z’)之间的关系为：

(x’，y’，z’)＝(x，y，z)*T_arr

其中，T_arr是上述测量物体方位的方法的三种情况下与初始方位相加的量的旋转矩阵形式。

三轴加速度传感器测量得到的加速度信号是重力加速度分量、向心加速度分量和线性加速度分量的矢量总和。由于重力加速度的方向、大小已知，并且静止坐标系A的一个轴通常选取铅垂方向，因此加速度信号中的重力加速度分量在静止坐标系A下的坐标值是已知的。

在加速度信号中剔除重力加速度分量是这样实现的。当物体处于运动状态时，取三轴加速度传感器的三轴方向为运动坐标系A’的三轴方向，这样对于三轴加速度传感器测量出来的三轴为运动坐标系A’三轴方向的加速度信号，可以通过坐标转换转换为三轴为静止坐标系A三轴方向的加速度信号，然后就可以方便地将其中的重力加速度分量剔除出来。

在剔除掉重力加速度分量的加速度信号后，进一步剔除掉向心加速度分量。向心加速度大小基于以下公式计算：A(point)＝k×w²×r。其中A(point)为向心加速度分量的大小。k是比例因子，用来与保持加速度传感器测量结果与公式计算结果保持一致。w为角速度，由三轴陀螺仪获得。r为旋转半径，一般根据系统的经验值来定，不同的运动状态具有不同的经验值。例如在通常的游戏系统中，如果整个手臂运动，转动半径为手臂长度，大约在65～85cm之间，如果是手腕运动，半径大约在10～20cm之间，如果是手肘运动，半径大约在30～50cm之间，根据实际情况来定。实际应用中，我们会要求物体在用户手中的握法，一般以物体的前端为圆弧运动的外延，也就是说，物体基本垂直于运动圆弧，垂直点在物体的前端，这样可以较好的排除滚动角部分对系统计算的不利影响。最后，获得的向心加速度的大小为A(point)＝k×(w₁ ²+w₂ ²)×r，其中，w₁为偏航角速度，w₂为俯仰角速度，均由陀螺仪获得。向心加速度的方向与物体实际转动的弧形的切线垂直，指向圆弧外侧，所以在以上条件的限制下，向心加速度的方向沿物体竖直指向物体前端。这样便得到了向心加速度分量的大小和方向，从而可在剔除掉重力加速度分量的加速度信号中剔除。

三轴加速度传感器所测量的加速度信号，在剔除重力加速度分量和向心加速度分量后，剩余的就是线性加速度分量。将该线性加速度分量与时间进行一次积分运算可以获得线性运动速度，将该线性加速度分量与时间进行二次积分运算可以获得线性运动位移。通常将静止坐标系A的原点设为物体的初始位置，这样该线性位移就是物体的目标位置。

当物体处于静止状态时，三轴加速度传感器测量的加速度信号就等于重力加速度分量，将该重力加速度分量通过坐标转换转换为三轴为运动坐标系A’三轴方向，便可以计算出以欧拉角形式表示的当前物体方位中的俯仰角f和滚动角θ。在此原理下，本发明仅示例性地给出如下一种计算方法：

$\mathrm{\θ}=\arctan \frac{a_x}{a_y},$ a_y≠0

$\sqrt{{a_x}^2+{a_y}^2}\≠0$

其中，arctan表示反正切函数，a_x、a_y、a_z分别是三轴加速度传感器的三轴采集的加速度信号。计算出的俯仰角f和滚动角θ，如果俯仰角f的绝对值大于80度，考虑到计算的稳定性，将滚动角θ设为欧拉角形式的上述第一方位的滚动角，再加上设为与第一方位中的偏航角相同的偏航角ψ，作为第二方位。

三轴地磁传感器在自身方位确定的前提下，可以测量地磁信号，处理器可以根据该地磁信号计算出以欧拉角形式表示的当前物体方位中偏航角ψ。计算方法有许多，本发明仅示例性地给出如下一种计算方法：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=T_{\mathrm{arr}}\left[\begin{array}{c}{a_x}^{\′\′}\\ {a_y}^{\′\′}\\ {a_z}^{\′\′}\end{array}\right]$

$\left[\begin{array}{c}y\\ i\\ j\end{array}\right]=k_1\left[\begin{array}{ccc}0& -m_z& m_y\\ m_z& 0& -m_x\\ -m_y& m_x& 0\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$

$\left[\begin{array}{c}x\\ m\\ n\end{array}\right]=k_2\left[\begin{array}{ccc}0& -j& i\\ j& 0& -y\\ -i& y& 0\end{array}\right]\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]$

$\mathrm{\ψ}=\arctan \frac{x}{y},$ y≠0

其中，x、y是两个中间量，a_x″a_y″a_z″是加速度传感器采集获得的原始值，a_xa_ya_z是经过坐标转换后的静态坐标对应的加速度输出，T_arr是姿态矩阵，m_xm_ym_z是磁传感器原始数据，k₁k₂是比例因子。计算出的偏航角ψ，再加上分设为与第一方位中的俯仰角、滚动角相同的俯仰角f和滚动角θ，作为第三方位。

处理器在计算第一方位、第二方位、第三方位的过程中涉及到将欧拉角转换为四元数。为了避免死角的产生，可以先进行试运算。当试运算中出现任何一个欧拉角从取值范围的一头跳变到另一头时为试运算失败，否则为试运算成功。出现试运算失败时，将所有欧拉角的取值范围整数倍扩大，再进行试运算，反复该步直至试运算成功。出现试运算成功时，再进行实际运算。

角度扩展的基本思路是基于边界运动趋势扩展。比如，原来的角度范围是-180～180度之间，如果需要扩展到-360～360，关键点是在边界点-180、180上，如果历史数据是由小变大的正数，如果突然出现一个较大的负数(比如-172度)，按照角度变化的大小规律可以分析，此时的角度还是按照先前的方向发展，这时对应的角度与实际的输入角度之间的关系如下式所示：

α^New＝α+360

其中α是之前输入的-180～180度的角度值，α^New是经过扩展的角度值。

如果历史数据是由大变小的负数，如果突然出现一个较大的正数(比如180度)，按照角度变化的大小规律可以分析，此时的角度还是按照先前的方向发展，这时对应的角度与实际的输入角度之间的关系如下式所示：

α^New＝α-360

其中α是之前输入的-180～180度的角度值，α^New是经过扩展的角度值。

如此类推，将相关边界点扩充到-360和360。

例如，原本滚动角f的取值范围是-180～180度，在试运算过程中俯仰角f从-180度这头跳跃到180度这一头，虽然对系统而言-180度与180度的意义相同，当试运算后四元数出现正负跳变就会造成死角。为了避免这种情况，将所有三个欧拉角的取值范围都进行整数倍(例如2倍)扩大，此时滚动角f的取值范围扩大到-360～360度，再进行试运算成功。此时将欧拉角换算成四元数时，四元数不再出现正负、或负正跳变，即已消除了跳变的死角区域，再进行正式运算。

请参阅图5，这是本发明测量物体位置和方位的方法的一个实施例的流程图。首先采集三轴陀螺仪输入的角速度、三轴加速度传感器输入的加速度值、三轴地磁传感器输入的地磁信号，然后进行信号滤波处理，接着判断三轴陀螺仪是否已经过自校准。如已经过自校准，则进入下一步。如未经过自校准，则等待三轴陀螺仪静止时进行自校准。

接着，对三轴陀螺仪输入的角速度与时间进行积分运算得到第一方位(因初始方位通常为零)，并对该第一方位进行欧拉角、四元数、旋转矩阵等各种表达形式的转换，转换过程中还包括试运算避免死角的方法。接着判断三轴地磁传感器是否已经过自校准。如已经过自校准，则进入下一步。如未经过自校准，则等待陀螺仪静止时进行自校准。考虑到磁传感器校准值计算需要较多的具有代表性的原始数据参与，如果数据不够，会对校准值的准确性造成较大的影响，所以校准值计算前先分析目前的特征点是否足够，如果不够，直接返回一个无效的校准值，继续采集三轴地磁传感器的地磁信号。

接着，判断物体持续运动是否在预设时间内，如果是则计算第三方位，如果否则直接进行下一步。

接着，判断物体是否有静止状态，如果是则计算第二方位，如果否则直接进行下一步。

接着，计算第二方位与第一方位的差值为补偿量一，计算第三方位与第一方位的差值为补偿量二。如果上述流程中未计算第二方位，则本步也不计算补偿量一。如果上述流程中未计算第三方位，则本步也不计算补偿量二。

最后，将补偿量一、补偿量二分为多个部分，按照每个时间段补偿一部分的方法实现对第一方位的补偿，最终使第一方位向第二方位、第三方位靠拢。或者，补偿也可以一次性进行。补偿时以四元数的形式进行。

本发明测量物体位置的方法，利用三轴加速度传感器所测量的加速度值，在去除重力加速度分量和向心加速度分量后，剩余的线性加速度分量与时间进行两次积分运算，得到线性位移，该线性位移与初始位置的矢量相加，得到物体的当前位置。

本发明测量物体方位的方法，以三轴陀螺仪测量的角速度与时间作积分运算得到的第一方位为基础，在物体处于静止的时刻以三轴加速度传感器测量的重力加速度计算的第二方位和三轴地磁传感器测量的地磁信号计算的第三方位为修正值，在物体持续运动一段时间内仅以三轴地磁传感器测量的地磁信号计算的第三方位为修正值，在物体持续运动一段时间后不进行补偿。

综上所述，本发明测量物体位置和方位的方法，可以利用三轴加速度传感器和三轴地磁传感器的测量结果，对三轴陀螺仪的测量结果出现的累计误差进行补偿(纠正)，从而获得较为准确的物体当前位置和方位。

Claims (4)

1.一种测量物体位置和方位的方法，所述物体包括游戏手柄、遥控器、鼠标；所述物体上包括三轴陀螺仪、三轴加速度传感器和三轴地磁传感器，其特征是，

其中，测量物体位置的方法是：三轴加速度传感器测量物体的加速度并输出给处理器，处理器将该加速度中剔除掉重力加速度分量和向心加速度分量，得到线性加速度分量；处理器将该线性加速度分量与时间进行两次积分运算得到物体在该时间内的线性位移；处理器将物体的初始位置与该线性位移相加，得到物体的当前位置；

其中，测量物体方位的方法是：三轴陀螺仪测量物体的角速度并输出给处理器，处理器将该角速度与时间进行积分运算得到物体在该时间内的角位移，处理器将物体的初始方位与该角位移相加称为第一方位；处理器至少给出四元数形式和欧拉角形式表示的第一方位；

当物体处于静止状态时，三轴加速度传感器测量得到的物体的加速度就是该物体的重力加速度，处理器根据此时的物体的加速度值计算物体的欧拉角形式的俯仰角和滚动角；当计算出的俯仰角的绝对值小于或等于80度时，将计算出的俯仰角、滚动角以及上述第一方位的偏航角作为第二方位；当计算出的俯仰角的绝对值大于80度时，将计算出的俯仰角、上述第一方位的滚动角和偏航角作为第二方位；处理器给出四元数形式表示的第二方位；

当物体处于静止状态时、或者当物体处于运动状态且持续运动的时间在预设值之内时，三轴地磁传感器测量地磁信号并输出给处理器，处理器根据该地磁信号计算物体的欧拉角形式的偏航角，并将俯仰角和滚动角分别设为欧拉角形式的上述第一方位的俯仰角和滚动角，作为第三方位；处理器给出四元数形式表示的第三方位；

当物体处于静止状态时，处理器计算第二方位与第一方位的差值作为补偿量一，还计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量一、补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间在预设值之内，处理器计算第三方位与第一方位的差值作为补偿量二，处理器将第一方位与补偿量二相加作为物体的方位；

当物体处于运动状态且持续运动的时间超出预设值，处理器将第一方位作为物体的方位。

2.根据权利要求1所述的测量物体位置和方位的方法，其特征是，所述计算第二方位与第一方位的差值作为补偿量一是以四元数形式的减法进行，所述计算第三方位与第一方位的差值作为补偿量二是以四元数形式的减法进行，所述将第一方位与补偿量一和补偿量二相加作为物体的方位是以四元数形式的加法进行，所述将第一方位与补偿量二相加作为物体的方位也是以四元数形式的加法进行。

3.根据权利要求1所述的测量物体位置和方位的方法，其特征是，将第一方位、第二方位、第三方位由欧拉角形式转换为四元数形式时，先进行试运算，当试运算中出现任何一个欧拉角从取值范围的一头跳变到另一头时为试运算失败，否则为试运算成功；

出现试运算失败时，将所有欧拉角的取值范围整数倍扩大，再进行试运算，反复该步直至试运算成功；

出现试运算成功时，再进行实际运算。

4.根据权利要求1所述的测量物体位置和方位的方法，其特征是，当三轴陀螺仪向所述处理器输出模拟信号的角速度，此时该模拟信号的角速度经过模数转换模块转换为数字信号后输出给处理器，此时该模数转换模块将采样时间传输给所述处理器，所述处理器计算第一方位时的积分时间即来自于该模数转换模块所输出的采样时间；

当三轴陀螺仪向所述处理器直接输出数字信号的角速度，此时该三轴陀螺仪内部必然具有模数转换模块，那么三轴陀螺仪还向所述处理器输出该模数转换模块的采样时间，此时所述处理器计算第一方位时的积分时间即来自于该模数转换模块所输出的采样时间。

Images