全机疲劳试验姿态监控系统

沈翔+郭善镜

摘 要： 为了改善现有全机疲劳试验中姿态监控的缺点，采用加速度传感器、陀螺仪、磁力计和DSP处理器，通过四元数法和二阶龙格?库塔法实时解算出飞机的姿态数据，然后经过扩展卡尔曼滤波器对多传感器数据进行融合滤波后得到准确的姿态数据，并通过无线传输模块发送至上位机，姿态数据超差后能够发出声光报警提醒试验控制人员。经试验，系统误差在0.58°以内。通过在某型号全机疲劳试验中的实际应用，该系统工作可靠、准确，满足试验要求。

关键词： 疲劳试验； 姿态解算； 四元数法； 扩展卡尔曼滤波

中图分类号： TN919?34； TP23 文献标识码： A 文章编号： 1004?373X（2014）22?0060?03

Attitude monitoring system of whole plan fatigue test

SHEN Xiang1， GUO Shan?jing2

（1. China Aircraft Strength Research Institute， Xian 710065， China； 2. The 213th Research Institute of China Ordnance Industry， Xian 710061， China）

Abstract： In order to improve the performance of the existing attitude monitoring system， the attitude data of plane was obtained with acceleration sensor， gyroscope， magnetometer and DSP processor through real?time calculation of quaternion algorithm and Runge?Kutta algorithm. The accurate attitude data was got by means of fusion and filtering of the extended Kalman filter with the data from multiple sensors. The data is send to upper computer through wireless transmission module. The system will reminding the test controller through sound and light alarm when the attitude error is greater than the threshold. In the test， the system error is within 0.58°. With the application in the fatigue test， the system is proved to be reliable and accurate， and meets the fatigue test requirements.

Keywords： fatigue test； attitude calculation； quaternion algorithm； extended Kalman filtering

0 引 言

在疲劳实验中需要将飞机的姿态保持在合理的范围内，否则会造成加载力与实际飞行受力不符的情况。姿态监控即监视飞机的俯仰角[θ]、航向角[φ]和横滚角[γ]，并实时显示，超出设定的阈值范围后报警。蔡明等采用电阻式位移传感器和数据采集卡，在Windows平台上实现了姿态测量[1]。目前的飞机姿态测量是通过在飞机起落架上安装力学传感器，通过传感器的受力偏差来估计飞机姿态的变化。

这两种方法采用的均为接触式传感器，调试计算复杂，尤其是在大型飞机上安装、调试更加困难。本系统采用加速度传感器、陀螺仪和磁力计，通过四元数法解算出飞机姿态，然后经扩展卡尔曼滤波器进行数据融合滤波后得到准确的姿态数据。

1 系统结构

姿态监控系统系统结构如图1所示，主要包括三轴加速度传感器、三轴陀螺仪、三轴磁力计、数据处理单元、无线通信模块以及上位机。

图1 姿态监控系统结构

数据处理单元采用浮点DSP处理器TMS320F28335，具有处理速度快实时性好的优点。三轴加速度传感器和磁力计采用六轴数字传感器FXOS8700CQ。该传感器集成了14位三轴加速度传感器和16位三轴磁力计，加速度测量范围[±2g/±4g/±8g]可选，磁力测量范围[±1 200 μT]，数据输出频率最高为[800 Hz]，其典型分辨率为0.1°。三轴陀螺仪采用ITG3400A，测量范围[±250 （°）/s/±500 （°）/s/±1 000 （°）/s]可选，片内集成16位ADC，数据输出频率[8 000 Hz]。

2 四元数法姿态解算[2?6]

姿态解算就是将载体上惯性传感器的输出实时转换成载体的姿态。

假设向量在世界坐标系[n]中的坐标为[xn]、[yn]、[zn]，在机体坐标系[b]中的坐标为[xb]、[yb]、[zb]。坐标系[n]经过[z→x→y]旋转后到坐标系[b]。

因此坐标变换公式为：

