CN103196443B - 基于光流和附加信息的飞行体姿态测量方法与系统 - Google Patents

Abstract

本发明公开了一种飞行体姿态测量方法与系统，主要是为了能够高效、可靠地测量飞行体姿态而设计。本发明所述方法包括：连续测量飞行体上若干点处下视方向光流信息；连续测量飞行体上若干点处下列附加信息的一种或几种：高度信息，距地面距离信息，部分姿态角度信息，部分姿态角角速度信息；基于测量得到的光流信息和附加信息解算出飞行体的运动姿态；将得到的运动姿态输出到外接设备。基于上述步骤，本发明能够测量飞行体姿态，可用于飞机、导弹的飞行姿态测量和控制、陀螺仪的校准、地表地形的测量，也可用于其他需要测量一个物体相对一平面的运动姿态的情形。

Description

基于光流和附加信息的飞行体姿态测量方法与系统

技术领域

本发明涉及航空领域，尤其涉及一种基于光流和附加信息的飞行体姿态测量方法与系统。

背景技术

飞机飞行姿态是指飞机在飞行过程中俯仰、滚转、偏航的程度，其可以量化为俯仰角、滚转角、偏航角。为保证飞机正常飞行，需要对飞机的飞行姿态进行测量和控制。

目前常用的飞机姿态测量方法是机械陀螺仪和光纤陀螺仪。机械陀螺仪依靠转子的空间指向稳定性确定飞机姿态，由于摩擦等原因的影响，转子轴向会逐渐偏离原来位置，随着时间的推移，输出的飞机姿态会逐渐偏离真实情况。光纤陀螺仪测量俯仰角速度、滚转角速度、偏航角速度，然后积分得到俯仰角、滚转角、偏航角，随着时间的推移，计算得到的角度值偏差会越来越大。为了保持姿态测量的精度，需要一种测量精度不随时间变化的测量方法来测量飞机飞行姿态，或使用该方法对陀螺仪进行校准。

对于小型无人机，陀螺仪价格高、重量大，且降落时易因受到机械冲击而发生损坏。因此，需要有一种价格低、重量轻、耐机械冲击的飞机飞行姿态测量系统。

目前有一些基于一维和二维图像识别的飞行体姿态测量方法，但其有较大的局限性。地平线识别方法可以给出飞行体滚转角，但差分得到的滚转角速度不够准确，在识别俯仰角、俯仰角速度和偏航角速度方面尚有困难，且受地平线处地形影响，不适合在地形起伏较大的地区使用，也不适合在城市楼群中、森林中和室内环境使用。飞行体起飞着陆和低空飞行时，红外地平仪受建筑、车辆、人、动物等地面物体影响，在早晨和傍晚，红外地平仪受太阳影响。

光流是空间运动物体在观测平面上所成的像的运动速度，光流法能够在不知道场景的任何信息的情况下，检测出运动对象，并判断其运动情况。目前，光流方法已经在飞行体姿态测量方面有所应用，如瑞士EcolePolytechnique Federale de Lausanne(EPFL)制成的一维光流信息飞机姿态感知和控制系统，可以测量偏航角速度，并以此积分出偏航角度；另外，专利申请“CN102654917A，运动体运动姿态感知方法及系统”中所述的运动体运动姿态感知方法及系统使用纯光流方法测量运动体的运动姿态，其虽能计算得到飞机姿态的全部信息，但需对所有光流传感器的信息进行联合求解，需要求解大规模超越方程组，该方程组解析解推导较为复杂，而其数值求解较慢，当飞行体控制需要较快的数据更新速度时，需要使用昂贵的高速计算芯片才能满足要求。

为了降低飞行体制造成本、方便飞行体设计，需要一种能够尽量使用飞行体已有设备和系统的飞行体姿态测量方法。

发明内容

对上述问题，本发明提供了一种能够提供测量精度不随时间推移而降低的，价格低廉，耐机械冲击的飞行体姿态测量方法与系统。

为实现上述目的，本发明采用以下技术方案：

本发明公开了一种飞行体姿态测量方法，其包括：

步骤1、测量飞行体下视方向的光流信息，同时测量附加信息；所述附加信息包括飞行体的位置信息或部分姿态信息；

步骤2、根据所测得的光流信息和附加信息，判断是否具有足够的正确测量信息；若足够则执行步骤3，否则发出报警信号；

步骤3、根据所测得的光流信息和附加信息解算飞行体的飞行姿态信息，并将得到的姿态信息输出。

本发明还公开了一种飞行体姿态测量系统，其包括：

光流信息测量单元，用于测量飞行体下视方向的光流信息；

附加信息测量单元，用于测量飞行体的附加信息；所述附加信息包括飞行体的位置信息或部分姿态信息；

测量信息检查器：用于检查光流信息测量单元和附加信息测量单元是否测出足够的正确测量信息，如果足够，则将足够的正确测量信息发送给飞行姿态解算器，否则发出报警信号；

飞行姿态解算器：用于根据从测量信息检查器接收到的光流信息和附加信息计算输出飞行体的姿态信息。

本发明由于采用以上技术方案，其具有以下优点：

1)本发明可配合速率陀螺，直接输出飞行体姿态角度、姿态角角速度、相对地面高度，无积分误差，输出值的精度不随时间推移而降低。

2)本发明中，所述姿态解算系统计算飞行体飞行姿态使用的算法为解代数方程组，不需要积分即可获得俯仰角、滚转角、俯仰角速度、滚转角速度、偏航角速度，因此上述五个姿态信息无积分误差，其测量精度不随时间的推移而变差，特别适合导弹等需要长时间依靠惯性导航飞行的飞行体的姿态测量和上述飞行体所搭载陀螺仪的校准；

3)本发明中，使用飞机偏航角速度积分得到飞机偏航角，故可以提供飞机偏航角信息；

4)本发明在附加信息测量系统使用特定种类的测量单元时，可以测量飞行体距地面的飞行高度；

5)本发明在附加信息测量系统使用特定种类的测量单元时，可以在室内、楼群中、树林中等环境中使用；

6)本发明中，所述光流测量系统，使用的光流传感器价格低、重量轻、耐机械冲击，可以适用于低成本小型无人飞行体，或其它动态系统的低成本姿态测量；

7)本发明中，可通过增加光流传感器个数和附加信息测量单元个数，作为冗余备份，确保任意时刻始终有足够多的正确的光流信息和附加信息，提高系统的可靠性；

8)本发明可与多种设备配合，最大限度利用飞行体原有设备。

本发明可提高飞行体性能，降低飞行体制造成本，减轻飞行体重量，因此，可广泛应用于飞机/导弹姿态测量和陀螺仪校准等领域，并可应用于任何动态系统的长时间低成本姿态测量。

附图说明

图1是本发明中飞行体姿态测量方法流程示意图；

图2是本发明中飞机参考系与地面坐标系之间的关系示意图；

图3是本发明中光流传感器参考系与地面坐标系之间的关系示意图；

图4是本发明中飞行体姿态测量系统结构图；

图5是本发明中飞行体姿态测量系统的一个优选实施例；

图6是本发明中飞行体姿态测量系统的另一优选实施例；

图7是本发明中飞行体姿态测量系统的另一优选实施例；

图8是本发明中飞行体姿态测量系统的另一优选实施例。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

