俯仰/滚转耦合的大角度姿态测量算法

摘要

基于加速度计或陀螺仪的测姿方法均存在大角度条件下姿态角误差放大、突变问题。对大俯仰角测量，通过预置欧拉旋转法可确定冗余加速度计的布置方式，降低了俯仰角测量误差。对大俯仰角条件下的滚转角测量，提出基于角速率阈值判定的陀螺解耦测姿算法。当俯仰角速率大于设定阈值时，采用角速度投影可钳制滚转角误差的漂移；当俯仰角速率小于设定阈值时，采用角速度积分可避免角速度投影造成的姿态误差放大。通过理论推导、分析和仿真，预置欧拉旋转法能有效避免大俯仰角条件下俯仰角姿态误差放大，陀螺解耦测姿算法能在振荡环境下长时间保持滚转角精确度。

关键词

双自由度; 欧拉角; 大角度; 奇异性; 多加速度计

风洞试验是获取飞行器气动力特性重要手段。在大迎角过失速试验中，气动力呈现严重的非对称、非线性和非定常特性，易诱发俯仰拉起的滚转或侧偏等耦合运动，严重影响姿态角测量精确度^[1]。目前风洞试验中姿态角的测量技术主要有：激光光栅法、视频测量法和基于惯性技术测量法等^[2-3]。激光光栅法需在模型表面嵌装光栅传感器，模型制造安装难度大。视频测量方法需具有处理庞大实时图像数据能力。基于惯性技术的测姿法，系统架构简单，维护方便。其中高精度陀螺测姿通过测量模型角速度再经积分运算获得姿态角，具有短时精度高的特点，高精度陀螺体积大，不能满足风洞试验要求^[4]。利用加速度计测姿是通过测量重力加速度的投影分量获取姿态角，在静态小角度条件下测姿精度高，但在大角度条件下重力投影的变化量难以分辨，导致测姿精度迅速降低^[5]，同时动态环境下加速度计敏感轴上叠加的杆臂干扰加速度，会对测姿精度造成极大影响^[6]。

文献[7-10]通过减小加速度计噪声来提高加速度计输出精度，但减小加速度计输出噪声只是改善大角度条件下测姿精度的一个方面。欧拉角投影转换才是造成大角度条件下加速度计测姿误差激增的主要原因。文献[11-12]提出预置欧拉旋转法来解决垂直钻井中姿态角的准确测量问题，但需假设井斜角和工具面角的单位微变相等。文献[13]将测姿范围进行了分段，使得不同方位布置的加速度计能够满足小角度测姿条件，但每次只使用一只加速度计测姿，姿态信息利用不完整。文献[14]利用正交的双轴和三轴加速度计测量姿态角，其中双轴加速度计只能测量俯仰角；三轴加速度计可同时对俯仰角和滚转角进行测量。正交的双轴和三轴加速度计俯仰角测姿误差与滚转角有关，精度还有待进一步提高。

本文针对俯仰/滚转的二自由度耦合转动，提出一种预置欧拉旋转的二自由度大量程测姿算法，最后通过仿真验证了姿态解算方法的正确性。由于动态环境下的杆臂效应由角运动产生，可视为高频干扰，可通过低通滤波或力学补偿抑制加速度计杆臂效应，因本文主要研究大角度测姿，未就该问题展开说明。

1 基于惯性技术的测姿原理及误差分析

设地理坐标系n系为ENU(东-北-天)，载体坐标系$\mathrm{b}$系，坐标系原点固定在载体质心处，其$x_{\mathrm{b}},y_{\mathrm{b}},z_{\mathrm{b}}$轴的指向分别为“前-右-下”。载体坐标系($\mathrm{b}$系)与地理坐标系(n系)之间的方向余弦矩阵${\mathit{C}}_{\mathrm{n}}^{\mathrm{b}}$为：