[xbybzb=Cbnxnynzn] （1）

四元数是一种超复数，可以描述一个坐标系或一个向量相对于某一个坐标系的旋转。四元数[Q]定义为：

[Q=q0+q1i+q2j+q3k] （3）

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的标量向量之和形式为：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元数的乘法表示为：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐标系变换如图2所示。

图2 坐标系变换图

四元数[Q]的共轭：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐标系[b]和[n]中的投影四元形式分别为：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐标系[n]按照四元数[Q]旋转后得到一个新的坐标系[b]，其投影关系可以表示为：

[pb=Q*?pn?Q] （8）

将式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

将式（9）按四元数展开，写成矩阵形式为：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飞机姿态角[θ]，[γ]，[φ]分别为：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全机疲劳试验海拔低，可以忽略地球自转因素的影响，那么四元数[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分别为机体坐标系下x、y和z轴的角速度。设T为采样周期，那么四元数微分方程的二阶龙格?库塔法计算公式为：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 扩展卡尔曼滤波数据融合

陀螺仪测量精度低，存在累计漂移误差，但是动态性能良好；加速度传感器和磁力计没有漂移问题，静态特性好，但是容易受机体振动和外部磁场等因素干扰，所以其动态可信度降低[7]，因此采用扩展卡尔曼滤波[8?11]进行数据融合。取系统的状态向量为：

[X=q0q1q2q3T]

根据式（11）可得状态方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

状态转移矩阵[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]为采样周期。[Γ]为[4×4]维的系统噪声矩阵；[W]为零均值的高斯白噪声，其方差阵为[Μ]。

根据加速度传感器和磁力计可以计算出飞机姿态角[θ]，[γ]，[φ]，根据式（2）计算出余弦矩阵[Cbn]，根据式（9），采用高斯?牛顿迭代法求解四元数[8]。以求得的四元数[qk=q0q1q2q3T]，作为扩展卡尔曼滤波器的观测量，则系统的量测方程为：

[Zk=HkXk+Vk] （16）

观测矩阵[Hk=I4×4]。[V]是与[W]不相关的观测噪声，其方差阵为[R]。根据扩展卡尔曼滤波器，[xk]是[xk]的无偏估计和最小方差估计。完整的滤波算法为：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

经过扩展卡尔曼滤波器后得到的四元数，代入到[Cbn]，由式（11），即可得出姿态角。

图3 仅利用陀螺仪时输出角度

4 试验结果

为了检测姿态监控系统的准确性，将其安装在单轴转动台上，转台调水平。由于飞机在试验过程中姿态变化是一个缓慢变化的过程，因此以一个相对缓慢的速度来旋转单轴转台，静止时采集的数据用来计算噪声针[M]和[R]。由于单轴转台水平，因此横滚角和俯仰角应该一直为[0°]，因此只利用偏航角来验证姿态监控系统的准确性。图3是仅利用陀螺仪输出的角速度采用四元数法解算的偏航角，可以看出随着时间的增长，偏航角开始发散。图4是采用扩展卡尔曼滤波后解算的偏航角，最大误差为[0.58°]。

图4 扩展卡尔曼滤波后输出角度

5 结 论

通过在某型号全机疲劳试验中的应用，姿态监控系统安装简便、校准方便，运行可靠，能够及时、准确、直观地反应飞机姿态变化，满足试验要求。

参考文献

[1] 蔡明，杨丛青，赵翔.直升机全机静力试验姿态测量系统软件设计分析[J].直升机技术，2005（4）：20?22.

[2] 袁信，郑谔.捷联式惯性导航原理[M].北京：航空专业教材编审组，1985.

[3] 李文亮.四元数矩阵[M].长沙：国防科技大学出版社，2002.

[4] 秦永元.惯性导航[M].北京：科学出版社，2006.

[5] 张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算[J].光学精密工程，2008，16（10）：1963?1970.

[6] 杜海龙，张荣辉，刘平，等.捷联惯导系统姿态解算模块的实现[J].光学精密工程，2008，16（10）：1956?1961.

[7] 刘星.多维MEMS惯性传感器的姿态解算算法研究[D].哈尔滨：哈尔滨工程大学，2013.

[8] 黄旭，王常虹，伊国兴，等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J].中国惯性学报，2005，13（2）：27?34.

