CN106989773A - 一种姿态传感器及姿态更新方法 - Google Patents

Abstract

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

Description

一种姿态传感器及姿态更新方法

技术领域

本发明涉及一种融合测距传感器阵列与陀螺仪、加速度计、磁力计的姿态更新方法，特别是针对步态分析中下肢姿态的测量，如脚、小腿等近地肢体的姿态。

背景技术

目前广泛使用的基于MEMS工艺的姿态传感器主要是惯性传感器(IMU：陀螺仪、加速度计)和磁力计，以InvenSense公司生产的MPU6050、MPU6500和MPU9150、MPU9250等型号的姿态传感器最为常用，前两种只包含陀螺仪和加速度计，后两种包含了陀螺仪、加速度计和磁力计。针对这类传感器，常用的姿态获取算法是互补卡尔曼滤波、扩展卡尔曼滤波等算法，状态变量的选取多为欧拉角和四元素。仅仅使用IMU更新姿态，无法确定地理位置上的北方，需要互补滤波算法融合磁力计的数据以修正偏航角，或者使用扩展卡尔曼滤波算法把磁力计的数据融合到欧拉角或四元素，但是这类方法在磁力计受到***强磁场干扰时，反而会引入新的误差。

在一些实验和工况环境下，根据具体情形的需要，可以使用其它传感器和测量方法替代姿态传感器获取姿态、位移等信息。在人体步态测量中，更加精确的方法是使用红外高速摄像头捕捉人体肢体的姿态。使用红外反光材料制作的标记点贴于人体肢体的各个生理位置，高速摄像头则捕捉人体各个生理位置的空间位置，根据人体模型将人体各肢体姿态还原出来。但这套***十分昂贵，对环境的光线条件要求高，只能在室内使用，而且其使用的空间范围有限。

发明内容

本发明的目的是克服现有技术的不足，提供一种姿态传感器及姿态更新方法。

本发明的目的是通过以下技术方案来实现的：一种姿态传感器，包括外壳、第一测距传感器、惯性传感器、磁力计、第二测距传感器、电源模块、CPU、WIFI模块、第三测距传感器，所述惯性传感器由陀螺仪和加速度计组成；第一测距传感器、第二测距传感器、第三测距传感器水平安装于外壳外部，且第一测距传感器、第二测距传感器、第三测距传感器的安装位置处于同一水平面内；惯性传感器、磁力计、电源模块、CPU、WIFI模块均置于外壳内部；CPU由电源模块供电，CPU通过总线分别连接第一测距传感器、惯性传感器、磁力计、第二测距传感器、WIFI模块和第三测距传感器，第一测距传感器、第二测距传感器和第三测距传感器测量自身距离地面的距离信息，陀螺仪测量姿态传感器的角速度，加速度计测量姿态传感器的加速度，磁力计测量姿态传感器的周边环境的磁场强度，CPU将采集的距离信息、角速度、加速度、磁场强度打包后通过WIFI模块传送至外部计算机。

一种上述姿态传感器的姿态更新方法，包括以下步骤：

