CN104880187A - 一种基于双摄像机的飞行器光流检测装置的运动估计方法 - Google Patents

Abstract

本发明涉及一种基于双摄像机的飞行器光流检测装置的运动估计方法，首先，前视与下视摄像机组成飞行器光流检测装置，实时采集前视和下视图像，分别计算前视和下视图像当前帧和前一帧之间对应多个特征点的光流值和其次，利用前视图像平移光流的分布特点及光流的解耦思想，计算飞行器的角速度，更新姿态角。再次，利用获得的角速度与获得下视图像平移光流。利用获得的姿态角，将各个特征点深度表征为飞行器离地高度的函数，建立新型平移运动估计方程，用UKF求解飞行器速度v。最终得到飞行器的六自由度运动估计。本发明适用于室内、低空或深空如火星探测飞行器的导航；涉及到的前视摄像机还可完成攻角、侧滑角的计算，障碍物规避等其他导航任务。

Description

一种基于双摄像机的飞行器光流检测装置的运动估计方法

技术领域

本发明涉及一种基于双摄像机的飞行器光流检测装置的运动估计方法，仅依靠视觉传感器，实现了飞行器的六自由度实时运动估计，适用于室内、低空或深空如火星探测飞行器的导航。

背景技术

由于对传感器的成本、体积、重量的限制，视觉导航成为无人机导航的重要发展方向。“光流”指的是由于观察者与外界环境的相对运动，在人眼或昆虫复眼的视网膜上形成的一系列流动着的图像光强信息。运动场反映真实场景中的三维运动，光流场是运动场在二维图像上的投影，光流蕴含着丰富的导航信息，可以由飞行过程中实时获取的图像序列计算出光流值，从而分析与估计相机和环境的相对运动。

光流导航经过多年的发展，已经成功应用于固定翼或旋翼无人机，成为重要的辅助导航或独立导航手段，可以实现无人机的自主避障、高度保持、自动着陆、目标检测和空中悬停等导航任务。利用前视摄像机的光流避障成为常用的低空无人机避障手段，而下视摄像机常作为运动估计或导航定位的手段。某些光流运动估计方法仅采用下视摄像机，再利用惯性器件得到的角速度计算下视摄像机的平移光流，进行飞行器速度估计，但是陀螺仪有漂移特性，导航时间长时精度会下降，并且没能充分利用周围环境的视觉信息。某些光流运动估计方法仅利用下视摄像机完成六自由度运动参数的估计，准确度较低，特别是速度估计易受到旋转运动的影响，当飞行器具有大幅度三维运动时，速度估计误差较大。还有些基于光流的速度估计与导航定位方法，在二维情形下进行速度估计与定位，局限性较大，并不能很好地应用于实际导航。前视摄像机比下视摄像机含有更丰富的信息，比下视摄像机能更好地估计旋转运动，不仅可以完成避障的任务，更可以和下视摄像机结合完成飞行器的姿态估计，或导航定位任务。

此外，现有的运动估计方程将每个特征点的深度作为状态量，每增加一个特征点的光流值作为量测量，系统就会增加一维，计算量大。总之，现有的基于单独下视摄像机的运动估计方法精确度较低，基于平飞假设的运动估计方法局限性较大，运动估计方程计算量有待降低。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于双摄像机的飞行器光流检测装置的运动估计方法，利用基于前视和下视摄像机的光流检测装置，可以实现飞行器的六自由度实时运动估计，能够最大限度的避免飞行器的三维旋转对速度估计的影响，并利用一种维数较低的运动估计方程作为量测方程求取三维速度，减少计算量。

本发明的技术解决方案为：一种基于双摄像机的飞行器光流检测装置的运动估计方法，通过下列步骤实现：

(1)采用前视与下视摄像机组成飞行器光流检测装置，飞行过程中实时采集前视和下视图像，分别计算前视和下视图像当前帧和前一帧之间对应多个特征点的光流值和

(2)利用前视图像平移光流分布特点和光流的解耦思想，建立非线性方程组求解飞行器的角速度，进而更新姿态角；

(3)利用获得的角速度计算下视图像旋转光流，再由计算下视图像平移光流。利用获得的姿态角，将下视图像的各个特征点的深度表征为飞行器离地高度的函数，建立新型平移运动估计方程，用UKF求解飞行器速度v；

