Summary of the invention
The object of the invention is to, for the problems referred to above, propose a kind of toothbrush that detects at cavity interior shift in position algorithm, to realize the clear advantage grasping cavity interior.
For achieving the above object, the technical solution used in the present invention is:
A kind of toothbrush that detects, at cavity interior shift in position algorithm, comprises
Six axle sensors are used to obtain the step of toothbrush at the acceleration of intraoral 3 axles and the angular velocity of 3 axles respectively;
Integration computing is carried out to the acceleration of 3 axles of above-mentioned acquisition and the angular velocity of 3 axles and obtains hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then draw the step of Eulerian angle by mean filter;
And Eulerian angle are sent to client, client captures the step of current toothbrush at mouth intracavitary locations according to the Eulerian angle obtained.
Preferably, integration computing is carried out to the acceleration of 3 axles of above-mentioned acquisition and the angular velocity of 3 axles and obtains hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then show that the step of Eulerian angle specifically comprises by mean filter:
The acceleration of 3 axles is carried out to the step of acceleration of gravity normalized;
Extract the step of the weight component in the equivalent cosine matrix of hypercomplex number;
The acceleration of 3 axles after degree of will speed up normalized and the weight component of extraction carry out the step that vector cross product calculates attitude error;
Attitude error is carried out to the step of integral and calculating;
Carry out complementary filter, and by the compensation of attitude error after above-mentioned integral and calculating on angular velocity, the step of correction angle rate integrating skew;
Single order runge kutta method is utilized to upgrade hypercomplex number, and to the step that the hypercomplex number after upgrading is normalized;
And
Hypercomplex number after above-mentioned normalized is converted to the step of Eulerian angle.
Simultaneously technical solution of the present invention also discloses and a kind ofly detects the system of toothbrush in cavity interior shift in position, comprises six axle sensors, data processor and the client that are arranged in toothbrush;
Described six axle sensors: for obtaining the acceleration of toothbrush at intraoral 3 axles and the angular velocity of 3 axles;
Described data processor: carry out integration computing for the acceleration of 3 axles to above-mentioned acquisition and the angular velocity of 3 axles and obtain hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then draw Eulerian angle by mean filter;
Described client: for receiving Eulerian angle data, thus catch current toothbrush at mouth intracavitary locations, and toothbrush is exported in a graphical form at mouth intracavitary locations and change in location.
Preferably, the data of described six axle sensors by bluetooth data transmission to data processor.
Technical scheme of the present invention has following beneficial effect:
Technical scheme of the present invention, by the algorithm monitors toothbrush of six axle sensor modules and technical solution of the present invention in cavity interior position change conditions constantly, can capture each action of toothbrush accurately, thus user better can grasp the situation of oneself cavity interior.
Below by drawings and Examples, technical scheme of the present invention is described in further detail.
Embodiment
Below in conjunction with accompanying drawing, the preferred embodiments of the present invention are described, should be appreciated that preferred embodiment described herein is only for instruction and explanation of the present invention, is not intended to limit the present invention.
As shown in Figure 1, a kind of toothbrush that detects, at cavity interior shift in position algorithm, comprises
Six axle sensors are used to obtain the step of toothbrush at the acceleration of intraoral 3 axles and the angular velocity of 3 axles respectively;
Integration computing is carried out to the acceleration of 3 axles of above-mentioned acquisition and the angular velocity of 3 axles and obtains hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then draw the step of Eulerian angle by mean filter;
And Eulerian angle are sent to client, client captures the step of current toothbrush at mouth intracavitary locations according to the Eulerian angle obtained.
Preferably, integration computing is carried out to the acceleration of 3 axles of above-mentioned acquisition and the angular velocity of 3 axles and obtains hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then show that the step of Eulerian angle specifically comprises by mean filter:
The acceleration of 3 axles is carried out to the step of acceleration of gravity normalized;
Extract the step of the weight component in the equivalent cosine matrix of hypercomplex number;
The acceleration of 3 axles after degree of will speed up normalized and the weight component of extraction carry out the step that vector cross product calculates attitude error;
Attitude error is carried out to the step of integral and calculating;
Carry out complementary filter, and by the compensation of attitude error after above-mentioned integral and calculating on angular velocity, the step of correction angle rate integrating skew;
Single order runge kutta method is utilized to upgrade hypercomplex number, and to the step that the hypercomplex number after upgrading is normalized;
And
Hypercomplex number after above-mentioned normalized is converted to the step of Eulerian angle.
As shown in Figure 2, a kind ofly detect the system of toothbrush in cavity interior shift in position, comprise six axle sensors, data processor and the client that are arranged in toothbrush;
Six axle sensors: for obtaining the acceleration of toothbrush at intraoral 3 axles and the angular velocity of 3 axles;
Data processor: carry out integration computing for the acceleration of 3 axles to above-mentioned acquisition and the angular velocity of 3 axles and obtain hypercomplex number, and infinitesimal analysis process is carried out to the hypercomplex number obtained, and then draw Eulerian angle by mean filter;
Client: for receiving Eulerian angle data, thus catch current toothbrush at mouth intracavitary locations, and toothbrush is exported in a graphical form at mouth intracavitary locations and change in location.
Wherein, the data of six axle sensors by bluetooth data transmission to data processor.
Client is the app installed within hardware, and hardware can be the smart machines such as smart mobile phone, flat board or bracelet.Toothbrush can select oracleen Intelligent toothbrush.
Technical scheme of the present invention can monitor toothbrush constantly in cavity interior shift in position method, so that can be more clear, people are made to observe oneself oral cavity and situations of teeth intuitively, this algorithm adopts six axle modules and Bluetooth data transfer, the current location of toothbrush in human oral cavity can be embodied more accurately, the acceleration of 3 axles is obtained respectively in six axle sensors, with the angular velocity of 3 axles, take after 6 axles divide other data, be integrated into row operation and obtain hypercomplex number, hypercomplex number is being carried out infinitesimal analysis process, each position data accurately of toothbrush translation and rotation can be obtained again by mean filter (disturbance fluctuation of each gear of toothbrush being filtered), this algorithm is used in the exploitation of mobile terminal, people can be made to know more intuitively in patterned mode current in the some positions of brush, and can according to the tooth-brushing habit of user for it formulates better toothbrushing regimen.
For existing technical matters, (space is imitated, the bit error rate is high, reaction velocity is slow) this algorithm utilizes six axles and enables bluetooth 4.0 transmission technology (every 50ms accepts to send out the speed of 15 bytes) by x, y, the algorithm process of z 3-axis acceleration and three axis angular rates, in being developed by this algorithm effect mobile terminal, can allow user more directly perceived in patterned mode, brush teeth clearly, and the situation that user brushes teeth for a long time can be shown.
Client and corresponding app, the graphical interfaces that can provide, can obtain the change of toothbrush diverse location state wherein.According to the algorithm of technical solution of the present invention, the change of 3d physical model in the state of interior volume and the monitoring of position can also be made.
Below utilize the code segment realizing algorithm of the present invention:
(1) data are taken data by Bluetooth transmission; Totally 12 bytes.Be 16 binary data
If the 3-axis acceleration read 16 system is respectively
acc_x_h,acc_x_l,acc_y_h,acc_y_l,acc_z_h,acc_z_l
The angular velocity of three axles read is
gyro_x_h,gyro_x_l,gyro_y_h,gyro_y_l,gyro_z_h,gyro_z,l
(2) selected datum mark O coordinate center (namely toothbrush position in oral cavity) on testee, builds three-dimensional system of coordinate O-XYZ, and setting along the six direction X forward of coordinate axis, X negative sense, Y forward, Y negative sense, Z forward, Z negative sense is reference direction,
Respectively x, y, z 3-axis acceleration is merged.
Formula 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
Formula 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) hypercomplex number is defined, such rule is had: hypercomplex number is all made up of three imaginary units i, j, k on real add between hypercomplex number, and they have following relation: i^2=j^2=k^2=-1, i^0=j^0=k^0=1, each hypercomplex number is 1, the linear combination of i, j and k, namely be that hypercomplex number generally can be expressed as a+bk+cj+di, wherein a, b, c, d are real numbers.
Formula 3:
If 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) acceleration of gravity normalization can be obtained according to the value in the acceleration direction obtained in formula 1, namely
Formula 4:
If norm=(Acc_x*Acc_x+Acc_y*Acc_y+Acc_z*Acc_z) extracts square root
If:
ax＝Acc_x/norm
ay＝Acc_y/norm
az＝Acc_z/norm
(5) weight component in the equivalent cosine matrix of hypercomplex number is extracted
Formula 5:
If:
vx=2*(q1q3-q0q2);
vy=2*(q0q1+q2q3);
vz=1-2*(q1*q1+q2*q2);
(6) vector cross product draws attitude error
ex=(ay*vz-az*vy);
ey=(az*vx-ax*vz);
ez=(ax*vy-ay*vx);
(7) integration is carried out to error
If ki=0.001//this is gyrostatic deviation
exInt=ex*Ki
eyInt=ey*Ki
ezInt=ez*Ki
(8) complementary filter, compensation of attitude error is on angular velocity, and correction angle rate integrating drifts about
gx+=Kp*ex+exInt;
gy+=Kp*ey+eyInt;
gz+=Kp*ez+ezInt;
(9) single order runge kutta method upgrades hypercomplex number
Q0+=(-q1*gx-q2*gy-q3*gz) * 0.5*deltaT; //delatT transfer rate
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) hypercomplex number normalization
norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
q0=q0/norm;
q1=q1/norm;
q2=q2/norm;
q3=q3/norm;
(11) hypercomplex number turns Eulerian angle
If RAD_TO_DEG=1/ is π;
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。
After obtaining attitude, by data (crab angle, the angle of pitch, roll angle) be sent in app, app, by the position of Eulerian angle, can get the state of toothbrush in human oral cavity inside accurately, so just can capture current position of brushing teeth, user can be allowed more to understand the situations of teeth of oneself.
Last it is noted that the foregoing is only the preferred embodiments of the present invention, be not limited to the present invention, although with reference to previous embodiment to invention has been detailed description, for a person skilled in the art, it still can be modified to the technical scheme described in foregoing embodiments, or carries out equivalent replacement to wherein portion of techniques feature.Within the spirit and principles in the present invention all, any amendment done, equivalent replacement, improvement etc., all should be included within protection scope of the present invention.