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

摘要

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

主权项

一种采用加速收敛算法的定位方法，其特征在于，包括以下步骤：1)获取加速度和地磁力的初始三维数值，计算出姿态四元数的初始值QA；2)获取角速度的三维数值，计算出姿态四元数的预测值QB＝QA+dQ/dt，其中Q为三维角速度的四元数；3)获取加速度和地磁力的当前三维数值，通过比对不同轴的加速度计值和磁力数值，计算得到姿态参数，所述姿态参数为罗德里格参数、方向余弦和欧拉角中的至少一种；4)当姿态参数中俯仰角的绝对值|θ|或Sin|θ|或Tan|θ|小于预设值时，将计算得出的姿态参数转换为姿态四元数的观测值QD；否则对加速度和磁力的三维数值进行往使往|θ|或Sin|θ|或Tan|θ|变小的方向旋转90°的矩阵运算，再进行欧拉角C的计算，之后将计算得出的姿态参数转换得到姿态四元数后再进行转回原角度θ的90°旋转运算得到姿态四元数的观测值QD；5)对QD和QB进行融合计算，得到姿态四元数的融合值QE，其中QD和QB融合的方法为KF、EKF、UKF、加权平均、最小二乘和有权值的最小二乘中的一种或者多种；6)再次获取角速度的三维数值，计算四元数的下一预测值QB1＝QE+dQ/dt；7)重复步骤3)至7)，获取四元数预测值QBn。