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

说明书

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

技术领域

本发明涉及惯导技术领域，尤其涉及一种九轴传感器的高动态轨迹处理方法及装置。

背景技术

根据卫生部在《第三次全国口腔健康流行病学研究报告》的数据显示，全民口腔病患病率高达97.6％。虽然刷牙是我国大部分国民每天必做的口腔护理(我国国民刷牙率在低年龄组达到82％，中年年龄组达到89.3％)，但是国民口腔仍不健康，原因在于大多数人没有科学刷牙，导致牙齿清洁度不够。如何是口腔健康的研究课题之一。对刷牙运动轨迹的计算，可作为刷牙方式的判断依据，是进行科学的有效的刷牙，减少口腔患病率的研究方向之一。随着微机械制造技术、微纳米技术和集成光学技术的高速发展，微机电(MEMS)惯性传感器组成的惯性测量单元(简称MEMS惯组)得到广泛的重视和快速发展，由于其低成本、小体积、重量轻的优势，在普通民用等领域迅速得到普及应用，越来越贴近人们的生活。牙刷使用者由于手腕摆动幅度过小，刷牙频率也因人而异，且牙刷可利用的空间有限性，这是对牙刷运动方式进行轨迹计算的难点，利用MEMS传感器的优点，可采集牙刷使用者在刷牙时牙刷尾部的传感器信息，从而进行轨迹计算。而虽然MEMS传感器的优点很多，是其缺点也比较明显：精度相对较低。为此，与磁强计的组合，是其提高精度的途径之一。磁强计对振动、冲击等不敏感，随温度等环境影响线性变化的特点也容易实现有效的补偿，而且它也兼具精度高和低成本的特点，与MEMS惯性传感器形成互补性。

如图1所示，现有技术中，轨迹解算过程主要包括姿态更新算法和导航计算两个过程。其中，根据MEMS陀螺仪和MEMS加速度计组成的MEMS惯性测量单元(MEMS-IMU，简称MIMU)测量得到的牙刷角运动信息和线运动信息来解算出牙刷的捷联姿态角，与MEMS加速度计和磁强计解算出的姿态角通过卡尔曼滤波器进行滤波，得到误差四元数，再与MIMU的姿态四元数结合建立牙刷的姿态转换矩阵，即实现由牙刷坐标系到导航坐标系的坐标转换，从而建立数学平台坐标系，为导航解算提供数学平台，将牙刷坐标系中的运动参数转化为导航坐标系中的运动参数。随着牙刷的运动，姿态转换矩阵也随之变化，根据矩阵的元素可确定牙刷的姿态角，再通过速度计算，进一步得到速度和位置信息。

由于刷牙运动属于高频率、小幅值的运动，一般的姿态解算和导航解算不能满足高动态要求。

发明内容

本发明实施例提供一种九轴传感器的高动态轨迹处理方法及装置，以适用于牙刷尾部运动的轨迹计算。

第一方面，本发明实施例提供了一种九轴传感器的高动态轨迹处理方法，所述九轴传感器安装于载体上，所述方法包括：

对时间区间[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为载体所在的纬度；