(4)在飞行器飞行过程中，不断重复(1)到(3)，直至导航结束。

本发明的原理是：光流场是运动场在二维图像上的投影，光流蕴含着丰富的导航信息，可以由飞行过程中实时获取的图像序列计算出光流值，从而分析与估计相机和环境的相对运动。前视图像的平移光流交于一点，利用光流的解耦思想可以用旋转光流代替平移光流，进而求得飞行器的角速度和姿态角。将角速度传入下视摄像机的运动估计，得到下视摄像机图像的平移光流作为量测量；将姿态角传入下视摄像机的运动估计，写出一种维数较低的运动估计方程作为量测方程求取飞行器的三维速度。

本发明与现有技术相比的优点在于：

(1)本发明通过前视与下视摄像机的结合，可以完成飞行器六自由度运动估计。将基于前视光流信息的运动估计结果传入下视运动估计，避免了三维旋转对速度估计的影响，提高了运动估计的精度，并且估计结果无漂移。将三维姿态用于深度的计算，适用于飞行器的三维导航，具有很强的实用性。此外，本发明用到的前视摄像机可以同时进行避障和运动估计，不需增加额外的设备，减少无人机的载荷。

(2)本发明涉及到的新型运动估计方程不以特征点的深度作为状态量，降低了系统维数。系统最少只有四个状态量，增加特征点只会增加量测信息，并不会增加系统维数，降低了计算量，提高了运算速率。

附图说明

图1为本发明的设计流程图；

图2为本发明涉及的摄像机安装方向及摄像机坐标系示意图；

图3为本发明涉及的摄像机成像及图像坐标系示意图；

图4为本发明涉及的前视图像平移光流交点示意图。

具体实施方式

如图1所示，本发明的具体实现步骤如下：

(1)飞行器机体安装了基于前视与下视摄像机的光流检测装置。前视摄像机的光轴与飞行器纵轴平行，并使图像横坐标与飞行器横轴平行，采集正前方图像。下视摄像机的光轴与前视摄像机光轴垂直，并使图像横坐标与飞行器横轴平行，采集正下方图像。摄像机安装方向如图2所示。

在飞行器飞行过程中实时采集前视和下视图像，用基于特征点的金字塔LK算法计算光流值。分别对前视和下视摄像机采集到的前一帧图像进行Harris角点检测，并计算亚像素级角点位置。在特征点处对图像分块处理，并用金字塔LK算法计算前一帧图像和当前帧之间特征点处的光流值和

(2)惯性坐标系F_e定义：惯性坐标系是与地球表面固连的坐标系，忽略地球的曲率和转动，将当地东北天坐标系认为是惯性坐标系，且惯性坐标系是固定坐标系。

机体坐标系F_b定义：原点取在飞行器的质心，OX_b轴与飞行器横轴重合，指向右侧为正；OY_b轴与飞行器纵轴重合，指向头部为正；OZ_b轴垂直于OX_bY_b平面，方向按右手坐标系确定。

前视摄像机坐标系F_c1定义：摄像机与飞行器固连，其坐标系的原点O与飞行器质心重合，OX_c1轴与OX_b轴重合，指向右侧为正；OZ_c1轴与摄像机的光轴重合指向飞行器头部为正；OY_c1轴垂直于OX_c1Z_c1平面，方向按右手直角坐标系确定。前视摄像机坐标系如图2所示。

图像坐标系及摄像机成像方式如图3所示。对应以上坐标系定义可以推导出对应前视摄像机的光流运动估计方程。

${\stackrel{\→}{V}}_{flow,F}=\left[\begin{array}{c}{\stackrel{\·}{u}}_i\\ {\stackrel{\·}{v}}_i\end{array}\right]=\left[\begin{array}{c}\left({\stackrel{\·}{u}}_{i,T}\right)+\left({\stackrel{\·}{u}}_{i,R}\right)\\ \left({\stackrel{\·}{v}}_{i,T}\right)+\left({\stackrel{\·}{v}}_{i,R}\right)\end{array}\right]=\left[\begin{array}{c}(-\frac{f}{Z_i^c}{v}_x+\frac{u_i}{Z_i^c}{v}_y)+(\frac{u_i{v}_i}{f}{\mathrm{\ω}}_x+v_i{\mathrm{\ω}}_y+(f+\frac{u_i^2}{f}\left){\mathrm{\ω}}_z\right)\\ (\frac{v_i}{Z_i^c}{v}_y+\frac{f}{Z_i^c}{v}_z)+\left(\right(f+\frac{v_i^2}{f}){\mathrm{\ω}}_x-u_i{\mathrm{\ω}}_y+\frac{u_i{v}_i}{f}{\mathrm{\ω}}_z)\end{array}\right]---\left(1\right)$