(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为加速度计测量得到的加速度信息，以右下角小标号x、y、z分别表示向量在x轴、y轴、z轴上的分量，以右下角小标号A代表加速度计，y_A,k-1为k-1时刻测量得到的加速度，为重力加速度的值，为姿态传感器的姿态矩阵，T为采样时间，Φ为四元素q的系数矩阵：

是陀螺仪测量得到的三维角速度；

(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为加速度计测量得到的加速度信息，y_M＝[y_M,x y_M,yy_M,z]^T为磁力计测量得到的磁场强度信息，d₁、d₂、d₃分别为第一测距传感器、第二测距传感器、第三测距传感器测量得到的距离信息，记测距传感器测量差值为d_ji＝d_j-d_i(i,j＝1,2,3)，d₂₁、d₃₂、d₁₃分别为第二测距传感器与第一测距传感器、第三测距传感器与第二测距传感器、第一测距传感器与第三测距传感器的距离差值信息；

H_A(k)为加速度y_A的系数矩阵：

其中，为重力加速度的三维向量值，q＝[q₀ q₁ q₂ q₃]^T为四元素；

H_M(k)为磁场强度y_M的系数矩阵：

其中，为地磁场强度的三维向量值；

记分别为第一测距传感、第二测距传感器、第三测距传感器到惯性传感器的空间距离向量，记向量为上述空间距离向量的统一表达形式，记测距传感器测量的距离信息统一表达为d_i(i＝1,2,3)，分别为d₁、d₂、d₃的系数矩阵，则距离信息d_i的系数矩阵统一表达形式为：

记P₁、P₂、P₃分别为第一测距传感器、第二测距传感器、第三测距传感器的空间位置，以表示第一测距传感器到第二测距传感器的空间向量，以表示第二测距传感器到第三测距传感器的空间向量，以表示第三测距传感器到第一测距传感器的空间向量，记测距传感器测量差值信息为d_ji＝d_j-d_i(i,j＝1,2,3)，测距传感器相互位置的空间向量为分别为测距传感器测量差值信息d₂₁、d₃₂、d₁₃的系数矩阵，它们的统一表达式为：

(3)根据卡尔曼滤波算法计算得到四元素；该步骤包括以下子步骤：

(3.1)使用陀螺仪测量得到的三维角速度信息和加速度计测量得到的加速度信息对状态变量X预测更新；

(3.2)使用加速度计测量得到的加速度信息、磁力计测量得到的磁场强度信息、第一测距传感器、第二测距传感器、第三测距传感器测量得到的距离信息、第二测距传感器与第一测距传感器、第三测距传感器与第二测距传感器、第一测距传感器与第三测距传感器的距离差值信息对状态变量X量测更新；

状态变量X包含四元素q，所以卡尔曼滤波后四元素的值已知；

(4)使用四元素计算得到姿态传感器的姿态矩阵和姿态角；

根据四元素q＝[q₀ q₁ q₂ q₃]^T计算姿态传感器的姿态矩阵为：

则根据该姿态矩阵可以计算姿态传感器的姿态角，姿态传感器的俯仰角θ、横滚角γ和方位角Ψ确定如下：

θ＝arcsinT₃₂

本发明的有益效果是，本发明在传统的融合IMU(陀螺仪和加速度计)和磁力计的姿态传感器上进行了改进，对原有结构部分充分保留，硬件上容易实现扩展。而且现在的基于MEMS工艺的测距传感器如激光测距传感器VL53L0X价格低廉，芯片体积小，方便安装于原有姿态传感器外壳周围，短距离内不会互相干扰，并且使用I2C通信，能够在同一CPU电路上同时挂载多个。这种新型的姿态传感器的姿态更新算法与常用的扩展卡尔曼更新算法十分相似，只是对其状态变量和量测变量做了补充，仍然不影响原算法的计算部分，可以简单地删减量测变量和量测方程系数矩阵的行来实现算法的转变。因为这种新型姿态传感器融合了3个测距传感器，而已知平面3个点的垂线到地面的长度，就可以准确地知道该平面的空间姿态，所以3个测距传感器数据对于姿态的矫正理论上是十分精确并且完全可以消除IMU的偏移误差的，算法的实现也并不复杂，相比传统的方法有很大的优势。

附图说明

图1为本发明姿态传感器的结构示意图；

图2为本发明各传感器的空间位置示意图；

图3为本发明姿态更新方法的简易流程图；

图中，外壳1、第一测距传感器2、惯性传感器3、陀螺仪4、加速度计5、磁力计6、第二测距传感器7、电源模块8、CPU9、WIFI模块10、第三测距传感器11。

具体实施方式

如图1所示，姿态传感器包括外壳1、第一测距传感器2、惯性传感器3、磁力计6、第二测距传感器7、电源模块8、CPU9、WIFI模块10、第三测距传感器11，所述惯性传感器3由陀螺仪4和加速度计5组成；第一测距传感器2、第二测距传感器7、第三测距传感器11水平安装于外壳1外部，且第一测距传感器2、第二测距传感器7、第三测距传感器11的安装位置处于同一水平面内；惯性传感器3、磁力计6、电源模块8、CPU9、WIFI模块10均置于外壳1内部；CPU9由电源模块8供电，CPU9通过总线分别连接第一测距传感器2、惯性传感器3、磁力计6、第二测距传感器7、WIFI模块10和第三测距传感器11，第一测距传感器2、第二测距传感器7和第三测距传感器11测量自身距离地面的距离信息，陀螺仪4测量姿态传感器的角速度，加速度计5测量姿态传感器的加速度，磁力计6测量姿态传感器的周边环境的磁场强度，CPU9将采集的距离信息、角速度、加速度、磁场强度打包后通过WIFI模块10传送至外部计算机。

如图2和图3所示，本发明所述的姿态传感器的姿态更新方法，包括以下步骤：

1、将姿态传感器绑定在被测下肢上，使得3个测距传感器的测量探头朝向地面，能够测量它们与地面的距离信息，姿态传感器开始测量，得到角速度、加速度、磁场强度和3个测距传感器的距离信息。

2、推导和建立融合角速度、加速度、磁场强度和距离信息的卡尔曼滤波算法，卡尔曼滤波算法包括预测更新和量测更新两个步骤。

第一步，状态向量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测量得到的三维角速度。

第二步，状态向量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,yy_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、根据卡尔曼滤波算法计算得到四元素。即：

第一步，使用陀螺仪4测量得到的三维角速度信息和加速度计5测量得到的加速度信息对状态变量X预测更新。

第二步，使用加速度计5测量得到的加速度信息、磁力计6测量得到的磁场强度信息、第一测距传感器2、第二测距传感器7、第三测距传感器11测量得到的距离信息、第二测距传感器7与第一测距传感器2、第三测距传感器11与第二测距传感器7、第一测距传感器2与第三测距传感器11的距离差值信息对状态变量X量测更新。

状态变量X包含四元素q，所以卡尔曼滤波后四元素的值已知。

4、使用四元素计算得到姿态传感器的姿态矩阵和姿态角。

根据四元素q＝[q₀ q₁ q₂ q₃]^T计算姿态传感器的姿态矩阵为：

则根据该姿态矩阵可以计算姿态传感器的姿态角，姿态传感器的俯仰角θ、横滚角γ和方位角Ψ确定如下：

θ＝arcsinT₃₂

Claims (2)

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)传送至外部计算机。

2.一种权利要求1所述的姿态传感器的姿态更新方法，其特征在于，包括以下步骤：

(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为状态方程系数矩阵：

$F=\left[\begin{array}{ccc}\mathrm{\Φ}& 0_3& 0_3\\ 0_4& I_3& T\·I_3\\ 0_4& 0_3& I_3\end{array}\right]$

$u=\left[\begin{array}{c}{0}_{4\×1}\\ \frac{T^2}{2}\·(C_b^n\·y_A+g^n)\\ T\·(C_b^n\·y_A+g^n)\end{array}\right]$

其中，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的系数矩阵：

$\mathrm{\Φ}=\left[\begin{array}{cccc}1& -\frac{1}{2}{\mathrm{\ω}}_x^{b}T& -\frac{1}{2}{\mathrm{\ω}}_y^{b}T& -\frac{1}{2}{\mathrm{\ω}}_z^{b}T\\ \frac{1}{2}{\mathrm{\ω}}_x^{b}T& 1& \frac{1}{2}{\mathrm{\ω}}_z^{b}T& -\frac{1}{2}{\mathrm{\ω}}_y^{b}T\\ \frac{1}{2}{\mathrm{\ω}}_y^{b}T& -\frac{1}{2}{\mathrm{\ω}}_z^{h}T& 1& \frac{1}{2}{\mathrm{\ω}}_x^{b}T\\ \frac{1}{2}{\mathrm{\ω}}_z^{b}T& \frac{1}{2}{\mathrm{\ω}}_y^{b}T& -\frac{1}{2}{\mathrm{\ω}}_x^{b}T& 1\end{array}\right]$

是陀螺仪(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为：

$H={\left[\begin{array}{cccccccc}{H}_A\left(k\right)& H_M\left(k\right)& H_{d_1}\left(k\right)& H_{d_2}\left(k\right)& H_{d_3}\left(k\right)& H_{d_{21}}\left(k\right)& H_{d_{32}}\left(k\right)& H_{d_{13}}\left(k\right)\end{array}\right]}^T;$

其中，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的系数矩阵：

$\begin{array}{c}{H}_A\left(k\right)=\frac{\∂h_A\left(k\right)}{\∂X\left(k\right)}\\ =2\·\left[\begin{array}{ccccc}{q}_0{g}_x^n+q_3{g}_y^n-q_2{g}_z^n& q_1{g}_x^n+q_2{g}_y^n+q_3{g}_z^n& -q_2{g}_x^n+q_1{g}_y^n-q_0{g}_z^n& -q_3{g}_x^n+q_0{g}_y^n+q_1{g}_z^n& 0_{1\×6}\\ -q_3{g}_x^n+q_0{g}_y^n+q_1{g}_z^n& q_2{g}_x^n-q_1{g}_y^n+q_0{g}_z^n& q_1{g}_x^n+q_2{g}_y^n+q_3{g}_z^n& -q_0{g}_x^n-q_3{g}_y^n+q_2{g}_z^n& 0_{1\×6}\\ q_2{g}_x^n-q_1{g}_y^n+q_0{g}_z^n& q_3{g}_x^n-q_0{g}_y^n-q_1{g}_z^n& q_0{g}_x^n+q_3{g}_y^n-q_2{g}_z^n& q_1{g}_x^n+q_2{g}_y^n+q_3{g}_z^n& 0_{1\×6}\end{array}\right]\end{array}$

其中，为重力加速度的三维向量值，q＝[q₀q₁q₂q₃]^T为四元素；

H_M(k)为磁场强度y_M的系数矩阵：

$\begin{array}{c}{H}_M\left(k\right)=\frac{\∂h_M\left(k\right)}{\∂X\left(k\right)}\\ =2\·\left[\begin{array}{ccccc}{q}_0{m}_x^n+q_3{m}_y^n-q_2{m}_z^n& q_1{m}_x^n+q_2{m}_y^n+q_3{m}_z^n& -q_2{m}_x^n+q_1{m}_y^n-q_0{m}_z^n& -q_3{m}_x^n+q_0{m}_y^n+q_1{m}_z^n& 0_{1\×6}\\ -q_3{m}_x^n+q_0{m}_y^n+q_1{m}_z^n& q_2{m}_x^n-q_1{m}_y^n+q_0{m}_z^n& q_1{m}_x^n+q_2{m}_y^n+q_3{m}_z^n& -q_0{m}_x^n-q_3{m}_y^n+q_2{m}_z^n& 0_{1\×6}\\ q_2{m}_x^n-q_1{m}_y^n+q_0{m}_z^n& q_3{m}_x^n-q_0{m}_y^n-q_1{m}_z^n& q_0{m}_x^n+q_3{m}_y^n-q_2{m}_z^n& q_1{m}_x^n+q_2{m}_y^n+q_3{m}_z^n& 0_{1\×6}\end{array}\right]\end{array}$

其中，为地磁场强度的三维向量值；

记分别为第一测距传感器(2)、第二测距传感器(7)、第三测距传感器(11)到惯性传感器(3)的空间距离向量，记向量为上述空间距离向量的统一表达形式，记测距传感器测量的距离信息统一表达为d_i(i＝1,2,3)， 分别为d₁、d₂、d₃的系数矩阵，则距离信息d_i的系数矩阵统一表达形式为：

$H_{d_i}\left(k\right)=\frac{\∂d_i}{\∂X}={\left[\begin{array}{c}\frac{q_2(q_0^2-q_1^2-q_2^2+q_3^2)+4q_0(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,x}-\frac{2q_1(q_0^2-q_1^2-q_2^2+q_3^2)-4q_0(q_0{q}_1-q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,y}-\frac{2q_0}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_z^n\\ -\frac{2q_3(q_0^2-q_1^2-q_2^2+q_3^2)+4q_1(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,x}-\frac{2q_0(q_0^2-q_1^2-q_2^2+q_3^2)+4q_1(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,y}+\frac{2q_1}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_z^n\\ \frac{2q_0(q_0^2-q_1^2-q_2^2+q_3^2)-4q_2(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,x}-\frac{2q_3(q_0^2-q_1^2-q_2^2+q_3^2)+4q_2(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,y}+\frac{2q_2}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_z^n\\ -\frac{2q_1(q_0^2-q_1^2-q_2^2+q_3^2)-4q_3(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,x}-\frac{2q_2(q_0^2-q_1^2-q_2^2+q_3^2)-4q_3(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{i,y}-\frac{2q_3}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_z^n\\ 0\\ 0\\ \frac{1}{q_0^2-q_1^2-q_2^2+q_3^2}\\ 0\\ 0\\ 0\end{array}\right]}^T$

记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₁₃的系数矩阵，它们的统一表达式为：

$H_{d_{ji}}\left(k\right)=\frac{\∂d_{ji}}{\∂X}={\left[\begin{array}{c}\frac{-2q_2(q_0^2-q_1^2-q_2^2+q_3^2)-4q_0(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,x}+\frac{2q_1(q_0^2-q_1^2-q_2^2+q_3^2)-4q_0(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,y}\\ \frac{2q_3(q_0^2-q_1^2-q_2^2+q_3^2)+4q_1(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,x}+\frac{2q_0(q_0^2-q_1^2-q_2^2+q_3^2)+4q_1(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,y}\\ \frac{-2q_0(q_0^2-q_1^2-q_2^2+q_3^2)+4q_2(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,x}+\frac{2q_3(q_0^2-q_1^2-q_2^2+q_3^2)+4q_2(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,y}\\ \frac{2q_1(q_0^2-q_1^2-q_2^2+q_3^2)-4q_3(q_1{q}_3-q_0{q}_2)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,x}+\frac{2q_2(q_0^2-q_1^2-q_2^2+q_3^2)-4q_3(q_0{q}_1+q_2{q}_3)}{{(q_0^2-q_1^2-q_2^2+q_3^2)}^2}{p}_{ij,y}\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]}^T.$

(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计算姿态传感器的姿态矩阵为：

$C_b^n=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_0{q}_3+q_1{q}_2)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

则根据该姿态矩阵可以计算姿态传感器的姿态角，姿态传感器的俯仰角θ、横滚角γ和方位角Ψ确定如下：

θ＝arcsinT₃₂

$\mathrm{\γ}=\arctan (-\frac{T_{31}}{T_{33}})$

$\mathrm{\Ψ}=\arctan (-\frac{T_{12}}{T_{22}}).$