CN112051868A - 智能电动牙刷轨迹的定位方法及系统 - Google Patents

Abstract

本发明公开了一种智能电动牙刷轨迹的定位及系统，该方法包括：获取智能电动牙刷的初始姿态信息、角速度和加速度；对角速度进行姿态计算，获得第一姿态四元数；根据第一姿态四元数和加速度确定第二姿态四元数；对第一姿态四元数及第二姿态四元数进行处理，获得姿态数据；根据初始姿态信息和姿态数据确定智能牙刷刷头所处的牙齿区域，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间。相较于现有技术，仅利用运动传感器确定牙刷刷头的定位信息，会导致定位精度差，而本发明根据智能电动牙刷的角速度和加速度确定智能牙刷的姿态数据，最后根据初始姿态信息和姿态数据确定刷头所处的牙齿区域，实现了精确定位智能牙刷刷头的所处区域。

Description

智能电动牙刷轨迹的定位方法及系统

技术领域

本发明涉及人工智能技术领域，尤其涉及一种智能电动牙刷轨迹的定位方法及系统。

背景技术

近些年大众对牙齿健康意识的提高，电动牙刷逐渐成为牙科领域增长最快的市场之一，并且日益增长，持续的创新和产品更新也推动了电动牙刷的发展，现有智能电动牙刷带有运动传感器，但存在定位精度差、产品成本较高，因此，如何在降低产品成本的同时，精确定位智能牙刷刷头的所处区域是亟待解决的技术问题。

上述内容仅用于辅助理解本发明的技术方案，并不代表承认上述内容是现有技术。

发明内容

本发明的主要目的在于提供一种智能电动牙刷轨迹的定位方法及系统，旨在解决如何在降低产品成本的同时，精确定位智能牙刷刷头的所处区域的技术问题。

为实现上述目的，本发明提供一种智能电动牙刷轨迹的定位方法，所述智能电动牙刷轨迹的定位方法包括以下步骤：

在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度；

对所述角速度进行姿态计算，获得第一姿态四元数；

根据所述第一姿态四元数和所述加速度确定第二姿态四元数；

对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计所述智能牙刷刷头在每个牙齿区域的停留时间。

优选地，所述对所述角速度进行姿态计算，获得第一姿态四元数的步骤，包括：

通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀i+q₁j+q₂k+q₃

式中，q₀、q₁、q₂、q₃为姿态四元数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

优选地，所述根据所述第一姿态四元数和所述加速度确定第二姿态四元数的步骤，包括：

根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

优选地，所述对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据的步骤，包括：

通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

优选地，所述根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域的步骤之后，还包括：

判断所述智能电动牙刷的工作状态是否为停止状态；

在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的所述智能电动牙刷刷头在每个牙齿区域的停留时间；

其中，所述牙齿区域为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面。

此外，为实现上述目的，本发明还提出一种智能电动牙刷轨迹的定位系统，所述智能电动牙刷轨迹的定位系统包括：

传感器数据获取模块，用于在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度；

第一四元数计算模块，用于对所述角速度进行姿态计算，获得第一姿态四元数；

第二四元数确定模块，用于根据所述第一姿态四元数和所述加速度确定第二姿态四元数；

四元数互补融合模块，用于对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

定位模块，用于根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计所述智能牙刷刷头在每个牙齿区域的停留时间。

优选地，所述第一四元数计算模块，用于通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀i+q₁j+q₂k+q₃

式中，q₀、q₁、q₂、q₃为姿态四元数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

优选地，所述第二四元数确定模块，用于根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

优选地，所述四元数互补融合模块，用于通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

优选地，所述系统还包括，运动轨迹生成模块；

所述运动轨迹生成模块，用于判断所述智能电动牙刷的工作状态是否为停止状态；

所述运动轨迹生成模块，还用于在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的所述智能电动牙刷刷头在每个牙齿区域的停留时间；

其中，所述牙齿区域为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面。