式中f为焦距，为特征点i对应的光流，为特征点i的平移光流，为特征点i的旋转光流，(u_i,v_i)为特征点i的像素坐标。(v_x,v_y,v_z)和(ω_x,ω_y,ω_z)为飞行器的速度与角速度在机体系下的表示，为特征点i对应三维点在前视相机系下的Z轴坐标，即深度。

由式(1)可知，图像特征点i对应的光流分为平移光流、旋转光流两部分，分别与速度、角速度有关，称为光流的解耦。其中，平移光流与特征点i对应的三维点的深度有关；旋转光流与特征点i的深度无关。

前视摄像机各个特征点的平移光流交于一点，满足式(2)。式中(u_i,v_i)为特征点i对应像素坐标，(p_x,p_y)为平移光流交点坐标。

$\frac{(u_i-p_x)}{(v_i-p_y)}=\frac{{\stackrel{\·}{u}}_{i,T}}{{\stackrel{\·}{v}}_{i,T}}---\left(2\right)$

由于光流可以分解为平移光流和旋转光流两部分，如式(1)。并且旋转光流与特征点深度无关，所以将式(3)代入式(2)得到旋转光流、(u_i,v_i)和(p_x,p_y)的关系式(4)。

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{i,T}={\stackrel{\·}{u}}_i-{\stackrel{\·}{u}}_{i,R}\\ {\stackrel{\·}{v}}_{i,T}={\stackrel{\·}{v}}_i-{\stackrel{\·}{v}}_{i,R}\end{array}\right.---\left(3\right)$

$({\stackrel{\·}{u}}_i-{\stackrel{\·}{u}}_{i,R})(v_i-p_y)-({\stackrel{\·}{v}}_i-{\stackrel{\·}{v}}_{i,R})(u_i-p_x)=0---\left(4\right)$

将旋转光流运动估计式代入式(4)，可得到ω、(u_i,v_i)和(p_x,p_y)的关系式。采集M个特征点联立非线性方程组，用广义逆法求解方程组的最小二乘解，得到角速度ω。对三轴角速度分别积分得到俯仰角、偏航角和滚转角。

(3)下视摄像机坐标系F_c2定义：摄像机与飞行器固连，其坐标系的原点O与飞行器质心重合，OX_c2轴与OX_b轴重合，指向右侧为正；OZ_c2轴与摄像机的光轴重合指向飞行器下方为正；OY_c2轴垂直于OX_c2Z_c2平面，方向按右手直角坐标系确定。下视摄像机坐标系如图2所示。

图像坐标系及摄像机成像方式如图3所示。对应以上坐标系定义推导出下视摄像机的运动估计方程。

${\stackrel{\→}{V}}_{flow,D}=\left[\begin{array}{c}{\stackrel{\·}{u}}_j\\ {\stackrel{\·}{v}}_j\end{array}\right]=\left[\begin{array}{c}\left({\stackrel{\·}{u}}_{j,T}\right)+\left({\stackrel{\·}{u}}_{j,R}\right)\\ \left({\stackrel{\·}{v}}_{j,T}\right)+\left({\stackrel{\·}{v}}_{j,R}\right)\end{array}\right]=\left[\begin{array}{c}(-\frac{f}{Z_j^c}{v}_x-\frac{u_j}{Z_j^c}{v}_z)+(\frac{u_j{v}_j}{f}{\mathrm{\ω}}_x+(\frac{u_j^2}{f}+f){\mathrm{\ω}}_y-v_j{\mathrm{\ω}}_z)\\ (\frac{f}{Z_j^c}{v}_y-\frac{v_j}{Z_j^c}{v}_z)+\left(\right(f+\frac{v_j^2}{f}){\mathrm{\ω}}_x+\frac{u_j{v}_j}{f}{\mathrm{\ω}}_y+u_j{\mathrm{\ω}}_z)\end{array}\right]$

