CN101598556A - 一种未知环境下无人机视觉/惯性组合导航方法 - Google Patents

Abstract

本发明一种未知环境下无人机视觉/惯性组合导航方法，该方法涉及到图像处理、惯性导航计算和滤波估计，因此，在进行主要导航方法之前首先需要进行坐标系定义、坐标系对准和滤波方程建立等。在本发明中所述的导航过程主要包括：图像获取并对图像进行预处理；通过SIFT方法进行特征点提取；特征点匹配；滤波方程更新；对更新后的滤波方程进行滤波估计五步骤来实现。本发明只要求当前的地面图像而不需要特定的外部信息配合，因此，理论上可以在任何环境(包括水下、遮挡、峡谷、地下等)下使用；具有隐蔽性好，精度高的优点。

Description

一种未知环境下无人机视觉/惯性组合导航方法

技术领域

本发明涉及导航技术领域，具体来说是一种未知环境下无人机视觉/惯性组合导航方法。

背景技术

无人机导航是指无人机依赖机载的导航设备和飞行控制系统来进行定位和控制并最终完成飞行任务的技术，它是无人机研制的关键技术之一。在无人机导航中常用的方法包括：遥控指挥、惯性导航系统(Inertial Navigation System，INS)、卫星导航、组合导航等。

目前也逐渐开展视觉导航系统的研究，但基本上都是以GPS卫星导航为主，其他几种导航方式为辅的方案。然而，由于无人机，特别是作战用小型无人机的应用电磁环境恶劣，遥控通讯链路难以维持，GPS信号也容易受到干扰而无法使用。为了提高无人机导航的适应性，经常采用的导航方法包括惯性导航、景象匹配导航等。

然而这些方法仍然存在一些问题。惯性导航虽然具有自主性好、抗干扰能力强等优点，然而，相对于无人机体积和载荷来讲，高精度的惯性导航系统体积庞大，重量和耗电量则远远超出了无人机的能力范围，同时，惯性导航系统还存在着误差累积等不足而难以单独应用，无法实现无人机的全自主导航。

景象匹配导航的原理是利用航天、航空等遥感手段获取飞行器规划路径下方的地物景象(基准图)并存储于机载计算机中；当携带相应传感器的飞行器飞过预定位置范围时，便即时测量出当地的地物景象(实时图)；将实时图和基准图在机载计算机中进行相关匹配即可以确定出当前飞行器的准确位置，然后把该位置参数送到惯导系统以修正惯导系统积累的误差，从而实现飞行器长时间的精确导航和制导。其特点是导航精度高，不存在INS中的误差累积问题，但是其缺点在于必须依赖于机载的匹配基准图才能对航空器进行导航定位，因此，其应用范围受到了较大的限制而不能称之为未知环境下的导航方法；同时，在景象匹配中如何对图像进行矫正和在巨大的图像数据库中进行图像匹配对无人机机载处理器也是一种考验。

本发明中提到的未知环境下的视觉导航技术是利用视觉传感器获得图像，并且通过图像处理方法得到无人机导航参数的一种方式，该方法所采用的传感器包括摄像头、惯性测量单元(IMU)等。视觉传感器具有轻便、低功耗、体积小等优点，十分适合在微小型无人机上使用，而本发明中所涉及到的IMU同样适用于无人机场合。此外，视觉导航作为自主导航的一种，工作在被动模式，不需要接收外部信号，因此具有隐蔽性好，抗干扰能力强等优点，将其与IMU相结合，可以同时克服单个摄像头无法得到景象深度信息、特征点匹配误差大，惯性导航系统存在误差累计问题等不足。目前基于视觉信息进行无人机导航是该领域研究的一个热点。

本发明与其他无人机应用中的视觉方法不同在于：在当前已公开发表的资料中，计算机视觉方法在无人机中的应用主要集中于自主着陆、目标识别以及作为载荷等。自主着陆中视觉方法仅仅应用于飞行器的着陆阶段，无法应用于任务执行阶段，而任务执行阶段是飞行器应用的主要阶段，也是体现无人机价值的阶段，并且无人机视觉自主着陆方法要求已知着陆场信息，仍然属于半自主的导航方式；目标识别则需要实现已知该目标信息，从而可以采用视觉方法得到目标相对于飞行器的位置信息，对于占视觉主要成分的自然环境却无能为力；将视觉作为载荷应用于无人机则完全没有考虑到视觉图像中包含的丰富的导航信息。因此，虽然有将视觉方法应用于导航方面的研究，但是这些方法无一例外均要求已知环境信息或者需要设置人工参考信息等，无法实现真正意义上的“未知环境”下的导航应用。

发明内容