本发明中，首先获取智能电动牙刷的初始姿态信息、角速度和加速度，然后对角速度进行姿态计算，获得第一姿态四元数，并根据第一姿态四元数和加速度确定第二姿态四元数，之后对第一姿态四元数及第二姿态四元数进行处理，获得姿态数据，最后根据初始姿态信息和姿态数据确定智能牙刷刷头所处的牙齿区域。相较于现有技术，仅利用运动传感器确定牙刷刷头的定位信息，会导致定位精度差，而本发明获取智能电动牙刷的初始姿态信息、角速度和加速度，之后根据角速度和加速度确定智能牙刷的姿态数据，提高了姿态解算的精度，最后根据初始姿态信息和姿态数据确定刷头所处的牙齿区域，实现了精确定位智能牙刷刷头的所处区域。

附图说明

图1为本发明智能电动牙刷轨迹的定位方法第一实施例的流程示意图；

图2为本发明智能电动牙刷轨迹的定位方法第二实施例的流程示意图；

图3为本发明智能电动牙刷轨迹的定位系统第一实施例的结构框图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参照图1，图1为本发明智能电动牙刷轨迹的定位方法第一实施例的流程示意图，提出本发明智能电动牙刷轨迹的定位方法第一实施例。

在第一实施例中，所述智能电动牙刷轨迹的定位方法包括以下步骤：

步骤S10：在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度。

需要说明的是，本实施例的执行主体是智能电动牙刷轨迹的定位设备，该设备具有数据处理、网络通信以及程序运行等功能，本实施例对此不做限制。

智能电动牙刷的初始姿态信息可以理解为电动牙刷刚启动时刷头的角度及位置信息等，本实施例并不加以限制。

通过陀螺仪和加速计分别进行角速度数据和加速度数据采集，可以理解为，陀螺仪和加速计分别用了三个16位的模数转换器，将其测量的模拟量转化为可输出的数字量，其数据即角速度和加速度的写入和读出均通过其芯片内部的寄存器实现。

假设角速度数据和加速度数据位于0x3B到0x47这14个字节的寄存器中，其中0x是十六进制表示，这些角速度数据和加速度数据会被动态更新。下面列出相关寄存器的地址，数据的名称：0x3B，加速度计的X轴分量ACC_X；0x3D，加速度计的Y轴分量ACC_Y；0x3F，加速度计的Z轴分量ACC_Z；0x41，当前温度TEMP；0x43，绕X轴旋转的角速度GYR_X；0x45，绕Y轴旋转的角速度GYR_Y；0x47，绕Z轴旋转的角速度GYR_Z；对于加速度计数据，加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数，分别表示器件在三个轴向上的加速度，三个加速度分量均以重力加速度g的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：2g、4g、8g、16g。

对于陀螺仪数据，绕X、Y和Z三个坐标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去，取正值时为顺时针旋转，取负值时为逆时针旋转。三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。

倍率可以理解为陀螺仪这个传感器的设定参数之一。

进一步地，为了改善在进行姿态解算时因电机震动造成加速度数据与角速度数据不准确的问题，需要将采集的角速度和加速度进行滤波处理，可以通过巴特沃斯滤波器进行数据处理，也就是说，利用巴特沃斯数字滤波器，对陀螺仪与加速度的原始数据进行低通滤波，利用经滤波后的角速度数据与加速度数据进行姿态解算更新得到两组姿态四元数。

巴特沃斯滤波器的设计与实现为：

(1)根据给定的通带截止频率、通带截止增益、阻带截止频率、阻带截止增益，计算巴特沃斯滤波器所需的最小阶数和截止频率。

(2)根据上述计算得到的阶数，计算巴特沃斯滤波器原型的零点，极点和增益。

(3)改变截止频率，将原型滤波器转换成带目标截止频率的滤波器。

(4)将模拟滤波器转换为数字滤波器，得到的分子分母系数即为数字滤波器系数。

(5)将所得的多项式系数写入单片机中，完成滤波。本发明所用算法为：

a(1)*y(k)＝b1*x(k)+b2*x(k-1)+b3*x(x-2)-a(2)*y(k-1)-a(3)*y(k-2)

式中，a(1)、a(2)、a(3)和b(1)、b(2)、b(3)为数字滤波器分子分母多项式系数，y(k)、y(k-1)、y(k-2)是滤波后的加速度数据，x(k)、x(k-1)、x(k-2)是加速度原始数据。

步骤S20：对所述角速度进行姿态计算，获得第一姿态四元数。

对所述角速度进行姿态计算，获得第一姿态四元数的步骤，通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀+q₁i+q₂j+q₃k

式中，q₀、q₁、q₂、q₃为姿态自然数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

