CN105466422A

CN105466422A - 一种检测牙刷在口腔内部位置变动算法

Info

Publication number: CN105466422A
Application number: CN201510871269.1A
Authority: CN
Inventors: 苗得雨
Original assignee: Oracleen Technology Co
Current assignee: Oracleen Technology Co
Priority date: 2015-12-02
Filing date: 2015-12-02
Publication date: 2016-04-06

Abstract

本发明公开了一种检测牙刷在口腔内部位置变动算法，使用六轴传感器分别获取牙刷在口腔内的3个轴的加速度和3个轴的角速度；对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角；以及将欧拉角发送至客户端，客户端根据获取的欧拉角捕捉到当前牙刷在口腔内位置。可以准确的捕获到牙刷的每一个动作，从而用户可以更好的掌握自己口腔内部的情况。

Description

一种检测牙刷在口腔内部位置变动算法

技术领域

本发明涉及数据处理领域，具体地，涉及一种检测牙刷在口腔内部位置变动算法。

背景技术

基于口腔业内的种种问题，用户无法能够清晰的了解自己口腔内部情况，并且无法在牙齿出现问题的情况下找到理想的解决方案。

发明内容

本发明的目的在于，针对上述问题，提出一种检测牙刷在口腔内部位置变动算法，以实现清楚掌握口腔内部的优点。

为实现上述目的，本发明采用的技术方案是：

一种检测牙刷在口腔内部位置变动算法，包括

使用六轴传感器分别获取牙刷在口腔内的3个轴的加速度和3个轴的角速度的步骤；

对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角的步骤；

以及将欧拉角发送至客户端，客户端根据获取的欧拉角捕捉到当前牙刷在口腔内位置的步骤。

优选的，对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角的步骤具体包括：

对3个轴的加速度进行重力加速度归一化处理的步骤；

提取四元数的等效余弦矩阵中的重力分量的步骤；

将加速度归一化处理后的3个轴的加速度与提取的重力分量进行进行向量叉积计算得出姿态误差的步骤；

对姿态误差进行积分计算的步骤；

进行互补滤波，并将上述积分计算后的姿态误差补偿到角速度上，修正角速度积分偏移的步骤；

利用一阶龙格库塔法更新四元数，并对更新后的四元数进行归一化处理的步骤；

以及

将上述归一化处理后的四元数转换为欧拉角的步骤。

同时本发明技术方案还公开一种检测牙刷在口腔内部位置变动的系统，包括设置在牙刷中的六轴传感器、数据处理器和客户端；

所述六轴传感器：用于获取牙刷在口腔内的3个轴的加速度和3个轴的角速度；

所述数据处理器：用于对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角；

所述客户端：用于接收欧拉角数据，从而捕捉当前牙刷在口腔内位置，并将牙刷在口腔内位置以及位置变化以图形的形式输出。

优选的，所述六轴传感器的数据通过蓝牙传输数据至数据处理器。

本发明的技术方案具有以下有益效果：

本发明的技术方案，通过六轴传感器模块以及本发明技术方案的算法监测牙刷在口腔内部位置时时变动情况，可以准确的捕获到牙刷的每一个动作，从而用户可以更好的掌握自己口腔内部的情况。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明实施例所述的检测牙刷在口腔内部位置变动算法的流程图；

图2为本发明实施例所述的检测牙刷在口腔内部位置变动的系统的原理框图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

如图1所示，一种检测牙刷在口腔内部位置变动算法，包括

对3个轴的加速度进行重力加速度归一化处理的步骤；

提取四元数的等效余弦矩阵中的重力分量的步骤；

对姿态误差进行积分计算的步骤；

以及

将上述归一化处理后的四元数转换为欧拉角的步骤。

如图2所示，一种检测牙刷在口腔内部位置变动的系统，包括设置在牙刷中的六轴传感器、数据处理器和客户端；

六轴传感器：用于获取牙刷在口腔内的3个轴的加速度和3个轴的角速度；

数据处理器：用于对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角；

客户端：用于接收欧拉角数据，从而捕捉当前牙刷在口腔内位置，并将牙刷在口腔内位置以及位置变化以图形的形式输出。

其中，六轴传感器的数据通过蓝牙传输数据至数据处理器。

客户端为在硬件中安装的app，硬件可以为智能手机、平板或手环等智能设备。牙刷可以选用oracleen智能牙刷。

本发明的技术方案可以时时监测牙刷在口腔内部位置变动法，以便能够更加清晰，直观的使人们观察自己的口腔与牙齿状况，本算法采用六轴模块与蓝牙数据传输，能够更加精确的体现牙刷在人体口腔中的当前位置，在六轴传感器中分别获得3个轴的加速度，和3个轴的角速度，拿到6轴分别的数据之后，整合进行运算得到四元数，将四元数在进行微积分处理，再通过均值滤波（将牙刷各个档位的干扰波动过滤）可以得到牙刷平移与转动的各个位置准确的数据，将此算法用在移动端开发中，以图形化的方式可以使人们更加直观的知道当前在刷某一个位置，并且可以根据用户的刷牙习惯为其制定更好的刷牙方案。

针对现有的技术问题（空间效，误码率高，反应速度慢）本算法利用六轴并启用蓝牙4.0传输技术（每50ms接受发15字节的速率）通过对x，y，z三轴加速度和三轴角速度的算法处理，将此算法作用移动终端开发中，可以让用户以图形化的方式更加直观，清晰的刷牙，以及可以放映用户长时间刷牙的状况。

