CN103940442A - 一种采用加速收敛算法的定位方法及装置 - Google Patents

Abstract

本发明提出一种采用加速收敛算法的定位方法，包括以下步骤：获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值Q_A；获取角速度的三维数值，计算出姿态四元数的预测值Q_B=Q_A+dQ/dt；获取加速度和地磁力的当前三维数值，计算出姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；通过相关的计算方法将计算得出的姿态参数转换为姿态四元数的观测值Q_D；对Q_D和Q_B进行融合计算，得到姿态四元数的融合值Q_E；再次获取角速度的三维数值，计算四元数的下一预测值Q_B1=Q_E+dQ/dt；重复以上相关步骤，获取四元数预测值Q_Bn。本发明采用了简单的运算方法得到定位结果，因此解决了由于运算问题产生的明显迟滞现象，能够实现快速收敛。

Description

一种采用加速收敛算法的定位方法及装置

技术领域

本发明涉及一种定位方法，尤其是一种采用加速收敛算法的定位方法及装置。

背景技术

目前多数MEMS（微机电系统）定位算法多数采用卡尔曼滤波或者扩展卡尔曼滤波算法，由于卡尔曼滤波算法运用高斯牛顿法，计算复杂，运算时间长。而扩展卡尔曼滤波收敛速度慢，在传感器慢速运动时输出比较准确，但是当快速运动时，收敛速度慢将导致计算值输出完全跟不上运动变化，出现计算值与实际值偏差太大，在空中鼠标的实际测试中反映的现象就是光标跑飞，并且需要很长时间光标才会回到实际位置的现象，因此这两种算法没办法用于快速运动的定位。

发明内容

针对以上现有技术的缺陷，本发明利用三维加速度传感器、陀螺仪、地磁仪来感测运动数据，并通过一种定位算法来加速收敛数据，实现快速精确定位。简单地说，本发明通过感测陀螺仪数据并计算得到四元数的预测值，再感测得到加速度传感器和地磁传感器数据并计算得到四元数的观测值，将两种数据融合得到最后定位数据，由于采用了简单的运算方法得到定位结果，因此解决了由于运算问题产生的明显迟滞现象，实现快速收敛。

具体地说，本发明所提出的定位方法包括以下步骤:

1）获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值Q_A；

2）获取角速度的三维数值，计算出姿态四元数的预测值Q_B=Q_A+dQ/dt，其中Q为三维角速度的四元数；

3）获取加速度和地磁力的当前三维数值，计算出姿态参数，如罗德里格参数、方向余弦或者欧拉角等；

4）将计算得出的姿态参数转换为姿态四元数的观测值Q_D；

5）对Q_D和Q_B进行融合计算，得到姿态四元数的融合值Q_E；

6）再次获取角速度的三维数值，计算四元数的下一预测值Q_B1=Q_E+dQ/dt；

7）重复步骤3）至6），获取四元数预测值Q_Bn。

作为以上技术方案的一种补充，所有计算出的四元数均以基准姿态为参考，其中以水平面向正北的姿态为基准姿态，在所述基准姿态上三维加速度的数值为g（0，0，最大），三维磁力的数值为M（最大，0，不管），基准姿态四元数为Q₀（1，0，0，0）。

作为以上技术方案的一种补充，在步骤3）中，通过比对不同轴的加速度数值和磁力数值计算出运动的空间姿态参数。

作为以上技术方案的一种补充，当姿态参数显示，俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，执行步骤4）的处理；否则对加速度和磁力的三维数值进行往使往|θ|变小的方向旋转90°的矩阵运算，再进行姿态参数的计算，之后执行步骤4）的运算得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值Q_D。

作为以上技术方案的一种补充，|θ|的预设值为70°或者80°或其他接近90°的值。

同理，Sin|θ|或Tan|θ|中的|θ|为70°或者80°或其他接近90°的值。

作为以上技术方案的一种补充，在步骤4）中，选取因旋转而产生的多个对应的四元数值中合适的四元数值作为观测值Q_D。

作为以上技术方案的一种补充，在步骤5）中，Q_D和Q_B融合的方法为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种。