[9] 杨淑洁，曾庆双，伊国兴.低成本无人机姿态测量系统研究[J].传感器与微系统，2012，31（2）：15?22.

[10] 杨峻巍.水下航行器导航及数据融合技术研究[D].哈尔滨：哈尔滨工程大学，2013.

[11] 乔相伟.基于四元数非线性滤波的飞行器姿态确定算法研究[D].哈尔滨：哈尔滨工程大学，2011.

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的标量向量之和形式为：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元数的乘法表示为：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐标系变换如图2所示。

图2 坐标系变换图

四元数[Q]的共轭：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐标系[b]和[n]中的投影四元形式分别为：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐标系[n]按照四元数[Q]旋转后得到一个新的坐标系[b]，其投影关系可以表示为：

[pb=Q*?pn?Q] （8）

将式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

将式（9）按四元数展开，写成矩阵形式为：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飞机姿态角[θ]，[γ]，[φ]分别为：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全机疲劳试验海拔低，可以忽略地球自转因素的影响，那么四元数[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分别为机体坐标系下x、y和z轴的角速度。设T为采样周期，那么四元数微分方程的二阶龙格?库塔法计算公式为：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 扩展卡尔曼滤波数据融合

陀螺仪测量精度低，存在累计漂移误差，但是动态性能良好；加速度传感器和磁力计没有漂移问题，静态特性好，但是容易受机体振动和外部磁场等因素干扰，所以其动态可信度降低[7]，因此采用扩展卡尔曼滤波[8?11]进行数据融合。取系统的状态向量为：

[X=q0q1q2q3T]

根据式（11）可得状态方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

状态转移矩阵[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]为采样周期。[Γ]为[4×4]维的系统噪声矩阵；[W]为零均值的高斯白噪声，其方差阵为[Μ]。

根据加速度传感器和磁力计可以计算出飞机姿态角[θ]，[γ]，[φ]，根据式（2）计算出余弦矩阵[Cbn]，根据式（9），采用高斯?牛顿迭代法求解四元数[8]。以求得的四元数[qk=q0q1q2q3T]，作为扩展卡尔曼滤波器的观测量，则系统的量测方程为：

[Zk=HkXk+Vk] （16）

观测矩阵[Hk=I4×4]。[V]是与[W]不相关的观测噪声，其方差阵为[R]。根据扩展卡尔曼滤波器，[xk]是[xk]的无偏估计和最小方差估计。完整的滤波算法为：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

经过扩展卡尔曼滤波器后得到的四元数，代入到[Cbn]，由式（11），即可得出姿态角。

图3 仅利用陀螺仪时输出角度

4 试验结果

为了检测姿态监控系统的准确性，将其安装在单轴转动台上，转台调水平。由于飞机在试验过程中姿态变化是一个缓慢变化的过程，因此以一个相对缓慢的速度来旋转单轴转台，静止时采集的数据用来计算噪声针[M]和[R]。由于单轴转台水平，因此横滚角和俯仰角应该一直为[0°]，因此只利用偏航角来验证姿态监控系统的准确性。图3是仅利用陀螺仪输出的角速度采用四元数法解算的偏航角，可以看出随着时间的增长，偏航角开始发散。图4是采用扩展卡尔曼滤波后解算的偏航角，最大误差为[0.58°]。

图4 扩展卡尔曼滤波后输出角度

5 结 论

通过在某型号全机疲劳试验中的应用，姿态监控系统安装简便、校准方便，运行可靠，能够及时、准确、直观地反应飞机姿态变化，满足试验要求。

参考文献

[1] 蔡明，杨丛青，赵翔.直升机全机静力试验姿态测量系统软件设计分析[J].直升机技术，2005（4）：20?22.

[2] 袁信，郑谔.捷联式惯性导航原理[M].北京：航空专业教材编审组，1985.

[3] 李文亮.四元数矩阵[M].长沙：国防科技大学出版社，2002.

[4] 秦永元.惯性导航[M].北京：科学出版社，2006.

[5] 张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算[J].光学精密工程，2008，16（10）：1963?1970.

[6] 杜海龙，张荣辉，刘平，等.捷联惯导系统姿态解算模块的实现[J].光学精密工程，2008，16（10）：1956?1961.

[7] 刘星.多维MEMS惯性传感器的姿态解算算法研究[D].哈尔滨：哈尔滨工程大学，2013.

[8] 黄旭，王常虹，伊国兴，等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J].中国惯性学报，2005，13（2）：27?34.

