CN102829779A - 一种飞行器多个光流传感器与惯导组合导航方法 - Google Patents

Abstract

一种飞行器多个光流传感器与惯导组合导航方法，它有四大步骤：步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；步骤四：用扩展卡尔曼滤波器估计惯导误差，并使用此误差对惯导数据进行修正，得到更为精确的导航数据。本发明使用3个光流传感器、1套微机械三轴速率陀螺和1套微机械三轴加速度计，功耗小、成本低，便于在小型飞行器上安装布置，不对外辐射电磁信号，提高了飞行器的隐蔽性，是一种自主式组合导航方法。

Description

一种飞行器多个光流传感器与惯导组合导航方法

技术领域：

本发明涉及一种飞行器多个光流传感器与惯导组合导航方法，具体涉及一种利用光流传感器和惯性器件实现飞行器自主式组合导航的方法。属于小型飞行器（Miniature aerial vehicles,MAVs)的组合导航技术领域。

背景技术：

在山谷、丛林、街道等复杂地域GPS信号变得不稳定，受到敌方干扰时甚至变得不可用，从而形成导航盲区(navigation gap)。那些使用GPS导航或者GPS/INS组合导航的飞行器将在未来战场中变得很脆弱，因此美国海、陆、空三军都对无GPS时的精确导航很感兴趣。我国在建成“北斗”导航系统之后，也将面临同样的问题，只有那些具备不依赖于外部信号的、完全自主的精确导航功能的飞行器才更有可能在未来战场中生存下来；另一方面，对于小型飞行器来说，其负载能力有限，因此机载设备也受到了重量、体积和功耗的限制，传统导航设备比如雷达、激光测距仪等都无法满足要求。在这种背景下，全被动式的光流技术，可以为解决这些问题提供全新的思路。

昆虫在移动时，周围环境的亮度模式在视网膜上形成一系列连续变化的图像，这一系列连续变化的信息不断“流过”视网膜，好像是一种光的“流”，故称这种图像亮度模式的表观运动为光流。国外的某些实验室，已经研制出了光流传感器的物理样机，并利用光流传感器实现了无人驾驶飞行器的自主避障、等高飞行、自动着陆、风速估计、目标检测和空中悬停，这些技术在探测、救灾等方面将有非常重要的应用价值。根据光流的定义和图1中所示的几何关系，可得出光流的表达式为：

$f=\frac{{v\cos }^2\mathrm{\θ}}{h}+\mathrm{\ω}---\left(1\right)$

式中，f为光流(1/s)，v为光流传感器的水平速度(m/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)。

由于光流与相对运动有关，也就是与昆虫身体的飞行高度、速度、姿态、姿态变化率有关，用到飞行器上，光流可以与惯导器件实现自主式组合导航，提高导航精度，即便没有GPS信号，也可能实时提供较高精度的导航数据。

将光流应用于飞行器组合导航，其优势体现在如下几个方面：

第一，光流传感器是被动式的，不会产生电磁辐射，故隐蔽性好，适合军事应用。当然，GPS也是被动式的，中高空的飞行器一般也会使用它，但对于超低空飞行，由于地形的散射、遮蔽等原因，GPS的测高功能将大打折扣。而光流感知则可以对周围环境作出即时的观察，可以提供一种无需先验知识的导航手段。

第二，光流传感器重量轻。对于小型飞行器来说，激光测距仪(Laser Rangefinders,LRF)和雷达都显得过于笨重。SICK LMS291是一款典型的激光测距仪，一般用于机器人领域，它的质量大约是4.5公斤。用于无人驾驶航空器(Unmanned Aerial Vehicle,UAV)上的最小的合成孔径雷达可能是美国圣地亚实验室(Sandia National Labs)制造的miniSAR，其质量约为4~5公斤。相比较而言，澳大利亚科学与技术局(Defence Science and Technology Organization)生产的能够检测图像运动以实现地形跟随的传感器就要小得多，其质量不足5g。

第三，体积小。许多飞行器的体积变得越来越小，这就限制了两个光学传感器之间的距离，双目立体视觉能力由此受限，而且立体视觉的计算量很大，难以保证实时性，这也会限制立体视觉在微型飞行器上的应用。而光流传感器却可以做得很小，计算量也很有限，可以方便地在飞行器上进行多点布置。另外，成本低、功耗小也是其重要优点。

发明内容：一种飞行器多个光流传感器与惯导组合导航方法

1、目的：本发明的目的是提供一种飞行器多个光流传感器与惯导组合导航方法，它使用3个光流传感器、1套微机械三轴速率陀螺和1套微机械三轴加速度计，体积小、重量轻、功耗小、成本低，便于在小型飞行器上安装布置，不对外辐射电磁信号，提高了飞行器的隐蔽性，是一种自主式组合导航方法。

2、技术方案：

本发明是一种飞行器多个光流传感器与惯导组合导航方法，该方法具体步骤如下：

步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；

导航坐标系选用ENU(East-North-Up,东北天)坐标系。该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天。

E、N、U三个方向的平台误差角方程分别为

${\stackrel{\·}{\mathrm{\φ}}}_E={\mathrm{\φ}}_N({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})-\frac{{\mathrm{\δV}}_N}{R_M+h}+\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}-{\mathrm{\ϵ}}_E$

${\stackrel{\·}{\mathrm{\φ}}}_N={-\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U\frac{V_N}{R_M+h}-{\mathrm{\δL\ω}}_{\mathrm{ie}}\sin L+\frac{{\mathrm{\δV}}_E}{R_N+h}-\mathrm{\δh}\frac{V_E}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_N---\left(2\right)$

${\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\φ}}_N\frac{V_N}{R_M+h}+\mathrm{\δL}({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)+\frac{{\mathrm{\δV}}_E}{R_N+h}+\tan L$

$-\mathrm{\δh}\frac{V_E\tan L}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_U$

式中：

${\mathrm{\ϵ}}_E=C_{11}{\mathrm{\ϵ}}_x^b+C_{21}{\mathrm{\ϵ}}_y^b+C_{31}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_N=C_{12}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{32}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_U=C_{13}{\mathrm{\ϵ}}_x^b+C_{23}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b$

C_ij(i＝1,2,3;j=1,2,3)为坐标变换矩阵中的子项，

为导航坐标系到本体坐标系的变换矩阵：

$C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)---\left(3\right)$

其中L、λ、h分别为纬度、经度和高度，V_E、V_N、V_U分别为东向、北向和天向的速度，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，

为三个陀螺的量测误差；ω_ie为地球自转角速度；R_M和R_N分别为地球的子午圈半径和卯酉圈半径。

E、N、U三个方向的速度误差方程分别为

$\mathrm{\δ}{\stackrel{\·}{V}}_E={\mathrm{\φ}}_U{f}_N-{\mathrm{\φ}}_N{f}_U+{\mathrm{\δV}}_E\frac{V_N\tan L-V_U}{R_N+h}+{\mathrm{\δV}}_N({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$-{\mathrm{\δV}}_U({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)$

$+\mathrm{\δh}\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}+{\▿}_E$

$\mathrm{\δ}{\stackrel{\·}{V}}_N={-\mathrm{\φ}}_U{f}_E+{\mathrm{\φ}}_E{f}_U-{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\δV}}_N\frac{V_U}{R_M+h}-{\mathrm{\δV}}_U\frac{V_N}{R_M+h}-$

$\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E+\mathrm{\δh}(\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2})+{\▿}_N$

$\mathrm{\δ}{\stackrel{\·}{V}}_U={\mathrm{\φ}}_N{f}_E-{\mathrm{\φ}}_E{f}_N+{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\δV}}_N\frac{{2V}_N}{R_M+h}-2{\mathrm{\δLV}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L-$

$\mathrm{\δh}(\frac{V_E^2}{{(R_N+h)}^2}+\frac{V_N^2}{{(R_M+h)}^2})+{\▿}_U---\left(4\right)$

式中：

${\▿}_E=C_{11}{\▿}_x^b+C_{21}{\▿}_y^b+C_{31}{\▿}_z^b$

${\▿}_N=C_{12}{\▿}_x^b+C_{22}{\▿}_y^b+C_{32}{\▿}_z^b$

${\▿}_U=C_{13}{\▿}_x^b+C_{23}{\▿}_y^b+C_{33}{\▿}_z^b$

f_E、f_N、f_U为E、N、U三个方向的比力，

为三个加速度计量测误差。E、N、U三个方向的位置误差方程分别为

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_N+h}\sec L+\mathrm{\δL}\frac{V_E}{R_N+h}\sec L\tan L-\mathrm{\δh}\frac{V_E\sec L}{{(R_N+h)}^2}---\left(5\right)$

$\mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δV}}_U$

于是，惯导误差方程可以写成：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(6\right)$

