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

权利要求书

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

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

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

3)获取加速度和地磁力的当前三维数值，通过比对不同轴的加速度计值和磁力数值，计算得到姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；

4)当姿态参数中俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，将计算得出的姿态参数转换为姿态四元数的观测值QD；否则对加速度和磁力的三维数值进行往使往|θ|或Sin|θ|或Tan|θ|变小的方向旋转90°的矩阵运算，再进行欧拉角C的计算，之后将计算得出的姿态参数转换得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值Q_D；

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

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

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

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

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

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

5.一种采用加速收敛算法的定位装置，其特征在于，包括感测单元、运算单元以及输出单元，其中所述感测单元包括三维陀螺仪、三维加速度传感器和三维地磁传感器；

所述运算单元用于执行以下步骤：

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

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

3)获取加速度和地磁力的当前三维数值，通过比对不同轴的加速度计值和磁力数值，计算得到姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；

4)当姿态参数中俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，将计算得出的姿态参数转换为姿态四元数的观测值QD；否则对加速度和磁力的三维数值进行往使往|θ|或Sin|θ|或Tan|θ|变小的方向旋转90°的矩阵运算，再进行欧拉角C的计算，之后将计算得出的姿态参数转换得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值Q_D；

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

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

7)重复步骤3)至7)，获取四元数预测值Q_Bn；

所述输出单元将计算得出的姿态数据用于自身控制和/或发送给外部设备进行相关控制。