本发明提出一种未知环境下无人机视觉/惯性组合导航方法，其目的是为了解决在未知环境下无人机的组合导航问题，它采用了计算机视觉方法，实现了在无人机执行任务过程中的抗干扰、全自主导航。该方法可在不依赖外部信息的情况下，只利用机载惯性导航系统、高度表、机载视觉系统等被动传感器和地面图像特征，为飞行器在任务执行过程中提供导航信息，该方法可以作为卫星导航等半自主导航方式的备份导航手段，并且还可以在干扰、遮挡情况下短时单独使用，从而提高飞行器的生存能力；也可以装载于有人飞机或者地面、水下机器人等载体上作为导航信息来源。

本发明一种未知环境下无人机视觉/惯性组合导航方法，该方法涉及到图像处理、惯性导航计算和滤波估计，因此，在进行主要导航方法之前首先需要进行坐标系定义、坐标系对准和滤波方程建立等。坐标系定义是进行此类导航方法的前提，在理论分析中的导航方法是建立在一定的坐标系关系基础上的，但是在实际中由于安装误差和安装条件的限制，难以做到与理论情况完全相符。因此，IMU测量到的载体加速度和角速度值与从视觉系统中提取到的此参数并非完全相同；同时，二者坐标原点也并非完全重合而是存在一定的偏差，这些因素在实际实现过程中都需要予以考虑。

在本发明中所述的导航过程主要通过5步来实现：

步骤一：图像获取并对图像进行预处理；

图像获取是由摄像头在机载计算机控制下完成的，由于载体环境和所用设备条件限制，所采集到的图像质量难以保证，如果对摄像头输出图像直接进行处理难免会影响该方法的效果甚至无法进行导航，因此，需要对图像进行预处理。

本发明中采用的图像为灰度图像，为了满足视觉导航系统对处理速度和信息量的要求采用了中值滤波方法对图像进行预处理。

步骤二：通过SIFT方法进行特征点提取；

视觉对惯性系统的约束主要体现在前后几帧图像中特征点坐标的变化上，因此，如何得到稳定的特征点成为该导航方法的首要问题，SIFT方法具有良好的稳定性、很高的匹配准确度，并且其特征点具有很好的分布特性，甚至在纹理不很明显的情况下也能得到充足的特征点。因此，SIFT方法在视觉导航中得到了广泛的应用。

SIFT方法进行特征点提取分为四步骤来实现：

1)检测尺度空间极值点；

2)精确确定极值点位置；将位于边缘的极值点删掉，余下的则为SIFT特征点；

3)为每个特征点指定方向参数

4)特征点描述向量的生成。

步骤三：特征点匹配；

为了得到特征点坐标变化关系，在进行特征点提取之后还需要对前后时刻的特征点对应关系进行匹配，这是决定该导航方法是否准确可靠最关键的一步。由于在第二步中已经得到了每个特征点对应的描述向量，因此，在此部分可以直接对前后两幅图像中的特征点进行匹配。

步骤四：滤波方程更新

根据步骤三中特征点匹配结果可以将特征点分为三类，这三类特征点可以代表实际过程中所有可能出现的情况，分别采用不同的策略对滤波方程进行更新。

步骤五：对更新后的滤波方程进行滤波估计；

经过第四步更新后的滤波方程已经包含了图像中特征点的所有运动信息，并且包含了惯性导航系统输出的惯性测量信息进行图像/惯性组合导航，即是对该方程进行滤波处理从而得到对无人机飞行状态的估计(位置、速度、姿态等)。然而，由以上方法建立的系统和观测方程为非线性方程，为了对其进行滤波处理必须采用非线性滤波方法，在本方案中采用了无迹卡尔曼滤波(UKF)方法。

本发明的优点在于：

1)适应性广，该方法只要求当前的地面图像而不需要特定的外部信息配合，因此，理论上可以在任何环境(包括水下、遮挡、峡谷、地下等)下使用；

2)隐蔽性好，该方法不需要外部信息，也不需要向外部发出信息，因此，可以具有很好的保密性；

3)精度高，当应用于侦察、通讯中继等小范围场合时由于机载摄像头可以得到活动范围的全景图像，因此导航精度可以达到很高的程度。

附图说明

图1为本发明准备工作中坐标系定义示意图；

图2为本发明准备工作中摄像机坐标系和IMU坐标系对准示意图；

图3为本发明一种未知环境下无人机视觉/惯性组合导航方法流程图。

具体实施方式

进行本方法前需要首先进行坐标系定义及对准工作：

1，坐标系的定义

如图1所示，坐标轴定义包括机体坐标系(o_bx_by_bz_b)、导航坐标系(o_nx_ny_nz_n)、摄像机坐标系(o_sx_sy_sz_s)和IMU坐标系(o_ix_iy_iz_i)的定义；

摄像机坐标系(o_sx_sy_sz_s)：在飞行器上捷联固定一下视觉传感器以获得一系列的地面图像，其坐标原点位于摄像机中心，o_sz_s沿摄像机光轴方向，o_sx_s和o_sy_s分别沿摄像机成像平面的长、宽方向，三者互相垂直成右手坐标系；