步骤S30：根据所述第一姿态四元数和所述加速度确定第二姿态四元数。

根据所述第一姿态四元数和所述加速度确定第二姿态四元数的步骤，根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

为了便于理解，以下进行举例说明：

在完成对陀螺仪和加速度计数据的采集之后，可以对四元数进行初始化了。首先需要计算初始的欧拉角，直接利用加速度的反正切求得俯仰角、横滚角和偏航角，由此得到一组初始的欧拉角。再利用欧拉角到四元数的公式：

q0_init＝cos(euler_yaw_init/2)*cos(euler_pitch_init/2)*cos(euler_roll_init/2)+sin(euler_yaw_init/2)*sin(euler_pitch_init/2)*sin(euler_roll_init/2)；

q1_init＝cos(euler_yaw_init/2)*cos(euler_pitch_init/2)*sin(euler_roll_init/2)-sin(euler_yaw_init/2)*sin(euler_pitch_init/2)*cos(euler_roll_init/2)；

q2_init＝cos(euler_yaw_init/2)*sin(euler_pitch_init/2)*cos(euler_roll_init/2)+sin(euler_yaw_init/2)*cos(euler_pitch_init/2)*sin(euler_roll_init/2)；

q3_init＝sin(euler_yaw_init/2)*cos(euler_pitch_init/2)*cos(euler_roll_init/2)-cos(euler_yaw_init/2)*sin(euler_pitch_init/2)*sin(euler_roll_init/2)；

计算出初始四元数。在此基础之上就可以进行对四元数的更新，此步骤完成了四元数的初始化。

进一步地，更新四元数，需要输入的参数为三个轴的加速度a_x、a_y、a_z和三个轴的角速度g_x、g_y、g_z，并且因为是以1g为基准单位，所以需要对加速度做归一化处理，即：

a_x*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

a_y*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

a_z*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

式中，invSqrt为求平方根的倒数，a_x、a_y、a_z为三个轴的加速度。

然后是计算由上一次四元数所得到的旋转矩阵所得出的重力分量，即：

v_x＝2q₁q₃-2q₀q₂

v_y＝2q₂q₃+2q₀q₁

v_z＝1-2q₀ ²+2q₃ ²

式中，v_x、v_y、v_z为重力分量，q₀、q₁、q₂、q₃为姿态自然数系数。

将a_x、a_y、a_z与v_x、v_y、v_z分别作差，就可得所需要的误差函数。再利用梯度下降法，其实质上就是求误差函数的导数，设定一个步长变量，用上一次的状态减去导数的负数乘步长，就可以逐步消除误差函数的值，步长越大，算法的动态性能就越好，能越快收敛，但静态性能差，步长越小，算法动态性能越差，收敛速度变慢，但起静态性能会变好。

也就是说，本发明通过陀螺仪的数据来获得四元数(旋转矩阵)，由此旋转矩阵计算当前重力在各个轴上的分量，再通过加速度计的数据获得实际测得的重力分量，两者做差得到误差函数，算法的目标就是尽力将误差函数的值趋近为0，以此来修正陀螺所获得的四元数，也就是修正了旋转矩阵，本质上就是修正了姿态，本发明采用的是梯度下降的方法修正误差函数，最后将陀螺仪所获得的四元数与加速度计所获取的四元数按照一定比例互补融合，就可以得到当前所需要的四元数。

步骤S40：对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据。

对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据的步骤，通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

可以理解的是，两组四元数，其分别由加速度计和陀螺仪的数据得到。之后通过对一个常量，该常量大于零小于一，来对加速度计求得的四元数和陀螺仪求得的四元数进行比例融合，该常量的取值应该由载体的运动速度决定，因为当载体高速运动时，应该更相信陀螺仪算出来的四元数，当载体低速运动时，应该更相信加速度计算出来的四元数。这于传感器的性能有关，陀螺仪在高速状态下的数据比加速度计更准确，而在低速状态下，加速度计的数据比陀螺仪更准确。

步骤S50：根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间。

牙齿区域可以为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面等，本实施例并不加以限制。

姿态数据可以为当智能电动牙刷停止工作时，当前刷头所处位置及角度等。