$${\mathit{C}}_{\mathrm{n}}^{\mathrm{b}}=\left[\begin{array}{ccc}\mathrm{c}\mathrm{o}\mathrm{s}\psi \mathrm{c}\mathrm{o}\mathrm{s}\theta & \mathrm{s}\mathrm{i}\mathrm{n}\psi \mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ -\mathrm{s}\mathrm{i}\mathrm{n}\psi \mathrm{c}\mathrm{o}\mathrm{s}\phi +\mathrm{c}\mathrm{o}\mathrm{s}\psi \mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\psi \mathrm{c}\mathrm{o}\mathrm{s}\phi +\mathrm{s}\mathrm{i}\mathrm{n}\psi \mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{s}\mathrm{i}\mathrm{n}\phi \\ \mathrm{s}\mathrm{i}\mathrm{n}\psi \mathrm{s}\mathrm{i}\mathrm{n}\phi +\mathrm{c}\mathrm{o}\mathrm{s}\psi \mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{c}\mathrm{o}\mathrm{s}\phi & -\mathrm{c}\mathrm{o}\mathrm{s}\psi \mathrm{s}\mathrm{i}\mathrm{n}\phi +\mathrm{s}\mathrm{i}\mathrm{n}\psi \mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{c}\mathrm{o}\mathrm{s}\phi & \mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{c}\mathrm{o}\mathrm{s}\phi \end{array}\right]$$

(1)

式中：$\psi$为绕偏航轴转动的方位角；$\theta$为绕俯仰轴转动的俯仰角；$\phi$为绕滚转轴转动的滚转角。

风洞试验中模型俯仰/滚转的双自由度耦合转动过程如图1所示。图中航姿参考系统(Attitude and Heading Reference System，AHRS)为惯性测姿单元。

图1  风洞测姿示意图

Fig.1  Schematic diagram of attitude measurement for wind tunnel test model

1.1 基于三轴加速度计的测姿原理及误差分析

静态条件下，三轴加速度计的测姿公式为：

$$\left\{\begin{array}{l}\theta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(-f_x^{\mathrm{b}}/\sqrt[]{{\left(f_y^{\mathrm{b}}\right)}^{\mathrm{2}}+{\left(f_z^{\mathrm{b}}\right)}^{\mathrm{2}}}\right),\hspace{1em}\theta \in \left(-\frac{\mathrm{\pi }}{\mathrm{2}},\frac{\mathrm{\pi }}{\mathrm{2}}\right)\\ \phi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(f_y^{\mathrm{b}}/f_z^{\mathrm{b}}\right),\hspace{1em}\phi \in \left(-\frac{\mathrm{\pi }}{\mathrm{2}},\frac{\mathrm{\pi }}{\mathrm{2}}\right)\end{array}\right.$$

(2)

式中$f_x^{\mathrm{b}},f_y^{\mathrm{b}},f_z^{\mathrm{b}}$为$x_{\mathrm{b}},y_{\mathrm{b}},z_{\mathrm{b}}$轴加速度计理想输出比力。设$\mathrm{\Delta }{f}_x^{\mathrm{b}},\mathrm{\Delta }{f}_y^{\mathrm{b}},\mathrm{\Delta }{f}_z^{\mathrm{b}}$为加速度计输出误差，当各个轴加速度计的输出误差相当，即$\mathrm{\Delta }{f}_x^{\mathrm{b}}\approx \mathrm{\Delta }{f}_y^{\mathrm{b}}\approx \mathrm{\Delta }{f}_z^{\mathrm{b}}=\mathrm{\Delta }A$，由式(2)可得俯仰角误差$\mathrm{\Delta }\theta$为：

$$\mathrm{\Delta }\theta =-\frac{\mathrm{\Delta }A}{g}\mathrm{c}\mathrm{o}\mathrm{s}\theta -\frac{\mathrm{\Delta }A}{g}\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\mathrm{45}°\right)$$

(3)

假定加速度计精度为${\mathrm{10}}^{-\mathrm{4}}g$量级时，利用等高线图观测俯仰角测姿误差绝对值$\left|\mathrm{\Delta }\theta \right|$，如图2所示。从图2可看到，$-\mathrm{90}°\le \theta \le \mathrm{0}°$时，$\left|\mathrm{\Delta }\theta \right|$较小；$\mathrm{0}°\le \theta ,\phi \le \mathrm{90}°$时，$\left|\mathrm{\Delta }\theta \right|$较大。