IMU坐标系(o_ix_iy_iz_i)：IMU出厂时定义的坐标系，三个坐标轴互相垂直，沿三个坐标轴分别各安装一个加速度计和一个陀螺仪敏感载体的加速度和角速度信息。

机体坐标系(o_bx_by_bz_b)：原点在飞行器重心，沿飞行器纵轴为x正方向，水平向右为y正方向，z方向与二者垂直并向下，同时，IMU的坐标系(o_ix_iy_iz_i)与该坐标系固连；

导航坐标系(o_nx_ny_nz_n)：为当地地理坐标系，原点位于导航起始位置，坐标轴分别沿北-东-地方向；

如图2所示，摄像机坐标系和IMU坐标系是机体坐标系经过简单的平移，而后依次水平转动ψ和俯仰转动θ得到的，ψ和θ分别是摄像机相对于机体坐标系的转动角。

2，坐标轴对准

坐标轴对准包括了IMU坐标系与机体坐标系、IMU坐标系与摄像机坐标系对准。IMU中加速度计和陀螺仪之间的安装误差可以忽略不计，在此情况下，可以采用加速度计输出对二者坐标系进行对准。为了得到IMU坐标系到摄像机坐标系的旋转关系，可以使所有传感器均敏感垂直方向，利用重力加速度信息对二者进行对准。

其对准方法如下：

首先，在摄像机视场中放置一垂直于水平面的摄像头标定靶；其次，采集摄像头中的标定靶图像，计算其中对应垂线的消失点得到摄像头测量到的垂直方向向量

并同时采集加速度计输出以得到当前加速度计相对于当地垂线方向向量

最后，将以上标定图像和加速度计输出数据代入标定方程中可以得到两个坐标系之间的对应关系。该方法不需要任何其他设备，而标定中所需要的标定靶是进行摄像头标定必须设备之一。因此，该标定方法具有使用方便的优点。其详细计算过程如下：

记q_i ^c为从惯性坐标系到摄像机坐标系的旋转四元数，于是，以上标定过程即选择q_i ^c使方程

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(q_i^c\stackrel{\→}{v}_i^I{q}_i^c\right)\stackrel{\→}{v}_i^C---\left(1\right)$

取得最大。在得到当前的旋转四元数q_i ^c之后可以根据