式中f为焦距，为特征点j对应的光流，为特征点j的平移光流，为特征点j的旋转光流，(u_j,v_j)为特征点j的像素坐标。(v_x,v_y,v_z)和(ω_x,ω_y,ω_z)为飞行器的速度与角速度在机体系下的表示，为特征点j对应三维点在前视相机系下的Z轴坐标，即深度。

利用前视摄像机得到的(ω_x,ω_y,ω_z)计算下视摄像机图像特征点j的旋转光流利用和计算下视摄像机图像对应特征点的平移光流如下式所示：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{j,T}={\stackrel{\·}{u}}_j-{\stackrel{\·}{u}}_{j,R}\\ {\stackrel{\·}{v}}_{j,T}={\stackrel{\·}{v}}_j-{\stackrel{\·}{v}}_{j,R}\end{array}\right.$

平移光流与特征点的深度有关，如下式所示。三维运动下各点无法视为相同，需要利用飞行器的姿态角、距地面垂直高度和像素坐标分别计算各特征点的深度。

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{j,T}=-\frac{f}{Z_j^c}{v}_x-\frac{u_j}{Z_j^c}{v}_z\\ {\stackrel{\·}{v}}_{j,T}=\frac{f}{Z_j^c}{\mathrm{\ν}}_x-\frac{v_j}{Z_j^c}{v}_y\end{array}\right.---\left(5\right)$

相机的投影中心与特征点j形成一条直线l_j，根据成像模型得到相应的直线方程。

$\left[\begin{array}{c}{u}_j\\ v_j\end{array}\right]=\frac{f}{Z_j^c}\left[\begin{array}{c}{X}_j^c\\ Y_j^c\end{array}\right]\→l_j:\left\{\begin{array}{c}\frac{X}{Z}=\frac{u_j}{f}\\ \frac{Y}{Z}=\frac{v_j}{f}\end{array}\right.$

假设飞行器下方的地面为水平，地面在惯性系下的坐标为当前飞行器质心在惯性系下的位置为则飞行器距地面垂直距离为惯性坐标系与下视摄像机坐标系的平移矩阵可以表示为：

$T_e^b=\left[\begin{array}{cccc}1& 0& 0& -X_b^e\\ 0& 1& 0& -Y_b^e\\ 0& 0& 1& -Z_b^e\\ 0& 0& 0& 1\end{array}\right]$

惯性坐标系与下视摄像机坐标系的旋转矩阵，可由前视摄像机得到的姿态角代入下式得到。式中的θ、ψ、γ分别为飞行器的俯仰角、偏航角和滚转角。

$\begin{array}{c}{R}_e^{c2}=C_b^{cb}*C_e^b\\ =C_b^{c2}*\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}& -\cos \mathrm{\θ}\sin \mathrm{\ψ}\\ -\sin \mathrm{\θ}\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\sin \mathrm{\ψ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}\cos \mathrm{\ψ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}& -\sin \mathrm{\θ}\sin \mathrm{\ψ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}\end{array}\right]\end{array}$

坐标表示采用齐次坐标形式，所以旋转矩阵应增加一维，写成下式的形式。

$R_e^{c2}=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& 0\\ R_{21}& R_{22}& R_{23}& 0\\ R_{31}& R_{32}& R_{33}& 0\\ 0& 0& 0& 1\end{array}\right]$

取地面上一点，在惯性坐标系下用齐次坐标的形式表示为该点在下视摄像机系下的坐标可用该坐标先平移后旋转得到，如下式所示：

$\left[\begin{array}{c}{X}_d^{c2}\\ Y_d^{c2}\\ Z_d^{c2}\\ 1\end{array}\right]=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& 0\\ R_{21}& R_{22}& R_{23}& 0\\ R_{31}& R_{32}& R_{33}& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{cccc}1& 0& 0& -X_b^e\\ 0& 1& 0& -Y_b^e\\ 0& 0& 1& -Z_b^e\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}{X}_d^e\\ Y_d^e\\ Z_d^e\\ 1\end{array}\right]$

取摄像机质心在地面的投影点求得该点在下视摄像机系下的坐标为(-R₁₃h,-R₂₃h,-R₃₃h,1)^T。同样的，地面法向量转换成下视摄像机坐标系下表示为将齐次坐标改写为普通坐标，写出点法式地面方程R₁₃(X+R₁₃h)+R₂₃(Y+R₂₃h)+R₃₃(Z+R₃₃h)＝0，即与直线方程联立，得到特征点j与摄像机的深度值：