本发明还提出了一种采用以上所述加速收敛算法的定位装置，该定位装置包括感测单元、运算单元以及输出单元，其中所述感测单元包括三维陀螺仪、三维加速度传感器和三维地磁传感器，所述感测单元感测数据并传送给运算单元，运算单元计算出融合后的四元数以得到装置的姿态数据，所述输出单元将计算得出的姿态数据发送给外部设备进行相关控制，其中所述运算单元执行以下步骤：

1）从三维加速度传感器和三维地磁传感器中获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值Q_A；

2）从三维陀螺仪中获取角速度的三维数值，计算出姿态四元数的预测值Q_B=Q_A+dQ/dt，其中Q为三维角速度的四元数；

3）从三维加速度传感器和三维地磁传感器中获取加速度和地磁力的当前三维数值，计算出姿态参数，如罗德里格参数、方向余弦或者欧拉角等。

4）将计算得出的姿态参数转换为合适的姿态四元数的观测值Q_D；

5）对Q_D和Q_B进行融合计算，得到姿态四元数的融合值Q_E；融合的方法为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种。

6）再次从三维陀螺仪中获取角速度的三维数值，计算四元数的下一预测值Q_B1=Q_E+dQ/dt；

7）重复步骤3）至6），获取四元数预测值Q_Bn。

作为以上技术方案的一种补充，所有计算出的四元数均以基准姿态为参考，其中以水平面向正北的姿态为基准姿态，在所述基准姿态上三维加速度的数值为g（0，0，最大），三维磁力的数值为M（最大，0，M_Z），基准姿态四元数为Q₀（1，0，0，0）。

作为以上技术方案的一种补充，在步骤3）中，通过比对不同轴的加速度计值，和磁力数值，得到相关姿态参数，如罗德里格参数、方向余弦或者欧拉角等。

作为以上技术方案的一种补充，当姿态参数顯示，俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，执行步骤4）的处理；否则对加速度和磁力的三维数值进行往使往|θ|变小的方向旋转90°的矩阵运算，再进行姿态参数欧拉角C的计算，之后执行步骤4）的运算得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值QD。

作为以上技术方案的一种补充，|θ|的预设值为70°或者80°。

同理，Sin|θ|或Tan|θ|中的|θ|为70°或者80°或其他接近90°的值。

作为以上技术方案的一种补充，在步骤4）中，选取因旋转而产生的多个对应的四元数值中合适的四元数值,作为观测值Q_D。

作为以上技术方案的一种补充，在步骤5）中，Q_D和Q_B融合的方法为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种。

利用得到的姿态数据可以进行如下应用：

一方面通过将姿态数据发送到外部进行的控制，例如：

俯仰角和航向角，经滚动角转动后可以转化为二维空中鼠标所需要的光标控制信号，实现空中鼠标功能；

通过姿态的定义及变化数据检测，实现姿态对连续量的控制，如音量调节、电视频道改变、亮度改变、对比度改变、画面移动及浏览、选曲、碟片的快进、快退、网页的上下移动；

实现装置与三维图形的联动控制，通过外部旋转装置来直接控制三维图形的旋转观察；

实现对三维游戏的控制；

另一方面可以直接对装置内部的三维姿态控制，比如：

对装置内部的三维画面、游戏进行直接的姿态控制。

附图说明

以下结合附图和实施例对本发明作进一步说明，其中：

图1为本发明示例用的空间轴向分布图；

图2为航向角、俯仰角和滚动角的示意图；

图3为本发明的方法流程图；

图4为本发明的装置方框图。

具体实施方式

参见图3所示的方法流程图，设备上电时，系统获取加速度和地磁力的初始三维数值，即读入初始姿态下的三维加速度传感器的值g（x，y，z）以及三维地磁传感器的值M（x，y，z），如图1所示，其中x=A，y=B，z=C，各代表三个轴（注意，本发明不拘泥于空间中X，Y，Z轴的具体指向，也不受左手坐标系或右手坐标系的限制，为方便描述下文采用图1所示的空间轴向分布作为示例，其中A轴方向为应用本发明的感测装置的前方）。

首先计算初始姿态的四元数，所有计算出的四元数都是以基准姿态为参考，基准姿态是将传感器所构成的模块面向正北极水平放置状态，此时三维加速度值为g（0，0，最大），三维地磁传感器的值为M（最大，0，M_Z）。

