[发明专利]一种采用加速收敛算法的定位方法及装置

说明书

技术领域

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

背景技术

目前多数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为三维角速度的四元数；