$Z_j^c=-\frac{hf}{R_{13}{u}_j+R_{23}{v}_j+R_{33}f}---\left(6\right)$

由式(6)代入式(5)得到三维运动下的新的平移光流运动估计式。

$\{\begin{array}{c}{\stackrel{\·}{u}}_{j,T}=-\frac{f}{Z_j^c}{v}_x-\frac{u_j}{Z_j^c}{v}_z=\frac{(R_{13}{u}_j+R_{23}{v}_j+R_{33}f)({\mathrm{fv}}_x+u_j{v}_z)}{hf}\\ {\stackrel{\·}{v}}_{j,T}=-\frac{f}{Z_j^c}{v}_y-\frac{u_j}{Z_j^c}{v}_z=\frac{(R_{13}{u}_j+R_{23}{v}_j+R_{33}f)(-{\mathrm{fv}}_x+v_j{v}_z)}{hf}\end{array}---\left(7\right)$

运用UKF滤波作为光流信息处理方法，实现飞行器速度的估计。滤波器设计过程如下：

状态方程：X_k+1＝AX_k+Bu_k+ω_k

观测方程：Y_k＝g(X_k)+γ_k

式中X_k＝(v_x,k,v_y,k,v_z,k,h_k)^T为视觉导航系统在k时刻的状态变量，其中v_k＝(v_x,k,v_y,k,v_z,k)^T为飞行器相对于机体轴的速度，h_k为飞行器相对地面的垂直高度；同样的，X_k+1＝(v_x,k+1,v_y,k+1,v_z,k+1,h_k+1)^T为视觉导航系统在k+1时刻的状态变量；ω_k为过程噪声，ω_k～N(0,Q)。为选取N个特征点计算出的平移光流，作为系统的观测向量；式(7)作为系统的量测方程中的g(X_k)；ν_k为量测噪声，ν_k～N(0,R)。状态方程如下式。

$\left\{\begin{array}{c}{v}_{x,k+1}=v_{x,k}+{\mathrm{\ω}}_{v_{x}k}\\ v_{y,k+1}=v_{y,k}+{\mathrm{\ω}}_{v_{y}k}\\ v_{z,k+1}=v_{z,k}+{\mathrm{\ω}}_{v_{z}k}\\ h_{k+1}=h_k+\mathrm{\Δ}t*{(C_b^e*v_k)}_z+{\mathrm{\ω}}_{hk}\end{array}\right.$

由给出的状态方程与观测方程，以迭代形式写出UKF滤波算法，进而得到飞行器速度。

(4)在飞行器飞行过程中，不断重复(1)到(3)，直至导航结束。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (6)

1.一种基于双摄像机的飞行器光流检测装置的运动估计方法，其特征在于包括以下步骤：

(1)采用前视与下视摄像机组成的飞行器光流检测装置，在飞行过程中飞行器光流检测装置实时采集前视和下视图像，并分别计算前视和下视图像当前帧和前一帧之间对应多个特征点的光流值和

(2)利用前视图像平移光流分布特点和光流的解耦思想，建立非线性方程组求解飞行器的角速度，进而更新姿态角；

(3)利用获得的角速度计算下视图像旋转光流，再由计算下视图像平移光流，利用获得的姿态角，将下视图像的各个特征点的深度表征为飞行器离地高度的函数，建立新型平移运动估计方程，用UKF求解飞行器速度v；

(4)在飞行器飞行过程中，不断重复(1)到(3)，直至导航结束。

2.根据权利要求1所述的一种基于双摄像机的飞行器光流检测装置的运动估计方法，其特征在于：所述步骤(1)光流检测装置的组成及安装结构为：光流检测装置由一个前视摄像机与一个下视摄像机组成，前视摄像机的光轴与飞行器纵轴平行，并使图像横轴与飞行器横轴平行，采集前方图像；下视摄像机的光轴与前视摄像机光轴垂直，并使图像横轴与飞行器横轴平行，采集飞行器下方的地面图像。

3.根据权利要求1所述的一种基于双摄像机的飞行器光流检测装置的运动估计方法，其特征在于：所述步骤(1)中涉及到的光流值计算方法为：采用基于特征点的金字塔LK光流算法，首先分别对前视和下视摄像机采集到的前一帧图像进行Harris角点检测，并计算亚像素级角点位置；然后在特征点处对图像分块处理，并用金字塔LK算法计算前一帧图像和当前帧之间特征点处的光流值和

4.根据权利要求1所述的一种基于双摄像机的飞行器光流检测装置及运动估计方法，其特征在于：所述步骤(2)中利用前视图像平移光流分布特点和光流的解耦思想，建立非线性方程组求解飞行器的角速度，进而更新姿态角的具体实现方法如下：

图像特征点i对应的光流分为平移光流、旋转光流两部分，分别与速度、角速度有关，称为光流的解耦，其中，平移光流与特征点i对应的三维点的深度有关；旋转光流与特征点i的深度无关；

前视摄像机各个特征点的平移光流交于一点，满足式(1)，式中(u_i,v_i)为特征点i对应像素坐标，(p_x,p_y)为平移光流交点坐标，为特征点i对应平移光流；

$\frac{(u_i-p_x)}{(v_i-p_y)}=\frac{{\stackrel{\·}{u}}_{i,T}}{{\stackrel{\·}{v}}_{i,T}}$

光流分解为平移光流和旋转光流两部分，如下式所示，式中为特征点i对应的总光流：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{i,T}={\stackrel{\·}{u}}_i-{\stackrel{\·}{u}}_{i,R}\\ {\stackrel{\·}{v}}_{i,T}={\stackrel{\·}{v}}_i-{\stackrel{\·}{v}}_{i,R}\end{array}\right.$

旋转光流与特征点深度无关，由以上两式得到旋转光流像素坐标(u_i,v_i)和交点坐标(p_x,p_y)的关系式：

$({\stackrel{\·}{u}}_i-{\stackrel{\·}{u}}_{i,R})(v_i-p_y)-({\stackrel{\·}{v}}_i-{\stackrel{\·}{v}}_{i,R})(u_i-p_x)=0$

将旋转光流运动估计式代入上式，得到飞行器角速度ω、(u_i,v_i)和(p_x,p_y)的关系式；采集M个特征点联立非线性方程组，用基于奇异值分解的广义逆法求解方程组的最小二乘解，得到角速度ω；对三轴角速度分别积分得到俯仰角、偏航角和滚转角。

5.根据权利要求1所述的一种基于双摄像机的飞行器光流检测装置及运动估计方法，其特征在于：所述步骤(3)中利用获得的角速度和姿态角计算速度的具体实现方法如下：

利用前视摄像机得到的(ω_x,ω_y,ω_z)计算下视摄像机图像特征点j的旋转光流利用和相减得到下视摄像机图像对应特征点的平移光流平移光流与特征点的深度有关，令f为摄像机的焦距，平移光流的运动估计如下式所示：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{j,T}=-\frac{f}{Z_j^c}{v}_x-\frac{u_j}{Z_j^c}{v}_z\\ {\stackrel{\·}{v}}_{j,T}=\frac{f}{Z_j^c}{v}_x-\frac{v_j}{Z_j^c}{v}_y\end{array}\right.$

三维运动下各点无法视为相同，需要利用飞行器的姿态角、距地面垂直高度和像素坐标分别计算各特征点的深度；相机的投影中心与特征点j形成一条直线l_j，根据成像模型得到相应的直线方程：

$\left[\begin{array}{c}{u}_j\\ v_j\end{array}\right]=\frac{f}{Z_j^c}\left[\begin{array}{c}{X}_j^c\\ Y_j^c\end{array}\right]\→l_j:\left\{\begin{array}{c}\frac{X}{Z}=\frac{u_j}{f}\\ \frac{Y}{Z}=\frac{v_j}{f}\end{array}\right.$

假设飞行器下方的地面为水平，地面在惯性系下的坐标为当前飞行器质心在惯性系下的位置为则飞行器距地面垂直距离为惯性坐标系与下视摄像机坐标系的平移矩阵可以表示为：

$T_e^b=\left[\begin{array}{cccc}1& 0& 0& -X_b^e\\ 0& 1& 0& -Y_b^e\\ 0& 0& 1& -Z_b^e\\ 0& 0& 0& 1\end{array}\right]$

惯性坐标系与下视摄像机坐标系的旋转矩阵，写成下式的形式：

$R_e^{c2}=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& 0\\ R_{21}& R_{22}& R_{23}& 0\\ R_{31}& R_{32}& R_{33}& 0\\ 0& 0& 0& 1\end{array}\right]$

取地面上一点，在惯性坐标系下用齐次坐标的形式表示为该点在下视摄像机系下的坐标可用该坐标先平移后旋转得到，如下式所示：

$\left[\begin{array}{c}{X}_d^{c2}\\ Y_d^{c2}\\ Z_d^{c2}\\ 1\end{array}\right]=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& 0\\ R_{21}& R_{22}& R_{23}& 0\\ R_{31}& R_{32}& R_{33}& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{cccc}1& 0& 0& -X_b^e\\ 0& 1& 0& -Y_b^e\\ 0& 0& 1& -Z_b^e\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}{X}_d^e\\ Y_d^e\\ Z_d^e\\ 1\end{array}\right]$