图1是本发明中飞行体姿态测量方法流程示意图，如图1所示，本发明公开了一种飞行体姿态测量方法。该方法包括以下步骤：

步骤1、测量飞行体下视方向的光流信息，同时测量附加信息；其中飞行体可以是飞机、小型无人机、导弹等飞行体；所述光流，指一个相机拍摄的连续图像中，图像中特征物体的移动情况，其正比于相机和被拍摄物体的横向相对速度，反比于相机和被拍摄物体的距离；所述光流信息可以通过一个或多个光流传感器获得，也可以通过摄像头连续捕捉图像计算获得；所述附加信息包括飞行体的位置信息和/或部分姿态信息，如飞行体上任意一点相对于地面的高度、飞行体上任意一点到地面上正对点的距离、飞行体的至少一个姿态角、飞行体的至少一个姿态角角速度等；所述飞行体上任意一点到地面上正对点，为从飞行体上这点沿一相对飞行体确定的方向延伸的射线与地面的交点。

步骤2、根据所测得的光流信息和附加信息，判断是否具有足够的正确测量信息；若足够则执行步骤3，否则发出报警信号；

步骤3、根据所测得的光流信息和附加信息解算飞行体的飞行姿态信息，并将得到的姿态信息输出至外接设备，其中所述姿态信息包括飞行体的俯仰角、滚转角、偏航角角速度、俯仰角角速度和滚转角角速度。

所述外接设备可以包括但不限于计算机、机载总线、通信设备等。所述测量信息不正确，其可能包括无信号、信号过大或过小等，其可能的原因包括但不限于光流传感器故障、附加信息测量单元故障、被测值超出光流传感器或附加信息测量单元等测量设备的量程等。

图2为本发明中飞机参考系与地面坐标系之间的关系示意图。以飞机为例，取飞机参考系为前-左-上坐标系，如图2所示，其中，对应飞机飞行前向，对应飞机左翼指向，对应飞机机体正上方。飞机参考系的原点在地面系OXYZ中的坐标为：R＝(x y z)^T，飞机平动速度为： $v={\left(\begin{array}{ccc}\stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right)}^T,$ 分别为三个方向的平动速度分量，符号上方的点表示相应物理量对时间的导数。

对于现有飞机姿态测量系统，陀螺仪测得的三轴角速度为绕三轴转动的角速度。故建立3-2-1转动系，三个姿态角依次为γβα，分别对应偏航角、俯仰角和滚转角。由飞机参考系到地面参考系的坐标转换矩阵为A，如下表示：

$A=\left(\begin{array}{ccc}\cos \mathrm{\γ}& -\sin \mathrm{\γ}& 0\\ \sin \mathrm{\γ}& \cos \mathrm{\γ}& 0\\ 0& 0& 1\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& -\sin \mathrm{\α}\\ 0& \sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right)$

图2和以下各个实施例中所使用的参考系均为正交直角坐标系，但任何采用其他坐标系的方法和系统也在本发明的保护之列，因使用公知的方法可在各种坐标系间对位置、速度、角度、角速度等量进行转换，故在各种坐标系下使用的方法和系统是等价的。

图3示出了光流传感器参考系与地面参考系之间的关系。如图3所示，各个量中上标含有“^”的表示飞机参考系中的量，上标含有“～”的表示光流传感器参考系中的量。光流传感器参考系各个坐标轴分别记为其指向分别与飞机参考系的三个坐标轴相同，第i个光流传感器在飞机参考系中的位置为 ${\hat{r}}_i={\left(\begin{array}{ccc}{\hat{x}}_i& {\hat{y}}_i& {\hat{z}}_i\end{array}\right)}^T.$ 记第i个光流传感器到其正对的地面上的点的距离为d_i。当光流传感器参考系的三个坐标轴的指向分别与飞机参考系的三个坐标轴相同时，从飞机参考系到地面参考系的坐标转换矩阵和从光流传感器参考系到地面参考系的坐标转换矩阵相同，相应的三个姿态角的数值也相同，相应的三个姿态角角速度的数值也相同，则两者可使用同样的坐标转换矩阵、姿态角和姿态角速度描述，且光流传感器测得的光流信息即光流传感器参考系中测得的光流信息也可直接作为飞机参考系中光流传感器位置处测得的光流信息使用，即

$I_i=\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)---\left(1\right)$

其中，为第i个光流传感器在其自身坐标系下测量得到的x方向光流值，为第i个光流传感器在其自身坐标系下测量得到的y方向光流值；为第i个光流传感器在飞机参考系中第i个光流传感器位置处测得的x方向光流值，为第i个光流传感器在飞机参考系中第i个光流传感器位置处测量得到的y方向光流值。

则第i个光流传感器测得的光流信息，可根据下面的光流方程表示，即为：

$I_i=-\frac{1}{d_i}{\mathrm{CA}}^T\·[\stackrel{\·}{A}\·{\left(\begin{array}{ccc}-{\hat{x}}_i& -{\hat{y}}_i& d_i\end{array}\right)}^T-{\left(\begin{array}{ccc}\stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right)}^T]---\left(2\right)$

式中

$C=\left(\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right),$ $d_i=\frac{z-\sin \mathrm{\β}\·{\hat{x}}_i+\sin \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{y}}_i}{\cos \mathrm{\α}\·\cos \mathrm{\β}}+{\hat{z}}_i$

$z=z_h+\sin \mathrm{\β}\·{\hat{x}}_h-\sin \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{y}}_h-\cos \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{z}}_h$

其中，表示矩阵A对时间求导，d_i表示第i个光流传感器到其正对的地面上的点的距离，z表示飞机参考系原点在地面系中的高度，为高度传感器在飞机参考系中的位置坐标，z_h是高度传感器测得的飞机距地面的相对高度。

将上述方程(2)写成分量形式，则为：

$\left(\begin{array}{c}{I}_{\mathrm{ix}}\\ I_{\mathrm{iy}}\end{array}\right)=\frac{1}{d_i}\left(\begin{array}{ccc}\cos \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\β}\·\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ -\cos \mathrm{\α}\·\sin \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\α}\·\cos \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\sin \mathrm{\γ}& \sin \mathrm{\α}\·\cos \mathrm{\β}\end{array}\right)\left(\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{z}\end{array}\right)$

$-\frac{1}{d_i}(\stackrel{\·}{\mathrm{\γ}}\left(\begin{array}{c}{\hat{y}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -{\hat{x}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\sin \mathrm{\β}\end{array}\right)+\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-{\hat{y}}_i\sin \mathrm{\α}+d_i\cos \mathrm{\α}\\ {\hat{x}}_i\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -d_i\end{array}\right))---\left(3\right)$

其中，分别表示飞机的偏航角、俯仰角和滚转角对时间的导数，其用于表征飞机的飞行姿态，但并不是飞机的“角速度”。而角速度值与的关系为：

$\left(\begin{array}{c}{\widehat{\mathrm{\ω}}}_1\\ {\widehat{\mathrm{\ω}}}_2\\ {\widehat{\mathrm{\ω}}}_3\end{array}\right)=\left(\begin{array}{ccc}-\cos \mathrm{\α}\sin \mathrm{\β}& 0& 1\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ \cos \mathrm{\α}\cos \mathrm{\β}& -\sin \mathrm{\α}& 0\end{array}\right)\left(\begin{array}{c}\stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\β}}\\ \stackrel{\·}{\mathrm{\α}}\end{array}\right)---\left(4\right)$