也可以理解为，对电动牙刷工作状态进行判断，判断其处于刷牙开始状态、刷牙进行状态、刷牙结束状态中的哪一种状态，之后记录用户开始刷牙时的初始位姿四元数实部w以及虚部x、y、z，比较w、x、y、z与实时位姿四元数curr_w、curr_x、curr_y、curr_z，根据两者的差别以及对口腔的先验知识，得到电动牙刷目前所处的牙齿区域。

根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域的步骤之后，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间，也就是说，判断所述智能电动牙刷的工作状态是否为停止状态，在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

也就是说，在智能电动牙刷处于启动状态时直至停止状态时，根据智能电动牙刷刷头的定位结果生成智能电动牙刷的运动轨迹路线，从而可以进行刷牙轨迹跟踪，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

本发明中，首先获取智能电动牙刷的初始姿态信息、角速度和加速度，然后对角速度进行姿态计算，获得第一姿态四元数，并根据第一姿态四元数和加速度确定第二姿态四元数，之后对第一姿态四元数及第二姿态四元数进行处理，获得姿态数据，最后根据初始姿态信息和姿态数据确定智能牙刷刷头所处的牙齿区域，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间。相较于现有技术，仅利用运动传感器确定牙刷刷头的定位信息，会导致定位精度差，而本发明获取智能电动牙刷的初始姿态信息、角速度和加速度，之后根据角速度和加速度确定智能牙刷的姿态数据，提高了姿态解算的精度，最后根据初始姿态信息和姿态数据确定刷头所处的牙齿区域，实现了精确定位智能牙刷刷头的所处区域。

此外，参照图2，图2为基于上述智能电动牙刷轨迹的定位方法第一实施例，提出本发明智能电动牙刷轨迹的定位方法第二实施例。

在第二实施例中，智能电动牙刷轨迹的定位方法中所述步骤S50之后，还包括：

步骤S60：判断所述智能电动牙刷的工作状态是否为停止状态。

智能电动牙刷的工作状态可以为刷牙开始状态、刷牙进行状态及刷牙结束状态等，本实施例并不加以限制。

步骤S70：在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

牙齿区域为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面等，本实施例并部加以限制。

也可以理解为，对电动牙刷工作状态进行判断，判断其处于刷牙开始状态、刷牙进行状态、刷牙结束状态中的哪一种状态，之后记录用户开始刷牙时的初始位姿四元数实部w以及虚部x、y、z，比较w、x、y、z与实时位姿四元数curr_w、curr_x、curr_y、curr_z，根据两者的差别以及对口腔的先验知识，得到电动牙刷目前所处的牙齿区域，之后在智能电动牙刷处于启动状态时直至停止状态时，根据智能电动牙刷刷头的定位结果生成智能电动牙刷的运动轨迹路线，从而可以进行刷牙轨迹跟踪，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

本发明中，判断所述智能电动牙刷的工作状态是否为停止状态，然后在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。实现了在低成本的情况下，完成了智能电动牙刷运动轨迹检测功能，从而生成精准的智能电动牙刷运动轨迹路线。

此外，参照图3，本发明实施例还提出一种智能电动牙刷轨迹的定位系统，所述智能电动牙刷轨迹的定位系统包括：

传感器数据获取模块3001，用于在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度。

需要说明的是，本实施例的执行主体是智能电动牙刷轨迹的定位设备，该设备具有数据处理、网络通信以及程序运行等功能，本实施例对此不做限制。

智能电动牙刷的初始姿态信息可以理解为电动牙刷刚启动时刷头的角度及位置信息等，本实施例并不加以限制。

通过陀螺仪和加速计分别进行角速度数据和加速度数据采集，可以理解为，陀螺仪和加速计分别用了三个16位的模数转换器，将其测量的模拟量转化为可输出的数字量，其数据即角速度和加速度的写入和读出均通过其芯片内部的寄存器实现。

假设角速度数据和加速度数据位于0x3B到0x47这14个字节的寄存器中，这些角速度数据和加速度数据会被动态更新。下面列出相关寄存器的地址，数据的名称：0x3B，加速度计的X轴分量ACC_X；0x3D，加速度计的Y轴分量ACC_Y；0x3F，加速度计的Z轴分量ACC_Z；0x41，当前温度TEMP；0x43，绕X轴旋转的角速度GYR_X；0x45，绕Y轴旋转的角速度GYR_Y；0x47，绕Z轴旋转的角速度GYR_Z；对于加速度计数据，加速度计的三轴分量ACC_X、ACC_Y和ACC_Z均为16位有符号整数，分别表示器件在三个轴向上的加速度，三个加速度分量均以重力加速度g的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：2g、4g、8g、16g。

