[发明专利]一种连续动态测量关节活动角度的电路及其方法

权利要求书

1.一种连续动态测量关节活动角度的电路，包括蓝牙WIFI主控模块，九轴惯性传感器模块、稳压电路模块和电源输入模块，其特征在于：

所述蓝牙WIFI主控模块通过I²C总线和九轴惯性传感器模块通信连接；

所述稳压电路模块和所述电源输入模块相连接，且稳压电路模块输出分别与蓝牙WIFI主控模块、低速高精度磁力计、高速度低精度磁力计的电源相连；

所述九轴惯性传感器模块包括传感器芯片、低速高精度磁力计和高速度低精度磁力计；

所述蓝牙WIFI主控模块包括主控芯片，且主控芯片为Wi-Fi和蓝牙双模的单芯片。

2.根据权利要求1所述的一种连续动态测量关节活动角度的电路，其特征在于：所述电源输入模块包括磁珠和电压输入接口，且电压输入接口选择micro USB。

3.根据权利要求1所述的一种连续动态测量关节活动角度的电路，其特征在于：所述主控芯片和传感器芯片通信连接。

4.根据权利要求1所述的一种连续动态测量关节活动角度的电路，其特征在于：所述电源输入模块的输出端与稳压电路模块的输入端相互连接，且稳压电路模块包括稳压芯片；

电源输入模块与外界的供电源相互连接，电源输入模块的电压输入稳压芯片的P1口，且稳压芯片设置有插槽，并且插槽选择micro USB。

5.根据权利要求1所述的一种连续动态测量关节活动角度的电路，其特征在于：所述稳压芯片的引脚1与电源输入模块的引脚1相连接，且稳压芯片的输出端与磁珠的输入端相互连接，所述磁珠的输出端分别与主控芯片、低速高精度磁力计和高速度低精度磁力计相互连接。

6.一种连续动态测量关节活动角度的方法，其特征在于：所述方法包括以下：

步骤S1，将两个测量关节复合运动角度的电路绑在关节两侧的肢体上，然后将采集到的角速度、加速度和磁力数据代入madgwick算法中，求得四元数q1(w1,x1,y1,z1)和q2(w2,x2,y2,z2)；

步骤S2，在两个测量关节复合运动角度的电路上分别定义三个矢量X(0,1,0,0)、Y(0,0,1,0)和Z(0,0,0,1)；

步骤S3，分别计算在两个测量关节复合运动角度的电路上的三个矢量经过四元数变换后的矢量：

计算方法是Q＝qPq-1，其中Q是经过四元数变换后的矢量，q是四元数，P是被转换的矢量，q-1是四元数的共轭；最终得到X1,Y1,Z1和X2,Y2,Z2；

步骤S4，分别将X1,Y1,Z1和X2,Y2,Z2做差得到X`,Y`,Z`。

7.根据权利要求6所述的一种连续动态测量关节活动角度的方法，其特征在于：所述步骤S4之后，所述方法还包括：

步骤S5，根据第四步得到的X`,Y`,Z`提出第一个元素后得到三个矢量，通过以下计算得到物体的俯仰角、横滚角和偏航角：

求俯仰角：选择x轴矢量x`(xx`,xy`,xz`)，将xz`除以1，求得俯仰角的正弦值，然后求正弦值的逆求得俯仰角值，即：

sinx＝xz`/1；

pitch＝arcsinx×180/π；

求偏航角：选择x轴矢量x`(xx`,xy`,xz`)，将xy`坐标除以1，求得偏航角的正弦值，然后求正弦值的逆求得偏航角值，即：

sinz＝xy`/1；

yaw＝arcsinz×180/π；

求横滚角：选择y轴矢量y`(yx`,yy`,yz`)，将yz`坐标除以1，求得横滚角的正弦值，然后求正弦值的逆求得横滚角值，即：

siny＝yz`/1；

roll＝arcsiny×180/π。

8.根据权利要求7所述的一种连续动态测量关节活动角度的方法，其特征在于：所述步骤S5中，其中算得到X1，Y1，Z1和X2，Y2，Z2的计算过程为：

首先求得四元数q(w,x,y,z)的共轭四元数q-1，即：

mould＝w*w+x*x+y*y+z*z；

q-1＝(w/mould,-x/mould,-y/mould,-z/mould)；

第二步计算旋转后的三个四元数，其中四元数的叉乘计算公式为：

q×x＝((wi*xi+wx*xi+wy*xz-wz*xy)；

(wi*xy-wx*xz+wy*xi+wz*xx)；

(wi*xz+wx*xy-wy*xx+wz*xi)；

(wi*xi-wx*xx-wy*xy-wz*xz))；

因此，根据X最终可以求得X1＝q×X×q-1得到变换后的物体x轴四元数X1(xw`，xx`，xy`，xz`)，同理，可求得Y1(yi`，yx`，yy`，yz`)和Z1(zi`，zx`，xy`，zz`)。