取摄像机质心在地面的投影点求得该点在下视摄像机系下的坐标为(-R₁₃h,-R₂₃h,-R₃₃h,1)^T；同样的，地面法向量转换成下视摄像机坐标系下表示为将齐次坐标改写为普通坐标，写出点法式地面方程R₁₃(X+R₁₃h)+R₂₃(Y+R₂₃h)+R₃₃(Z+R₃₃h)＝0，即与直线方程联立，得到特征点j与摄像机的深度值：

$Z_j^c=-\frac{\mathrm{hf}}{R_{13}{u}_j+R_{23}{v}_j+R_{33}f}$

将上式代入平移光流的运动估计式得到三维运动下的新的平移光流运动估计式：

$\left\{\begin{array}{c}{\stackrel{\·}{u}}_{j,T}=-\frac{f}{Z_j^c}{v}_x-\frac{u}{Z_j^c}{v}_z=\frac{(R_{13}{u}_j+R_{23}{v}_j+R_{33}f)(f{v}_x+u_j{v}_z)}{\mathrm{hf}}\\ {\stackrel{\·}{v}}_{j,T}=\frac{f}{Z_j^c}{v}_y-\frac{v}{Z_j^c}{v}_z=\frac{(R_{13}{u}_j+R_{23}{v}_j+R_{33}f)(-f{v}_y+v_j{v}_z)}{\mathrm{hf}}\end{array}\right.$

运用UKF滤波作为光流信息处理方法，实现飞行器速度的估计，滤波器设计过程如下：

状态方程：X_k+1＝AX_k+Bu_k+ω_k

观测方程：Y_k＝g(X_k)+γ_k

式中X_k＝(v_x,k,v_y,k,v_z,k,h_k)^T为视觉导航系统在k时刻的状态变量，其中v_k＝(v_x,k,v_y,k,v_z,k)^T为飞行器相对于机体轴的速度，h_k为飞行器相对地面的垂直高度；同样的，X_k+1＝(v_x,k+1,v_y,k+1,v_z,k+1,h_k+1)^T为视觉导航系统在k+1时刻的状态变量；ω_k为过程噪声，ω_k～N(0,Q)；为选取N个特征点计算出的平移光流，作为系统的观测向量；三维运动下的新的平移光流运动估计式作为系统的量测方程中的g(X_k)；ν_k为量测噪声，ν_k～N(0,R)；状态方程如下式：

$\left\{\begin{array}{c}{v}_{x,k+1}=v_{x,k}+{\mathrm{\ω}}_{v_{x}k}\\ v_{y,k+1}=v_{y,k}+{\mathrm{\ω}}_{v_{y}k}\\ v_{z,k+1}=v_{z,k}+{\mathrm{\ω}}_{v_{z}k}\\ h_{k+1}=h_k+\mathrm{\Δt}*{(C_b^e*v_k)}_z+{\mathrm{\ω}}_{\mathrm{hk}}\end{array}\right.$

由给出的状态方程与观测方程，以迭代形式写出UKF滤波算法，进而得到飞行器速度。

6.根据权利要求1所述的一种基于双摄像机的飞行器光流检测装置及运动估计方法，其特征在于：所述步骤(3)中涉及到的新型运动估计方程不以特征点的深度作为状态量，将各个特征点的深度写成飞行器距地面垂直高度的函数，由此写出平移运动估计方程，降低了系统维数；系统最少只有四个状态量，增加特征点只会增加量测信息，并不会增加系统维数，降低了计算量，提高了运算速率。