对于陀螺仪数据，绕X、Y和Z三个坐标轴旋转的角速度分量GYR_X、GYR_Y和GYR_Z均为16位有符号整数。从原点向旋转轴方向看去，取正值时为顺时针旋转，取负值时为逆时针旋转。三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。

倍率可以理解为陀螺仪这个传感器的设定参数之一。

进一步地，为了改善在进行姿态解算时因电机震动造成加速度数据与角速度数据不准确的问题，需要将采集的角速度和加速度进行滤波处理，可以通过巴特沃斯滤波器进行数据处理，也就是说，利用巴特沃斯数字滤波器，对陀螺仪与加速度的原始数据进行低通滤波，利用经滤波后的角速度数据与加速度数据进行姿态解算更新得到两组姿态四元数。

巴特沃斯滤波器的设计与实现为：

(1)根据给定的通带截止频率、通带截止增益、阻带截止频率、阻带截止增益，计算巴特沃斯滤波器所需的最小阶数和截止频率。

(2)根据上述计算得到的阶数，计算巴特沃斯滤波器原型的零点，极点和增益。

(3)改变截止频率，将原型滤波器转换成带目标截止频率的滤波器。

(4)将模拟滤波器转换为数字滤波器，得到的分子分母系数即为数字滤波器系数。

(5)将所得的多项式系数写入单片机中，完成滤波。本发明所用算法为：

a(1)*y(k)＝b1*x(k)+b2*x(k-1)+b3*x(x-2)-a(2)*y(k-1)-a(3)*y(k-2)

式中，a(1)、a(2)、a(3)和b(1)、b(2)、b(3)为数字滤波器分子分母多项式系数，y(k)、y(k-1)、y(k-2)是滤波后的加速度数据，x(k)、x(k-1)、x(k-2)是加速度原始数据

第一四元数计算模块3002，用于对所述角速度进行姿态计算，获得第一姿态四元数。

对所述角速度进行姿态计算，获得第一姿态四元数的步骤，通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀+q₁i+q₂j+q₃k

式中，q₀、q₁、q₂、q₃为姿态自然数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

第二四元数确定模块3003，用于根据所述第一姿态四元数和所述加速度确定第二姿态四元数。

根据所述第一姿态四元数和所述加速度确定第二姿态四元数的步骤，根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

为了便于理解，以下进行举例说明：

在完成对陀螺仪和加速度计数据的采集之后，可以对四元数进行初始化了。首先需要计算初始的欧拉角，直接利用加速度的反正切求得俯仰角、横滚角和偏航角，由此得到一组初始的欧拉角。再利用欧拉角到四元数的公式：

q0_init＝cos(euler_yaw_init/2)*cos(euler_pitch_init/2)*cos(euler_roll_init/2)+sin(euler_yaw_init/2)*sin(euler_pitch_init/2)*sin(euler_roll_init/2)；

q1_init＝cos(euler_yaw_init/2)*cos(euler_pitch_init/2)*sin(euler_roll_init/2)-sin(euler_yaw_init/2)*sin(euler_pitch_init/2)*cos(euler_roll_init/2)；

q2_init＝cos(euler_yaw_init/2)*sin(euler_pitch_init/2)*cos(euler_roll_init/2)+sin(euler_yaw_init/2)*cos(euler_pitch_init/2)*sin(euler_roll_init/2)；

q3_init＝sin(euler_yaw_init/2)*cos(euler_pitch_init/2)*cos(euler_roll_init/2)-cos(euler_yaw_init/2)*sin(euler_pitch_init/2)*sin(euler_roll_init/2)；

计算出初始四元数。在此基础之上就可以进行对四元数的更新，此步骤完成了四元数的初始化。

进一步地，更新四元数，需要输入的参数为三个轴的加速度a_x、a_y、a_z和三个轴的角速度g_x、g_y、g_z，并且因为是以1g为基准单位，所以需要对加速度做归一化处理，即：

a_x*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

a_y*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

a_z*＝invSqrt(a_x*a_x+a_y*a_y+a_z*a_z)

式中，invSqrt为求平方根的倒数，a_x、a_y、a_z为三个轴的加速度。