[9] 杨淑洁，曾庆双，伊国兴.低成本无人机姿态测量系统研究[J].传感器与微系统，2012，31（2）：15?22.

[10] 杨峻巍.水下航行器导航及数据融合技术研究[D].哈尔滨：哈尔滨工程大学，2013.

[11] 乔相伟.基于四元数非线性滤波的飞行器姿态确定算法研究[D].哈尔滨：哈尔滨工程大学，2011.

式中：[q0，q1，q2，q3∈R]，[i2=j2=k2=-1，ij=k=-ji]。[Q]的标量向量之和形式为：

[Q=q0+q] （4）

式中：[q=q1i+q2j+q3k]。

四元数的乘法表示为：

[Q1?Q2=q01q02-q1?q2+q01q2+q02q1+q1×q2] （5）

坐标系变换如图2所示。

图2 坐标系变换图

四元数[Q]的共轭：

[Q*=q0-q1i-q2j-q3k] （6）

向量[p]在坐标系[b]和[n]中的投影四元形式分别为：

[pb=0+xbi+ybj+zbkpn=0+xni+ynj+znk] （7）

坐标系[n]按照四元数[Q]旋转后得到一个新的坐标系[b]，其投影关系可以表示为：

[pb=Q*?pn?Q] （8）

将式（7）代入式（8）中得：

[xbi+ybj+zbk=q0-q1i-q2j-q3k? xni+ynj+znk?q0+q1i+q2j+q3k] （9）

将式（9）按四元数展开，写成矩阵形式为：

[xbybzb=q21+q20-q23-q222q1q2+q0q32q1q3-q0q22q1q2-q0q3q22-q23+q20-q212q2q3+q0q12q1q3+q0q22q2q3-q0q1q23-q22-q21+q20xnynzn=Cbnxnynzn] （10）

飞机姿态角[θ]，[γ]，[φ]分别为：

[θ=arcsin2q2q3+q0q1γ=arctan-2q1q3-q0q2q23+q20-q22-q21φ=arctan-2q1q2-q0q3q22-q23+q20-q21] （11）

由于全机疲劳试验海拔低，可以忽略地球自转因素的影响，那么四元数[Q]具有如下微分方程[4]：

[Q?=12ΩbnbQ] （12）

[Ωbnb=0-ωbx-ωby-ωbzωbx0ωbz-ωbyωby-ωbz0ωbxωbzωby-ωbx0] （13）

式中：[ωbx]，[ωby]，[ωbz]分别为机体坐标系下x、y和z轴的角速度。设T为采样周期，那么四元数微分方程的二阶龙格?库塔法计算公式为：

[K1=ΩbnbtQtY=Qt+TΩbnbtQtK2=Ωbnbtt+TYQt+T=Qt+T2*K1+K2] （14）

3 扩展卡尔曼滤波数据融合

陀螺仪测量精度低，存在累计漂移误差，但是动态性能良好；加速度传感器和磁力计没有漂移问题，静态特性好，但是容易受机体振动和外部磁场等因素干扰，所以其动态可信度降低[7]，因此采用扩展卡尔曼滤波[8?11]进行数据融合。取系统的状态向量为：