$C_i^c=\left[\begin{array}{ccc}{q}_1^2+q_2^2-q_3^2-q_4^2& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& q_1^2-q_2^2+q_3^2-q_4^2& 2(q_3{q}_4-q_1{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_3{q}_4+q_1{q}_2)& q_1^2-q_2^2-q_3^2+q_4^2\end{array}\right]---\left(2\right)$

得到惯性坐标系到摄像机坐标系的旋转矩阵，其中q₁ q₂ q₃ q₄为四元数q_i ^c的四个分量。从而，对于任意IMU坐标系下的坐标点Xⁱ而言都有其在摄像机坐标系下X^c为：

$X^c=C_i^c*X^i---\left(3\right)$

通过上式即可实现两坐标系下坐标的互相转换。

3，建立滤波方程

此处滤波方程包括了状态方程和观测方程，其建立方法分别如下：

(1)状态方程的建立

状态方程包括了飞行器和特征点的动态模型，它们可以写成如下的一阶差分模型：

$X_{a\left(k\right)}=f\left(X_{a(k-1)}\right)=\left[\begin{array}{c}{x}_{v\left(k\right)}\\ x_{m\left(k\right)}\end{array}\right]=\left[\begin{array}{c}{f}_v(x_{v(k-1)},u_{\left(k\right)},w_{v\left(k\right)})\\ f_m(x_{m(k-1)},w_{m\left(k\right)})\end{array}\right]---\left(4\right)$

此处X_a(k)为第k步状态向量，x_v(k)为飞行器运动状态，包括位置、速度和姿态，可以通过惯性导航方程解算得到；

$x_v\left(k\right)=\left[\begin{array}{c}{P}_k^n\\ V_k^n\\ {\mathrm{\ψ}}_k^n\end{array}\right]=\left[\begin{array}{c}{P}_k^n+V_{k-1}^{n}T\\ V_{k-1}^n+(C_b^n{f}^b+g^n)T\\ {\mathrm{\ψ}}_{k-1}^n+E_b^n{\mathrm{\ω}}^{b}T\end{array}\right]+w_v\left(k\right)---\left(5\right)$

C_b ⁿ为从载体坐标系到导航坐标系的转移矩阵，E_b ⁿ为载体角速率到欧拉角速率的转移矩阵，w_v(k)为系统过程噪声，T为滤波周期。

x_m(k)为视觉获取的特征点三维位置向量，对于第i个特征点而言，在假设场景不变前提下，其状态方程为

x_mi(k)＝x_mi(k-1)    (6)

(2)观测方程的建立

观测量由三部分组成：相对高度z_h、航向z_ψ和摄像机坐标系下的特征点像素坐标z_mi。其中高度和航向观测量直接从其对应的状态向量中输出，即：

$\left[\begin{array}{c}{z}_h\\ z_{\mathrm{\ψ}}\end{array}\right]\left[\begin{array}{ccccc}& 1& & 0& \\ 0_{2\×2}& & 0_{2\×5}& & ...\\ & 0& & 1& \end{array}\right]x_k+\left[\begin{array}{c}{v}_{h\left(k\right)}\\ v_{\mathrm{\ψ}\left(k\right)}\end{array}\right]---\left(7\right)$

特征点像素坐标部分观测方程构建如下：

设视觉传感器内参数为

$A=\left[\begin{array}{ccc}{f}_x& \mathrm{\γ}& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]---\left(8\right)$

其中，f_x，f_y分别为视觉传感器在x，y方向的焦距，u₀，v₀为光学中心，γ为不垂直因子。则对于第i个特征点有摄像机坐标系下系统观测值u，v为：

$z_{\mathrm{mi}}=\left[\begin{array}{c}u\\ v\end{array}\right]=\left[\begin{array}{c}\frac{x}{z}{f}_x+u_0\\ \frac{y}{z}{f}_y+v_0\end{array}\right]---\left(9\right)$

此处，x，y，z为特征点在摄像机坐标系中坐标P_ms(k) ^s的分量，它可以从导航坐标系下的飞行器和特征点坐标中得到：

$P_{\mathrm{ms}\left(k\right)}^s=C_b^s{C}_n^b[x_{\mathrm{mi}}^n-{P^n}_{\left(k\right)}-C_b^n{P}_{\mathrm{sb}}^b]---\left(10\right)$

C_b ^s(k)为从机体坐标系到摄像机坐标系的转移矩阵，x_mi ⁿ(k)为状态向量中当前特征点对应的分量，C_n ^b(k)为从导航坐标系到机体坐标系的转移矩阵，P_sb ^b为视觉传感器中心与飞行器重心的偏差量，Pⁿ(k)为当前的载体(无人机)位置坐标。

将以上高度z_h、航向z_ψ和特征点像素坐标z_mi观测量合成得到系统观测方程为：

$z_{\left(k\right)}=\left[\begin{array}{c}{z}_h\\ z_{\mathrm{\ψ}}\\ z_{\mathrm{mi}}\end{array}\right]=h(x_{\left(k\right)},v_{\left(k\right)})---\left(11\right)$

h(.，.)为当前时刻非线性观测模型，v(k)为观测噪声。

(3)无人机视觉/惯性组合导航具体实现方法

本发明的具体步骤如图3所示：

步骤一：图像获取并对图像进行预处理；

当前对灰度图像的处理较为成熟，同时运算速度较快，因此，本发明中采用了灰度图像进行处理，通过公式：

Y＝0.299R+0.596G+0.211B    (12)

将获取到的彩色图像转化为灰度图像，可得到图像的灰度值Y，其中R G B分别表示图像中红、绿、蓝三种颜色分量；

然后对图像信息进行滤波处理，一般情况下认为图像中包含的噪声近似为高斯白噪声，可以采用中值滤波对图像的噪声进行抑制，即通过邻域中亮度的中间值代替图像的当前点。中值滤波具有很好的低通特性，同时还可以保留图像的边缘细节信息，而邻域中亮度的中值不受个别噪声毛刺的影响，因此，中值滤波十分适合于此类图像处理；

步骤二：通过SIFT方法进行特征点提取；

视觉对惯性系统的约束主要体现在前后几帧图像中特征点坐标的变化上，因此，需要对预处理后的图像提取特征点；

SIFT方法进行特征点提取分为四步骤来实现：

1)检测尺度空间极值点；

首次，定义图像尺度空间函数为L(x，y，σ)，输入图像用I(x，y)表示，对输入图像进行卷积操作，则有 $L(x,y,\mathrm{\σ})=G(x,y,\mathrm{\σ})\⊗I(x,y).$ 其中 $G(x,y,\mathrm{\σ})=\frac{1}{2\mathrm{\π}{\mathrm{\σ}}^2}{e}^{-(x^2+y^2)}/{2\mathrm{\σ}}^2$ 是尺度可变高斯函数，(x，y)是空间坐标，σ是尺度坐标。采用不同的σ对图像进行高斯卷积，得到高斯图像金字塔。

其次，对高斯图像金字塔中相邻尺度的两个高斯图像相减得到高斯差分图像

$D(x,y,\mathrm{\σ})=(G(x,y,\mathrm{k\σ})-G(x,y,\mathrm{\σ}))\⊗I(x,y)=L(x,y,\mathrm{k\σ})-L(x,y,\mathrm{\σ})$

最后，将高斯差分图像中每一个像素都与其周围点相比较，得到极值点；

2)精确确定极值点位置；将位于边缘的极值点删掉，余下的则为SIFT特征点；

3)为每个特征点指定方向参数