图2  俯仰角姿态误差等高线图

Fig.2  Contour diagram of pitch angle error

由式(2)，可得滚转角的测姿误差为：

$$\mathrm{\Delta }\phi =\mathrm{\Delta }A\left(\frac{\mathrm{c}\mathrm{o}\mathrm{s}\phi }{g\mathrm{c}\mathrm{o}\mathrm{s}\theta }-\frac{\mathrm{s}\mathrm{i}\mathrm{n}\phi }{g\mathrm{c}\mathrm{o}\mathrm{s}\theta }\right)$$

(4)

由式(4)可见，在俯仰角为大角度时，滚转角仍存在较大的测姿误差。当$\theta \to \pm \mathrm{90}°$时，加速度计无法获得准确的滚转角。

1.2 基于陀螺仪的测姿原理及误差分析

由欧拉微分方程，欧拉角的角速率${\left[\dot{\psi },\dot{\theta },\dot{\phi }\right]}^{\mathrm{T}}$与载体系相对地理坐标系的角速度${\mathit{\omega }}_{\mathrm{n}\mathrm{b}}^{\mathrm{b}}={\left[{\omega }_{\mathrm{n}\mathrm{b}x}^{\mathrm{b}},{\omega }_{\mathrm{n}\mathrm{b}\mathrm{y}}^{\mathrm{b}},{\omega }_{\mathrm{n}\mathrm{b}\mathrm{z}}^{\mathrm{b}}\right]}^{\mathrm{T}}$的关系为：

$$\left\{\begin{array}{l}\dot{\psi }=\left({\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}\mathrm{s}\mathrm{i}\mathrm{n}\phi +{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}\mathrm{c}\mathrm{o}\mathrm{s}\phi \right)/\mathrm{c}\mathrm{o}\mathrm{s}\theta \\ \dot{\theta }={\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}\mathrm{c}\mathrm{o}\mathrm{s}\phi -{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}\mathrm{s}\mathrm{i}\mathrm{n}\phi \\ \dot{\phi }={\omega }_{\mathrm{n}\mathrm{b}x}^{\mathrm{b}}+\dot{\psi }\mathrm{s}\mathrm{i}\mathrm{n}\theta \end{array}\right.$$

(5)

风洞测姿模型的位移角速度${\mathit{\omega }}_{\mathrm{e}\mathrm{n}}^{\mathrm{n}}$可视为0，有${\mathit{\omega }}_{\mathrm{n}\mathrm{b}}^{\mathrm{b}}\approx {\mathit{\omega }}_{\mathrm{i}\mathrm{b}}^{\mathrm{b}}$，即${\mathit{\omega }}_{\mathrm{n}\mathrm{b}}^{\mathrm{b}}$可由陀螺仪测得的角速度近似。但欧拉角在俯仰角$\theta =\pm \mathrm{90}°$时存在奇异。

2 改进的二自由度测姿

2.1 基于加速度计的预置欧拉旋转俯仰角测姿

由式(2)可知，当俯仰角为大角度时，俯仰角测姿误差中的第一项$\mathrm{c}\mathrm{o}\mathrm{s}\theta$为小值，测姿误差主要由第2项，即滚转角引起。本文基于预置欧拉旋转，将滚转角预先旋转一个角度$\lambda$，降低滚转角对俯仰角姿态误差的影响。设测姿单元预先绕载体$\mathrm{b}$系的$x_{\mathrm{b}}$轴转动$\lambda$角，构成预置${\mathrm{b}}_{\mathrm{1}}$系。预置${\mathrm{b}}_{\mathrm{1}}$系与载体$\mathrm{b}$系之间的方向余弦矩阵${\mathit{C}}_{\mathrm{b}}^{{\mathrm{b}}_{\mathrm{1}}}$为：

$${\mathit{C}}_{\mathrm{b}}^{{\mathrm{b}}_{\mathrm{1}}}=\left[\begin{array}{ccc}\mathrm{1}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{c}\mathrm{o}\mathrm{s}\lambda & -\mathrm{s}\mathrm{i}\mathrm{n}\lambda \\ \mathrm{0}& \mathrm{s}\mathrm{i}\mathrm{n}\lambda & \mathrm{c}\mathrm{o}\mathrm{s}\lambda \end{array}\right]$$

(6)

预置欧拉旋转示意图，如图3所示。可得预置${\mathrm{b}}_{\mathrm{1}}$系下各轴加速度计的理想输出比力为：

图3  预置欧拉旋转示意图

Fig.3  Schematic diagram of pre-set Euler rotation

$${\mathit{f}}^{{\mathrm{b}}_{\mathrm{1}}}=\left[\begin{array}{c}{f}_x^{{\mathrm{b}}_{\mathrm{1}}}\\ f_y^{{\mathrm{b}}_{\mathrm{1}}}\\ f_z^{{\mathrm{b}}_{\mathrm{1}}}\end{array}\right]=-{\mathit{C}}_{\mathrm{n}}^{{\mathrm{b}}_{\mathrm{1}}}{g}^{\mathrm{n}}=g\left[\begin{array}{c}-\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ \mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\lambda \right)\\ \mathrm{c}\mathrm{o}\mathrm{s}\theta \mathrm{c}\mathrm{o}\mathrm{s}\left(\phi +\lambda \right)\end{array}\right]$$