式中，为飞机参考系中飞机角速度的三个分量，即滚转角角速度、俯仰角角速度和偏航角角速度。

对于上述式(3)，第i个光流传感器测得的光流信息是一个二维向量方程。因此，使用两个光流传感器的光流信息，可联立各个向量方程，消去 $v={\left(\begin{array}{ccc}\stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right)}^T,$ 如下列公式所示：

$\left(\begin{array}{c}{d}_j{I}_{\mathrm{jx}}-d_i{I}_{\mathrm{ix}}\\ d_j{I}_{\mathrm{jy}}-d_i{I}_{\mathrm{iy}}\end{array}\right)=\stackrel{\·}{\mathrm{\γ}}\left(\begin{array}{c}({\hat{y}}_i-{\hat{y}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -({\hat{x}}_i-{\hat{x}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\sin \mathrm{\β}\end{array}\right)$

$+\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-({\hat{y}}_i-{\hat{y}}_j)\sin \mathrm{\α}+(d_i-d_j)\cos \mathrm{\α}\\ ({\hat{x}}_i-{\hat{x}}_j)\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -(d_i-d_j)\end{array}\right)---\left(5\right)$

式中，下标中含有i的量对应第i个传感器的相应数值，下标中含有j的量对应第j个传感器的相应数值。再将式(2)给出的d_i和d_j计算公式代入式(5)，此时，式(5)中除了光流信息外，只含飞机的滚转角α、俯仰角β、滚转角对时间求导值俯仰角对时间求导值偏航角对时间求导值飞机参考系原点在地面系中的高度z。据此可得飞机参考系中飞机的角速度 $\mathrm{\ω}=\left[\begin{array}{ccc}{\widehat{\mathrm{\ω}}}_1& {\widehat{\mathrm{\ω}}}_2& {\widehat{\mathrm{\ω}}}_3\end{array}\right],$ 如下列公式所示：

${\widehat{\mathrm{\ω}}}_1=-\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\sin \mathrm{\β}+\stackrel{\·}{\mathrm{\α}}$

${\widehat{\mathrm{\ω}}}_2=\stackrel{\·}{\mathrm{\γ}}\sin \mathrm{\α}+\stackrel{\·}{\mathrm{\β}}\cos \mathrm{\α}$

${\widehat{\mathrm{\ω}}}_3=\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\cos \mathrm{\β}-\stackrel{\·}{\mathrm{\β}\sin }\mathrm{\α}$

式中，分别为滚转角速度、俯仰角速度和偏航角速度。

当光流传感器个数多于4个且不在同一条直线上时，由上述消去 $v={\left(\begin{array}{ccc}\stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right)}^T$ 的公式，得到关于光流信息、飞机参考系原点在地面系中的高度、飞机参考系中光流传感器的位置、飞机姿态角度和角速度的方程组。代入光流信息、飞机参考系原点在地面系中的高度、飞机参考系中光流传感器位置，可得飞机姿态角度和角速度。

光流信息由光流测量系统测量得到，飞机参考系中光流传感器位置已知，使用附加信息得到飞机参考系原点在地面系中的高度，将这些信息代入上述方程组，即可解得飞机姿态角度和角速度。

上述光流算法使用了飞机飞行高度作为输入，基于上述关系式可导出高度与其他与飞机飞行有关的信息的关系，故也可以使用其他信息代替高度信息输入光流算法，例如距地面距离信息、部分姿态角度信息、部分姿态角角速度信息等，这些信息即为本发明所述的附加信息。

当光流传感器参考系各个坐标轴与飞机参考系各个坐标轴方向不相同时，须将上述光流方程转换到飞机参考系中，记A_i为第i个光流传感器自身坐标系到飞行体参考系中的坐标转换矩阵，记光流传感器参考系中测得的光流为：

$\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)$

则有飞机参考系中的光流为：

$\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=A_i\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)$

上述坐标系转换的方法为飞行器设计、飞行器控制、以及汽车、建筑、土木、计算机等各个领域和学科的专业人员所公知。

作为一个例子，当光流传感器参考系的坐标轴的指向与飞机参考系的坐标轴相同，而光流传感器参考系的两个坐标轴的指向与飞机参考系的两个坐标轴不同时，对正交直角坐标系，光流传感器参考系可由飞机参考系绕轴转动ζ角度获得，仍记光流传感器参考系中测得的光流为：

$\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)$

此时，第i个光流传感器自身坐标系到飞行体参考系中的坐标转换矩阵为：

$A_i=\left(\begin{array}{cc}\cos \mathrm{\ζ}& -\sin \mathrm{\ζ}\\ \sin \mathrm{\ζ}& \cos \mathrm{\ζ}\end{array}\right)$

则飞机参考系中的光流为：

$\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=A_i\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)=\left(\begin{array}{cc}\cos \mathrm{\ζ}& -\sin \mathrm{\ζ}\\ \sin \mathrm{\ζ}& \cos \mathrm{\ζ}\end{array}\right)\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)$

光流方程相应变为：

$\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=\frac{1}{d_i}\left(\begin{array}{ccc}\cos \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\β}\·\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ -\cos \mathrm{\α}\·\sin \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\α}\·\cos \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\sin \mathrm{\γ}& \sin \mathrm{\α}\·\cos \mathrm{\β}\end{array}\right)\left(\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{z}\end{array}\right)$

$-\frac{1}{d_i}(\stackrel{\·}{\mathrm{\γ}}\left(\begin{array}{c}{\hat{y}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -{\hat{x}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\sin \mathrm{\β}\end{array}\right)+\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-{\hat{y}}_i\sin \mathrm{\α}+d_i\cos \mathrm{\α}\\ {\hat{x}}_i\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -d_i\end{array}\right))---\left(7\right)$

即

$\left(\begin{array}{cc}\cos \mathrm{\ζ}& -\sin \mathrm{\ζ}\\ \sin \mathrm{\ζ}& \cos \mathrm{\ζ}\end{array}\right)\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right)=\frac{1}{d_i}\left(\begin{array}{ccc}\cos \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\β}\·\sin \mathrm{\γ}& -\sin \mathrm{\β}\\ -\cos \mathrm{\α}\·\sin \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\cos \mathrm{\γ}& \cos \mathrm{\α}\·\cos \mathrm{\γ}+\sin \mathrm{\α}\·\sin \mathrm{\β}\·\sin \mathrm{\γ}& \sin \mathrm{\α}\·\cos \mathrm{\β}\end{array}\right)\left(\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{z}\end{array}\right)$

$-\frac{1}{d_i}(\stackrel{\·}{\mathrm{\γ}}\left(\begin{array}{c}{\hat{y}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -{\hat{x}}_i\cos \mathrm{\β}\·\cos \mathrm{\α}+d_i\sin \mathrm{\β}\end{array}\right)+\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-{\hat{y}}_i\sin \mathrm{\α}+d_i\cos \mathrm{\α}\\ {\hat{x}}_i\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -d_i\end{array}\right))---\left(8\right)$

同样，使用两个光流传感器测得的光流信息，可得消去 $v={\left(\begin{array}{ccc}\stackrel{\·}{x}& \stackrel{\·}{y}& \stackrel{\·}{z}\end{array}\right)}^T$ 后的式子：

