[发明专利]一种九轴传感器的高动态轨迹处理方法及装置

摘要

本发明提供一种九轴传感器的高动态轨迹处理方法及装置，九轴传感器安装于载体上，该方法包括：对时间区间[t_k,t_k+T]上的载体角速度进行曲线拟合，得到根据计算等效旋转矢量根据计算姿态四元数对姿态四元素进行实时更新，获得姿态角，并建立姿态转换矩阵根据所述姿态角和姿态转换矩阵确定所述载体的位置信息，得到所述载体的轨迹。本发明提供了一种高动态的轨迹算法，可以适用于牙刷尾部运动的轨迹计算。

主权项

1.一种九轴传感器的高动态轨迹处理方法，其特征在于，所述九轴传感器安装于载体上，所述方法包括：对时间区间[t_k，t_k+T]上的载体角速度进行曲线拟合，得到t_k为k时刻的时间点，T为导航周期，为载体角速度，τ为三子样的时间点，a₀为常数，a₁为一次拟合系数，a₂为二次拟合系数；根据计算等效旋转矢量为等效旋转矢量子样数，h为姿态更新周期；根据计算姿态四元数Φ为效旋转矢量的绝对值；对姿态四元素进行实时更新，获得姿态角，并建立姿态转换矩阵Δλ为从s_k时刻到s_k+D时经度增量，ΔL为从s_k时刻到s_k+D时纬度增量，D为导航坐标系更新周期，L为载体所在的纬度；根据所述姿态角和姿态转换矩阵确定所述载体的位置信息，得到所述载体的轨迹。