(7)

则${\mathrm{b}}_{\mathrm{1}}$系下基于预置欧拉旋转法的俯仰角测姿误差为：

$$\mathrm{\Delta }\theta =-\frac{\mathrm{\Delta }A}{g}\mathrm{c}\mathrm{o}\mathrm{s}\theta -\frac{\mathrm{\Delta }A}{g}\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\lambda +\mathrm{45}°\right)$$

(8)

当俯仰角为$-\mathrm{90}°\le \theta \le \mathrm{0}°$时，要使俯仰角误差绝对值$\left|\mathrm{\Delta }\theta \right|$最小，需$\mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\lambda +\mathrm{45}°\right)\ge \mathrm{0}$，根据滚转角测量范围，进而可求得预置欧拉旋转角为${\lambda }_{\mathrm{1}}=\mathrm{45}°$。当俯仰角为$\mathrm{0}°\le \theta \le \mathrm{90}°$时，要使$\left|\mathrm{\Delta }\theta \right|$最小，需$\mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\lambda +\mathrm{45}°\right)\le \mathrm{0}$，根据滚转角测量范围，可求得预置欧拉旋转角为${\lambda }_{\mathrm{2}}=-\mathrm{135}°$，即：

$$\left\{\begin{array}{l}{\lambda }_{\mathrm{1}}=\mathrm{45}°,\hspace{1em}\hspace{1em}-\mathrm{90}°\le \theta \le \mathrm{0}°\\ {\lambda }_{\mathrm{2}}=-\mathrm{135}°,\hspace{1em}\mathrm{0}°\le \theta \le \mathrm{90}°\end{array}\right.$$

(9)

根据式(8)，本文采用2套三轴加速度计分别对应预置欧拉旋转角，其空间指向如图4所示。

图4  两套三轴加速度计空间指向示意图

Fig.4  Schematic diagram of the spatial orientations of two sets of triaxial accelerometers

当俯仰角为$-\mathrm{90}°\le \theta \le \mathrm{0}°$时，以预置旋转${\lambda }_{\mathrm{1}}$角的三轴加速度计$A_{\mathrm{1}},A_{\mathrm{2}},A_{\mathrm{3}}$联立求解俯仰角；当俯仰角为$\mathrm{0}°\le \theta \le \mathrm{90}°$时，以预置旋转${\lambda }_{\mathrm{2}}$角的三轴加速度计$A_{\mathrm{4}},A_{\mathrm{5}},A_{\mathrm{6}}$联立求解俯仰角。假定各个轴加速度计的输出误差相当，即认为$\mathrm{\Delta }{f}_x^{\mathrm{b}}\approx \mathrm{\Delta }{f}_y^{\mathrm{b}}\approx \mathrm{\Delta }{f}_z^{\mathrm{b}}=\mathrm{\Delta }A$，则俯仰角的测姿误差$\mathrm{\Delta }\theta$可表示为：

$$\mathrm{\Delta }\theta =\left\{\begin{array}{cc}-\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi -\mathrm{90}°\right)\right)/g,& \mathrm{0}°\le \theta \le \mathrm{90}°\\ -\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\mathrm{90}°\right)\right)/g,& -\mathrm{90}°\le \theta \le \mathrm{0}°\end{array}\right.$$