在以特征点为中心的邻域窗口内采样，并用直方图统计邻域像素的梯度方向。直方图的峰值代表了该特征点处邻域梯度的主方向，即作为该特征点的方向。当存在另一个相当于主峰值80％能量的峰值时，则将这个方向认为是该特征点的辅方向。

4)特征点描述向量的生成；首先将坐标轴旋转为特征点的方向，计算每个特征点邻域中梯度方向直方图并根据位置排序，然后将特征向量长度归一化，该向量就是SIFT特征点描述向量。

步骤三：特征点匹配；

每一个SIFT特征点描述向量均代表了该特征点唯一的特征，对特征点进行匹配即对其特征点描述向量进行匹配。由于SIFT特征点提取方法提取出的特征点具有很高的鲁棒性，对图像的旋转、缩放、平移、光线以及遮挡等具有不变性。用128维的高维度来对特征点进行描述，使得特征点描述符之间具有很大的差异性，因此可以直接利用特征点之间的几何特性进行特征点的匹配；当前应用较多的为基于欧几里德距离的最近邻法作为衡量标准，相似性的进行图像匹配，该方法具体实现过程如下：

设第一幅图像中第i个特征点，第二幅图像中的第j个特征点a_i，b_j的特征描述符为：

a_i＝[α_i0，α_i1，……，α_i127]    (13)

b_j＝[β_j0，β_j1，……，β_j127]    (14)

则这两个特征点描述符的欧几里德距离为

$D_{\mathrm{ij}}=\sqrt{\underset{k=0}{\overset{127}{\mathrm{\Σ}}}{({\mathrm{\α}}_{\mathrm{ik}}-{\mathrm{\β}}_{\mathrm{ik}})}^2}---\left(15\right)$

将a_i与第二幅图像中所有的特征点计算距离，存入数组D_i·，并将这些距离从小到大排序，如果最小的距离D_i0与次最小距离d_i1的比值在某一个阈值内(τ)，即d_i0＜＝τ×D_i1，则认为匹配成功。把当前帧中所有的特征点都与前一帧进行匹配便得到特征点的匹配关系。

步骤四：滤波方程更新

根据步骤三中特征点匹配结果可以将特征点分为三类：

A、在前后两帧图像中都出现并且可以成功匹配的特征点认为是匹配成功的特征点，对其进行处理即对该特征点对应的观测量进行更新。首先在观测方程(11)中找到与该匹配特征点对应的滤波观测值分量z_i，而后用当前时刻该特征点在摄像机坐标系中的像素坐标代替该分量。

B、仅在第一帧图像中出现，而在当前帧图像中无法找到与之对应的匹配特征点认为是跟踪丢失的特征点；

从摄像机中采集到的视觉图像中提取的特征点数量十分庞大，随着导航时间的延长将会导致滤波方程中状态维数不断膨胀，这对机载处理器的处理能力提出了很高的要求。因此，需要对特征点进行管理，随着飞行器的运动将不再可见的特征点从滤波方程中删除。由于滤波方程中各特征点之间互相独立，因此，删除特征点只需要将其对应的状态向量及其在滤波方差阵中对应的行和列删除即可。即分别从状态方程(4)中将该丢失的特征点在系统状态向量X_a(k)和系统方程f(X_a(k-1))中所对应的状态分量删除，从观测方程(11)中将该丢失的特征点在观测向量z和观测方程h(χ_i，·)中所对应的分量删除即可。

C、在当前帧图像中新出现的特征点认为是新的特征点，对其进行初始化。

所述初始化包括状态向量初始化和观测向量的初始化，通过以下三步完成：

a、得到新特征点在导航坐标系下坐标

$x_{\mathrm{mi}}^n\left(k\right)=P^n\left(k\right)+C_b^n\left(k\right)P_{\mathrm{sb}}^b+C_b^n{C}_s^b{P}_{\mathrm{ms}}^s\left(k\right)---\left(16\right)$

此处Pⁿ(k)为k时刻无人机位置坐标，C_b ⁿ为从载体坐标系到导航坐标系的转移矩阵，P_sb ^b为视觉传感器中心与飞行器重心的偏差量，C_s ^b为从摄像机坐标系到机体坐标系的转移矩阵，P_ms ^s(k)为在摄像机坐标系下的特征点位置与摄像机位置之差，它可以通过观测得到：

$P_{\mathrm{ms}}^s\left(k\right)={\left[\begin{array}{ccc}\frac{u-u_0}{f_x}h& \frac{v-v_0}{f_y}h& h\end{array}\right]}^T---\left(17\right)$

h为当前时刻飞行器的飞行高度，u₀，v₀为摄像机成像平面的光学中心，f_x，f_y分别为视觉传感器在x，y方向的焦距，u，v为该特征点在摄像机坐标系下的像素坐标。

b、对状态向量及新特征点对应的滤波估计误差方差子矩阵进行初始化；

将新特征点坐标x_mi ⁿ(k)作为状态变量加入到原系统的状态方程(4)式中的状态向量X_a中得到新的状态向量X_a ⁺与新的估计误差方差阵P_a ⁺：