然后是计算由上一次四元数所得到的旋转矩阵所得出的重力分量，即：

v_x＝2q₁q₃-2q₀q₂

v_y＝2q₂q₃+2q₀q₁

v_z＝1-2q₀ ²+2q₃ ²

式中，v_x、v_y、v_z为重力分量，q₀、q₁、q₂、q₃为姿态自然数系数。

将a_x、a_y、a_z与v_x、v_y、v_z分别作差，就可得所需要的误差函数。再利用梯度下降法，其实质上就是求误差函数的导数，设定一个步长变量，用上一次的状态减去导数的负数乘步长，就可以逐步消除误差函数的值，步长越大，算法的动态性能就越好，能越快收敛，但静态性能差，步长越小，算法动态性能越差，收敛速度变慢，但起静态性能会变好。

也就是说，本发明通过陀螺仪的数据来获得四元数(旋转矩阵)，由此旋转矩阵计算当前重力在各个轴上的分量，再通过加速度计的数据获得实际测得的重力分量，两者做差得到误差函数，算法的目标就是尽力将误差函数的值趋近为0，以此来修正陀螺所获得的四元数，也就是修正了旋转矩阵，本质上就是修正了姿态，本发明采用的是梯度下降的方法修正误差函数，最后将陀螺仪所获得的四元数与加速度计所获取的四元数按照一定比例互补融合，就可以得到当前所需要的四元数。

四元数互补融合模块3004，用于对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据。

对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据的步骤，通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

可以理解的是，两组四元数，其分别由加速度计和陀螺仪的数据得到。之后通过对一个常量，该常量大于零小于一，来对加速度计求得的四元数和陀螺仪求得的四元数进行比例融合，该常量的取值应该由载体的运动速度决定，因为当载体高速运动时，应该更相信陀螺仪算出来的四元数，当载体低速运动时，应该更相信加速度计算出来的四元数。这于传感器的性能有关，陀螺仪在高速状态下的数据比加速度计更准确，而在低速状态下，加速度计的数据比陀螺仪更准确。

定位模块3005，用于根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间。

牙齿区域可以为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面等，本实施例并不加以限制。

姿态数据可以为当智能电动牙刷停止工作时，当前刷头所处位置及角度等。

也可以理解为，对电动牙刷工作状态进行判断，判断其处于刷牙开始状态、刷牙进行状态、刷牙结束状态中的哪一种状态，之后记录用户开始刷牙时的初始位姿四元数实部w以及虚部x、y、z，比较w、x、y、z与实时位姿四元数curr_w、curr_x、curr_y、curr_z，根据两者的差别以及对口腔的先验知识，得到电动牙刷目前所处的牙齿区域。

根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域的步骤之后，判断所述智能电动牙刷的工作状态是否为停止状态，在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

也就是说，在智能电动牙刷处于启动状态时直至停止状态时，根据智能电动牙刷刷头的定位结果生成智能电动牙刷的运动轨迹路线，从而可以进行刷牙轨迹跟踪，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。

本发明中，首先获取智能电动牙刷的初始姿态信息、角速度和加速度，然后对角速度进行姿态计算，获得第一姿态四元数，并根据第一姿态四元数和加速度确定第二姿态四元数，之后对第一姿态四元数及第二姿态四元数进行处理，获得姿态数据，根据初始姿态信息和姿态数据确定智能牙刷刷头所处的牙齿区域，实时在线统计智能牙刷刷头在每个牙齿区域的停留时间。并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的智能电动牙刷刷头在每个牙齿区域的停留时间。相较于现有技术，仅利用运动传感器确定牙刷刷头的定位信息，会导致定位精度差，而本发明获取智能电动牙刷的初始姿态信息、角速度和加速度，之后根据角速度和加速度确定智能牙刷的姿态数据，提高了姿态解算的精度，最后根据初始姿态信息和姿态数据确定刷头所处的牙齿区域，实现了精确定位智能牙刷刷头的所处区域。

本发明智能电动牙刷轨迹的定位系统的其他实施例或具体实现方式可参照上述各方法实施例，此处不再赘述。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。在列举了若干系统的单元权利要求中，这些系统中的若干个可以是通过同一个硬件项来具体体现。词语第一、第二、以及第三等的使用不表示任何顺序，可将这些词语解释为名称。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如只读存储器镜像(Read Only Memory image，ROM)/随机存取存储器(Random AccessMemory，RAM)、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种智能电动牙刷轨迹的定位方法，其特征在于，所述智能电动牙刷轨迹的定位方法包括以下步骤：