利用基准姿态值计算出欧拉角，并转换为四元数，得到基准四元数值Q₀（1，0，0，0）。

以基准姿态为原始基准，根据初始姿态的三维加速度传感器以及三维地磁传感器的值计算得到初始姿态四元数Q_A。

读取三维陀螺仪数据，得到三轴角速度（ωx、ωy、ωz），并以此计算三维陀螺仪的四元数Q，再通过对Q的求导，再与Q_A累积作为四元素的预测值Q_B，即Q_B=Q_A+dQ/dt。

将以下用罗德里格参数欧拉角作为姿态参数描述本发明的一个实施例。

以罗德里格参数表示姿态，罗德里格参数按照如下定义：

$\mathrm{\σ}=\left[\begin{array}{ccc}{\mathrm{\σ}}_1& {\mathrm{\σ}}_2& {\mathrm{\σ}}_3\end{array}\right]=e\tan \frac{\mathrm{\θ}}{4}$

其中e和θ分别表示旋转轴和旋转角。罗德里格参数和姿态四元数转换关系如下：

${\mathrm{\σ}}_i=\frac{q_i}{1+q_0},i=\mathrm{1,2,3}$

$q_0=\frac{1-{\left|\mathrm{\σ}\right|}^2}{1+{\left|\mathrm{\σ}\right|}^2},q_i=\frac{{2\mathrm{\σ}}_i}{1+{\left|\mathrm{\σ}\right|}^2},i=\mathrm{1,2,3}$