$X_a^{+}=f_i\left(x_{\mathrm{aug}}\right)=\left[\begin{array}{c}{X}_a\\ x_{\mathrm{mi}}^n\left(k\right)\end{array}\right]---\left(18\right)$

$P_a^{+}=\▿f_{\mathrm{xaug}}{P}_{\mathrm{aug}}\▿f_{\mathrm{xaug}}^T---\left(19\right)$

其中，

$\▿f_{\mathrm{xaug}}=\frac{\∂f\left(x_a^{+}\right)}{\∂x_a^{+}}{|x}_{\mathrm{aug}}---\left(20\right)$

$P_{\mathrm{aug}}=\left[\begin{array}{cc}P& 0\\ 0& R\end{array}\right]---\left(21\right)$

P为加入该新特征点之前的估计误差协方差阵，R为新特征点对应的视觉传感器的观测误差协方差阵。

c、观测向量的初始化

观测向量的初始化即将新特征点的观测值加入到观测向量中，

$z^{+}=\left[\begin{array}{c}{z}_{\left(k\right)}\\ z_{\mathrm{mi}}^s\end{array}\right]---\left(22\right)$

其中，z_(k)为原来观测向量，z_mi ^s为摄像机坐标系下新特征点的观测值。

步骤五：对更新后的滤波方程进行滤波估计；

此处更新后的滤波方程指的是状态方程(4)和观测方程(11)，对其进行滤波计算即可得到对无人机飞行状态的估计(位置、速度、姿态等)。

UKF滤波方法结合了UT(Unscented Transformation)变换的思想。它首先通过选择一组权值不同、且能够代表随机状态变量统计特性的采样点；其次，将这些采样点带入非线性函数处理后重构出新的统计特性(如均值和方差)；最后将得到的均值、方差和量测方程代入到Kalman滤波器的逐步递推过程中就构成了UKF滤波。因此，UKF本质上是非线性的，不需要近似线性化，避免了EKF(扩展Kalman滤波)中线性化所带来的误差，而且其计算复杂度与EKF相当，适合应用于视觉导航等领域。

采用UKF方法对系统状态方程(4)和观测方程(11)构成的滤波模型进行滤波，滤波输出的结果即为视觉/惯性组合导航系统对飞行器状态的估计，该估计结果可以返回到控制系统对无人机进行导航定位、控制等。

Claims (7)

1、一种未知环境下无人机视觉/惯性组合导航方法，在进行导航过程之前首先进行坐标系定义及坐标轴对准工作，并建立系统运动状态方程及观测方程，其特征在于：该导航方法通过下列步骤来实现：

步骤一：图像获取并对图像进行预处理；

图像预处理指的是对原始图像进行灰度矫正、去噪声处理，根据无人机机载视觉系统特点，采用灰度图像进行处理，通过公式：

Y＝0.299R+0.596G+0.211B    (1)

将获取到的彩色图像转化为灰度图像Y，式中RGB分别表示图像中红、绿、蓝三种颜色分量；

然后对图像信息进行滤波处理，一般情况下，图像中包含的噪声近似为高斯自噪声，采用中值滤波进行滤波处理，对图像的噪声进行抑制，通过邻域中亮度的中值代替图像的当前点；

步骤二：通过SIFT方法进行特征点提取；

视觉对惯性系统的约束主要体现在图像特征点坐标上，通过SIFT方法对预处理的图像提取特征点；

步骤三：特征点匹配；

每一个SIFT特征点描述向量均代表了该特征点唯一的特征，对特征点进行匹配即对其特征点描述向量进行匹配；用128维的高维度来对特征点进行描述，使得特征点描述符之间具有很大的差异性；用基于欧几里德距离的最近邻法作为衡量图像相似性的度量进行图像匹配；

步骤四：滤波方程更新

根据步骤三中特征点匹配结果将特征点分为三类：

A、在前后两帧图像中均出现并且成功匹配的特征点认为是匹配成功的特征点，对匹配成功的特征点进行处理即对该特征点对应的观测值进行更新；

首先在滤波观测向量z中找到与该匹配特征点对应的分量，而后用当前时刻该特征点在摄像机坐标系中的像素坐标代替该分量，并且根据当前惯性传感器输出重新建立滤波方程，滤波方程包括状态方程和观测方程；

B、仅在第一帧图像中出现，而在当前帧图像中无法找到与之对应的匹配特征点认为是跟踪丢失的特征点，对其进行删除；

由于滤波方程中各地标点之间互相独立，因此，删除地标点只需要删除其对应的状态向量及在滤波方差阵中对应的行和列；

C、在当前帧图像中新出现的特征点认为是新的特征点，对其进行初始化；

步骤五：对更新后的滤波方程进行UKF滤波估计；

首先通过选择一组权值不同、且能够代表随机状态变量统计特性的sigma点；其次，将这些采样点代入非线性函数处理后，重构出新的统计特性；最后将得到的均值、方差和测量方程代入到Kalman滤波器的逐步递推过程中就构成了UKF滤波。