[X=q0q1q2q3T]

根据式（11）可得状态方程：

[Xk=I4×4+12TΩbnbk-1Xk-1+Γk-1Wk-1] （15）

状态转移矩阵[Φk，k-1=I4×4+12TΩnnbk-1]，其中[T]为采样周期。[Γ]为[4×4]维的系统噪声矩阵；[W]为零均值的高斯白噪声，其方差阵为[Μ]。

根据加速度传感器和磁力计可以计算出飞机姿态角[θ]，[γ]，[φ]，根据式（2）计算出余弦矩阵[Cbn]，根据式（9），采用高斯?牛顿迭代法求解四元数[8]。以求得的四元数[qk=q0q1q2q3T]，作为扩展卡尔曼滤波器的观测量，则系统的量测方程为：

[Zk=HkXk+Vk] （16）

观测矩阵[Hk=I4×4]。[V]是与[W]不相关的观测噪声，其方差阵为[R]。根据扩展卡尔曼滤波器，[xk]是[xk]的无偏估计和最小方差估计。完整的滤波算法为：

[xk，k-1=Φk，k-1xkxk=xk，k-1+Kkzk-zk，k-1zk，k-1=Hkxk，k-1Kk=Pk，k-1HTkHkPk，k-1HTk+VkRkVTk-1Pk，k-1=Φk，k-1Pk-1ΦTk，k-1+Γk，k-1Mk-1ΓTk，k-1Pk=I4×4-KkHkPk，k-1] （17）

经过扩展卡尔曼滤波器后得到的四元数，代入到[Cbn]，由式（11），即可得出姿态角。

图3 仅利用陀螺仪时输出角度

4 试验结果

为了检测姿态监控系统的准确性，将其安装在单轴转动台上，转台调水平。由于飞机在试验过程中姿态变化是一个缓慢变化的过程，因此以一个相对缓慢的速度来旋转单轴转台，静止时采集的数据用来计算噪声针[M]和[R]。由于单轴转台水平，因此横滚角和俯仰角应该一直为[0°]，因此只利用偏航角来验证姿态监控系统的准确性。图3是仅利用陀螺仪输出的角速度采用四元数法解算的偏航角，可以看出随着时间的增长，偏航角开始发散。图4是采用扩展卡尔曼滤波后解算的偏航角，最大误差为[0.58°]。

图4 扩展卡尔曼滤波后输出角度

5 结 论

通过在某型号全机疲劳试验中的应用，姿态监控系统安装简便、校准方便，运行可靠，能够及时、准确、直观地反应飞机姿态变化，满足试验要求。

参考文献

[1] 蔡明，杨丛青，赵翔.直升机全机静力试验姿态测量系统软件设计分析[J].直升机技术，2005（4）：20?22.

[2] 袁信，郑谔.捷联式惯性导航原理[M].北京：航空专业教材编审组，1985.

[3] 李文亮.四元数矩阵[M].长沙：国防科技大学出版社，2002.

[4] 秦永元.惯性导航[M].北京：科学出版社，2006.

[5] 张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算[J].光学精密工程，2008，16（10）：1963?1970.

[6] 杜海龙，张荣辉，刘平，等.捷联惯导系统姿态解算模块的实现[J].光学精密工程，2008，16（10）：1956?1961.

[7] 刘星.多维MEMS惯性传感器的姿态解算算法研究[D].哈尔滨：哈尔滨工程大学，2013.

[8] 黄旭，王常虹，伊国兴，等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J].中国惯性学报，2005，13（2）：27?34.

[9] 杨淑洁，曾庆双，伊国兴.低成本无人机姿态测量系统研究[J].传感器与微系统，2012，31（2）：15?22.

[10] 杨峻巍.水下航行器导航及数据融合技术研究[D].哈尔滨：哈尔滨工程大学，2013.

[11] 乔相伟.基于四元数非线性滤波的飞行器姿态确定算法研究[D].哈尔滨：哈尔滨工程大学，2011.