$C_b^n=\frac{1}{{(1+{\mathrm{\σ}}_1^2+{\mathrm{\σ}}_2^2+{\mathrm{\σ}}_3^2)}^2}\left[\begin{array}{ccc}{\mathrm{\Σ}}^2+4({\mathrm{\σ}}_1^2-{\mathrm{\σ}}_2^2-{\mathrm{\σ}}_3^2)& 8{\mathrm{\σ}}_1{\mathrm{\σ}}_2-4\mathrm{\Σ}{\mathrm{\σ}}_3& 8{\mathrm{\σ}}_1{\mathrm{\σ}}_3+4\mathrm{\Σ}{\mathrm{\σ}}_2\\ 8{\mathrm{\σ}}_1{\mathrm{\σ}}_2+4\mathrm{\Σ}{\mathrm{\σ}}_3& {\mathrm{\Σ}}^2+4({\mathrm{\σ}}_2^2-{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_3^2)& 8{\mathrm{\σ}}_2{\mathrm{\σ}}_3-4\mathrm{\Σ}{\mathrm{\σ}}_1\\ 8{\mathrm{\σ}}_1{\mathrm{\σ}}_3-4\mathrm{\Σ}{\mathrm{\σ}}_2& 8{\mathrm{\σ}}_2{\mathrm{\σ}}_3+4\mathrm{\Σ}{\mathrm{\σ}}_1& {\mathrm{\Σ}}^2+4({\mathrm{\σ}}_3^2-{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_2^2)\end{array}\right]$

在另一个优选实例中，不采用罗德里格参数作为姿态参数，而以方向余弦作为姿态参数描述空间姿态。根据定义，用向量I,J,K表示地球坐标，用向量i,j,k表示机体坐标，下式中：

$[i^G,j^G,k^G]=\left[\begin{array}{ccc}I.i& I.j& I.k\\ J.i& J.j& J.k\\ K.i& K.j& K.k\end{array}\right]=\left[\begin{array}{ccc}\cos (I,i)& \cos (I,j)& \cos (I,k)\\ \cos (J,i)& \cos (J,j)& \cos (J,k)\\ \cos (K,i),& \cos (K,j)& \cos (K,k)\end{array}\right]={\mathrm{DCM}}^G$

${\stackrel{\·}{C}}_b^n=C_b^n{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bk}}$

${\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bk}}=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_{\mathrm{nbz}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b& 0& {-\mathrm{\ω}}_{\mathrm{nbx}}^b\\ {-\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

|i|是i单位向量的模（长度），cos(I，i)是I向量和i向量的夹角的余弦值。由于|I|=1并且|i|=1(由定义，它们是单位向量).我们可以得到:cos(I，i)=|I||i|cos(I，i)=I，i，它表示i向量在地球坐标系I轴即X轴上的投影长度。

这个矩阵称为方向余弦矩阵（Direction Cosine Matrix）。原因是显而易见的-它包含了所有机身和地球向量的余弦值。用该矩阵也可以描述机体的空间姿态。

在另一个优选实例中使用欧拉角作为姿态参数。

读取当前三维加速度传感器值以及三维地磁传感器值，利用重力加速度在三轴的分量计算姿态角（即滚动角和俯仰角），再利用地磁传感器的值计算得到与正北极的夹角（即航向角），将两种数据融合，计算欧拉角C（Ψ、θ、Φ）得到姿态数据，其中参见图2，Ψ为航向角Yaw，θ为俯仰角Pitch，Φ为滚动角Roll，具体计算过程如下所述：

1）计算滚动角Φ：根据重力加速度垂直向下的原理，比对B轴和C轴的加速度计数值，利用Tan roll=（Accel[B]/Accel[C]）得到滚动角并保存，其中Accel为加速度值；

2）计算俯仰角θ：根据计算出来的航向角，将三个维度的加速度计和磁力计数值经过三维矩阵旋转运算，得到旋转后的传感器数值；同样再根据重力加速度垂直向下的原理，比对旋转运算后的A轴和C轴的加速度计数值，利用公式Tan pitch=（Accel[A]/Accel[C]）得到俯仰角并保存；

3）计算航向角Ψ：根据计算出来的俯仰角，将三个维度的加速度计和磁力计数值经过三维矩阵旋转运算，得到旋转后的传感器数值；根据地球磁场始终指向正北方的原理，比对旋转运算后的A轴和B轴磁力计数据，利用Tan yaw=（Mag[A]/Mag[B]）得到航向角并保存，其中Mag为地磁传感器值；

4）奇异点判断处理：当|θ|远小于90°（即Sin|θ|或Tan|θ|小于Sin|90°|或Tan|90°|），，当然，其它合适的预设值也是可行的（例如70/80°，即Sin|70/80°|或Tan|70/80°|），按正常情况继续下一步骤处理；否则对传感器的数据值进行往回旋转90°（往|θ|变小的方向）矩阵运算三维加速度传感器值和三维地磁传感器值，处理之后再进行以上的步骤1）至3）开始计算欧拉角C，之后进行下一步骤，运算得到四元数后再（前面）作用(运算,为加或减或乘)上转回原角度θ的90度旋转四元数，其中采用的四元数旋转微分方程如下所示：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\λ}}\\ {\stackrel{\·}{P}}_1\\ {\stackrel{\·}{P}}_2\\ {\stackrel{\·}{P}}_3\end{array}\right]=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_x/2& -{\mathrm{\ω}}_y/2& -{\mathrm{\ω}}_z/2\\ {\mathrm{\ω}}_x/2& 0& {\mathrm{\ω}}_z/2& -{\mathrm{\ω}}_y/2\\ {\mathrm{\ω}}_y/2& -{\mathrm{\ω}}_z/2& 0& {\mathrm{\ω}}_x/2\\ {\mathrm{\ω}}_z/2& {\mathrm{\ω}}_y/2& {-\mathrm{\ω}}_x/2& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\λ}\\ P_1\\ P_2\\ P_3\end{array}\right]$

其中参数λ也可以用P₀表示。

这是因为俯仰角θ在靠近90°时计算会产生很大的误差，因此采用先将数据旋转90°变成小于45°的角计算后再旋转回去的方式比较精确。

接着，将欧拉角根据四元数运算转换成四元数，即四元数的观测值Q_D（q0，q1，q2，q3）=（cos R/2，asin R/2，bsin R/2，csinR/2）。

一个四元数可以表示一个旋转，R表示该旋转的角度，a，b，c分别表示该旋转的旋转轴在三个坐标轴上的分量。具体到这个实例中，根据运算步骤，R分别用来表示三个欧拉角，a，b，c分别为（1，0，0），（0，1，0）和（0，0，1）。

然后对Q_D和Q_B进行加权平均融合运算，得到融合后的四元数融合值Q_E，即Q_E=W1*Q_B+W2*Q_D。