2、根据权利要求1所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：所述步骤二中通过SIFT方法进行特征点提取，其具体方法如下：

1)检测尺度空间极值点；将输入图像通过不同尺度的高斯核函数连续卷积和降采样，形成高斯金字塔图像；然后再对相邻尺度的两个高斯图像相减得到高斯差分图像；将高斯差分图像中每一个像素都与其周围点相比较，得到极值点；

2)精确确定极值点位置；将位于边缘的极值点删掉，余下的则为SIFT特征点；

3)为每个特征点指定方向参数；通过梯度直方图的统计计算，为特征点赋予方向参数；

4)特征点描述向量的生成；首先将坐标轴旋转为特征点的方向，计算每个特征点邻域中梯度方向直方图并以根据位置排序，然后将特征向量长度归一化，该向量就是SIFT特征点描述向量。

3、根据权利要求1所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：所述步骤四的状态方程包括飞行器和地标点的动态模型，它们写成如下的一阶差分模型：

$X_{a\left(k\right)}=f\left(X_{a(k-1)}\right)=\left[\begin{array}{c}{x}_v\left(k\right)\\ x_m\left(k\right)\end{array}\right]=\left[\begin{array}{c}{f}_v(x_v(k-1),u\left(k\right),w_v\left(k\right))\\ f_m(x_m(k-1),w_m\left(k\right)\end{array}\right]---\left(2\right)$

此处X_a为状态向量，x_v(k)为飞行器运动状态，包括位置、速度和姿态，通过惯性导航方程解算得到；

$x_v\left(k\right)=\left[\begin{array}{c}{P}_k^n\\ V_k^n\\ {\mathrm{\ψ}}_k^n\end{array}\right]=\left[\begin{array}{c}{P}_k^n+V_{k-1}^{n}T\\ V_{k-1}^n+(C_b^n{f}^b+g^n)T\\ {\mathrm{\ψ}}_{k-1}^n+E_b^n{\mathrm{\ω}}^{b}T\end{array}\right]+w_v\left(k\right)---\left(3\right)$

C_b ⁿ为方向余弦矩阵，E_b ⁿ为载体角速率到欧拉角速率的转移矩阵，w_v(k)为系统过程噪声，T为滤波周期；

x_m(k)为视觉获取的地标点三维位置向量，对于第i个地标点而言，在假设场景不变前提下，其状态方程为

x_mi(k)＝x_mi(k-1)。    (4)

4、根据权利要求1所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：所述步骤四的观测方程中观测量由高度及摄像机坐标系下的地标点坐标两部分组成，其构建方法如下：

设视觉传感器内参数为

$A=\left[\begin{array}{ccc}{f}_x& \mathrm{\γ}& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]---\left(5\right)$

其中，f_x，f_y分别为视觉传感器在x，y方向的焦距，u₀，v₀为光学中心，γ为不垂直因子；则系统观测值u，v为：

$z_i\left(k\right)=\left[\begin{array}{c}u\\ v\end{array}\right]=\begin{array}{}\end{array}\begin{array}{}\end{array}\left[\begin{array}{c}\frac{x}{z}{f}_x+u_0\\ \frac{y}{z}{f}_y+v_0\end{array}\right]---\left(6\right)$

此处，[x y z]^T为地标点在摄像机坐标系中坐标P_ms ^s(k)的分量，从导航坐标系下的飞行器和地标点位置中得到：

$P_{\mathrm{ms}}^s\left(k\right)=C_b^s\left(k\right)C_n^b\left(k\right)[x_{\mathrm{mi}}^n\left(k\right)-P^n\left(k\right)-C_b^n\left(k\right)P_{\mathrm{sb}}^b]---\left(7\right)$

C_b ^s(k)为从机体坐标系到摄像机坐标系的转移矩阵，x_mi ⁿ(k)为状态向量中当前特征点对应的分量，C_n ^b(k)为从导航坐标系到机体坐标系的转移矩阵，P_sb ^b为视觉传感器中心与飞行器重心的偏差量，Pⁿ(k)为当前的载体(无人机)位置坐标；

由上得到观测方程为：

z(k)＝h(x(k)，v(k))    (8)

h(.，.)为当前时刻非线性观测模型，v(k)为观测噪声。

5、根据权利要求1所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：

步骤四C中所述初始化通过两步完成：

a、得到新地标点在导航坐标系下坐标

$x_{\mathrm{mi}}^n\left(k\right)=g(X\left(k\right),z\left(k\right))=P^n\left(k\right)+C_b^n\left(k\right)P_{\mathrm{sb}}^b+C_b^n{C}_s^b{P}_{\mathrm{ms}}^s\left(k\right)---\left(9\right)$

此处Pⁿ(k)为当前时刻无人机位置坐标，P_ms ^s(k)为在摄像机坐标系下的地标点与摄像机坐标之差，它通过观测得到：

$P_{\mathrm{ms}}^s\left(k\right)={\left[\begin{array}{ccc}\frac{u-u_0}{f_x}h& \frac{v-v_0}{f_y}h& h\end{array}\right]}^T---\left(10\right)$

h为当前时刻飞行器的飞行高度；

b、对状态向量及新地标点对应的滤波估计误差协方差子矩阵进行初始化；

将新地标点坐标x_mi ⁿ(k)作为状态变量加入到原系统的状态方程(2)式中的状态向量X_a中得到新的状态向量X_a ⁺与新的估计误差方差阵P_a ⁺：

$X_a^{+}=f_i\left(x_{\mathrm{aug}}\right)=\left[\begin{array}{c}{X}_a\\ x_{\mathrm{mi}}^n\left(k\right)\end{array}\right]---\left(11\right)$

$P_a^{+}=\▿f_{\mathrm{xaug}}{P}_{\mathrm{aug}}\▿f_{\mathrm{xaug}}^T---\left(12\right)$

其中，

${\▿f}_{\mathrm{xaug}}=\frac{\∂f\left(x_a^{+}\right)}{\∂x_a^{+}}|x_{\mathrm{aug}}={\hat{x}}_{\mathrm{aug}}---\left(13\right)$

$P_{\mathrm{aug}}=\left[\begin{array}{cc}P& 0\\ 0& R\end{array}\right]---\left(14\right)$

P为加入该新地标点之前的估计误差方差阵，R为新地标点对应的视觉传感器的观测方差阵。

6、根据权利要求1所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：

所述坐标系的定义，包括机体坐标系(o_bx_by_bz_b)、导航坐标系(o_nx_ny_nz_n)、摄像机坐标系(o_sx_sy_sz_s)的定义；

机体坐标系(o_bs_by_bz_b)原点在飞行器重心，沿飞行器纵轴向前为x正方向，水平向右为y正方向，z方向与二者垂直并向下，同时，认为IMU(惯性测量单元)的坐标系(o_ix_iy_iz_i)与该坐标系固连；

导航坐标系(o_nx_ny_nz_n)为当地地理坐标系，原点位于导航起始位置，坐标轴分别沿北-东-地方向；

摄像机坐标系(o_sx_sy_sz_s)在飞行器上捷联固定一下视觉传感器以获得一系列的地面图像，其坐标原点位于摄像机中心，o_sz_s沿摄像机光轴方向，o_sx_s和o_sy_s分别沿摄像机成像平面的长、宽方向，三者互相垂直成右手坐标系；

摄像机坐标系是机体坐标系经过简单的平移，而后依次水平转动ψ和俯仰转动θ得到的，ψ和θ分别是摄像机相对于机体坐标系的转动角。

7、根据权利要求6所述的一种未知环境下无人机视觉/惯性组合导航方法，其特征在于：所述的导航坐标系和摄像机坐标系之间的旋转关系标定方法如下：

首先，令所有传感器均敏感垂直方向，在摄像机视场中放置一垂直于水平面的摄像头标定靶；其次，采集摄像头中的标定靶图像，并计算其中对应垂线的消失点得到摄像头对应的垂直方向，对于加速度计而言，当加速度计输出等于重力加速度时认为该传感器方向即为垂直方向；最后，将以上标定图像和加速度计输出数据代入标定方程中得到两个坐标系之间的对应关系；

假设

为惯性传感器敏感到的垂直方向向量，而为与惯性传感器固连的摄像头测量到的垂直方向向量，记q_i ^c为从惯性坐标系到摄像机坐标系的旋转四元数，于是，以上标定过程即选择q_i ^c使方程

$\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\left(q_i^c\stackrel{\→}{v}_i^I{q}_i^c\right)\stackrel{\→}{v}_i^C---\left(15\right)$

取得最大，

从而，当得到当前的q_i ^c旋转四元数之后根据

$C_i^c=\left[\begin{array}{ccc}{q}_1^2+q_2^2-q_3^2-q_4^2& 2(q_2{q}_3-q_1{q}_4)& 2(q_2{q}_4+q_1{q}_3)\\ 2(q_2{q}_3+q_1{q}_4)& q_1^2-q_2^2+q_3^2-q_4^2& 2(q_3{q}_4-q_1{q}_2)\\ 2(q_2{q}_4-q_1{q}_3)& 2(q_3{q}_4+q_1{q}_2)& q_1^2-q_2^2-q_3^2+q_4^2\end{array}\right]---\left(16\right)$

得到惯性坐标系到摄像机坐标系的旋转矩阵，其中q₁…q₄为四元数q_i ^c的四个分量，从而，对于任意惯性坐标系下的坐标点Xⁱ而言都有其在摄像机坐标系下X^c为：

$X^c=C_i^c*X^i---\left(17\right)$

即通过上式将两坐标系下的坐标进行互相转换，同时也对二者进行对准。

Images