$\left(\begin{array}{c}{d}_j{\hat{I}}_{\mathrm{jx}}-d_i{\hat{I}}_{\mathrm{ix}}\\ d_j{\hat{I}}_{\mathrm{jy}}-d_i{\hat{I}}_{\mathrm{iy}}\end{array}\right)=\stackrel{\·}{\mathrm{\γ}}\left(\begin{array}{c}({\hat{y}}_i-{\hat{y}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -({\hat{x}}_i-{\hat{x}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\sin \mathrm{\β}\end{array}\right)$

$+\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-({\hat{y}}_i-{\hat{y}}_j)\sin \mathrm{\α}+(d_i-d_j)\cos \mathrm{\α}\\ ({\hat{x}}_i-{\hat{x}}_j)\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -(d_i-d_j)\end{array}\right)---\left(9\right)$

可见，该式与上述光流传感器参考系的三个坐标轴的指向分别与飞机参考系的三个坐标轴相同时的情况相同。

因此，通过测量得到的光流信息和所述附加信息，将其带入上述式(5)或(9)中，即可得到飞行体的姿态信息。

上述推导过程中，以光流传感器测量得到的光流信息为例，本发明的方案中也可以通过摄像头连续捕捉图像计算获得光流信息，而根据摄像头连续捕捉图像计算获得的所述光流信息和附加信息获得飞行体姿态信息的方式与上面记载的类似，在此不再赘述。

图4示出了本发明中飞行体姿态测量系统结构图，如图4所示，本发明还提出了一种飞行体姿态测量系统，其包括：光流测量单元、附加信息测量单元、测量信息检查器和飞行姿态解算器；其中光流测量单元包括若干个光流传感器，用于测量飞行体的光流信息；附加信息测量单元包括测量飞行体位置信息和/或姿态信息的测量单元，如高度测量单元、距离测量单元、姿态角测量单元、姿态角角速度测量单元；测量信息检查器用于检查光流测量单元和附加信息测量单元测得的光流信息和附加信息，并筛选出可用的光流信息和附加信息进行输出，其输出接飞行姿态解算器和外接设备；所述飞行姿态解算器用于根据所述光流测量单元和附加信息测量单元测得的光流信息和附加信息计算飞行体的姿态信息，并输出至外接设备，以实现对飞行体飞行姿态的控制。其中，为了防止系统中的某一个传感器损坏而导致整个系统失效，通常同时使用多个同种或不同种类传感器进行测量，由测量信息检查器筛选出可用的测量信息，进行姿态解算，提升系统的可靠性。例如，光流传感器可以使用10个甚至20个进行测量，选出其中5个传感器的输出进行姿态解算。可以同时使用两个高度传感器，一个工作异常时使用另一个输出的信号。使用一个高度传感器和一个陀螺仪，当陀螺仪工作正常时，使用陀螺仪输出的角速度信息计算飞机姿态；当陀螺仪工作不正常时，使用高度传感器测量的高度信息进行飞机姿态解算。

所述高度测量单元包括若干个高度传感器，用于测量飞行体相对于地面的高度信息。所述高度传感器，可以包括但不限于气压高度计、GPS高度传感器等。气压高度计便宜，轻便，适用于平坦地面的上空，也可用于在机场等有地面辅助设备的空域低空飞行和起飞着陆；GPS测量高度计可以在任意空域测量绝对高度，故适用于所有已知地面绝对高度信息的空域(这里的“绝对高度”指“海拔”)。

所述距离测量单元包括若干个距离传感器，其用于测量飞机上安装距离传感器处距离地面上正对点的距离，这里距离的定义与图3中光流传感器到地面上正对点的距离d_i的定义相同。因为距离传感器是有指向的，其指向与飞机平面垂直或成一确定夹角而与地面不垂直，故这里的“距离”指距离传感器沿着其指向正对地面上的点到距离传感器的距离。这是激光测距仪的工作方式。所述距离传感器，可以包括但不限于激光测距仪、超声测距仪、双目摄像机测距仪等。激光测距仪精度高，可在室内和/或楼群中使用，同时适用于平原、戈壁以及各种地面植被较为低矮的空域，也适合在机场、公路等平整度较好的区域起飞着陆时使用；超声测距仪体积小，重量轻，价格非常便宜，特别适合在各种地面起飞着陆使用(这里的“各种地面”，其平整度应满足飞机起飞的基本要求)，也适用于各种室内空间。

所述姿态角测量单元包括若干个姿态角传感器，其用于测量飞机的姿态角，可以包括飞机的俯仰角和滚转角之一。利用本发明提出的上述方案，如果飞行体系统中，某些姿态信息的测量值不够精确，就可以利用测量得到的俯仰角或滚转角之一和光流信息，通过式(5)或(9)解算得到其它的姿态信息。所述姿态角传感器，可以包括但不限于倾角计、基于地平线识别的姿态测量装置、陀螺仪、红外地平仪等。在平原地区，因地平线较平，基于地平线识别的姿态测量装置有着很好的精度和稳定性，特别适合平原地区的使用，且最大程度兼顾了高低空工作环境。

所述姿态角角速度测量单元包括若干个姿态角角速度传感器，其用于测量飞行体的姿态角角速度，可以包括飞行体俯仰角角速度、滚转角角速度、偏航角角速度中的一个或几个。同样，利用本发明提出的上述方案，如果飞行体系统中，某些姿态信息的测量值不够精确，就可以利用测量得到的俯仰角角速度、滚转角角速度、偏航角角速度之一和光流信息，通过式(5)或(9)解算得到其它的姿态信息。所述姿态角角速度传感器，可以包括但不限于陀螺仪、基于地平线识别的姿态测量装置、红外地平仪等。陀螺仪广泛应用于飞行体姿态测量，可以精确获得飞行体姿态角速度信息，配合光流方法，可以实现无积分误差的高精度飞行体姿态测量，适合所有光流传感器能正常工作的空域。

所述测量信息检查器和飞行姿态解算器可以通过硬件或软件实现的设备来实现，如通用计算机设备、FPGA、单片机、DSP等。

下面通过几个实施例来说明本发明提出的飞行体姿态测量系统

图5示出了本发明中飞行体姿态测量系统的一个优选实施例，以飞机为例，如5所示，该系统采用了十个光流传感器，它们两两一组固定在飞机机身前部、中部、后部和左右两侧五个位置，其中两两为一组的光流传感器中的一个作为备份光流传感器。具体地，光流传感器01和02位于机身中部，光流传感器11和12位于机身前部，光流传感器21和22位于机身左部，光流传感器31和32位于机身后部，光流传感器41和42位于机身右部，所有光流传感器固定指向垂直于飞机平面方向向下的方向，各个传感器的指向相互平行。另外，该系统中附加信息测量单元采用了一个高度传感器，其位于机身上，用于测量高度传感器所在位置距地面的高度，所述高度传感器还可以包括多个，以精确测量飞机的高度信息，或互为备份。各个光流传感器和高度传感器测量的信息分别传递到测量信息检查器，由测量信息检查器从机身每个位置的两个光流传感器测量信号中选取一个正确的信息，得到五个正确的光流信息，与高度信息一起传递到飞行姿态解算器，解算飞行姿态，并将飞行姿态传递给外接设备；若某一位置的两个光流传感器测量信息均不正确，或高度信息不正确，此时无法解算全部或部分飞行姿态，则向外接设备发出报警信号。该优选实施例展示了一种光流传感器组和高度传感器组的布置方案，其他任何使用不同光流传感器组和高度传感器组排布方式的基于光流传感器和高度信息的姿态测量系统也均在本发明的保护范围之内。本优选实施例同时展示了一种对光流传感器进行备份的方法，显然使用测量信息检查器可以实现其他形式的光流传感器冗余备份，也可实现高度传感器的备份，本实施例同时展示了一种测量信息检查器的实现方法，对于部分光流传感器失效导致无法解算部分姿态信息的情况也可在发出报警信号的同时计算和输出仍可解算的姿态信息，其他任何使用不同实现方法实现测量信息检查器功能的基于光流传感器和高度信息的姿态测量系统也均在本发明的保护范围之内。

当所述5组光流传感器处于飞机参考系中平面上且成十字形布置时，第i个光流传感器处的光流在光流传感器参考系中的值 ${\left(\begin{array}{cc}{\tilde{I}}_{\mathrm{ix}}& {\tilde{I}}_{\mathrm{iy}}\end{array}\right)}^T$ 和在飞机参考系中的值 ${\left(\begin{array}{cc}{\hat{I}}_{\mathrm{ix}}& {\hat{I}}_{\mathrm{iy}}\end{array}\right)}^T$ 相同，简记为(I_ix I_iy)^T。记前后左右四组光流传感器到中间一组光流传感器的距离为m，高度信息传感器在飞机系中的位置坐标为 ${\left(\begin{array}{ccc}{\hat{x}}_h& {\hat{y}}_h& {\hat{z}}_h\end{array}\right)}^T.$ 若高度传感器装在中间一组光流传感器正前或正后方，即 测量得到的高度信息为z_h，则根据上述方程飞行体姿态为：

$\mathrm{\β}=\arcsin [\frac{1}{1+\frac{(2I_{0x}-I_{1x}-I_{3x}){\hat{x}}_h}{(I_{3x}-I_{1x})m}}\·\frac{(2I_{0x}-I_{1x}-I_{3x})z_h}{(I_{3x}-I_{1x})m}]$

$\mathrm{\α}=\arcsin [\frac{(2I_{0x}-I_{2x}-I_{4x})z}{(I_{2x}-I_{4x})m}/\sqrt{1-{\sin }^2\mathrm{\β}}]$

$z=z_h+\sin \mathrm{\β}\·{\hat{x}}_h$

${\widehat{\mathrm{\ω}}}_1=-\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\sin \mathrm{\β}+\stackrel{\·}{\mathrm{\α}}$

${\widehat{\mathrm{\ω}}}_2=\stackrel{\·}{\mathrm{\γ}}\sin \mathrm{\α}+\stackrel{\·}{\mathrm{\β}}\cos \mathrm{\α}$

${\widehat{\mathrm{\ω}}}_3=\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\cos \mathrm{\β}+\stackrel{\·}{\mathrm{\β}\sin }\mathrm{\α}$

式中，

$\stackrel{\·}{\mathrm{\γ}}=\frac{(I_{0x}{I}_{2x}+I_{0x}{I}_{4x}-2I_{2x}{I}_{4x})z}{(I_{4x}-I_{2x})m}/(1-{\sin }^2\mathrm{\β})$

$\stackrel{\·}{\mathrm{\α}}=\frac{(I_{0y}-I_{2y})z}{m\sin \mathrm{\α}\·\cos \mathrm{\β}}-I_{2y}-\stackrel{\·}{\mathrm{\γ}}\·\sin \mathrm{\β}$

$\stackrel{\·}{\mathrm{\β}}=-\frac{(I_{0x}-I_{3x})z}{m\sqrt{1-{\sin }^2\mathrm{\α}}\·\sin \mathrm{\β}}+\frac{I_{3x}+\stackrel{\·}{\mathrm{\γ}\·}\sin \mathrm{\α}\·\sqrt{1-{\sin }^2\mathrm{\β}}}{\sqrt{1-{\sin }^2\mathrm{\α}}}$

其中，使用3-2-1转动系，三个姿态角依次为γβα，分别对应偏航角、俯仰角和滚转角，其对时间的导数分别为三个姿态角角速度依次为分别对应滚转角角速度、俯仰角角速度和偏航角角速度。I_ix表示第i组传感器沿飞机飞行前方(即图2中的方向和图3中的方向)的光流大小，I_iy表示第i组传感器沿飞机飞行左方的光流大小(即图2中的方向和图3中的方向)。z表示飞机参考系原点在地面系中的高度。m为周围4组传感器到中间一组传感器即飞机参考系原点的距离。

图6示出了本发明中飞行体姿态测量系统的又一优选实施例，以飞机为例，如6所示，该系统采用了十个光流传感器，它们两两一组固定在飞机机身前部、中部、后部和左右两侧五个位置。具体地，光流传感器01和02位于机身中部，光流传感器11和12位于机身前部，光流传感器21和22位于机身左部，光流传感器31和32位于机身后部，光流传感器41和42位于机身右部，所有光流传感器固定于飞机平面内，指向垂直于飞机平面方向向下的方向，各个传感器指向相互平行。该系统中附加信息单元采用了一个距离传感器，其固定于机身，其指向与光流传感器指向平行，与飞机平面垂直而与地面不垂直，测量得到该距离传感器与地面通过其指向的距离；各个光流传感器和距离传感器测量的信息分别传递到测量信息检查器，由测量信息检查器从机身每个位置的两个光流传感器测量信号中选取一个正确的信息，得到五个正确的光流信息，与距离信息一起传递到飞行姿态解算器，解算飞行姿态，并将飞行姿态传递给外接设备；若某一位置的两个光流传感器测量信息均不正确，或距离信息不正确，此时无法解算全部或部分飞行姿态，则向外接设备发出报警信号。本实施例展示了一种光流测量单元和距离测量单元的布置方案，其他任何使用不同光流测量单元和距离测量单元排布方式的基于光流信息和距离信息的姿态测量系统也均在本发明的保护范围之内。

当所述5组光流传感器成十字形布置时，记前后左右四组光流传感器到中间一组光流传感器的距离为m，距离传感器在飞机系中的位置坐标为 ${\left(\begin{array}{ccc}{\hat{x}}_d& {\hat{y}}_d& {\hat{z}}_d\end{array}\right)}^T,$ 距离传感器的指向与光流传感器相同，为飞机参考系的方向。则距离传感器测得数据，即距离传感器沿着其指向正对地面上的点到距离传感器的距离为d_d，其可以用下式表示：

$d_d=\frac{z-\sin \mathrm{\β}\·{\hat{x}}_d+\sin \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{y}}_d}{\cos \mathrm{\α}\·\cos \mathrm{\β}}+{\hat{z}}_d$

令距离传感器位置满足飞机姿态为

$\mathrm{\α}=\arcsin [\frac{1}{1+\frac{(2I_{0x}-I_{2x}-I_{4x}){\hat{y}}_d}{(I_{2x}-I_{4x})m}}\·\frac{(2I_{0x}-I_{2x}-I_{4x})(d_d-{\hat{z}}_d)}{(I_{2x}-I_{4x})m}]$

$z=m\sin \mathrm{\α}/(\frac{{(2I_{0x}-I_{1x}-I_{3x})}^2}{{(I_{3x}-I_{1x})}^2}{\sin }^2\mathrm{\α}+\frac{{(2I_{0x}-I_{2x}-I_{4x})}^2}{{(I_{2x}-I_{4x})}^2})$

$\mathrm{\β}=\arcsin \frac{(2I_{0x}-I_{1x}-I_{3x})z}{(I_{3x}-I_{1x})m}$

${\widehat{\mathrm{\ω}}}_1=-\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\sin \mathrm{\β}+\stackrel{\·}{\mathrm{\α}}$

${\widehat{\mathrm{\ω}}}_2=\stackrel{\·}{\mathrm{\γ}}\sin \mathrm{\α}+\stackrel{\·}{\mathrm{\β}}\cos \mathrm{\α}$

${\widehat{\mathrm{\ω}}}_3=\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\cos \mathrm{\β}-\stackrel{\·}{\mathrm{\β}}\sin \mathrm{\α}$

式中，

$\stackrel{\·}{\mathrm{\γ}}=\frac{(I_{0x}{I}_{2x}+I_{0x}{I}_{4x}-2I_{2x}{I}_{4x})z}{(I_{4x}-I_{2x})m}/(1-{\sin }^2\mathrm{\β})$

$\stackrel{\·}{\mathrm{\α}}=\frac{(I_{0y}-I_{2y})z}{m\sin \mathrm{\α}\·\cos \mathrm{\β}}-I_{2y}-\stackrel{\·}{\mathrm{\γ}}\·\sin \mathrm{\β}$

$\stackrel{\·}{\mathrm{\β}}=-\frac{(I_{0x}-I_{3x})z}{m\sqrt{1-{\sin }^2\mathrm{\α}}\·\sin \mathrm{\β}}+\frac{I_{3x}+\stackrel{\·}{\mathrm{\γ}\·}\sin \mathrm{\α}\·\sqrt{1-{\sin }^2\mathrm{\β}}}{\sqrt{1-{\sin }^2\mathrm{\α}}}$

其中，使用3-2-1转动系，三个姿态角依次为γβα，分别对应偏航角、

俯仰角和滚转角，其对时间的导数分别为三个姿态角角速度依次为分别对应滚转角角速度、俯仰角角速度和偏航角角速度。I_ix表示第i组传感器沿飞机飞行前方(即图2中的方向和图3中的方向)的光流大小，I_iy表示第i组传感器沿飞机飞行左方的光流大小(即图2中的方向和图3中的方向)。z表示飞机参考系原点在地面系中的高度。m为周围4组传感器到中间一组传感器即飞机参考系原点的距离。

图7示出了本发明中飞行体姿态测量系统的又一优选实施例，以飞机为例，如7所示，该系统采用了十个光流传感器，其两两一组固定在飞机机身前部、中部、后部和左右两侧五个位置，具体地，光流传感器01和02位于机身中部，光流传感器11和12位于机身前部，光流传感器21和22位于机身左部，光流传感器31和32位于机身后部，光流传感器41和42位于机身右部，所有光流传感器固定于飞机平面内，指向垂直于飞机平面方向向下的方向，各个传感器指向相互平行；该系统的附加信息测量单元使用基于地平线识别的姿态角测量单元，其包括一个前视单目相机，固定于飞机上指向飞机飞行前方，其还包括一个滚转角识别器，用于从相机拍摄的图像中识别出飞机滚转角；各个光流传感器和姿态角传感器测量的信息分别传递到测量信息检查器，由测量信息检查器从机身每个位置的两个光流传感器测量信号中选取一个正确的信息，得到五个正确的光流信息，与滚转角信息一起传递到飞行姿态解算器，解算飞行姿态，并将飞行姿态传递给外接设备；若某一位置的两个光流传感器测量信息均不正确，或滚转角信息不正确，此时无法解算全部或部分飞行姿态，则向外接设备发出报警信号。本优选实施例展示了一种光流测量单元和姿态角测量单元的布置方案，其他任何使用不同光流测量单元和姿态角测量单元排布方式的基于光流信息和姿态角信息的姿态测量系统也均在本发明的保护范围之内。

当所述5组光流传感器成十字形布置时，记前后左右四组光流传感器到中间一组光流传感器的距离为m，所述姿态角传感器测得的滚转角为α，则此时飞机姿态为：

$z=m\left|\sin \mathrm{\α}\right|/(\frac{{(2I_{0x}-I_{1x}-I_{3x})}^2}{{(I_{3x}-I_{1x})}^2}{\sin }^2\mathrm{\α}+\frac{{(2I_{0x}-I_{2x}-I_{4x})}^2}{{(I_{2x}-I_{4x})}^2})$

$\mathrm{\β}=\arcsin \frac{(2I_{0x}-I_{1x}-I_{3x})z}{(I_{3x}-I_{1x})m}$

${\widehat{\mathrm{\ω}}}_1=-\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\sin \mathrm{\β}+\stackrel{\·}{\mathrm{\α}}$

${\widehat{\mathrm{\ω}}}_2=\stackrel{\·}{\mathrm{\γ}}\sin \mathrm{\α}+\stackrel{\·}{\mathrm{\β}}\cos \mathrm{\α}$

${\widehat{\mathrm{\ω}}}_3=\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\cos \mathrm{\β}-\stackrel{\·}{\mathrm{\β}}\sin \mathrm{\α}$

式中，

$\stackrel{\·}{\mathrm{\γ}}=\frac{(I_{0x}{I}_{2x}+I_{0x}{I}_{4x}-2I_{2x}{I}_{4x})z}{(I_{4x}-I_{2x})m}/(1-{\sin }^2\mathrm{\β})$

$\stackrel{\·}{\mathrm{\α}}=\frac{(I_{0y}-I_{2y})z}{m\sin \mathrm{\α}\·\cos \mathrm{\β}}-I_{2y}-\stackrel{\·}{\mathrm{\γ}}\·\sin \mathrm{\β}$

$\stackrel{\·}{\mathrm{\β}}=-\frac{(I_{0x}-I_{3x})z}{m\sqrt{1-{\sin }^2\mathrm{\α}}\·\sin \mathrm{\β}}+\frac{I_{3x}+\stackrel{\·}{\mathrm{\γ}}\·\sin \mathrm{\α}\·\sqrt{1-{\sin }^2\mathrm{\β}}}{\sqrt{1-{\sin }^2\mathrm{\α}}}$

其中，使用3-2-1转动系，三个姿态角依次为γβα，分别对应偏航角、俯仰角和滚转角，其对时间的导数分别为三个姿态角角速度依次为分别对应滚转角角速度、俯仰角角速度和偏航角角速度。|sinα|表示α的正弦的绝对值。I_ix表示第i组传感器沿飞机飞行前方(即图2中的方向和图3中的方向)的光流大小，I_iy表示第i组传感器沿飞机飞行左方的光流大小(即图2中的方向和图3中的方向)。z表示飞机参考系原点在地面系中的高度。m为周围4组传感器到中间一组传感器即飞机参考系原点的距离。

图8示出了本发明中飞行体姿态测量系统的又一优选实施例，如图8所示，该系统采用十个光流传感器，其两两一组固定在飞机机身前部、中部、后部和左右两侧五个位置，具体地，光流传感器01和02位于机身中部，光流传感器11和12位于机身前部，光流传感器21和22位于机身左部，光流传感器31和32位于机身后部，光流传感器41和42位于机身右部，所有光流传感器固定于飞机平面内，指向垂直于飞机平面方向向下的方向，各个传感器指向相互平行；该系统中附加信息测量单元为姿态角角速度测量单元，使用陀螺仪，用于测量飞机的偏航角速度；各个光流传感器和姿态角角速度传感器测量的信息分别传递到测量信息检查器，由测量信息检查器从机身每个位置的两个光流传感器测量信号中选取一个正确的信息，得到五个正确的光流信息，与偏航角速度一起传递到飞行姿态解算器，解算飞行姿态，并将飞行姿态传递给外接设备；若某一位置的两个光流传感器测量信息均不正确，或偏航角速度信息不正确，此时无法解算全部或部分飞行姿态，则向外接设备发出报警信号。本优选实施例展示了一种光流测量单元和姿态角角速度测量单元的布置方案，其他任何使用不同光流测量单元和姿态角角速度测量单元排布方式的基于光流信息和姿态角角速度信息的姿态测量系统也均在本发明的保护范围之内。

如图8所示的实施例适用于配合传统机械陀螺仪或光纤陀螺，实现无积分误差的精确姿态测量。

当所述5组光流传感器成十字形布置时，记前后左右四组光流传感器到中间一组光流传感器的距离为m，姿态角角速度测量单元测得的偏航角角速度为则此时飞行体姿态为：

$\mathrm{\β}=\arcsin \frac{-\mathrm{\ξ}+\sqrt{{\mathrm{\ξ}}^2+4{\widehat{\mathrm{\ω}}}_3^2}}{2{\widehat{\mathrm{\ω}}}_3}$

$z=\frac{I_{3x}\text{-}{I}_{1x}}{2I_{0x}-I_{1x}-I_{3x}}m\·\sin \mathrm{\β}$

$\mathrm{\α}=\arcsin [\frac{(2I_{0x}-I_{2x}-I_{4x})z}{(I_{2x}-I_{4x})m}/\sqrt{1-{\sin }^2\mathrm{\β}}]$

${\widehat{\mathrm{\ω}}}_1=-\stackrel{\·}{\mathrm{\γ}}\cos \mathrm{\α}\sin \mathrm{\β}+\stackrel{\·}{\mathrm{\α}}$

${\widehat{\mathrm{\ω}}}_2=\stackrel{\·}{\mathrm{\γ}}\sin \mathrm{\α}+\stackrel{\·}{\mathrm{\β}}\cos \mathrm{\α}$

式中，

$\stackrel{\·}{\mathrm{\α}}=\frac{(I_{0y}-I_{2y})z}{m\sin \mathrm{\α}\·\cos \mathrm{\β}}-I_{2y}-{\widehat{\mathrm{\ω}}}_3\·\sin \mathrm{\β}$

$\stackrel{\·}{\mathrm{\β}}=-\frac{(I_{0x}-I_{x3})z}{m\sqrt{1-{\sin }^2\mathrm{\α}}\·\sin \mathrm{\β}}+\frac{I_{3x}+{\widehat{\mathrm{\ω}}}_3\·\sin \mathrm{\α}\sqrt{1-{\sin }^2\mathrm{\β}}}{\sqrt{1-{\sin }^2\mathrm{\α}}}$

$\stackrel{\·}{\mathrm{\γ}}={\widehat{\mathrm{\ω}}}_3$

$\mathrm{\ξ}=\frac{(I_{0x}{I}_{2x}+I_{0x}{I}_{4x}-2I_{2x}{I}_{4x})(I_{3x}-I_{1x})}{(I_{4x}-I_{2x})(2I_{0x}-I_{1x}-I_{3x})}$

其中，使用3-2-1转动系，三个姿态角依次为γβα，分别对应偏航角、俯仰角和滚转角，其对时间的导数分别为三个姿态角角速度依次为分别对应滚转角角速度、俯仰角角速度和偏航角角速度。I_ix表示第i组传感器沿飞机飞行前方(即图2中的方向和图3中的方向)的光流大小，I_iy表示第i组传感器沿飞机飞行左方的光流大小(即图2中的方向和图3中的方向)。z表示飞机参考系原点在地面系中的高度。m为周围4组传感器到中间一组传感器即飞机参考系原点的距离。

本发明公开的上述基于光流和高度信息的飞行器姿态测量系统及方法，适用于但不限于起飞、巡航、盘旋、机动、着陆等各种工况下飞机、导弹或其它飞行器飞行姿态的测量，适用于飞机、导弹或其它飞行器携带的陀螺仪的校准，也适用于其它各种未经特别说明但可适用本系统的依据光流信息和高度/距离信息测量一个运动物体相对另一个物体或环境姿态的情况。

本发明的上述系统和方法还可直接用于任何两运动平面间相互姿态关系的测量。对于光照不足的场合，可以配合辅助光源使用，也可通过合理选择光流传感器工作频段改善光流传感器工作效果。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种飞行体姿态测量方法，其包括：

步骤1、测量飞行体下视方向的光流信息，同时测量附加信息；所述附加信息包括飞行体的位置信息或部分姿态信息；

步骤2、根据所测得的光流信息和附加信息，判断是否具有足够的正确测量信息；若足够则执行步骤3，否则发出报警信号；

步骤3、根据所测得的光流信息和附加信息解算飞行体的飞行姿态信息，并将得到的姿态信息输出；

步骤3中通过下面的方程解算得到飞行体的姿态信息：

$\begin{array}{c}\left(\begin{array}{c}{d}_j{\hat{I}}_{\mathrm{jx}}-d_i{\hat{I}}_{\mathrm{ix}}\\ d_j{\hat{I}}_{\mathrm{jy}}-d_i{\hat{I}}_{\mathrm{iy}}\end{array}\right)=\stackrel{\·}{\mathrm{\γ}}\begin{array}{c}\end{array}\left(\begin{array}{c}{\hat{y}}_i-{\hat{y}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -({\hat{x}}_i-{\hat{x}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\sin \mathrm{\β}\end{array}\right)\\ +\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-({\hat{y}}_i-{\hat{y}}_j)\sin \mathrm{\α}+(d_i-d_j)\cos \mathrm{\α}\\ ({\hat{x}}_i-{\hat{x}}_j)\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -(d_i-d_j)\end{array}\right)\end{array}$

$\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=A_i\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right),\left(\begin{array}{c}{\hat{I}}_{\mathrm{jx}}\\ {\hat{I}}_{\mathrm{jy}}\end{array}\right)=A_j\left(\begin{array}{c}{\tilde{I}}_{\mathrm{jx}}\\ {\tilde{I}}_{\mathrm{jy}}\end{array}\right)$

其中，为第i个光流信息测量点处局部光流信息测量坐标系下x方向上的光流值，为第i个光流信息测量点处局部光流信息测量坐标系下y方向上的光流值；为第i个光流传感器在飞机参考系中第i个光流传感器位置处测得的x方向光流值，为第i个光流传感器在飞机参考系中第i个光流传感器位置处测量得到的y方向光流值；为第j个光流信息测量点处局部光流信息测量坐标系下x方向上的光流值，为第j个光流信息测量点处局部光流信息测量坐标系下y方向上的光流值；为第j个光流传感器在飞机参考系中第j个光流传感器位置处测得的x方向光流值，为第j个光流传感器在飞机参考系中第j个光流传感器位置处测量得到的y方向光流值；A_i和A_j分别表示第i个和第j个光流信息测量点处局部光流信息测量坐标系到飞行体参考系中的坐标转换矩阵；d_i和d_j分别表示测量第i个光流值的位置点和测量第j个光流值的位置点到其正对的地面上的

点的距离；α、β、γ分别表示飞行体的滚转角、俯仰角和偏航角， 分别表示飞行体的滚转角、俯仰角和偏航角对时间的导数；和分别为测量第i个光流值的位置点在飞行体参考系中的x方向坐标和y方向坐标，和分别为第j个光流值的位置点在飞行体参考系中的横坐标和竖坐标；

其中，d_i的表达式如下式所示：

$d_i=\frac{z-\sin \mathrm{\β}\·{\hat{x}}_i+\sin \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{y}}_i}{\cos \mathrm{\α}\·\cos \mathrm{\β}}+{\hat{z}}_i$

式中，z为飞行体参考系原点在地面参考系中的高度，为测量第i个光流值的位置点在飞行体参考系中的z方向坐标。

2.如权利要求1所述的方法，其特征在于，所述光流信息通过多个光流传感器获得，或通过摄像头连续捕捉图像计算获得；所述姿态信息包括飞行体的俯仰角、滚转角、偏航角角速度、俯仰角角速度和滚转角角速度。

3.如权利要求1所述的方法，其特征在于，所述附加信息为下列信息的一个或若干个的组合：飞行体上任意一点相对于地面的高度、飞行体上任意一点到地面上正对点的距离、飞行体的俯仰角和滚转角中的至少一个、飞行体的至少一个姿态角角速度。

4.如权利要求1或3所述的方法，其特征在于，所述飞行体上任意一点到地面上正对点，为从飞行体上这点沿一相对飞行体确定的方向延伸的射线与地面的交点。

5.如权利要求1或3所述的方法，其特征还在于，当所述附加信息为飞行体相对地面的姿态角或姿态角速度时，解算飞行体的飞行姿态信息之前根据所述光流信息和姿态角或姿态角速度计算得到飞行体相对地面的高度。

6.一种飞行体姿态测量系统，其包括：

光流信息测量单元，用于测量飞行体下视方向的光流信息；

附加信息测量单元，用于测量飞行体的附加信息；所述附加信息包括飞行体的位置信息或部分姿态信息；

测量信息检查器：用于检查光流信息测量单元和附加信息测量单元是否测出足够的正确测量信息，如果足够，则将足够的正确测量信息发送给飞行姿态解算器，否则发出报警信号；

飞行姿态解算器：用于根据从测量信息检查器接收到的光流信息和附加信息计算输出飞行体的姿态信息；

其中通过下面的方程解算得到飞行体的姿态信息：

$\begin{array}{c}\left(\begin{array}{c}{d}_j{\hat{I}}_{\mathrm{jx}}-d_i{\hat{I}}_{\mathrm{ix}}\\ d_j{\hat{I}}_{\mathrm{jy}}-d_i{\hat{I}}_{\mathrm{iy}}\end{array}\right)=\stackrel{\·}{\mathrm{\γ}}\begin{array}{c}\end{array}\left(\begin{array}{c}{\hat{y}}_i-{\hat{y}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\cos \mathrm{\β}\·\sin \mathrm{\α}\\ -({\hat{x}}_i-{\hat{x}}_j)\cos \mathrm{\β}\·\cos \mathrm{\α}+(d_i-d_j)\sin \mathrm{\β}\end{array}\right)\\ +\stackrel{\·}{\mathrm{\β}}\left(\begin{array}{c}-({\hat{y}}_i-{\hat{y}}_j)\sin \mathrm{\α}+(d_i-d_j)\cos \mathrm{\α}\\ ({\hat{x}}_i-{\hat{x}}_j)\sin \mathrm{\α}\end{array}\right)+\stackrel{\·}{\mathrm{\α}}\left(\begin{array}{c}0\\ -(d_i-d_j)\end{array}\right)\end{array}$

$\left(\begin{array}{c}{\hat{I}}_{\mathrm{ix}}\\ {\hat{I}}_{\mathrm{iy}}\end{array}\right)=A_i\left(\begin{array}{c}{\tilde{I}}_{\mathrm{ix}}\\ {\tilde{I}}_{\mathrm{iy}}\end{array}\right),\left(\begin{array}{c}{\hat{I}}_{\mathrm{jx}}\\ {\hat{I}}_{\mathrm{jy}}\end{array}\right)=A_j\left(\begin{array}{c}{\tilde{I}}_{\mathrm{jx}}\\ {\tilde{I}}_{\mathrm{jy}}\end{array}\right)$

其中，为第i个光流信息测量点处局部光流信息测量坐标系下x方向上的光流值，为第i个光流信息测量点处局部光流信息测量坐标系下y方向上的光流值；为第i个光流传感器在飞机参考系中第i个光流传感器位置处测得的x方向光流值，为第i个光流传感器在飞机参考系中第i个光流传感器位置处测量得到的y方向光流值；为第j个光流信息测量点处局部光流信息测量坐标系下x方向上的光流值，为第j个光流信息测量点处局部光流信息测量坐标系下y方向上的光流值；为第j个光流传感器在飞机参考系中第j个光流传感器位置处测得的x方向光流值，为第j个光流传感器在飞机参考系中第j个光流传感器位置处测量得到的y方向光流值；A_i和A_j分别表示第i个和第j个光流信息测量点处局部光流信息测量坐标系到飞行体参考系中的坐标转换矩阵；d_i和d_j分别表示测量第i个光流值的位置点和测量第j个光流值的位置点到其正对的地面上的点的距离；α、β、γ分别表示飞行体的滚转角、俯仰角和偏航角， 分别表示飞行体的滚转角、俯仰角和偏航角对时间的导数；和分别为测量第i个光流值的位置点在飞行体参考系中的x方向坐标和y方向坐标，和分别为第j个光流值的位置点在飞行体参考系中的横坐标和竖坐标；

其中，d_i的表达式如下式所示：

$d_i=\frac{z-\sin \mathrm{\β}\·{\hat{x}}_i+\sin \mathrm{\α}\·\cos \mathrm{\β}\·{\hat{y}}_i}{\cos \mathrm{\α}\·\cos \mathrm{\β}}+{\hat{z}}_i$

式中，z为飞行体参考系原点在地面参考系中的高度，为测量第i个光流值的位置点在飞行体参考系中的z方向坐标。

7.如权利要求6所述的系统，其特征在于，所述光流信息测量单元包括：光流传感器，带有计算光流功能的摄像头，摄像头和光流计算设备的组合。

8.如权利要求6所述的系统，其特征在于，所述附加信息测量单元，为下列测量单元的一个或若干个的组合：高度测量单元、距离测量单元、姿态角度测量单元、姿态角角速度测量单元。

9.如权利要求6所述的系统，其特征在于，所述附加信息为下列信息的一个或若干个的组合：飞行体上某点相对于地面的高度、飞行体上某点到地面上正对点的距离、飞行体的某个姿态角、飞行体的某个姿态角角速度。