客户端及相应的app，可以提供的图形界面，在其中可以得到牙刷不同位置状态的改变。根据本发明技术方案的算法，还可以做出3d物理模型在空间内部的状态的改变和位置的监测。

以下是利用实现本发明算法的部分编码：

（1）将数据通过蓝牙传输拿到数据；共12个字节。均为16进制数据

设读到的三轴加速度16进制分别为

acc_x_h,acc_x_l,acc_y_h,acc_y_l,acc_z_h,acc_z_l

读到的三轴的角速度为

gyro_x_h,gyro_x_l,gyro_y_h,gyro_y_l,gyro_z_h,gyro_z,l

（2）于被测物体上选定测量基准点O坐标中心（也就是口腔内牙刷位置），构建三维坐标系O-XYZ,设定沿坐标轴的六个方向X正向、X负向、Y正向、Y负向、Z正向、Z负向为基准方向,

分别将x，y，z三轴加速度进行合并。

式1:

Acc_x=(acc_x_h＊256)+acc_x_l

Acc_y=(acc_y_h＊256)+acc_y_l

Acc_z=(acc_z_h＊256)+acc_z_l

式2:

gyro_x=(gyro_x_h＊256)+gyro_x_l

gyro_y=(gyro_y_h＊256)+gyro_y_l

gyro_z=(gyro_z_h＊256)+gyro_z_l

（3）定义四元数，四元数之间有这样的规则：四元数都是由实数加上三个虚数单位i、j、k组成，而且它们有如下的关系：i^2=j^2=k^2=-1，i^0=j^0=k^0=1,每个四元数都是1、i、j和k的线性组合，即是四元数一般可表示为a+bk+cj+di，其中a、b、c、d是实数。

式3:

设q0＝1，q1＝0，q2＝0，q3＝0

q0q0＝q0＊q0

q0q1＝q0＊q1

q0q2＝q0＊q2

q0q3＝q0＊q3

q1q1＝q1＊q1

q1q2＝q1＊q2

q1q3＝q1＊q3

q2q2＝q2＊q2

q2q3＝q2＊q3

q3q3＝q3＊q3

（4）根据式1中得到的加速度方向的值可以求出重力加速度归一化，即

式4:

设norm＝(Acc_x＊Acc_x+Acc_y＊Acc_y+Acc_z＊Acc_z)开平方

设：

ax＝Acc_x/norm

ay＝Acc_y/norm

az＝Acc_z/norm

(5)提取四元数的等效余弦矩阵中的重力分量

式5:

设：

vx=2*(q1q3-q0q2);

vy=2*(q0q1+q2q3);

vz=1-2*(q1*q1+q2*q2);

(6)向量叉积得出姿态误差

ex=(ay*vz-az*vy);

ey=(az*vx-ax*vz);

ez=(ax*vy-ay*vx);

(7)对误差进行积分

设ki＝0.001//此为陀螺仪的偏差

exInt=ex*Ki

eyInt=ey*Ki

ezInt=ez*Ki

(8)互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移

gx+=Kp*ex+exInt;

gy+=Kp*ey+eyInt;

gz+=Kp*ez+ezInt;

(9)一阶龙格库塔法更新四元数

q0+=(-q1*gx-q2*gy-q3*gz)*0.5*deltaT;//delatT传输速率

q1+=(q0*gx+q2*gz-q3*gy)*0.5*deltaT;

q2+=(q0*gy-q1*gz+q3*gx)*0.5*deltaT;

q3+=(q0*gz+q1*gy-q2*gx)*0.5*deltaT;

(10)四元数归一化

norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);

q0=q0/norm;

q1=q1/norm;

q2=q2/norm;

q3=q3/norm;

(11)四元数转欧拉角

设RAD_TO_DEG＝1/π；

GyroAngle.z=tan2(2.0f*(q1*q2-q0*q3),2.0f*(q1q1+q2q2)-1)*RAD_TO_DEG;//YAW；

GyroAngle.x=sin(-2*q1*q3+2*q0*q2)*RAD_TO_DEG;//pitch；

GyroAngle.y=tan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q+1)*RAD_TO_DEG;//roll。

得到姿态之后，将数据（偏航角，俯仰角，滚动角）发送到app内，app通过欧拉角的位置，可以准确的获取到牙刷在人体口腔内部的状态，这样就可以捕捉到当前刷牙的部位，可以让用户更了解自己的牙齿状况。

最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种检测牙刷在口腔内部位置变动算法，其特征在于，包括

2.根据权利要求1所述的检测牙刷在口腔内部位置变动算法，其特征在于，对上述获取的3个轴的加速度和3个轴的角速度进行整合运算得到四元数，并对得到的四元数进行微积分处理，然后再通过均值滤波从而得出欧拉角的步骤具体包括：

对3个轴的加速度进行重力加速度归一化处理的步骤；

提取四元数的等效余弦矩阵中的重力分量的步骤；

对姿态误差进行积分计算的步骤；

以及

将上述归一化处理后的四元数转换为欧拉角的步骤。

3.一种检测牙刷在口腔内部位置变动的系统，其特征在于，包括设置在牙刷中的六轴传感器、数据处理器和客户端；

4.根据权利要求3所述的检测牙刷在口腔内部位置变动的系统，其特征在于，所述六轴传感器的数据通过蓝牙传输数据至数据处理器。