在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度；

对所述角速度进行姿态计算，获得第一姿态四元数；

根据所述第一姿态四元数和所述加速度确定第二姿态四元数；

对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计所述智能牙刷刷头在每个牙齿区域的停留时间。

2.如权利要求1所述的方法，其特征在于，所述对所述角速度进行姿态计算，获得第一姿态四元数的步骤，包括：

通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀i+q₁j+q₂k+q₃

式中，q₀、q₁、q₂、q₃为姿态四元数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

3.如权利要求2所述的方法，其特征在于，所述根据所述第一姿态四元数和所述加速度确定第二姿态四元数的步骤，包括：

根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

4.如权利要求3所述的方法，其特征在于，所述对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据的步骤，包括：

通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

5.如权利要求1所述的方法，其特征在于，所述根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域的步骤之后，还包括：

判断所述智能电动牙刷的工作状态是否为停止状态；

在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的所述智能电动牙刷刷头在每个牙齿区域的停留时间；

其中，所述牙齿区域为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面。

6.一种智能电动牙刷轨迹的定位系统，其特征在于，所述智能电动牙刷轨迹的定位系统包括：

传感器数据获取模块，用于在智能电动牙刷的工作状态为启动状态时，获取所述智能电动牙刷的初始姿态信息、角速度和加速度；

第一四元数计算模块，用于对所述角速度进行姿态计算，获得第一姿态四元数；

第二四元数确定模块，用于根据所述第一姿态四元数和所述加速度确定第二姿态四元数；

四元数互补融合模块，用于对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

定位模块，用于根据所述初始姿态信息和所述姿态数据确定所述智能牙刷刷头所处的牙齿区域，实时在线统计所述智能牙刷刷头在每个牙齿区域的停留时间。

7.如权利要求6所述的系统，其特征在于，所述第一四元数计算模块，用于通过预设四元数微分公式对所述角速度进行姿态计算，获得第一姿态四元数；

所述预设四元数微分公式为：

q₀＝0.5*(-q₁*g_x-q₂*g_y-q₃*g_z)

q₁＝0.5*(q₀*g_x+q₂*g_y-q₃*g_y)

q₂＝0.5*(q₀*g_x-q₁*g_z+q₃*g_x)

q₃＝0.5*(q₀*g_x+q₁*g_y-q₂*g_x)

q＝q₀i+q₁j+q₂k+q₃

式中，q₀、q₁、q₂、q₃为姿态四元数系数，g_x、g_y、g_z为角速度分量，q为第一姿态四元数，i、j和k为虚部单位。

8.如权利要求7所述的系统，其特征在于，所述第二四元数确定模块，用于根据所述第一姿态四元数和所述加速度，通过预设梯度下降公式计算第二姿态四元数；

所述预设梯度下降公式为：

式中，s为第二姿态四元数，a_x、a_y、a_z为加速度分量。

9.如权利要求8所述的系统，其特征在于，所述四元数互补融合模块，用于通过预设融合公式对所述第一姿态四元数及所述第二姿态四元数进行融合处理，获得所述智能电动牙刷的姿态数据；

所述预设融合公式为：

q_f＝αs+(1-α)q

式中，q_f为智能电动牙刷的姿态数据，α为自然数。

10.如权利要求6所述的系统，其特征在于，所述系统还包括，运动轨迹生成模块；

所述运动轨迹生成模块，用于判断所述智能电动牙刷的工作状态是否为停止状态；

所述运动轨迹生成模块，还用于在所述智能电动牙刷的状态为停止状态时，根据所述牙齿区域生成智能电动牙刷运动轨迹路线，并根据口腔模型，离线配准牙刷运动轨迹路线，获得修正的所述智能电动牙刷刷头在每个牙齿区域的停留时间；

其中，所述牙齿区域为左边外侧面、左边下咬合面、左边下内侧面、左边上咬合面、左边上内侧面、中间外侧面、中间下内侧面、中间上内侧面、右边外侧面、右边下咬合面、右边下内侧面、右边上咬合面、右边上内侧面。

Images