这里W1、W2为系数，W1+W2=1，且W1>=0，W2>=0，W1、W2不需为定值可以随Q_B、Q_D、欧拉角或传感器值变化，但是也可以采用经验法则或测试所定出来的权值。

当然，数据融合的方法也可以是EKF、UKF、最小二乘和有权值的最小二乘中的一种或者多种。

再次读取当前三维陀螺仪数据，计算三维陀螺仪的四元数，作为四元素的预测值Q_B1，即Q_B1=Q_E+dQ/dt。

重复上述相关步骤获得四元数的一系列预测值Q_Bn。

与此同时，通过红外测距或者超声测距等测距装置，得到该应用以上方法的装置与输出屏幕之间的距离。同时将修正后的四元数值Q_Bn再实时变换成欧拉角H（ψ1、θ1、Φ1），采用公式如下所示：

$\begin{array}{c}M=\left[\begin{array}{ccc}{M}_{00}& M_{01}& M_{02}\\ M_{10}& M_{11}& M_{12}\\ M_{20}& M_{21}& M_{22}\end{array}\right]\\ =\left[\begin{array}{ccc}1-{2y}^2-{2z}^2& 2\mathrm{xy}-2\mathrm{wz}& 2\mathrm{xz}+2\mathrm{wy}\\ 2\mathrm{xy}+2\mathrm{wz}& 1-{2x}^2-2z^2& 2\mathrm{yz}-2\mathrm{wx}\\ 2\mathrm{xz}-2\mathrm{wy}& 2\mathrm{yz}+2\mathrm{wx}& 1-{2x}^2-2y^2\end{array}\right]\end{array}$