(10)

在加速度计精度为${\mathrm{10}}^{-\mathrm{4}}g$量级时，基于预置欧拉旋转法的俯仰角测姿误差如图5所示。从图5可见基于预置欧拉旋转的测姿方法，可有效抑制大俯仰角时俯仰角误差激增问题。

图5  基于预置欧拉旋转法的俯仰角姿态误差示意图

Fig.5  Contour diagram of pitch angle error based on the pre-set Euler rotation method

2.2 大俯仰角条件下基于陀螺仪的滚转角测姿

当俯仰角$\theta \to \pm \mathrm{90}°$时，加速度计无法准确获取滚转角。本文采用陀螺仪进行滚转角测姿。对于方位角不变的二自由度转动，理想方位角速率为$\dot{\psi }=\mathrm{0}$。而根据式(5)所示的欧拉微分方程，在俯仰角$\theta =\pm \mathrm{90}°$时出现“奇异点”，导致方位角和滚转角突变，此奇异点为欧拉微分算法引起的算法奇异。

针对方位角不变的二自由度转动，本文提出欧拉微分解耦算法，算法中陀螺仪不再跟踪方位角变化，将式(5)改写为：

$$\left\{\begin{array}{l}{\omega }_{\mathrm{n}\mathrm{b}x}^{\mathrm{b}}=\dot{\phi }\\ {\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}=\dot{\theta }\mathrm{c}\mathrm{o}\mathrm{s}\phi \\ {\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}=-\dot{\theta }\mathrm{s}\mathrm{i}\mathrm{n}\phi \end{array}\right.$$

(11)

2.2.1 基于陀螺仪的解耦积分法

由式(11)，滚转角可由$x_{\mathrm{b}}$轴陀螺仪输出角速度积分获取：

$$\phi =\int {\omega }_{\mathrm{n}\mathrm{b}x}^{\mathrm{b}}\mathrm{d}t+{\phi }_{\mathrm{0}}$$

(12)

式中${\phi }_{\mathrm{0}}$为初始滚转角，可通过初始对准获取。由式(12)可求取滚转角，但长时间积分会出现累积的姿态误差。由于风洞试验模型体积有限，无法安装高精确度陀螺仪。低精确度的微机电系统(Micro-Electro-Mechanical System，MEMS)陀螺仪，积分造成的累积误差会更大。

2.2.2 基于陀螺仪的角速度解耦投影法

本文针对俯仰/滚转耦合的二自由度运动提出一种角速度投影求解滚转角的新算法。根据式(11)，在俯仰角速率$\dot{\theta }\ne \mathrm{0}$时，滚转角可由$y$轴陀螺仪与$z$轴陀螺仪联立获取：

$$\phi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(-{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}/{\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}\right)$$

(13)

由式(13)可得滚转角测量误差：

$$\mathrm{\Delta }\phi =-\frac{\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}\mathrm{c}\mathrm{o}\mathrm{s}\phi +\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}\mathrm{s}\mathrm{i}\mathrm{n}\phi }{\dot{\theta }}$$

(14)

由式(14)可见，滚转角测量误差不仅受到$y,z$轴陀螺仪输出误差的影响，同时与俯仰角速率$\dot{\theta }$有关，俯仰角速率$\dot{\theta }$越大，滚转角误差越小。设MEMS陀螺仪测量误差为0.2°/s，滚转角测量误差$\mathrm{\Delta }\phi$随俯仰角速率的变化趋势如图6所示。

图6  滚转角测量误差随俯仰角速率的变化趋势

Fig.6  Variation trend of roll angle measurement error with pitch rate

当俯仰角速率$\dot{\theta }\to \mathrm{0}$时，${\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}},{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}$均为0，滚转角为：

