[发明专利]一种姿态传感器及姿态更新方法

摘要

本发明公开了一种姿态传感器及姿态更新方法，它包括外壳、3个测距传感器、陀螺仪、加速度计、磁力计、WIFI模块、CPU、电源模块。陀螺仪和加速度计又称惯性传感器，与磁力计融合，常用于导航系统的姿态更新。CPU由电源模块供电，CPU连接WIFI模块、惯性传感器、磁力计和3个测距传感器，将传感器测量的角速度、加速度、磁场强度、距离信息通过WIFI模块传送至计算机。通过姿态更新方法融合角速度、加速度、磁场强度和距离数据对姿态进行更新。本发明结构简单，成本低廉，适合对人体下肢姿态的长时间测量，其姿态更新方法易于扩展，且有效地解决了陀螺仪和加速度计的漂移问题。

主权项

1.一种姿态传感器的姿态更新方法，所述姿态传感器包括外壳(1)、第一测距传感器(2)、惯性传感器(3)、磁力计(6)、第二测距传感器(7)、电源模块(8)、CPU(9)、WIFI模块(10)、第三测距传感器(11)，所述惯性传感器(3)由陀螺仪(4)和加速度计(5)组成；第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)水平安装于外壳(1)外部，且第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)的安装位置处于同一水平面内；惯性传感器(3)、磁力计(6)、电源模块(8)、CPU(9)、WIFI模块(10)均置于外壳(1)内部；CPU(9)由电源模块(8)供电，CPU(9)通过总线分别连接第一测距传感器(2)、惯性传感器(3)、磁力计(6)、第二测距传感器(7)、WIFI模块(10)和第三测距传感器(11)，第一测距传感器(2)、第二测距传感器(7)和第三测距传感器(11)测量自身距离地面的距离信息，陀螺仪(4)测量姿态传感器的角速度，加速度计(5)测量姿态传感器的加速度，磁力计(6)测量姿态传感器的周边环境的磁场强度，CPU(9)将采集的距离信息、角速度、加速度、磁场强度信息打包后通过WIFI模块(10)传送至外部计算机；其特征在于，包括以下步骤：(1)将姿态传感器绑定在被测下肢上，使得3个测距传感器的测量探头朝向地面，能够测量它们与地面的距离信息，姿态传感器开始测量，得到角速度、加速度、磁场强度和3个测距传感器的距离信息；(2)推导和建立融合角速度、加速度、磁场强度和距离信息的卡尔曼滤波算法；该步骤包括以下两个子步骤；(2.1)状态向量X的预测更新；确定状态向量X＝[q p v]^T，其中，q为四元素，p为姿态传感器的位移，v为姿态传感器的速度，其状态方程用于状态向量X的预测更新：X_k/k‑1＝F·X_k‑1+u_k‑1其中，X_k/k‑1为k时刻预测更新后的状态变量，X_k‑1为k‑1时刻的状态变量，u为输入量，u_k‑1为k‑1时刻的输入量，I表示单位矩阵，F为状态方程系数矩阵：其中，y_A＝[y_A,x y_A,y y_A,z]^T为加速度计(5)测量得到的加速度信息，以右下角小标号x、y、z分别表示向量在x轴、y轴、z轴上的分量，以右下角小标号A代表加速度计，y_A,k‑1为k‑1时刻测量得到的加速度，为重力加速度的值，为姿态传感器的姿态矩阵，T为采样时间，Φ为四元素q的系数矩阵：是陀螺仪(4)测量得到的三维角速度；(2.2)状态向量X的量测更新；用量测变量Z对预测更新后的状态变量X_k/k‑1进行量测更新，即：X_k＝X_k/k‑1+K_k·(Z‑H·X_k/k‑1)其中，X_k为k时刻的量测更新后的状态变量，K_k为滤波增益，量测向量Z＝[y_A y_M d₁ d₂ d₃ d₂₁ d₃₂ d₁₃]^T，量测方程系数矩阵H为：其中，y_A＝[y_A,x y_A,y y_A,z]^T为加速度计(5)测量得到的加速度信息，y_M＝[y_M,x y_M,y y_M,z]^T为磁力计(6)测量得到的磁场强度信息，d₁、d₂、d₃分别为第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)测量得到的距离信息，记测距传感器测量差值为d_ji＝d_j‑d_i(i,j＝1,2,3)，d₂₁、d₃₂、d₁₃分别为第二测距传感器(7)与第一测距传感器(2)、第三测距传感器(11)与第二测距传感器(7)、第一测距传感器(2)与第三测距传感器(11)的距离差值信息；H_A(k)为加速度y_A的系数矩阵：其中，为重力加速度的三维向量值，q＝[q₀ q₁ q₂ q₃]^T为四元素；H_M(k)为磁场强度y_M的系数矩阵：其中，为地磁场强度的三维向量值；记分别为第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)到惯性传感器(3)的空间距离向量，记向量为上述空间距离向量的统一表达形式，记测距传感器测量的距离信息统一表达为d_i(i＝1,2,3)，分别为d₁、d₂、d₃的系数矩阵，则距离信息d_i的系数矩阵统一表达形式为：记P₁、P₂、P₃分别为第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)的空间位置，以表示第一测距传感器(2)到第二测距传感器(7)的空间向量，以表示第二测距传感器(7)到第三测距传感器(11)的空间向量，以表示第三测距传感器(11)到第一测距传感器(2)的空间向量，记测距传感器测量差值信息为d_ji＝d_j‑d_i(i,j＝1,2,3)，测距传感器相互位置的空间向量为分别为测距传感器测量差值信息d₂₁、d₃₂、d₁₃的系数矩阵，它们的统一表达式为：(3)根据卡尔曼滤波算法计算得到四元素；该步骤包括以下子步骤：(3.1)使用陀螺仪(4)测量得到的三维角速度信息和加速度计(5)测量得到的加速度信息对状态变量X预测更新；(3.2)使用加速度计(5)测量得到的加速度信息、磁力计(6)测量得到的磁场强度信息、第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)测量得到的距离信息、第二测距传感器(7)与第一测距传感器(2)、第三测距传感器(11)与第二测距传感器(7)、第一测距传感器(2)与第三测距传感器(11)的距离差值信息对状态变量X量测更新；状态变量X包含四元素q；(4)使用四元素计算得到姿态传感器的姿态矩阵和姿态角；根据四元素q＝[q₀ q₁ q₂ q₃]^T计算姿态传感器的姿态矩阵为：则根据该姿态矩阵可以计算姿态传感器的姿态角，姿态传感器的俯仰角θ、横滚角γ和方位角Ψ确定如下：θ＝arcsinT₃₂