$\begin{array}{c}R({\mathrm{\θ}}_1,{\mathrm{\θ}}_2,{\mathrm{\θ}}_3)=R({\mathrm{\θ}}_1,x)R({\mathrm{\θ}}_2,y)R({\mathrm{\θ}}_3,z)\\ =\left[\begin{array}{cccc}{c}_2{c}_3& c_2{s}_3& -s_2& 0\\ s_1{s}_2{c}_3-c_1{s}_3& s_1{s}_2{s}_3+c_1{c}_3& s_1{c}_2& 0\\ c_1{s}_2{c}_3+s_1{s}_3& c_1{s}_2{s}_3-s_1{c}_3& c_1{c}_2& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$

或者是

$\mathrm{yaw}=\arctan \left(\frac{2(q_0{q}_3+q_1{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right).$

pitch=arcsin(2(q₀q₂-q₃q₁)).

$\mathrm{roll}=\arctan \left(\frac{2(q_0{q}_1+q_2{q}_3)}{q_0^2-q_1^2-q_2^2+q_3^2}\right).$

其中，yaw为ψ1，pitch为θ1，roll为Φ1。

再将其中的俯仰角θ1和航向角ψ1与装置输出屏幕上的光点位置根据计算出的距离一一对应，实现绝对角度位置的输出，其中利用的将距离与欧拉角转化为屏幕绝对位置输出的公式如下：

Screen_X=(Screen_Resolutin_X/X_Length)*tan(ψ1)*Distance；

Screen_Y=(Screen_Resolutin_Y/Y_Length)*tan(θ1)*Distance；

其中Screen_X，Screen_Y分别为屏幕中光点的X、Y轴坐标，Screen_Resolutin_X与Screen_Resolutin_Y分别屏幕的X、Y轴向分辨率，X_Length和Y_Length分别为X、Y轴上长度，Distance为应用以上方法的装置与输出屏幕之间的距离。

在另一优选实施例中，不使用测距装置，而是通过手动定位预先存入初始值，并可设置多级的敏感度调节。

在一个优选的实施例中，新的Q_D观测值在修正使用上也可以不完全取代旧有（其它方法计算出来的）观测值Q_F，也可以是：Q_E=W1*Q_B+W2*Q_D+W3*Q_F,其中W1+W2+W3=1，W1>=0，W2>=0，W3>=0，W1、W2、W3不需为定值，而是可以随Q_B、Q_D、欧拉角或传感器值变化,也可以是由经验法则或测试所定出来的权值决定。

数据融合的方法可以为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种。

以上内容通过具体实施例详细描述了本发明所提出的采用加速收敛算法的定位方法，除此之外，本发明还提出了相应的装置，通过执行以上所述方法生成姿态数据。所述采用加速收敛算法的定位装置由感测单元、运算单元和输出单元构成，如图4所示，其中感测单元包括三维陀螺仪、三维加速度传感器和三维地磁传感器，运算单元可以是任何具有需要的运算能力的处理器，输出单元可以是与外部设备或其它内部设备兼容的数据接口。感测单元感测数据并传送给运算单元，运算单元利用以上所述的快速定位算法计算融合出所需的四元数，并可将四元数转换成欧拉角，得到装置的姿态数据，具体运算步骤可参见以上所述的方法及附图3。

利用得到的姿态数据可以广泛的应用。一方面通过将姿态数据发送到外部进行的控制，如：

1）俯仰角θ1和航向角ψ1，经滚动角Φ1转动后可以转化为二维空中鼠标所需要的光标控制信号，实现空中鼠标功能；

2）通过姿态的定义及变化数据检测，实现姿态对连续量的控制，如音量调节、电视频道改变、亮度改变、对比度改变、画面移动及浏览、选曲、碟片的快进、快退、网页的上下移动等；

3）实现装置与三维图形的联动控制，通过外部旋转装置来直接控制三维图形的旋转观察；

4）实现对三维游戏的控制。

另一方面可以直接对装置内部的三维姿态控制，如对装置内部的三维画面、游戏进行直接的姿态控制。

本发明除了上述实施方式之外，其等同技术方案也应当在其保护范围之内，在此不再一一叙述。

Claims (8)

1.一种采用加速收敛算法的定位方法，其特征在于，包括以下步骤：

1）获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值Q_A；

2）获取角速度的三维数值，计算出姿态四元数的预测值Q_B=Q_A+dQ/dt，其中Q为三维角速度的四元数；

3）获取加速度和地磁力的当前三维数值，计算出姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；

4）将计算得出的姿态参数转换为姿态四元数的观测值Q_D；

5）对Q_D和Q_B进行融合计算，得到姿态四元数的融合值Q_E；

6）再次获取角速度的三维数值，计算四元数的下一预测值Q_B1=Q_E+dQ/dt；

7）重复步骤3）至6），获取四元数预测值Q_Bn。

2.根据权利要求1所述的采用加速收敛算法的定位方法，其特征在于，所有计算出的四元数均以基准姿态为参考，其中以水平面向正北的姿态为基准姿态，在所述基准姿态上三维加速度的数值为g（0，0，最大），三维磁力的数值为M（最大，0，不管），基准姿态四元数为Q₀（1，0，0，0）。

3.根据权利要求1所述的采用加速收敛算法的定位方法，其特征在于，在步骤3）中，通过比对不同轴的加速度计值和磁力数值，计算得到姿态参数。

4.根据权利要求3所述的采用加速收敛算法的定位方法，其特征在于，当姿态参数中俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，执行步骤4）的处理；否则对加速度和磁力的三维数值进行往使往|θ|或Sin|θ|或Tan|θ|变小的方向旋转90°的矩阵运算，再进行欧拉角C的计算，之后执行步骤4）的运算得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值Q_D。

5.根据权利要求5所述的采用加速收敛算法的定位方法，其特征在于，|θ|的预设值为70°或者80°。

6.根据权利要求1所述的采用加速收敛算法的定位方法，其特征在于，在步骤4）中，选取因旋转姿态参数而产生的多个对应的四元数值中合适的值作为观测值Q_D。

7.根据权利要求1所述的采用加速收敛算法的定位方法，其特征在于，在步骤5）中，Q_D和Q_B融合的方法为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种。

8.一种采用加速收敛算法的定位装置，其特征在于，包括感测单元、运算单元以及输出单元，其中所述感测单元包括三维陀螺仪、三维加速度传感器和三维地磁传感器，所述感测单元感测数据并传送给运算单元，运算单元计算出融合后的四元数以得到装置的姿态数据，所述输出单元将计算得出的姿态数据用于自身控制和/或发送给外部设备进行相关控制。