$$\phi =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}\left(-\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}z}^{\mathrm{b}}/\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}y}^{\mathrm{b}}\right)$$

(15)

根据式(14)，当俯仰角速率$\dot{\theta }\to \mathrm{0}$时，滚转角的计算值只与陀螺仪的随机误差相关，解算得出的滚转角无意义，俯仰角速率$\dot{\theta }\to \mathrm{0}$时，角速度投影法不能测量滚转角。由于积分运算只能在短时间内保证精度，而角速度投影法没有累积误差，可以保证长时间的姿态精度，但需要满足俯仰角速率足够大的条件，综合考虑积分运算法与角速度投影法，本文进而提出一种新算法，如图7所示。当俯仰角速率$\dot{\theta }$大于设定俯仰角速率阈值${\dot{\theta }}_{\mathrm{t}}$时，采用角速度投影法获取滚转角姿态，否则采用积分法。角速率阈值应根据测姿时间和角速度变化来选取。角速度变化较大，姿态误差主要由积分造成，而积分运算时间应尽可能短，否则引起的漂移太大，此时俯仰角速率阈值应设为小值；角速度变化较小，姿态误差主要由角速度投影造成，此时俯仰角速率阈值应设为大值。

图7  滚转角姿态测量算法流程图

Fig.7  Flow of roll angle attitude measurement algorithm

3 仿真验证

3.1 俯仰角测姿

本文假定加速度计精度为${\mathrm{10}}^{-\mathrm{4}}g$量级，将基于预置欧拉旋转与基于三轴加速度计的俯仰角误差进行对比。

当俯仰角范围为$\mathrm{0}°\le \theta \le \mathrm{90}°$，基于预置欧拉旋转的俯仰角误差$\mathrm{\Delta }{\theta }_{\mathrm{P}}$与基于三轴加速度计的俯仰角误差$\mathrm{\Delta }{\theta }_{\mathrm{T}}$为：

$$\left\{\begin{array}{l}\mathrm{\Delta }{\theta }_{\mathrm{T}}=-\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\mathrm{45}°\right)\right)/g\\ \mathrm{\Delta }{\theta }_{\mathrm{P}}=-\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi -\mathrm{90}°\right)\right)/g\end{array}\right.$$

当俯仰角为$-\mathrm{90}°\le \theta \le \mathrm{0}°$，基于预置欧拉旋转的俯仰角误差$\mathrm{\Delta }{\theta }_{\mathrm{P}}$与基于三轴加速度计的俯仰角误差$\mathrm{\Delta }{\theta }_{\mathrm{T}}$为：

$$\left\{\begin{array}{l}\mathrm{\Delta }{\theta }_{\mathrm{T}}=-\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\mathrm{45}°\right)\right)/g\\ \mathrm{\Delta }{\theta }_{\mathrm{P}}=-\mathrm{\Delta }A\left(\mathrm{c}\mathrm{o}\mathrm{s}\theta +\sqrt[]{\mathrm{2}}\mathrm{s}\mathrm{i}\mathrm{n}\theta \mathrm{s}\mathrm{i}\mathrm{n}\left(\phi +\mathrm{90}°\right)\right)/g\end{array}\right.$$

仿真结果如图8所示，可见基于预置欧拉旋转的俯仰角测姿精度有明显提升。

图8  俯仰角测姿误差仿真对比图

Fig.8  Simulation comparison of pitch angle error

3.2 滚转角测姿

设各轴陀螺仪输出误差为$\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}x}^{\mathrm{b}}=$$\mathrm{\Delta }{\omega }_{\mathrm{n}\mathrm{b}\mathrm{z}}^{\mathrm{b}}=$0.2°/s，初始滚转角${\phi }_{\mathrm{0}}=\mathrm{0}°$。当真实俯仰角速率分别为10°/s,20°/s,60°/s，真实滚转角速率为0°/s时，分别采用欧拉微分法、积分解耦算法、角速度投影解耦算法求取滚转角。欧拉微分法滚转角误差为$\mathrm{\Delta }{\phi }_{\mathrm{E}}$；积分解耦算法滚转角误差为$\mathrm{\Delta }{\phi }_{\mathrm{I}}$；角速度投影解耦算法滚转角误差为$\mathrm{\Delta }{\phi }_{\mathrm{P}}$。上述方法求解的滚转角姿态误差如图9所示。

