本发明涉及姿态检测领域，具体而言，涉及一种姿态角获取方法及装置。
背景技术：
在飞行控制领域、穿戴式设备领域、人体姿态动作检测领域和机械运动控制领域，为了实现精准控制，姿态角、欧拉角的检测是必要的，现有技术常常通过图像识别、激光雷达数据识别来获取姿态角，但是在一些较为复杂环境，视觉障碍物常常造成姿态检测失效；另一种做法是通过在运动体上固定上检测传感器，即，捷联惯导方式，如陀螺仪(包括光电陀螺、机械陀螺和电子陀螺仪芯片等)，通过时间积分等方式获取姿态角，但是这种方法造成累计误差较大的缺点，高精度陀螺的体积较大造价较高，对于高速运动的运动体，这种方法响应慢、跟随效果差；还有一些现有技术采用姿态融合算法，但是这种算法计算复杂，难以达到实时响应的要求。
技术实现要素：
本发明为了克服上述技术问题中的至少一项，提供了基于扩展卡尔曼滤波的多传感器数据姿态融合算法及装置，具体地，本发明第一方面，提供了一种姿态获取方法，包括如下步骤：
s110、获取当前时刻角速度数据、加速度数据和地磁场强度数据；
s120、获取上一时刻系统状态量，根据当前时刻角速度数据和上一时刻系统状态量求取系统状态矩阵值；
s130、获取上一时刻系统误差协方差矩阵值，根据上一时刻系统误差协方差矩阵值和所述系统状态矩阵值获取当前时刻系统误差协方差矩阵值；
s140、根据所述当前时刻地磁场强度数据和上一时刻系统状态量，建立系统测量方程h，利用所述上一时刻系统状态量对所述系统测量方程求偏导，获取系统测量方程的雅克比矩阵值；
s150、根据所述当前时刻系统误差协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；
s160、根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量和所述系统测量方程获取当前时刻系统状态量，所述系统测量量表示为y＝(axayazmxmymz)t，其中，y表示系统测量量，ax、ay、az表示当前时刻加速度数据在芯片坐标系下x、y、z轴的分量，mx、my、mz表示当前时刻地磁场强度数据在芯片坐标系下x、y、z轴的分量；
s170、根据所述当前时刻系统状态量中的四元数获取姿态角。
本发明第二方面，提供了一种姿态获取控制器，包括外部数据缓存单元、内部数据缓存单元和与所述外部数据缓存单元、内部数据缓存单元连接的计算单元，所述外部数据缓存单元用于缓存当前时刻角速度数据、加速度数据和地磁场强度数据；所述内部数据缓存单元用于缓存上一时刻系统状态量和上一时刻系统误差协方差矩阵值，所述计算单元用于执行上述的姿态角获取方法获取姿态角。
本发明第三方面，提供了一种姿态角获取装置，包括地磁传感器、角速度传感器和加速度传感器，还包括上述控制器，所述控制器与所述地磁传感器、角速度传感器和加速度传感器连接。
本发明第四方面，提供了一种可穿戴设备，包括上述的姿态角获取装置，所述姿态角获取装置用于获取穿戴者骨骼的姿态角。
本发明中的方法及装置动态响应快，可以捕捉高于5hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好，用加速度与磁强数据校准姿态，实时计算陀螺仪的误差，可以在短时间(30s内)的姿态提取中只使用陀螺仪数据，从而摆脱复杂的姿态融合算法，适合磁场异常的环境使用。
附图说明
通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，在附图中：
图1为本发明一些实施例中的姿态角获取方法的流程示意图；
图2为本发明一些实施例中的姿态角获取方法的子步骤的流程示意图；
图3为本发明一些实施例中的姿态角获取方法的流程示意图；
图4为本发明一些实施例中的控制器系统结构图；
图5为本发明一些实施例中的姿态获取装置的系统结构图；
图6为本发明一些实施例中的用于测量脊椎姿态角的可穿戴设备示意图；
图7为本发明一些实施例中的用于测量手臂姿态角的可穿戴设备示意图；
图8为本发明一些实施例中的用于测量手姿态角的可穿戴设备示意图；
图9为本发明一些实施例中的姿态角获取方法中的导航坐标系的定义示意图；
图10为本发明一些实施例中的姿态角获取方法中的芯片坐标系的定义示意图；
图11为本发明一些实施例中的姿态角获取方法中的芯片坐标系的定义示意图。
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
本发明基于扩展卡尔曼滤波的思想，对姿态角的获取方法进行改进，改进后的算法利用每次迭代进行姿态角的输出，保证了每次获取姿态角的过程简化，时间成本小，动态跟随性好，误差也较小。具体实施方式如下描述。
实施例一
如图1所示，本发明的实施例提供了一种姿态角获取方法，
s110、获取当前时刻角速度数据、加速度数据和地磁场强度数据；
所述角速度数据可以通过陀螺仪或陀螺仪芯片进行采集，所述加速度数据可以通过加速度计或者加速度计芯片进行测量，所述地磁场强度数据可以通过磁力计或者磁强计芯片进行测量。所述加速度计测量获取的数据在导航坐标系下，始终朝向地球的地，经过反向处理指向地球天；所述磁力计测量获取的数据在导航坐标系下始终朝向地球的南，经过反向处理后，指向地球的北，如图9所示，为导航坐标系的定义，三个直角坐标轴包括指向地球东、地球北和地球天；如图10所示，为与所述陀螺仪芯片和加速度计固定的芯片坐标系，所述角速度数据、加速度数据从图10中的芯片坐标系中进行读取；如图11所示，为与所述磁力计固定的坐标系，所述地磁场强度数据从图11中的坐标系中读出。本发明中的所述姿态角即为图9坐标系中定义的航向角、横滚角和俯仰角。上述获取当前时刻角速度数据、加速度数据和地磁场强度数据的步骤包括：对传感器原始数据预处理，将加速度计采集的数据和磁力计采集的数据进行归一化处理，即，将其测量的三维向量转化为模为1的向量，作为加速度数据以及地磁场强度数据。
s120、获取上一时刻系统状态量，根据当前时刻角速度数据和上一时刻系统状态量求取系统状态矩阵值；
可以通过一阶龙格库塔更新四元数的方法求取系统状态矩阵，龙格库塔更新四元数的原理如下：
用采集到的角速度数据w，更新四元数qn，下标n表示n时刻的四元数，获得新的四元数qn+1，下标n+1表示n+1时刻的四元数，ωx、ωy、ωz表示x、y、z轴角速度。
这时有：qn+1＝f(qn)；建立系统方程f，表达如下：
对方程f求x的偏导数，得到矩阵方程f，
x表示系统状态量，所述系统状态量表示为x＝(q0q1q2q3δωxδωyδωz)t，q0、q1、q2、q3表示四元数，δωx、δωy、δωz表示系统计算的陀螺仪x、y、z轴角速度的误差，δt表示采样时间间隔；
s130、获取上一时刻系统误差协方差矩阵值，根据上一时刻系统误差协方差矩阵值和所述系统状态矩阵值获取当前时刻系统误差协方差矩阵值p；
具体地，可以通过下式进行求取，p＝f·p·f-1+q，等式左面的p表示当前时刻系统误差协方差矩阵值，等式右面的p表示上一时刻系统误差协方差矩阵值，为初始设定值，或者是上一次迭代计算留存的值；q表示过程激励噪声协方差，可以设定为：
s140、根据所述当前时刻地磁场强度数据和上一时刻系统状态量，建立系统测量方程h，利用所述上一时刻系统状态量对所述系统测量方程求偏导，即，对系统测量方程求取关于上一时刻系统状态量的偏导数，获取系统测量方程的雅克比矩阵值；
s150、根据所述当前时刻系统误差协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；
具体可采用下式进行计算，
s160、根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量y和所述系统测量方程获取当前时刻系统状态量，所述系统测量量表示为y＝(axayazmxmymz)t，其中，y表示系统测量量，ax、ay、az表示当前时刻加速度数据在芯片坐标系(图10)下x、y、z轴的分量，mx、my、mz表示当前时刻地磁场强度数据在芯片坐标系(图11)下x、y、z轴的分量；
具体可采用下式进行计算求取，x＝x+k(y-h)，等式左面的x表示当前时刻系统状态量，等式右面的x表示上一时刻系统状态量；
s170、根据所述当前时刻系统状态量中的四元数获取姿态角。
本发明中的姿态角获取方法通过将上一次执行时记录的中间状态量，迭代到当前的计算中，不需要每次都进行复杂的计算，仅将记录的中间状态量引入到计算过程中，简化了计算，保证了实时性和跟随性。本发明实施例中的系统测量量采用加速度传感器测量到的加速度数据，和地磁传感器测量的地磁场强度数据，由于加速度传感器所测量的方向始终指向地球地，地磁传感器测量的方向始终指向南，经过简单反向处理即可与东北天坐标系一致，这样使得系统测量量的整体方向与东北天导航坐标系一致，仅需简单的变换符号就能实现转换，简化了计算。
本发明中的方法，通过迭代对姿态角进行更新，具体地，还包括如下步骤：
s180、根据所述当前时刻系统状态量更新当前时刻系统误差协方差矩阵值；
具体地，采用如下的计算方式进行更新，p＝(i-k·h)·p·(i-k·h)t+k·r·kt，其中，式中，i表示单位矩阵，等式左面的p表示更新后的当前时刻系统误差协方差矩阵，等式右面的p表示当前时刻系统误差协方差矩阵。
当前计算时，当当前时刻为初始时刻时，所述上一时刻系统误差协方差矩阵值为初始设定值；当当前时刻不为初始时刻时，所述上一时刻系统误差协方差矩阵值为上一时刻执行所述姿态角获取方法中步骤s180所获取的当前时刻系统误差协方差矩阵值；
所述系统误差协方差矩阵的初始设定值，可以人为设定，例如可以设置为：
所述系统状态量表示为x＝(q0q1q2q3δωxδωyδωz)t，x表示系统状态量，q0、q1、q2、q3表示四元数，δωx、δωy、δωz表示系统计算的陀螺仪x、y、z轴角速度的误差；当当前时刻为初始时刻时，所述上一时刻系统状态量中的四元数为根据初始时刻获取的加速度数据和地磁场强度数据所获取的四元数，所述上一时刻系统状态量中的角速度误差为初始设定值，具体参照实施例二中的步骤，即，通过初始时刻的过加速度数据和地磁场强度数据建立姿态余弦矩阵，通过姿态余弦矩阵获取四元数；当当前时刻不为初始时刻时，所述上一时刻系统状态量为上一时刻执行所述姿态角获取方法中步骤s160所获取的当前时刻系统状态量；通过迭代计算当前时刻的姿态角，将每次迭代计算的姿态角都进行反馈利用，单次计算的计算量减小。
所述步骤s140包括，根据上一时刻系统状态量中的四元数、当前时刻地磁场强度数据，建立系统测量方程，利用所述上一时刻系统状态量对所述系统测量方程求偏导获取系统测量方程的雅克比矩阵值。所述系统测量方程表示为
根据上一时刻系统状态量中四元数获取姿态余弦矩阵
根据所述姿态余弦矩阵
将所述当前时刻地磁场强度数据
去除当前时刻地磁场强度数据在导航坐标系下的x轴分量，通过如下公式将地磁场强度数据转化到芯片坐标系中
根据所述第一向量h(1～3)和第二向量h(46)组合形成系统测量方程如下：
所述系统测量方程的雅克比矩阵表示为
本发明实施例的姿态角获取方法可以直接通过四元数计算姿态角，即将四元数转换成姿态余弦矩阵，然后通过姿态余弦矩阵获取姿态角，另一方面，本发明中实施例在最后通过四元数获取姿态角的同时，考虑到具体运用场景中的误差，例如引入穿戴误差矩阵，将从四元数获取到的姿态余弦矩阵与穿戴误差矩阵做运算，获得更新后的姿态余弦矩阵，进而获取姿态角，具体地，如图2所示，所述步骤s170包括：
s171、根据所述当前时刻系统状态量中的四元数获取第一姿态余弦矩阵c；
s172、根据所述第一姿态余弦矩阵c和穿戴误差矩阵ct获取第二姿态余弦矩阵c·ct；
s173、根据所述第二姿态余弦矩阵c·ct求解欧拉角，所述欧拉角为所述姿态角。
所述步骤s120中采用一阶龙格库塔方法建立系统状态方程，利用上一时刻系统状态量对所述系统状态方程求偏导获取所述系统状态矩阵值。
本发明中的方法及装置动态响应快，可以捕捉高于5hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好，用加速度与磁强数据校准姿态，实时计算陀螺仪的误差，可以在短时间(30s内)的姿态提取中只使用陀螺仪数据，从而摆脱复杂的姿态融合算法，适合磁场异常的环境使用。
需要说明的是，本发明中的当前时刻，即为当前姿态角计算时刻；上一时刻为，迭代计算中的上一次姿态角计算时刻；初始时刻为迭代计算中第一次姿态角计算时刻。
实施例二
如图3所示，本发明实施例提供了一种姿态角获取方法，基于卡尔曼滤波进行多传感器数据进行融合，算法动态响应快，可以捕捉高于5hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好，用加速度与磁强数据校准姿态，实时计算陀螺仪的误差，可以在短时间(30s内)的姿态提取中只使用陀螺仪数据，从而摆脱复杂的姿态融合算法，适合磁场异常的环境使用；所述姿态角获取方法的步骤如下所述。
进行角速度数据、加速度数据和地磁场强度数据采集，其中，获取到的传感器数据包含陀螺仪采集到的三个轴的角度数据ω＝(ωxωyωz)(芯片坐标系下)，加速度传感器采集到的加速度数据a＝(axayaz)，磁强计采集到的地磁场强度数据m＝(mxmymz)，多传感器数据融合算法解算姿态的具体方法如下：
a)由加速度数据a，地磁场强度数据m，获得传感器初始姿态矩阵cb
由初始姿态矩阵求解出初始姿态的航向角，俯仰角，横滚角(定义见图9)，以及相应的四元数向量qc，下标c表示初始时刻的四元数。
b)用采集到的角速度数据w，更新四元数qn，下标n表示n时刻的四元数，获得新的四元数qn+1，下标n+1表示n+1时刻的四元数，如下式所示：qn+1＝f(qn)
c)建立导航系统的系统方程f，
将δw与qn+1联合成一个向量x，作为状态量，其中，qn(x)表示n时刻的四元数中第x个量的值，t表示相邻两次陀螺仪采样时间间隔，wx表示本次陀螺仪采样的角速度值的x轴分量，wy表示本次陀螺仪采样的角速度值的y轴分量，wz表示本次陀螺仪采样的角速度值的z轴分量，δωx表示系统计算的陀螺仪x轴角速度的误差，δωy表示系统计算的陀螺仪y轴角速度的误差，δωz表示系统计算的陀螺仪z轴角速度的误差。
d)对方程f求x的偏导数，得到矩阵方程f，
e)建立系统测量量y＝[am]t
f)将地磁场数据和加速度数据转化到导航坐标系中，建立系统测量方程如下：
g)计算h方程组的对x的偏导数，得到矩阵方程h，
如图3所示，使用ekf(扩展卡尔曼滤波)方法对建立的方程进行迭代求解，获得最新的姿态余弦矩阵，用穿戴误差矩阵ct右乘姿态余弦矩阵，计算出欧拉角，完成一次角度数据计算。
过程如下：
利用p＝f·p·f-1+q计算系统误差协方差；
利用
利用x＝x+k(y-h)更新系统状态量；
由系统状态量中的四元数计算姿态余弦矩阵，再根据穿戴误差矩阵对姿态余弦矩阵进行修正，最后根据修正后的姿态余弦矩阵修正欧拉角。
穿戴误差矩阵ct通过传感器穿戴的初始化中进行求取，当传感器穿戴在身体上后，保持身体标准站姿(可以背贴墙站立)，传感器数据计算获得的姿态矩阵为t0，而身体处的当前实际姿态矩阵为ta，计算矩阵
本次计算获取完姿态角之后，利用p＝(i-k·h)·p·(i-k·h)t+k·r·kt对系统误差协方差进行更新；为下一次迭代做好准备。
本发明中的方法及装置动态响应快，可以捕捉高于5hz的人体动作，静态误差在±0.1°，对人体动作跟随效果好，用加速度与磁强数据校准姿态，实时计算陀螺仪的误差，可以在短时间(30s内)的姿态提取中只使用陀螺仪数据，从而摆脱复杂的姿态融合算法，适合磁场异常的环境使用，克服了穿戴误差。
实施例三
如图4所示，基于上述实施例一或者实施例二中姿态角获取方法，本发明实施例还提供了一种控制器200，包括外部数据缓存单元210、内部数据缓存单元220和与所述外部数据缓存单元210、内部数据缓存单元220连接的计算单元230，所述外部数据缓存单元210用于缓存当前时刻角速度数据、加速度数据和地磁场强度数据；所述内部数据缓存单元220用于缓存上一时刻系统状态量和上一时刻系统误差协方差矩阵值，所述计算单元用于执行上述的姿态角获取方法获取姿态角，这时上述的姿态角获取方法演变成程代码在所述计算单元230中进行计算。所述控制器200可以为cpu、mcu、mips、arm、dsp、fpga、asic或者其他控制器。
实施例四
如图5所示，基于上述实施例三中的控制器300，本发明实施例还提供了一种姿态角获取装置300，包括地磁传感器310、角速度传感器320和加速度传感器330，还包括上述控制器200，所述控制器200与所述地磁传感器310、角速度传感器320和加速度传感器330连接。具体地，所述地磁传感器310、角速度传感器320和加速度传感器330与所述外部数据缓存单元210进行连接。实际运用过程中，可以将所述地磁传感器310、角速度传感器320和加速度传感器330集成在一体，制作在同一pcb上，节省了占用空间，从而减小对被侧体的运动的妨碍。
具体地，所述地磁传感器310包括磁力计，所述角速度传感器320包括陀螺仪芯片，所述加速度传感器330包括加速度计。上述传感器可以采用传感器芯片，从而进行集成。
本发明实施例中姿态角获取装置300可以运用在飞行器、无人车、无人船等自主导航设备上，同时也可以运用在可穿戴设备上。
实施例五
基于上述实施例四中的姿态角获取装置300，本发明实施例还提供了一种可穿戴设备，包括上述的姿态角获取装置300，所述姿态角获取装置300用于获取穿戴者骨骼的姿态角。如图6所示，可将所述可穿戴设备400可为作用到人体脊椎上设备，如背背佳，用于测量脊椎姿态角，从而获取弯曲程度。如图7所示，所述可穿戴设备400可为作用到手臂上的臂袖，获取小臂与大臂的姿态；如图8所示，所述可穿戴设备400还可为数据手套，用于采集手指、手腕的姿态角；所述可穿戴设备还可为智能手表、智能眼镜、ar/vr眼镜等设备。
需要说明的是，可穿戴设备中的姿态角获取装置300中的控制器200，可以集成为一个，总体计算各个传感单元的姿态角，也可进行分布式设置，分别计算。
通过本发明实施例中的姿态获取装置，使得本发明实施例中的可穿戴设备反馈数据及时，跟随性好，误差小，计算快等优点。
在本发明中，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。