式中， $X={[\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\ϵ}}_{\mathrm{cx}},{\mathrm{\ϵ}}_{\mathrm{cy}},{\mathrm{\ϵ}}_{\mathrm{cz}}{,\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T.$

状态向量X共18维。其中δL、δλ、δh分别为纬度误差、经度误差和高度误差，δV_E、δV_N、δV_U分别为东向、北向和天向的速度误差，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，ε_cx，ε_cy，ε_cz为三个陀螺的随机常值偏差；ε_rx，ε_ry，ε_rz为三个陀螺的随机漂移（一阶马尔科夫过程）；

为三个加速度计的随机偏差（一阶马尔科夫过程）。

系统噪声为

w=[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

其中ω_gx，ω_gy，ω_gz为陀螺随机白噪声漂移；ω_rx，ω_ry，ω_rz为陀螺一阶马尔科夫驱动白噪声；ω_ax，ω_ay，ω_az为加速度计一阶马尔科夫驱动白噪声。

系统噪声分配阵为

$G=\left[\begin{array}{ccc}{0}_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ C_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]$

F中的非零元素为

$F_{\mathrm{1,3}}=\frac{{-V}_N}{{(R_M+h)}^2}$ $F_{\mathrm{1,5}}=\frac{1}{R_M+h}$ $F_{\mathrm{2,1}}=\frac{V_E\sec L\tan L}{R_N+h}$ $F_{\mathrm{2,3}}=\frac{{-V}_E\sec L}{{(R_N+h)}^2}$

$F_{\mathrm{2,4}}=\frac{\sec L}{R_N+h}$ F_3,6=1 $F_{\mathrm{4,1}}={2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L$

$F_{\mathrm{4,3}}=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}$ $F_{\mathrm{4,4}}=\frac{V_N\tan L-V_U}{R_N+h}$ $F_{\mathrm{4,5}}={2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h}$

$F_{4,6}={-2\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$ F_4,8=-f_U F_4,9=f_N F_4，16=C₁₁ F_4，17=C₂₁

F_4，18=C₃₁ $F_{\mathrm{5,1}}=-V_E({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)$ $F_{\mathrm{5,3}}=\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{5,4}}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$ $F_{\mathrm{5,5}}=-\frac{V_U}{R_M+h}$ $F_{\mathrm{5,6}}=-\frac{V_N}{R_M+h}$ F_5,7=f_U

F_5,9=-f_E F_5，16=C₁₂ F_5，17=C₂₂ F_5，18=C₃₂ F_6,1=-2V_Eω_ie sin L

$F_{\mathrm{6,3}}=-\frac{{V_E}^2}{{(R_N+h)}^2}-\frac{{V_N}^2}{{(R_M+h)}^2}$ $F_{\mathrm{6,4}}={2\mathrm{\ω}}_{\mathrm{ie}}\cos L+2\frac{V_E}{R_N+h}$ $F_{\mathrm{6,5}}=\frac{{2V}_N}{R_M+h}$

F_6,7=-f_N F_6,8=f_E F_6，16=C₁₃ F_6，17=C₂₃ F_6，18=C₃₃

$F_{\mathrm{7,5}}=-\frac{1}{R_M+h}$ $F_{\mathrm{7,8}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{7,9}}={-\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$

F_7，10=-C₁₁ F_7，11=-C₂₁ F_7，12=-C₃₁ F_7，13=-C₁₁ F_7，14=-C₂₁ F_7，15=-C₃₁

F_8,1=-ω_ie sin L $F_{\mathrm{8,3}}=-\frac{V_E}{{(R_N+h)}^2}$ $F_{\mathrm{8,4}}=\frac{1}{R_N+h}$ $F_{\mathrm{8,7}}={-\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R_N+h}$

F_8，10=-C₁₂ F_8，11=-C₂₂ F_8，12=-C₃₂ F_8，13=-C₁₂

F_8，14=-C₂₂ F_8，15=-C₃₂ $F_{\mathrm{9,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h}$ $F_{\mathrm{9,3}}=-\frac{V_E\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{9,4}}=\frac{\tan L}{R_N+h}$ $F_{\mathrm{9,7}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{9,8}}=\frac{V_N}{R_M+h}$ F_9，10=-C₁₃

F_9，11=-C₂₃ F_9，12=-C₃₃ F_9，13=-C₁₃ F_9，14=-C₂₃ F_9，15=-C₃₃

$F_{\mathrm{14,14}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{15,15}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{16,16}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{17,17}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{18,18}}=-\frac{1}{{\mathrm{\τ}}_A}$

步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；

将3个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；其中，“多点布置”是指，光流传感器要安装在飞行器的不同位置，典型位置是头部、中间、尾部和翼梢；“距离要尽量远”是指，安装在头部、尾部或者翼梢的光流传感器，在不影响其它机载设备的情况下，要尽量靠近机体的最前端、最后端或者侧端，这样就保证了头部、尾部及翼梢光流传感器间的距离尽可能大些。

在推导光流传感器的量测方程之前，先定义几个坐标系，如图4所示：

导航坐标系(S_n):为了与惯导统一，选用ENU(East-North-Up,东北天)坐标系。该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天。

本体坐标系(S_b):本体坐标系固连在MAV上，其原点在MAV的质心处，y轴指向MAV的前方，z轴沿MAV纵向对称面朝上，x轴按右手定则确定。

光流传感器坐标系(S_f):光流传感器坐标系固连在光流传感器上，其原点在镜头的焦点处，z轴沿光轴方向指向外，x轴和y轴分别与测得的两个正交方向的光流重合。

于是，光流传感器的量测值为：

$f_f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)---\left(7\right)$

这里,V_nf和ω_nf分别是光流传感器相对于导航坐标系的速度矢量和角速度矢量，下标f，x和f,y分别代表光流传感器坐标系中的x分量和y分量。d_fg为光流传感器的焦点沿z_f到地面的距离。

令r_nb为S_b相对于S_n的位置矢量，r_bf为S_f相对于S_b的位置矢量，于是光流传感器的速度矢量可表示为：

$V_{\mathrm{nf}}=\frac{{\mathrm{dr}}_{\mathrm{nf}}}{\mathrm{dt}}=\frac{d}{\mathrm{dt}}(r_{\mathrm{nb}}+r_{\mathrm{bf}})=\frac{{\mathrm{dr}}_{\mathrm{nb}}}{\mathrm{dt}}+\frac{{\mathrm{dr}}_{\mathrm{bf}}}{\mathrm{dt}}---\left(8\right)$

将速度矢量向S_f中投影：

${\left(V_{\mathrm{nf}}\right)}_f=C_n^f\frac{d{\left(r_{\mathrm{nb}}\right)}_n}{\mathrm{dt}}+C_b^f(\frac{d{\left(r_{\mathrm{bf}}\right)}_b}{\mathrm{dt}}+{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b)$

$=C_n^f{V}_n+C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b\×{\left(r_{\mathrm{bf}}\right)}_b---\left(9\right)$

$=C_n^f{V}_n+C_b^f({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×{\left(r_{\mathrm{bf}}\right)}_b$

本体坐标系到光流传感器坐标系的转换矩阵为：Y(μ)->X(η)

$C_b^f=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\η}& \sin \mathrm{\η}\\ 0& -\sin \mathrm{\η}& \cos \mathrm{\η}\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}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\cos \mathrm{\μ}\end{array}\right)---\left(10\right)$

这里,μ和η是光流传感器的安装角，它们是光流传感器坐标系相对于本体坐标系的欧拉角，也就是说，将本体坐标系沿y_b轴转动角度μ，然后再沿x_b轴转动角度η，即可得到光流传感器坐标系。由于μ和η是常值，故为常值矩阵。

设z_f的方向矢量为k_f，即(k_f)_f=(0 0 1)^T，于是将k_f向S_n投影得：

${\left(k_f\right)}_n=C_f^n{\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f---\left(11\right)$

z_f和z_n间夹角的余弦值为-(k_f)_n，z，于是：

(k_f)_n，z=C₁₃cosηsinμ-C₂₃sinη+C₃₃cosηcosμ        (12)

        =C₁₃T₃₁+C₂₃T₃₂+C₃₃T₃₃

光流传感器沿其光轴方向到地面的距离为：

$d_{\mathrm{fg}}=\left|\frac{{\left(r_{\mathrm{nf}}\right)}_{n,z}}{{\left(k_f\right)}_{n,z}}\right|$

$=-\frac{{(r_{\mathrm{nb}}+r_{\mathrm{bf}})}_{n,z}}{{\left(k_f\right)}_{n,z}}$

                                         (13)

$=-\frac{{\left(r_{\mathrm{nb}}\right)}_{n,z}+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

$=-\frac{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

而

${\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_f={\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_f$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b$

$=C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b---\left(14\right)$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{\left({\mathrm{\ω}}_{\mathrm{in}}\right)}_n$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n$

于是，光流传感器的量测方程为

$f_f=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)---\left(15\right)$

$=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}\×{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_x\end{array}\right)$

步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；

事实上，一个光流传感器可以同时测出正交方向上的两个光流分量，其量测输出可记为

$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)---\left(16\right)$

求取光流误差方程为

δf=H_fX+v(t)                                            (17)

v(t)为量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v^T(τ)]=r_vδ(t-τ)，r_v为v(t)的方差强度阵。

在推导光流量测方程的线化系数之前，首先对光流的量测方程进行合理的简化。

光流的平动部分，即

和基本上与

是一个量级的，就一般飞行器来说，其量级大于10^-3s^-1，而|ω_ie|的量级为10^-5rad/s，

量级不大于10^-5s^-1；另一方面，光流传感器是有噪声的，10^-5s^-1的数值会被量测噪声湮没，于是，光流量测方程(53)可简化为：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(18\right)$

现在求取典型安装位置和角度情况下的光流误差方程。

1、安装在纵轴纵面内

(r_bf)_b=(0,r_y,0)^T，μ=π，于是：

$C_b^f=\left(\begin{array}{ccc}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\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)=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)---\left(19\right)$

由于 $C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right),$ 于是，由式(49)得：

${\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ T_{23}\\ -T_{22}\end{array}\right)$

$=\left(\begin{array}{c}{C}_{21}{T}_{23}-C_{31}{T}_{22}\\ C_{22}{T}_{23}-C_{32}{T}_{22}\\ C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)$

于是，

(k_f)_n,z=C₂₃T₂₃-C₃₃T₂₂                                (20)

由式(41)，

$C_b^n{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)=\left(\begin{array}{c}{C}_{21}{r}_y\\ C_{22}{r}_y\\ C_{23}{r}_y\end{array}\right),$

于是

${\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z=C_{23}{r}_y---\left(21\right)$

而，

$C_n^f{V}_n=C_b^f{C}_n^b{V}_n$

$=\begin{array}{c}\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\end{array}\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)$

$=\begin{array}{c}\end{array}\left(\begin{array}{ccc}-C_{11}& -C_{12}& {-C}_{13}\\ C_{21}{T}_{22}+C_{31}{T}_{23}& C_{22}{T}_{22}+C_{32}{T}_{23}& C_{23}{T}_{22}+C_{33}{T}_{23}\\ C_{21}{T}_{23}-C_{31}{T}_{22}& C_{22}{T}_{23}-C_{32}{T}_{22}& C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)---\left(22\right)$

$=\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-C_{13}{V}_U\\ (C_{21}{T}_{22}+C_{31}+T_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N+(C_{23}{T}_{22}+C_{33}{T}_{23})V_U\\ (C_{21}{T}_{23}-C_{31}{T}_{22})V_E+(C_{22}{T}_{23}-C_{32}{T}_{22})V_N+(C_{23}{T}_{23}-C_{33}{T}_{22})V_U\end{array}\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{ccc}0& {-\mathrm{\ω}}_{\mathrm{ib},z}^b& {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b& 0& {-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ {-\mathrm{\ω}}_{\mathrm{ib},y}^b& {\mathrm{\ω}}_{\mathrm{ib},x}^b& 0\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)$

                                     (23)

$=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ 0\\ {\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b\\ -T_{22}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}{r}_y\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)=\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},y}^b-T_{22}{\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)---\left(24\right)$

将式(56)~(60)代入光流公式(54)得：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(25\right)$

展开并写成分量形式：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-\\ C_{13}{V}_U+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\end{array}\right)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(26\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E-(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +(C_{23}{T}_{22}+C_{33}{T}_{23})V_U+{T_{23}\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(27\right)$

对于飞机来说，其水平速度一般要远大于其垂直速度，故上式可以简化为：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×({-C}_{11}{V}_E-C_{12}{V}_N+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(28\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(29\right)$

方程(64)(65)未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定（以式(64)表示的x向光流为例）：

$f_x+{\mathrm{\δf}}_x=-\frac{({\hat{C}}_{23}{T}_{23}-{\hat{C}}_{33}{T}_{22})}{h+\mathrm{\δh}+{\hat{C}}_{23}{r}_y}\×(-{\hat{C}}_{11}(V_E+{\mathrm{\δV}}_E)-{\hat{C}}_{12}(V_N+{\mathrm{\δV}}_N)+({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b)r_y)+---\left(30\right)$

$(T_{22}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)+T_{23}({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b))$

式(66)中，

由下式确定：

${\hat{C}}_n^b=C_n^b(I+{\mathrm{\Φ}}_{\×}^n)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right)---\left(31\right)$

$=\left(\begin{array}{ccc}{C}_{11}+{\mathrm{\φ}}_U{C}_{12}-{\mathrm{\φ}}_N{C}_{13}& {-\mathrm{\φ}}_U{C}_{11}+C_{12}+{\mathrm{\φ}}_E{C}_{13}& {\mathrm{\φ}}_N{C}_{11}-{\mathrm{\φ}}_E{C}_{12}+C_{13}\\ C_{21}+{\mathrm{\φ}}_U{C}_{22}-{\mathrm{\φ}}_N{C}_{23}& {-\mathrm{\φ}}_U{C}_{21}+C_{22}+{\mathrm{\φ}}_E{C}_{23}& {\mathrm{\φ}}_N{C}_{21}-{\mathrm{\φ}}_E{C}_{22}+C_{23}\\ C_{31}+{\mathrm{\φ}}_U{C}_{32}-{\mathrm{\φ}}_N{C}_{33}& {-\mathrm{\φ}}_U{C}_{31}+C_{32}+{\mathrm{\φ}}_E{C}_{33}& {\mathrm{\φ}}_N{C}_{31}+{\mathrm{\φ}}_E{C}_{32}+C_{33}\end{array}\right)$

式(66)减去式(64)，并略去高阶小项，即可得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (32)

H_fx是一个1×12的行阵，它的各个子项都很复杂，这里只给出相对简单的第一项，其它的不再一一列出。

$H_{\mathrm{fx}}\left(\mathrm{1,1}\right)=\frac{1}{{(h+C_{23}{r}_y)}^2}\left(\begin{array}{c}(C_{23}{T}_{23}-C_{33}{T}_{22}{)\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y+(C_{33}{T}_{22}{C}_{11}-C_{23}{T}_{23}{C}_{11})V_E+\\ (C_{33}{T}_{22}{C}_{12}-C_{23}{T}_{23}{C}_{12})V_N\end{array}\right)$

同理可以得到

δf_y=H_fy[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (33)

合并可得光流误差方程为：

δf=H_f[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (34)

2、安装在横轴纵面内

(r_bf)_b=(r_x,0,0)^T，η=0，跟前文的推导类似，光流公式可以简化为：

$f_x=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{13}{r}_x}\×\left(\begin{array}{c}(C_{11}{T}_{11}+C_{31}{T}_{13})V_E+(C_{12}{T}_{11}+C_{32}{T}_{13})V_N\\ {-T}_{13}{\mathrm{\ω}}_{\mathrm{ib},y}^b{r}_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b---\left(35\right)$

$f_y=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{23}{r}_x}\×\left(\begin{array}{c}{C}_{21}{V}_E+C_{22}{V}_N\\ +{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_x\end{array}\right)-T_{11}{\mathrm{\ω}}_{\mathrm{ib},x}^b-T_{13}{\mathrm{\ω}}_{\mathrm{ib},z}^b---\left(36\right)$

上述方程未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定（以式(71)表示的x向光流为例）：

$f_x+{\mathrm{\δf}}_x=\frac{({\hat{C}}_{13}{T}_{13}-{\hat{C}}_{33}{T}_{11})}{h+\mathrm{\δh}+{\hat{C}}_{13}{r}_x}\×\left(\begin{array}{c}({\hat{C}}_{11}{T}_{11}+{\hat{C}}_{31}{T}_{13})(V_E+{\mathrm{\δV}}_E)+({\hat{C}}_{12}{T}_{11}+{\hat{C}}_{32}{T}_{13})\\ \×(V_N+{\mathrm{\δV}}_N)-T_{13}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)r_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b---\left(37\right)$

式(73)减去(71)，并略去高阶小项，即可得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (38)

这与式(68)的形式是完全相同的。

最终，安装在飞行器纵轴纵面内的光流传感器和安装在横轴纵面内的光流传感器可以组合到一起，成为组合导航系统的量测方程：

Z=HX+v(t)

这里，Z=δf，H可以由H_f扩展得到，v(t)表示光流传感器的量测噪声。

步骤四：用扩展卡尔曼滤波器估计惯导误差，并使用此误差对惯导数据进行修正，得到更为精确的导航数据。

3个光流传感器在MAV上的安装位置和方向，以(x_b y_b z_b μη)的形式给出，形成矩阵M_3×5，设：

$M_{3\×5}=\left(\begin{array}{ccccc}0& 2& 0& \mathrm{\π}& -\frac{\mathrm{\π}}{6}\\ 2& 0& 0& \frac{5\mathrm{\π}}{6}& 0\\ -2& 0& 0& \frac{7\mathrm{\π}}{6}& 0\end{array}\right)---\left(39\right)$

惯导误差初值X₀=0，按图5所示的组合导航原理框图，经数值仿真，得到的量测数据滤波效果如图6~8所示。图6表明，组合导航的纬度误差比纯惯导小一个数量级，纬度误差减小至1/3，高度误差几近于0，综合纬度误差和经度误差可以算得，组合导航的位置误差大约是纯惯导的1/6；图7表明，组合导航有效地抑制了纯惯导的速度发散，这其实也是减小位置误差的主要原因；图8表明，组合导航可以减小东向和北向的平台误差角，但对天向平台误差角没有发挥抑制作用。

图6~8证明，本发明所提的这种光流与惯导组合导航，可以有效地抑制纯惯导的发散，提高导航精度。

3、优点及功效：本发明是一种利用光流传感器和惯性器件实现飞行器自主式组合导航的方法，其优点是：（1）测量元件体积小、重量轻、功耗小、成本低，便于在飞行器上布置、安装和使用；（2）测量元件不对外辐射电磁信号，有利于飞行器完成隐蔽性任务；（3）自主式组合导航，无需GPS等外界信号的支持；（4）导航精度高于纯惯导5～10倍。

附图说明：

图1是光流传感器测量关系图

图1中，v为光流传感器的水平速度(rad/s)，h为光流传感器距离地面的高度(m)，θ为光轴与铅垂方向的夹角(rad)，ω为光流传感器的旋转速度(rad/s)；

图2是光流传感器在飞行器上的布置方案示意图

图3是本发明流程框图

图4是各坐标系的关系图

图4中，S_n表示导航坐标系，S_b表示本体坐标系，S_f表示光流传感器坐标系。r_nb为S_b相对于S_n的位置矢量，r_bf为S_f相对于S_b的位置矢量，r_nf为S_f相对于S_n的位置矢量；

图5是光流与惯导组合导航原理框图

图6是组合导航与纯惯导的位置误差对比

图6中，δL、δλ、δh分别为纬度误差、经度误差和高度误差

图7是组合导航与纯惯导的速度误差对比

图7中，δV_E、δV_N、δV_U分别为东向、北向和天向的速度误差

图8是组合导航与纯惯导的平台误差角对比

图8中，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角。

具体实施方式：

根据图1所示的光流传感器测量关系图和图2所示的光流传感器在MAV上的配置方案示意图，我们提出了一种利用光流传感器和惯性器件实现飞行器自主式组合导航的方法。光流传感器可以测得飞行器前方、下方和侧方的光流信息，利用这些光流信息与惯导信息组合，提高导航精度。

为了降低问题的复杂程度，简化系统数学模型，作出如下假设：

1)飞行器周围环境的质地纹理是杂乱的，光流是可测的；

2)每个光流传感器都能正常工作，它们的输出含有量测噪声，但不存在完全错误的野值；

3)光流传感器的视场角很小，测得的信息为镜头轴线上的光流信息；

基于以上假设，见图3，本发明是一种利用光流传感器和惯性器件实现飞行器自主式组合导航的方法，该方法具体步骤如下：

步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；

这里，导航坐标系选用ENU(East-North-Up,东北天)坐标系。该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天。

E、N、U三个方向的平台误差角方程分别为

${\stackrel{\·}{\mathrm{\φ}}}_E={\mathrm{\φ}}_N({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})-\frac{{\mathrm{\δV}}_N}{R_M+h}+\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}-{\mathrm{\ϵ}}_E$

${\stackrel{\·}{\mathrm{\φ}}}_N={-\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U\frac{V_N}{R_M+h}-{\mathrm{\δL\ω}}_{\mathrm{ie}}\sin L+\frac{{\mathrm{\δV}}_E}{R_N+h}-\mathrm{\δh}\frac{V_E}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_N$

                                                          (40)

${\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\φ}}_N\frac{V_N}{R_M+h}+\mathrm{\δL}({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)+\frac{{\mathrm{\δV}}_E}{R_N+h}+\tan L$

$-\mathrm{\δh}\frac{V_E\tan L}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_U$

式中：

${\mathrm{\ϵ}}_E=C_{11}{\mathrm{\ϵ}}_x^b+C_{21}{\mathrm{\ϵ}}_y^b+C_{31}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_N=C_{12}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{32}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_U=C_{13}{\mathrm{\ϵ}}_x^b+C_{23}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b$

C_ij(i=1,2,3;j=1,2,3)为坐标变换矩阵

中的子项，为导航坐标系到本体坐标系的变换矩阵：

$C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)---\left(41\right)$

其中L、λ、h分别为纬度、经度和高度，V_E、V_N、V_U分别为东向、北向和天向的速度，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，

为三个陀螺的量测误差；ω_ie为地球自转角速度；R_M和R_N分别为地球的子午圈半径和卯酉圈半径。

E、N、U三个方向的速度误差方程分别为

$\mathrm{\δ}{\stackrel{\·}{V}}_E={\mathrm{\φ}}_U{f}_N-{\mathrm{\φ}}_N{f}_U+{\mathrm{\δV}}_E\frac{V_N\tan L-V_U}{R_N+h}+{\mathrm{\δV}}_N({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$-{\mathrm{\δV}}_U({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)$

$+\mathrm{\δh}\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}+{\▿}_E$

$\mathrm{\δ}{\stackrel{\·}{V}}_N={-\mathrm{\φ}}_U{f}_E+{\mathrm{\φ}}_E{f}_U-{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\δV}}_N\frac{V_U}{R_M+h}-{\mathrm{\δV}}_U\frac{V_N}{R_M+h}-$

$\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E+\mathrm{\δh}(\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2})+{\▿}_N$

$\mathrm{\δ}{\stackrel{\·}{V}}_U={\mathrm{\φ}}_N{f}_E-{\mathrm{\φ}}_E{f}_N+{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\δV}}_N\frac{{2V}_N}{R_M+h}-2{\mathrm{\δLV}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L-$

$\mathrm{\δh}(\frac{V_E^2}{{(R_N+h)}^2}+\frac{V_N^2}{{(R_M+h)}^2})+{\▿}_U---\left(42\right)$

式中：

${\▿}_E=C_{11}{\▿}_x^b+C_{21}{\▿}_y^b+C_{31}{\▿}_z^b$

${\▿}_N=C_{12}{\▿}_x^b+C_{22}{\▿}_y^b+C_{32}{\▿}_z^b$

${\▿}_U=C_{13}{\▿}_x^b+C_{23}{\▿}_y^b+C_{33}{\▿}_z^b$

f_E、f_N、f_U为E、N、U三个方向的比力，

为三个加速度计量测误差。E、N、U三个方向的位置误差方程分别为

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_N+h}\sec L+\mathrm{\δL}\frac{V_E}{R_N+h}\sec L\tan L-\mathrm{\δh}\frac{V_E\sec L}{{(R_N+h)}^2}---\left(43\right)$

$\mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δV}}_U$

于是，惯导误差方程可以写成：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(44\right)$

式中， $X={[\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\ϵ}}_{\mathrm{cx}},{\mathrm{\ϵ}}_{\mathrm{cy}},{\mathrm{\ϵ}}_{\mathrm{cz}}{,\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T.$

状态向量X共18维。其中δL、δλ、δh分别为纬度误差、经度误差和高度误差，δV_E、δV_N、δV_U分别为东向、北向和天向的速度误差，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，ε_cx，ε_cy，ε_cz为三个陀螺的随机常值偏差；ε_rx，ε_ry，ε_rz为三个陀螺的随机漂移（一阶马尔科夫过程）；

为三个加速度计的随机偏差（一阶马尔科夫过程）。

系统噪声为

w=[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

其中ω_gx，ω_gy，ω_gz为陀螺随机白噪声漂移；ω_rx，ω_ry，ω_rz为陀螺一阶马尔科夫驱动白噪声；ω_ax，ω_ay，ω_az为加速度计一阶马尔科夫驱动白噪声。

系统噪声分配阵为

$G=\left[\begin{array}{ccc}{0}_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ C_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]$

F中的非零元素为

$F_{\mathrm{1,3}}=\frac{{-V}_N}{{(R_M+h)}^2}$ $F_{\mathrm{1,5}}=\frac{1}{R_M+h}$ $F_{\mathrm{2,1}}=\frac{V_E\sec L\tan L}{R_N+h}$ $F_{\mathrm{2,3}}=\frac{{-V}_E\sec L}{{(R_N+h)}^2}$

$F_{\mathrm{2,4}}=\frac{\sec L}{R_N+h}$ F_3,6=1 $F_{\mathrm{4,1}}={2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L$

$F_{\mathrm{4,3}}=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}$ $F_{\mathrm{4,4}}=\frac{V_N\tan L-V_U}{R_N+h}$ $F_{\mathrm{4,5}}={2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h}$

$F_{4,6}={-2\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$ F_4,8=-f_U F_4,9=f_N F_4，16=C₁₁ F_4，17=C₂₁

F_4，18=C₃₁ $F_{\mathrm{5,1}}=-V_E({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)$ $F_{\mathrm{5,3}}=\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{5,4}}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$ $F_{\mathrm{5,5}}=-\frac{V_U}{R_M+h}$ $F_{\mathrm{5,6}}=-\frac{V_N}{R_M+h}$ F_5,7=f_U

F_5,9=-f_E F_5，16=C₁₂ F_5，17=C₂₂ F_5，18=C₃₂ F_6,1=-2V_Eω_iesinL

$F_{\mathrm{6,3}}=-\frac{{V_E}^2}{{(R_N+h)}^2}-\frac{{V_N}^2}{{(R_M+h)}^2}$ $F_{\mathrm{6,4}}={2\mathrm{\ω}}_{\mathrm{ie}}\cos L+2\frac{V_E}{R_N+h}$ $F_{\mathrm{6,5}}=\frac{{2V}_N}{R_M+h}$

F_6,7=-f_N F_6,8=f_E F_6，16=C₁₃ F_6，17=C₂₃ F_6，18=C₃₃

$F_{\mathrm{7,5}}=-\frac{1}{R_M+h}$ $F_{\mathrm{7,8}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{7,9}}={-\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$

F_7，10=-C₁₁ F_7，11=-C₂₁ F_7，12=-C₃₁ F_7，13=-C₁₁ F_7，14=-C₂₁ F_7，15=-C₃₁

F_8,1=-ω_iesinL $F_{\mathrm{8,3}}=-\frac{V_E}{{(R_N+h)}^2}$ $F_{\mathrm{8,4}}=\frac{1}{R_N+h}$ $F_{\mathrm{8,7}}={-\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R_N+h}$

F_8，10=-C₁₂ F_8，11=-C₂₂ F_8，12=-C₃₂ F_8，13=-C₁₂

F_8，14=-C₂₂ F_8，15=-C₃₂ $F_{\mathrm{9,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h}$ $F_{\mathrm{9,3}}=-\frac{V_E\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{9,4}}=\frac{\tan L}{R_N+h}$ $F_{\mathrm{9,7}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{9,8}}=\frac{V_N}{R_M+h}$ F_9，10=-C₁₃

F_9，11=-C₂₃ F_9，12=-C₃₃ F_9，13=-C₁₃ F_9，14=-C₂₃ F_9，15=-C₃₃

$F_{\mathrm{14,14}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{15,15}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{16,16}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{17,17}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{18,18}}=-\frac{1}{{\mathrm{\τ}}_A}$

步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；

在推导光流传感器的量测方程之前，先定义几个坐标系，如图4所示：

导航坐标系(S_n):为了与惯导统一，选用ENU(East-North-Up,东北天)坐标系。该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天。

本体坐标系(S_b):本体坐标系固连在MAV上，其原点在MAV的质心处，y轴指向MAV的前方，z轴沿MAV纵向对称面朝上，x轴按右手定则确定。

光流传感器坐标系(S_f):光流传感器坐标系固连在光流传感器上，其原点在镜头的焦点处，z轴沿光轴方向指向外，x轴和y轴分别与测得的两个正交方向的光流重合。

于是，光流传感器的量测值为：

$f_f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)---\left(45\right)$

这里,V_nf和ω_nf分别是光流传感器相对于导航坐标系的速度矢量和角速度矢量，下标f，x和f,y分别代表光流传感器坐标系中的x分量和y分量。d_fg为光流传感器的焦点沿z_f到地面的距离。

令r_nb为S_b相对于S_n的位置矢量，r_bf为S_f相对于S_b的位置矢量，于是光流传感器的速度矢量可表示为：

$V_{\mathrm{nf}}=\frac{{\mathrm{dr}}_{\mathrm{nf}}}{\mathrm{dt}}=\frac{d}{\mathrm{dt}}(r_{\mathrm{nb}}+r_{\mathrm{bf}})=\frac{{\mathrm{dr}}_{\mathrm{nb}}}{\mathrm{dt}}+\frac{{\mathrm{dr}}_{\mathrm{bf}}}{\mathrm{dt}}---\left(46\right)$

将速度矢量向S_f中投影：

${\left(V_{\mathrm{nf}}\right)}_f=C_n^f\frac{d{\left(r_{\mathrm{nb}}\right)}_n}{\mathrm{dt}}+C_b^f(\frac{d{\left(r_{\mathrm{bf}}\right)}_b}{\mathrm{dt}}+{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b)$

$=C_n^f{V}_n+C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b\×{\left(r_{\mathrm{bf}}\right)}_b---\left(47\right)$

$=C_n^f{V}_n+C_b^f({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×{\left(r_{\mathrm{bf}}\right)}_b$

本体坐标系到光流传感器坐标系的转换矩阵为：Y(μ)->X(η)

$C_b^f=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\η}& \sin \mathrm{\η}\\ 0& -\sin \mathrm{\η}& \cos \mathrm{\η}\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}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\cos \mathrm{\μ}\end{array}\right)---\left(48\right)$

这里,μ和η是光流传感器的安装角，它们是光流传感器坐标系相对于本体坐标系的欧拉角，也就是说，将本体坐标系沿y_b轴转动角度μ，然后再沿x_b轴转动角度η，即可得到光流传感器坐标系。由于μ和η是常值，故

为常值矩阵。

设z_f的方向矢量为k_f，即(k_f)_f=(0 0 1)^T，于是将k_f向S_n投影得：

${\left(k_f\right)}_n=C_f^n{\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f---\left(49\right)$

z_f和z_n间夹角的余弦值为-(k_f)_n，z，于是：

(k_f)_n，z=C₁₃cosηsinμ-C₂₃sinη+C₃₃cosηcosμ        (50)

        =C₁₃T₃₁+C₂₃T₃₂+C₃₃T₃₃

光流传感器沿其光轴方向到地面的距离为：

$d_{\mathrm{fg}}=\left|\frac{{\left(r_{\mathrm{nf}}\right)}_{n,z}}{{\left(k_f\right)}_{n,z}}\right|$

$=-\frac{{(r_{\mathrm{nb}}+r_{\mathrm{bf}})}_{n,z}}{{\left(k_f\right)}_{n,z}}$

                                (51)

$=-\frac{{\left(r_{\mathrm{nb}}\right)}_{n,z}+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

$=-\frac{{h+\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

而

${\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_f={\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_f$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b$

$=C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b---\left(52\right)$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{\left({\mathrm{\ω}}_{\mathrm{in}}\right)}_n$

${=C}_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n$

于是，光流传感器的量测方程为

$f_f=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)$

                                     (53)

$=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}\×{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_x\end{array}\right)$

步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；

在推导光流量测方程的线化系数之前，首先对光流的量测方程进行合理的简化。光流的平动部分，即

和

基本上与

是一个量级的，就一般飞行器来说，其量级大于10^-3s^-1，而|ω_ie|的量级为10^-5rad/s，

量级不大于10^-5s^-1；另一方面，光流传感器是有噪声的，10^-5s^-1的数值会被量测噪声湮没，于是，光流量测方程(53)可简化为：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(54\right)$

现在求取典型安装位置和角度情况下的光流误差方程。

1、安装在纵轴纵面内

(r_bf)_b=(0,r_y,0)^T，μ=π，于是：

$C_b^f=\left(\begin{array}{ccc}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\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)=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)---\left(55\right)$

由于 $C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right),$ 于是，由式(49)得：

${\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ T_{23}\\ -T_{22}\end{array}\right)$

$=\left(\begin{array}{c}{C}_{21}{T}_{23}-C_{31}{T}_{22}\\ C_{22}{T}_{23}-C_{32}{T}_{22}\\ C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)$

于是，

(k_f)_n,z=C₂₃T₂₃-C₃₃T₂₂                (56)

由式(41)，

$C_b^n{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)=\left(\begin{array}{c}{C}_{21}{r}_y\\ C_{22}{r}_y\\ C_{23}{r}_y\end{array}\right),$

于是

${\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z=C_{23}{r}_y---\left(57\right)$

而，

$C_n^f{V}_n=C_b^f{C}_n^b{V}_n$

$=\begin{array}{c}\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\end{array}\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)$

$=\begin{array}{c}\end{array}\left(\begin{array}{ccc}-C_{11}& -C_{12}& {-C}_{13}\\ C_{21}{T}_{22}+C_{31}{T}_{23}& C_{22}{T}_{22}+C_{32}{T}_{23}& C_{23}{T}_{22}+C_{33}{T}_{23}\\ C_{21}{T}_{23}-C_{31}{T}_{22}& C_{22}{T}_{23}-C_{32}{T}_{22}& C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)---\left(58\right)$

$=\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-C_{13}{V}_U\\ (C_{21}{T}_{22}+C_{31}+T_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N+(C_{23}{T}_{22}+C_{33}{T}_{23})V_U\\ (C_{21}{T}_{23}-C_{31}{T}_{22})V_E+(C_{22}{T}_{23}-C_{32}{T}_{22})V_N+(C_{23}{T}_{23}-C_{33}{T}_{22})V_U\end{array}\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{ccc}0& {-\mathrm{\ω}}_{\mathrm{ib},z}^b& {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b& 0& {-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ {-\mathrm{\ω}}_{\mathrm{ib},y}^b& {\mathrm{\ω}}_{\mathrm{ib},x}^b& 0\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)$

                                              (59)

$=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ 0\\ {\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b\\ -T_{22}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}{r}_y\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)=\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},y}^b-T_{22}{\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)---\left(60\right)$

将式(56)~(60)代入光流公式(54)得：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(61\right)$

展开并写成分量形式：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-\\ C_{13}{V}_U+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\end{array}\right)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(62\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +(C_{23}{T}_{22}+C_{33}{T}_{23})V_U+T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(63\right)$

对于飞机来说，其水平速度一般要远大于其垂直速度，故上式可以简化为：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×({-C}_{11}{V}_E-C_{12}{V}_N+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(64\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(65\right)$

方程(64)(65)未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定（以式(64)表示的x向光流为例）：

$f_x+{\mathrm{\δf}}_x=-\frac{({\hat{C}}_{23}{T}_{23}-{\hat{C}}_{33}{T}_{22})}{h+\mathrm{\δh}+{\hat{C}}_{23}{r}_y}\×(-{\hat{C}}_{11}(V_E+{\mathrm{\δV}}_E)-{\hat{C}}_{12}(V_N+{\mathrm{\δV}}_N)+({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b)r_y)+---\left(66\right)$

$(T_{22}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)+T_{23}({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b))$

式(66)中，

由下式确定：

${\hat{C}}_n^b=C_n^b(I+{\mathrm{\Φ}}_{\×}^n)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right)---\left(67\right)$

$=\left(\begin{array}{ccc}{C}_{11}+{\mathrm{\φ}}_U{C}_{12}-{\mathrm{\φ}}_N{C}_{13}& {-\mathrm{\φ}}_U{C}_{11}+C_{12}+{\mathrm{\φ}}_E{C}_{13}& {\mathrm{\φ}}_N{C}_{11}-{\mathrm{\φ}}_E{C}_{12}+C_{13}\\ C_{21}+{\mathrm{\φ}}_U{C}_{22}-{\mathrm{\φ}}_N{C}_{23}& {-\mathrm{\φ}}_U{C}_{21}+C_{22}+{\mathrm{\φ}}_E{C}_{23}& {\mathrm{\φ}}_N{C}_{21}-{\mathrm{\φ}}_E{C}_{22}+C_{23}\\ C_{31}+{\mathrm{\φ}}_U{C}_{32}-{\mathrm{\φ}}_N{C}_{33}& {-\mathrm{\φ}}_U{C}_{31}+C_{32}+{\mathrm{\φ}}_E{C}_{33}& {\mathrm{\φ}}_N{C}_{31}+{\mathrm{\φ}}_E{C}_{32}+C_{33}\end{array}\right)$

式(66)减去式(64)，并略去高阶小项，即可得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (68)

H_fx是一个1×12的行阵，它的各个子项都很复杂，这里只给出相对简单的第一项，其它的不再一一列出。

$H_{\mathrm{fx}}\left(\mathrm{1,1}\right)=\frac{1}{{(h+C_{23}{r}_y)}^2}\left(\begin{array}{c}(C_{23}{T}_{23}-C_{33}{T}_{22}{)\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y+(C_{33}{T}_{22}{C}_{11}-C_{23}{T}_{23}{C}_{11})V_E+\\ (C_{33}{T}_{22}{C}_{12}-C_{23}{T}_{23}{C}_{12})V_N\end{array}\right)$

同理可以得到

δf_y=H_fy[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (69)

合并可得光流误差方程为：

δf=H_f[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (70)

2、安装在横轴纵面内

(r_bf)_b=(r_x,0,0)^T，η=0，跟前文的推导类似，光流公式可以简化为：

$f_x=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{13}{r}_x}\×\left(\begin{array}{c}(C_{11}{T}_{11}+C_{31}{T}_{13})V_E+(C_{12}{T}_{11}+C_{32}{T}_{13})V_N\\ {-T}_{13}{\mathrm{\ω}}_{\mathrm{ib},y}^b{r}_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b---\left(71\right)$

$f_y=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{23}{r}_x}\×\left(\begin{array}{c}{C}_{21}{V}_E+C_{22}{V}_N\\ +{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_x\end{array}\right)-T_{11}{\mathrm{\ω}}_{\mathrm{ib},x}^b-T_{13}{\mathrm{\ω}}_{\mathrm{ib},z}^b---\left(72\right)$

上述方程未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定（以式(71)表示的x向光流为例）：

$f_x+{\mathrm{\δf}}_x=\frac{({\hat{C}}_{13}{T}_{13}-{\hat{C}}_{33}{T}_{11})}{h+\mathrm{\δh}+{\hat{C}}_{13}{r}_x}\×\left(\begin{array}{c}({\hat{C}}_{11}{T}_{11}+{\hat{C}}_{31}{T}_{13})(V_E+{\mathrm{\δV}}_E)+({\hat{C}}_{12}{T}_{11}+{\hat{C}}_{32}{T}_{13})\\ \×(V_N+{\mathrm{\δV}}_N)-T_{13}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)r_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b---\left(73\right)$

式(73)减去(71)，并略去高阶小项，即可得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_ry ε_cz ε_rx ε_ry ε_rz]^T    (74)

这与式(68)的形式是完全相同的。

最终，安装在飞行器纵轴纵面内的光流传感器和安装在横轴纵面内的光流传感器可以组合到一起，成为组合导航系统的量测方程：

Z=HX+v(t)

这里，Z=δf，H可以由H_f扩展得到，v(t)表示光流传感器的量测噪声。

步骤四：用扩展卡尔曼滤波器估计惯导误差，并使用此误差对惯导数据进行修正，得到更为精确的导航数据。

3个光流传感器在MAV上的安装位置和方向，以(x_b y_b z_b μη)的形式给出，形成矩阵M_3×5，设：

$M_{3\×5}=\left(\begin{array}{ccccc}0& 2& 0& \mathrm{\π}& -\frac{\mathrm{\π}}{6}\\ 2& 0& 0& \frac{5\mathrm{\π}}{6}& 0\\ -2& 0& 0& \frac{7\mathrm{\π}}{6}& 0\end{array}\right)---\left(75\right)$

惯导误差初值X₀=0，按图5所示的组合导航原理框图，经数值仿真，得到的量测数据滤波效果如图6~8所示。图6表明，组合导航的纬度误差比纯惯导小一个数量级，纬度误差减小至1/3，高度误差几近于0，综合纬度误差和经度误差可以算得，组合导航的位置误差大约是纯惯导的1/6；图7表明，组合导航有效地抑制了纯惯导的速度发散，这其实也是减小位置误差的主要原因；图8表明，组合导航可以减小东向和北向的平台误差角，但对天向平台误差角没有发挥抑制作用。

图6~8证明，本发明所提的这种光流与惯导组合导航，可以有效地抑制纯惯导的发散，提高导航精度。

Claims (2)

1.一种飞行器多个光流传感器与惯导组合导航方法，其特征在于：该方法具体步骤如下：

步骤一：将微机械三轴速率陀螺和三轴加速度计安装到飞行器上，组成捷联式惯性导航系统，建立惯导误差方程；

导航坐标系选用ENU即东北天坐标系，该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天；

E、N、U三个方向的平台误差角方程分别为

${\stackrel{\·}{\mathrm{\φ}}}_E={\mathrm{\φ}}_N({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})-\frac{{\mathrm{\δV}}_N}{R_M+h}+\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}-{\mathrm{\ϵ}}_E$

${\stackrel{\·}{\mathrm{\φ}}}_N={-\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\φ}}_U\frac{V_N}{R_M+h}-{\mathrm{\δL\ω}}_{\mathrm{ie}}\sin L+\frac{{\mathrm{\δV}}_E}{R_N+h}-\mathrm{\δh}\frac{V_E}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_N$

                                                         （2）

${\stackrel{\·}{\mathrm{\φ}}}_U={\mathrm{\φ}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\φ}}_N\frac{V_N}{R_M+h}+\mathrm{\δL}({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)+\frac{{\mathrm{\δV}}_E}{R_N+h}+\tan L$

$-\mathrm{\δh}\frac{V_E\tan L}{{(R_N+h)}^2}-{\mathrm{\ϵ}}_U$

式中：

${\mathrm{\ϵ}}_E=C_{11}{\mathrm{\ϵ}}_x^b+C_{21}{\mathrm{\ϵ}}_y^b+C_{31}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_N=C_{12}{\mathrm{\ϵ}}_x^b+C_{22}{\mathrm{\ϵ}}_y^b+C_{32}{\mathrm{\ϵ}}_z^b$

${\mathrm{\ϵ}}_U=C_{13}{\mathrm{\ϵ}}_x^b+C_{23}{\mathrm{\ϵ}}_y^b+C_{33}{\mathrm{\ϵ}}_z^b$

C_ij(i=1,2,3;j=1,2,3)为坐标变换矩阵

中的子项，

为导航坐标系到本体坐标系的变换矩阵：

$C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)---\left(3\right)$

其中L、λ、h分别为纬度、经度和高度，V_E、V_N、V_U分别为东向、北向和天向的速度，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，

为三个陀螺的量测误差；ω_ie为地球自转角速度；R_M和R_N分别为地球的子午圈半径和卯酉圈半径；

E、N、U三个方向的速度误差方程分别为

$\mathrm{\δ}{\stackrel{\·}{V}}_E={\mathrm{\φ}}_U{f}_N-{\mathrm{\φ}}_N{f}_U+{\mathrm{\δV}}_E\frac{V_N\tan L-V_U}{R_N+h}+{\mathrm{\δV}}_N({2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$

$-{\mathrm{\δV}}_U({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L)$

$+\mathrm{\δh}\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}+{\▿}_E$

$\mathrm{\δ}{\stackrel{\·}{V}}_N={-\mathrm{\φ}}_U{f}_E+{\mathrm{\φ}}_E{f}_U-{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)-{\mathrm{\δV}}_N\frac{V_U}{R_M+h}-{\mathrm{\δV}}_U\frac{V_N}{R_M+h}-$

$\mathrm{\δL}({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_E+\mathrm{\δh}(\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2})+{\▿}_N$

$\mathrm{\δ}{\stackrel{\·}{V}}_U={\mathrm{\φ}}_N{f}_E-{\mathrm{\φ}}_E{f}_N+{2\mathrm{\δV}}_E({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h})+{\mathrm{\δV}}_N\frac{{2V}_N}{R_M+h}-2{\mathrm{\δLV}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L-$

$\mathrm{\δh}(\frac{V_E^2}{{(R_N+h)}^2}+\frac{V_N^2}{{(R_M+h)}^2})+{\▿}_U---\left(4\right)$

式中：

${\▿}_E=C_{11}{\▿}_x^b+C_{21}{\▿}_y^b+C_{31}{\▿}_z^b$

${\▿}_E=C_{11}{\▿}_x^b+C_{21}{\▿}_y^b+C_{31}{\▿}_z^b$

${\▿}_N=C_{12}{\▿}_x^b+C_{22}{\▿}_y^b+C_{32}{\▿}_z^b$

f_E、f_N、f_U为E、N、U三个方向的比力，

为三个加速度计量测误差；E、N、U三个方向的位置误差方程分别为

$\mathrm{\δ}\stackrel{\·}{L}=\frac{{\mathrm{\δV}}_N}{R_M+h}-\mathrm{\δh}\frac{V_N}{{(R_M+h)}^2}$

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{{\mathrm{\δV}}_E}{R_N+h}\sec L+\mathrm{\δL}\frac{V_E}{R_N+h}\sec L\tan L-\mathrm{\δh}\frac{V_E\sec L}{{(R_N+h)}^2}---\left(5\right)$

$\mathrm{\δ}\stackrel{\·}{h}={\mathrm{\δV}}_U$

于是，惯导误差方程写成：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{Gw}---\left(6\right)$

式中， $X={[\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\δV}}_E,{\mathrm{\δV}}_N,{\mathrm{\δV}}_U,{\mathrm{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,{\mathrm{\ϵ}}_{\mathrm{cx}},{\mathrm{\ϵ}}_{\mathrm{cy}},{\mathrm{\ϵ}}_{\mathrm{cz}}{,\mathrm{\ϵ}}_{\mathrm{rx}},{\mathrm{\ϵ}}_{\mathrm{ry}},{\mathrm{\ϵ}}_{\mathrm{rz}},{\▿}_x,{\▿}_y,{\▿}_z]}^T;$

状态向量X共18维，其中δL、δλ、δh分别为纬度误差、经度误差和高度误差，δV_E、δV_N、δV_U分别为东向、北向和天向的速度误差，φ_E、φ_N、φ_U分别为东向、北向和天向的平台误差角，ε_cx，ε_cy，ε_cz为三个陀螺的随机常值偏差；ε_rx，ε_ry，ε_rz为三个陀螺的随机漂移；

为三个加速度计的随机偏差；

系统噪声为

w=[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

其中ω_gx，ω_gy，ω_gz为陀螺随机白噪声漂移；ω_rx，ω_ry，ω_rz为陀螺一阶马尔科夫驱动白噪声；ω_ax，ω_ay，ω_az为加速度计一阶马尔科夫驱动白噪声；

系统噪声分配阵为

$G=\left[\begin{array}{ccc}{0}_{6\×3}& 0_{6\×3}& 0_{6\×3}\\ C_b^n& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}\end{array}\right]$

F中的非零元素为

$F_{\mathrm{1,3}}=\frac{{-V}_N}{{(R_M+h)}^2}$ $F_{\mathrm{1,5}}=\frac{1}{R_M+h}$ $F_{\mathrm{2,1}}=\frac{V_E\sec L\tan L}{R_N+h}$ $F_{\mathrm{2,3}}=\frac{{-V}_E\sec L}{{(R_N+h)}^2}$

$F_{\mathrm{2,4}}=\frac{\sec L}{R_N+h}$ F_3,6=1 $F_{\mathrm{4,1}}={2\mathrm{\ω}}_{\mathrm{ie}}(V_N\cos L+V_U\sin L)+\frac{V_E{V}_N}{R_N+h}{\sec }^{2}L$

$F_{\mathrm{4,3}}=\frac{V_E{V}_U-V_E{V}_N\tan L}{{(R_N+h)}^2}$ $F_{\mathrm{4,4}}=\frac{V_N\tan L-V_U}{R_N+h}$ $F_{\mathrm{4,5}}={2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E\tan L}{R_N+h}$

F_4,8=-f_U F_4,9=f_N F_4，16=C₁₁ F_4，17=C₂₁

F_4，18=C₃₁ $F_{\mathrm{5,1}}=-V_E({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)$ $F_{\mathrm{5,3}}=\frac{V_N{V}_U}{{(R_M+h)}^2}+\frac{V_E^2\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{5,4}}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L)$ $F_{\mathrm{5,5}}=-\frac{V_U}{R_M+h}$ $F_{\mathrm{5,6}}=-\frac{V_N}{R_M+h}$ F_5,7=f_U

F_5,9=-f_E F_5，16=C₁₂ F_5，17=C₂₂ F_5，18=C₃₂ F_6,1=-2V_Eω_iesinL

$F_{\mathrm{6,3}}=-\frac{{V_E}^2}{{(R_N+h)}^2}-\frac{{V_N}^2}{{(R_M+h)}^2}$ $F_{\mathrm{6,4}}={2\mathrm{\ω}}_{\mathrm{ie}}\cos L+2\frac{V_E}{R_N+h}$ $F_{\mathrm{6,5}}=\frac{{2V}_N}{R_M+h}$

F_6,7=-f_N F_6,8=f_E F_6，16=C₁₃ F_6，17=C₂₃ F_6，18=C₃₃

$F_{\mathrm{7,5}}=-\frac{1}{R_M+h}$ $F_{\mathrm{7,8}}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_N+h}\tan L$ $F_{\mathrm{7,9}}={-\mathrm{\ω}}_{\mathrm{ie}}\cos L-\frac{V_E}{R_N+h}$

F_7，10=-C₁₁ F_7，11=-C₂₁ F_7，12=-C₃₁ F_7，13=-C₁₁ F_7，14=-C₂₁ F_7，15=-C₃₁

F_8,1=-ω_iesinL $F_{\mathrm{8,3}}=-\frac{V_E}{{(R_N+h)}^2}$ $F_{\mathrm{8,4}}=\frac{1}{R_N+h}$ $F_{\mathrm{8,7}}={-\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{V_E\tan L}{R_N+h}$

F_8，10=-C₁₂ F_8，11=-C₂₂ F_8，12=-C₃₂ F_8，13=-C₁₂

F_8，14=-C₂₂ F_8，15=-C₃₂ $F_{\mathrm{9,1}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E{\sec }^{2}L}{R_N+h}$ $F_{\mathrm{9,3}}=-\frac{V_E\tan L}{{(R_N+h)}^2}$

$F_{\mathrm{9,4}}=\frac{\tan L}{R_N+h}$ $F_{\mathrm{9,7}}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}$ $F_{\mathrm{9,8}}=\frac{V_N}{R_M+h}$ F_9，10=-C₁₃

F_9，11=-C₂₃ F_9，12=-C₃₃ F_9，13=-C₁₃ F_9，14=-C₂₃ F_9，15=-C₃₃

$F_{\mathrm{14,14}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{15,15}}=-\frac{1}{{\mathrm{\τ}}_G}$ $F_{\mathrm{16,16}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{17,17}}=-\frac{1}{{\mathrm{\τ}}_A}$ $F_{\mathrm{18,18}}=-\frac{1}{{\mathrm{\τ}}_A};$

步骤二：将3个光流传感器多点布置在飞行器上，建立光流传感器的量测方程；

将3个光流传感器多点布置在飞行器上，在空间允许的情况下，各传感器间的距离要尽量远，并指向不同方向，这样做有利于提高后续的估计精度；其中，“多点布置”是指，光流传感器要安装在飞行器的不同位置，典型位置是头部、中间、尾部和翼梢；“距离要尽量远”是指，安装在头部、尾部或者翼梢的光流传感器，在不影响其它机载设备的情况下，要尽量靠近机体的最前端、最后端或者侧端，这样就保证了头部、尾部及翼梢光流传感器间的距离尽可能大些；

在推导光流传感器的量测方程之前，先定义几个坐标系：

导航坐标系(S_n):为了与惯导统一，选用ENU东北天坐标系，该坐标系与地球表面固连，x轴指东，y轴指北，z轴指天；

本体坐标系(S_b):本体坐标系固连在MAV上，其原点在MAV的质心处，y轴指向MAV的前方，z轴沿MAV纵向对称面朝上，x轴按右手定则确定；

光流传感器坐标系(S_f):光流传感器坐标系固连在光流传感器上，其原点在镜头的焦点处，z轴沿光轴方向指向外，x轴和y轴分别与测得的两个正交方向的光流重合；

于是，光流传感器的量测值为：

$f_f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)---\left(7\right)$

这里,V_nf和ω_nf分别是光流传感器相对于导航坐标系的速度矢量和角速度矢量，下标f，x和f,y分别代表光流传感器坐标系中的x分量和y分量，d_fg为光流传感器的焦点沿z_f到地面的距离；

令r_nb为S_b相对于S_n的位置矢量，r_bf为S_f相对于S_b的位置矢量，于是光流传感器的速度矢量表示为：

$V_{\mathrm{nf}}=\frac{{\mathrm{dr}}_{\mathrm{nf}}}{\mathrm{dt}}=\frac{d}{\mathrm{dt}}(r_{\mathrm{nb}}+r_{\mathrm{bf}})=\frac{{\mathrm{dr}}_{\mathrm{nb}}}{\mathrm{dt}}+\frac{{\mathrm{dr}}_{\mathrm{bf}}}{\mathrm{dt}}---\left(8\right)$

将速度矢量向S_f中投影：

${\left(V_{\mathrm{nf}}\right)}_f=C_n^f\frac{d{\left(r_{\mathrm{nb}}\right)}_n}{\mathrm{dt}}+C_b^f(\frac{d{\left(r_{\mathrm{bf}}\right)}_b}{\mathrm{dt}}+{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b)$

$=C_n^f{V}_n+C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b\×{\left(r_{\mathrm{bf}}\right)}_b---\left(9\right)$

$=C_n^f{V}_n+C_b^f({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×{\left(r_{\mathrm{bf}}\right)}_b$

本体坐标系到光流传感器坐标系的转换矩阵为：Y(μ)->X(η)

$C_b^f=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\η}& \sin \mathrm{\η}\\ 0& -\sin \mathrm{\η}& \cos \mathrm{\η}\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}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\cos \mathrm{\μ}\end{array}\right)---\left(10\right)$

这里,μ和η是光流传感器的安装角，它们是光流传感器坐标系相对于本体坐标系的欧拉角，也就是说，将本体坐标系沿y_b轴转动角度μ，然后再沿x_b轴转动角度η，即得到光流传感器坐标系；由于μ和η是常值，故

为常值矩阵；

设z_f的方向矢量为k_f，即(k_f)_f=(0 0 1)^T，于是将k_f向S_n投影得：

${\left(k_f\right)}_n=C_f^n{\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f---\left(11\right)$

z_f和z_n间夹角的余弦值为-(k_f)_n，z，于是：

(k_f)_n，z=C₁₃cosηsinμ-C₂₃sinη+C₃₃cosηcosμ            （12）

        =C₁₃T₃₁+C₂₃T₃₂+C₃₃T₃₃

光流传感器沿其光轴方向到地面的距离为：

$d_{\mathrm{fg}}=\left|\frac{{\left(r_{\mathrm{nf}}\right)}_{n,z}}{{\left(k_f\right)}_{n,z}}\right|$

$=-\frac{{(r_{\mathrm{nb}}+r_{\mathrm{bf}})}_{n,z}}{{\left(k_f\right)}_{n,z}}$

                                  （13）

$=-\frac{{\left(r_{\mathrm{nb}}\right)}_{n,z}+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

$=-\frac{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}{{\left(k_f\right)}_{n,z}}$

而

${\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_f={\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_f$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{nb}}\right)}_b$

$=C_b^f{({\mathrm{\ω}}_{\mathrm{ib}}-{\mathrm{\ω}}_{\mathrm{in}})}_b---\left(14\right)$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{\left({\mathrm{\ω}}_{\mathrm{in}}\right)}_n$

$=C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n$

于是，光流传感器的量测方程为

$f_f=\left(\begin{array}{c}\frac{(V_{\mathrm{nf}}{)}_{f,x}}{d_{\mathrm{fg}}}+{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,y}\\ \frac{{\left(V_{\mathrm{nf}}\right)}_{f,y}}{d_{\mathrm{fg}}}-{\left({\mathrm{\ω}}_{\mathrm{nf}}\right)}_{f,x}\end{array}\right)$ (15)；

$=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}\×{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{({\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b-C_b^f{C}_n^b{({\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})}_n)}_x\end{array}\right)$

步骤三：根据光流传感器的量测方程，建立线性化的光流误差方程，作为组合导航系统的量测方程；

事实上，一个光流传感器可以同时测出正交方向上的两个光流分量，其量测输出可记为

$f=\left(\begin{array}{c}{f}_x\\ f_y\end{array}\right)---\left(16\right)$

求取光流误差方程为

δf=H_fX+v(t)                    (17)

v(t)为量测噪声，假设其为均值为0的白噪声，即E[v(t)]＝0，且E[v(t)v^T(τ)]=r_vδ(t-τ)，r_v为v(t)的方差强度阵；

在推导光流量测方程的线化系数之前，首先对光流的量测方程进行合理的简化，

光流的平动部分，即

和基本上与

是一个量级的，就一般飞行器来说，其量级大于10^-3s^-1，而|ω_ie|的量级为10^-5rad/s，

量级不大于10^-5s^-1；另一方面，光流传感器是有噪声的，10^-5s^-1的数值会被量测噪声湮没，于是，光流量测方程（17）简化为：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(18\right)$

现在求取典型安装位置和角度情况下的光流误差方程：

1、安装在纵轴纵面内

(r_bf)_b=(0,r_y,0)^T，μ＝π，于是：

$C_b^f=\left(\begin{array}{ccc}\cos \mathrm{\μ}& 0& -\sin \mathrm{\μ}\\ \sin \mathrm{\η}\sin \mathrm{\μ}& \cos \mathrm{\η}& \sin \mathrm{\η}\cos \mathrm{\μ}\\ \cos \mathrm{\η}\sin \mathrm{\μ}& -\sin \mathrm{\η}& \cos \mathrm{\η}\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)=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)---\left(19\right)$

由于 $C_n^b=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right),$ 于是，由式（13）得：

${\left(k_f\right)}_f=C_b^n{C}_f^b{\left(k_f\right)}_f$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ T_{23}\\ -T_{22}\end{array}\right)$

$=\left(\begin{array}{c}{C}_{21}{T}_{23}-C_{31}{T}_{22}\\ C_{22}{T}_{23}-C_{32}{T}_{22}\\ C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)$

于是，

(k_f)_n，z=C₂₃T₂₃-C₃₃T₂₂                (20)

由式（5），

$C_b^n{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{31}\\ C_{12}& C_{22}& C_{32}\\ C_{13}& C_{23}& C_{33}\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)=\left(\begin{array}{c}{C}_{21}{r}_y\\ C_{22}{r}_y\\ C_{23}{r}_y\end{array}\right),$

于是

${\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z=C_{23}{r}_y---\left(21\right)$

而，

$C_n^f{V}_n=C_b^f{C}_n^b{V}_n$

$=\begin{array}{c}\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\end{array}\left(\begin{array}{ccc}{C}_{11}& C_{21}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)$

$=\begin{array}{c}\end{array}\left(\begin{array}{ccc}-C_{11}& -C_{12}& {-C}_{13}\\ C_{21}{T}_{22}+C_{31}{T}_{23}& C_{22}{T}_{22}+C_{32}{T}_{23}& C_{23}{T}_{22}+C_{33}{T}_{23}\\ C_{21}{T}_{23}-C_{31}{T}_{22}& C_{22}{T}_{23}-C_{32}{T}_{22}& C_{23}{T}_{23}-C_{33}{T}_{22}\end{array}\right)\left(\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right)---\left(22\right)$

$=\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-C_{13}{V}_U\\ (C_{21}{T}_{22}+C_{31}+T_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N+(C_{23}{T}_{22}+C_{33}{T}_{23})V_U\\ (C_{21}{T}_{23}-C_{31}{T}_{22})V_E+(C_{22}{T}_{23}-C_{32}{T}_{22})V_N+(C_{23}{T}_{23}-C_{33}{T}_{22})V_U\end{array}\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×{\left(r_{\mathrm{bf}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& -T_{22}\end{array}\right)\left(\begin{array}{ccc}0& {-\mathrm{\ω}}_{\mathrm{ib},z}^b& {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b& 0& {-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ {-\mathrm{\ω}}_{\mathrm{ib},y}^b& {\mathrm{\ω}}_{\mathrm{ib},x}^b& 0\end{array}\right)\left(\begin{array}{c}0\\ r_y\\ 0\end{array}\right)$

                                       (23)

$=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ 0\\ {\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)=\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b\\ -T_{22}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}{r}_y\right)$

$C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b=\left(\begin{array}{ccc}-1& 0& 0\\ 0& T_{22}& T_{23}\\ 0& T_{23}& {-T}_{22}\end{array}\right)\left(\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ {\mathrm{\ω}}_{\mathrm{ib},y}^b\\ {\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)=\left(\begin{array}{c}{-\mathrm{\ω}}_{\mathrm{ib},x}^b\\ T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b\\ T_{23}{\mathrm{\ω}}_{\mathrm{ib},y}^b-T_{22}{\mathrm{\ω}}_{\mathrm{ib},z}^b\end{array}\right)---\left(24\right)$

将式（20）~（24）代入光流公式（18）得：

$f_f=\left(\begin{array}{c}-\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_x}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}+{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_y\\ -\frac{{\left(k_f\right)}_{n,z}{(C_n^f{V}_n+C_b^f{{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\×\left(r_{\mathrm{bf}}\right)}_b)}_y}{h+{\left[C_b^n{\left(r_{\mathrm{bf}}\right)}_b\right]}_z}-{\left(C_b^f{\left({\mathrm{\ω}}_{\mathrm{ib}}\right)}_b\right)}_x\end{array}\right)---\left(25\right)$

展开并写成分量形式：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}{-C}_{11}{V}_E-C_{12}{V}_N-\\ C_{13}{V}_U+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y\end{array}\right)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(26\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E-(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +(C_{23}{T}_{22}+C_{33}{T}_{23})V_U+{T_{23}\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(27\right)$

对于飞机来说，其水平速度一般要远大于其垂直速度，故上式简化为：

$f_x=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×({-C}_{11}{V}_E-C_{12}{V}_N+{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y)+(T_{22}{\mathrm{\ω}}_{\mathrm{ib},y}^b+T_{23}{\mathrm{\ω}}_{\mathrm{ib},z}^b)---\left(28\right)$

$f_y=-\frac{(C_{23}{T}_{23}-C_{33}{T}_{22})}{h+C_{23}{r}_y}\×\left(\begin{array}{c}(C_{21}{T}_{22}+C_{31}{T}_{23})V_E+(C_{22}{T}_{22}+C_{32}{T}_{23})V_N\\ +T_{23}{\mathrm{\ω}}_{\mathrm{ib},x}^b{r}_y\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},x}^b---\left(29\right)$

方（28）（29）未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定，以式（28）表示的x向光流为例：

$f_x+{\mathrm{\δf}}_x=-\frac{({\hat{C}}_{23}{T}_{23}-{\hat{C}}_{33}{T}_{22})}{h+\mathrm{\δh}+{\hat{C}}_{23}{r}_y}\×(-{\hat{C}}_{11}(V_E+{\mathrm{\δV}}_E)-{\hat{C}}_{12}(V_N+{\mathrm{\δV}}_N)+({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b)r_y)+---\left(30\right)$

$(T_{22}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)+T_{23}({\mathrm{\ω}}_{\mathrm{ib},z}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},z}^b))$

式（30）中，由下式确定：

${\hat{C}}_n^b=C_n^b(I+{\mathrm{\Φ}}_{\×}^n)$

$=\left(\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right)\left(\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right)---\left(31\right)$

$=\left(\begin{array}{ccc}{C}_{11}+{\mathrm{\φ}}_U{C}_{12}-{\mathrm{\φ}}_N{C}_{13}& {-\mathrm{\φ}}_U{C}_{11}+C_{12}+{\mathrm{\φ}}_E{C}_{13}& {\mathrm{\φ}}_N{C}_{11}-{\mathrm{\φ}}_E{C}_{12}+C_{13}\\ C_{21}+{\mathrm{\φ}}_U{C}_{22}-{\mathrm{\φ}}_N{C}_{23}& {-\mathrm{\φ}}_U{C}_{21}+C_{22}+{\mathrm{\φ}}_E{C}_{23}& {\mathrm{\φ}}_N{C}_{21}-{\mathrm{\φ}}_E{C}_{22}+C_{23}\\ C_{31}+{\mathrm{\φ}}_U{C}_{32}-{\mathrm{\φ}}_N{C}_{33}& {-\mathrm{\φ}}_U{C}_{31}+C_{32}+{\mathrm{\φ}}_E{C}_{33}& {\mathrm{\φ}}_N{C}_{31}+{\mathrm{\φ}}_E{C}_{32}+C_{33}\end{array}\right)$

（30）减去式（28），并略去高阶小项，即得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (32)

H_fx是一个1×12的行阵，它的各个子项都很复杂，这里只给出相对简单的第一项，其它的不再一一列出，

$H_{\mathrm{fx}}\left(\mathrm{1,1}\right)=\frac{1}{{(h+C_{23}{r}_y)}^2}\left(\begin{array}{c}(C_{23}{T}_{23}-C_{33}{T}_{22}{)\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_y+(C_{33}{T}_{22}{C}_{11}-C_{23}{T}_{23}{C}_{11})V_E+\\ (C_{33}{T}_{22}{C}_{12}-C_{23}{T}_{23}{C}_{12})V_N\end{array}\right)$

同理得到

δf_y=H_fy[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (33)

合并可得光流误差方程为：

δ_f=H_f[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (34)

2.安装在横轴纵面内

(r_bf)_b=(r_x,0,0)^T，η=0，跟前文的推导类似，光流公式简化为：

$f_x=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{13}{r}_x}\×\left(\begin{array}{c}(C_{11}{T}_{11}+C_{31}{T}_{13})V_E+(C_{12}{T}_{11}+C_{32}{T}_{13})V_N\\ {-T}_{13}{\mathrm{\ω}}_{\mathrm{ib},y}^b{r}_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b---\left(35\right)$

$f_y=\frac{(C_{13}{T}_{13}-C_{33}{T}_{11})}{h+C_{23}{r}_x}\×\left(\begin{array}{c}{C}_{21}{V}_E+C_{22}{V}_N\\ +{\mathrm{\ω}}_{\mathrm{ib},z}^b{r}_x\end{array}\right)-T_{11}{\mathrm{\ω}}_{\mathrm{ib},x}^b-T_{13}{\mathrm{\ω}}_{\mathrm{ib},z}^b---\left(36\right)$

上述方程未考虑任何误差，而实际系统中总存在各种误差，所以实际的光流应由下述方程确定，以式（35）表示的x向光流为例：

$f_x+{\mathrm{\δf}}_x=\frac{({\hat{C}}_{13}{T}_{13}-{\hat{C}}_{33}{T}_{11})}{h+\mathrm{\δh}+{\hat{C}}_{13}{r}_x}\×\left(\begin{array}{c}({\hat{C}}_{11}{T}_{11}+{\hat{C}}_{31}{T}_{13})(V_E+{\mathrm{\δV}}_E)+({\hat{C}}_{12}{T}_{11}+{\hat{C}}_{32}{T}_{13})\\ \×(V_N+{\mathrm{\δV}}_N)-T_{13}({\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b)r_x\end{array}\right)+{\mathrm{\ω}}_{\mathrm{ib},y}^b+{\mathrm{\δ\ω}}_{\mathrm{ib},y}^b---\left(37\right)$

式（37）减去（35），并略去高阶小项，即得到光流的误差方程：

δf_x=H_fx[δh δV_E δV_N φ_E φ_N φ_U ε_cx ε_cy ε_cz ε_rx ε_ry ε_rz]^T    (38)

这与式（32）的形式是完全相同的；

最终，安装在飞行器纵轴纵面内的光流传感器和安装在横轴纵面内的光流传感器组合到一起，成为组合导航系统的量测方程：

Z=HX+v(t)

这里，Z=δf，H由H_f扩展得到，v(t)表示光流传感器的量测噪声；

步骤四：用扩展卡尔曼滤波器估计惯导误差，并使用此误差对惯导数据进行修正，得到更为精确的导航数据；

3个光流传感器在MAV上的安装位置和方向，以(x_b y_b z_b μη)的形式给出，形成矩阵M_3×5，设：

$M_{3\×5}=\left(\begin{array}{ccccc}0& 2& 0& \mathrm{\π}& -\frac{\mathrm{\π}}{6}\\ 2& 0& 0& \frac{5\mathrm{\π}}{6}& 0\\ -2& 0& 0& \frac{7\mathrm{\π}}{6}& 0\end{array}\right)---\left(39\right)$

惯导误差初值X₀=0，按组合导航原理框图，经数值仿真，得到的量测数据滤波效果是组合导航的纬度误差比纯惯导小一个数量级，纬度误差减小至1/3，高度误差几近于0，综合纬度误差和经度误差得到，组合导航的位置误差是纯惯导的1/6；组合导航有效地抑制了纯惯导的速度发散，提高了导航精度，这其实也是减小位置误差的主要原因；组合导航减小东向和北向的平台误差角，但对天向平台误差角没有发挥抑制作用。

Images