图9  匀速转动条件下滚转角测姿误差仿真对比

Fig.9  Simulation comparison of roll angle error under the condition of uniform rotation

由图9可知：a) 当俯仰角$\theta =\pm \mathrm{90}°$时，欧拉微分法求解的滚转角发生突变；b) 积分解耦算法求解的滚转角没有突变，随着时间的增加，姿态误差累积增大；3) 角速度投影解耦算法求解的滚转角能保持长时间的姿态精度稳定性，姿态误差与俯仰角速率$\dot{\theta }$有关，在俯仰角速率$\dot{\theta }$较大时，滚转角测姿精确度高；$\dot{\theta }$较小时，滚转角测姿精确度较低。

匀速转动条件下，只要俯仰角速率较大，滚转角测姿精确度就会比较高，但是在角振荡环境下，俯仰角的变化为正弦变化：

$$\theta ={\theta }_{\mathrm{m}}\mathrm{s}\mathrm{i}\mathrm{n}\left(\mathrm{2}\mathrm{\pi }ft\right)$$

(16)

式中：${\theta }_{\mathrm{m}}$为俯仰角振荡幅值；$f$为俯仰角振荡频率。

俯仰角速率为：

$$\dot{\theta }=\mathrm{2}\mathrm{\pi }f{\theta }_{\mathrm{m}}\mathrm{c}\mathrm{o}\mathrm{s}\left(\mathrm{2}\mathrm{\pi }ft\right)$$

(17)

假设俯仰角振荡幅值为40°，振荡频率为0.5 Hz，此时俯仰角速率的变化如图10所示。当俯仰角速率逐渐趋近0时，角速度投影法的滚转角误差会突变，如图11所示。从图11可见，振荡环境下基于角速度投影法求解滚转角的精确度难以满足实际测姿需求，而积分法只能保证短时测姿精确度，为此采用角速度投影结合陀螺积分算法求取滚转角。

图10  振荡环境下俯仰角速率变化

Fig.10  Pitch rate change under oscillation environment

图11  振荡环境下基于角速度投影法的滚转角误差

Fig.11  Roll angle error based on the angular velocity projection method under oscillation environment

假定俯仰角速率阈值为${\dot{\theta }}_t=\mathrm{0.5}°/\mathrm{s}$，当俯仰角速率大于阈值${\dot{\theta }}_t$时，角速度投影法信噪比高，采用角速度投影法求取滚转角可抑制滚转角误差的漂移；当俯仰角速率小于等于阈值${\dot{\theta }}_{\mathrm{t}}$时，角速度投影法信噪比低，采用陀螺积分算法求取滚转角可避免角速度投影法造成的姿态误差放大。新算法求解的滚转角姿态误差$\mathrm{\Delta }{\phi }_{\mathrm{G}}$如图12所示。从图12可以看到，在振荡环境下滚转角测姿新算法能有效地避免俯仰角速率较小时，采用角速度投影解耦算法造成的姿态误差突变问题，能在振荡环境下长时间保持较高的精确度。

图12  振荡环境下基于新算法的滚转角误差

Fig.12  Roll angle error based on the new algorithm under oscillation environment

4 结论

本文对大量程条件下俯仰/滚转二自由度耦合的姿态测量方法进行研究，分析在大角度条件下基于陀螺仪和加速度计的测姿方式中姿态误差放大的原因。本文提出的预置欧拉旋转的俯仰角测姿算法，确定冗余加速度计的布置方式，实现在大角度条件下俯仰角的高精度测量。提出的积分运算与角速度投影相结合的滚转角测姿新算法，在俯仰角速率大于设定阈值时可抑制滚转角误差的漂移，在俯仰角速率小于等于设定阈值时可避免角速度投影法造成的姿态误差放大，从而能在振荡环境下长时间保持较高的精度。