CN102435188A - 一种用于室内环境的单目视觉/惯性全自主导航方法 - Google Patents

Abstract

本发明公开了一种用于室内环境的单目视觉/惯性全自主导航方法，属于视觉导航和惯性导航领域。该方法是基于图像的局部不变特性获得特征点信息，利用摄像机运动产生的视差形成的对极几何，求解基础矩阵，利用标定的摄像机内参求解本质矩阵，根据本质矩阵获得摄像机位姿信息，最后将视觉导航信息与惯导信息结合获得相对精确、可靠的导航信息，并对空间特征点进行3D重构，获得环境信息地图，完成载体的自主导航。本发明实现了载体在陌生室内环境中不依赖合作目标的自主导航，算法可靠性高，实现成本低。

Description

一种用于室内环境的单目视觉/惯性全自主导航方法

技术领域

本发明涉及一种导航方法，尤其涉及一种用于室内环境的单目视觉/惯性全自主导航方法，属于视觉导航和惯性导航领域。

背景技术

随着微小型无人机、小型自主机器人等技术的发展，利用先进的导航方式在室内环境中实现自主导航驾驶/飞行是其应用的先决技术条件，在此技术基础上可进行危险地形探测、反恐侦查、室内目标搜寻等任务，具有重要意义。

要在陌生室内环境中实现自主导航需要考虑的主要要素有：1、克服室内环境下GPS等无线信号无法有效获取；2、除了载体本身位姿信息还需要感知外界环境信息，实现避障、路径规划等；3、微小型无人机本身负载问题。国外著名高校进行了诸多的研究，其中以美国麻省理工大学为优，其于1996开始进行小型无人机导航的早期研究，经过十多年的发展，形成了具有领先水平的室内微小型无人机导航制导与控制技术，其采用惯性、激光测距仪、视觉和声纳等传感器，实现飞行器的高精度室内自主导航技术。国内近些年也针对于飞行器视觉导航开展了大量的研究，如浙江大学、哈尔滨工业大学等都利用基于合作目标的图像信息完成飞行器着陆、飞行器对接时的位姿测定，但是目前的视觉导航主要都集中于已知环境地图信息进行追踪或者根据合作目标进行位姿检测。

在陌生环境中，对于非合作目标，获得载体本身的位姿信息需要通过前后图像的匹配特征点进行，载体的运动信息包含在前后图像匹配特征点的投影关系中。由于载体运动会导致采集图像的平移、旋转、尺度、视角、错切、光照、扭曲等变化，因此必须要考虑图像匹配算法的高可靠性。常见的特征点提取算法有Harris法，但是其特征对于上述的图像变换情况点匹配的鲁棒性不够；而在普通图像匹配中采用较为广泛的尺度不变特征提取(SIFT：ScaleInvariant Feature Transform)算法，其在同类算子中具有较强的鲁棒性和较好的实时性，但是此算法过程中仍保留部分误匹配，匹配结果不够精确。

发明内容

本发明针对现有室内导航技术存在的不足，而提出一种适用于室内陌生环境下的结合单目视觉和惯性导航的组合导航方法。

该方法包括如下步骤：

步骤1：对载体上的摄像机进行内参标定，获得空间特征点从世界坐标系到图像坐标系的投影关系，并对摄像机的畸变进行非线性优化；

步骤2：利用摄像机采集序列图像，基于尺度不变特征提取算法提取摄像机采集到的序列图像前后两帧中的空间特征点信息；

步骤3：根据步骤2获得的空间特征点信息进行图像初始匹配，得到初始匹配结果；依据空间特征点匹配信息量自适应调整步骤2中尺度不变特征提取算法的影响因子，获得至少7对匹配特征点；

步骤4：根据步骤3得到的初始匹配结果，基于两帧图像之间视差产生的对极几何约束，求解基础矩阵，通过对极几何约束剔除错误匹配，保留正确匹配特征点信息；

步骤5：根据步骤1标定获得的摄像机内参，结合步骤4中基础矩阵的求解结果，求解本质矩阵；利用奇异值分解，根据本质矩阵求解摄像机位姿变换的旋转矩阵和位移向量，并对所求旋转矩阵进行解算获得视觉位姿变换信息；

步骤6：利用惯性器件获得载体的惯性导航信息，将该惯性导航信息与步骤5获得的视觉位姿变换信息进行比较，保留视觉位姿变换信息中与惯性器件相近的数据作为正解，同时对摄像机坐标系与载体坐标系之间的安装误差进行修正，并将惯导与视觉导航获得的信息结合得到融合导航信息，同时获得载体本身的平移及转动速度；

步骤7：根据步骤6得到的融合导航信息，结合步骤4中保留的正确匹配特征点信息，利用步骤1中获得的投影关系进行空间特征点3D重构，从而获得准确的环境信息，完成载体在环境中的自主导航。

与现有技术相比，本发明具有如下技术效果：

1、采用单目视觉，简化了算法，节约了硬件实现成本。

2、适用于陌生室内环境、非合作目标的情况。

3、针对单目视觉获得位姿信息过程中解的非唯一性及直接获得的位移信息为非量纲问题(即位移信息为各方向的比例关系)，结合惯性导航器件，可简化位姿信息解算过程中错误解的剔除，同时获得位移长度单位，且保证在当视觉导航信息无法有效获取时仍可获得有效的位姿信息，提高了导航信息的可靠性。

4、利用获得的载体本身的绝对位姿信息及保留的匹配特征点信息，完成特征点的3D重构，构造环境信息地图，可实现了路径规划、避障功能，从而实现载体安全有效地自主导航。

附图说明

图1为本发明的方法流程图。

图2为图像平面、摄像机与世界坐标系之间的投影关系图。

图3(a)是摄像机坐标系与载体坐标系的安装示意图；图3(b)为摄像机坐标系与载体坐标系之间的安装误差示意图。

图4为立体视觉对极几何关系示意图。

具体实施方式

本发明的方法流程如图1所示，主要包括如下步骤：

步骤1：对载体上的摄像机进行内参标定，获得空间特征点从世界坐标系到图像坐标系的投影关系，并对摄像机的畸变进行非线性优化；

步骤2：利用摄像机采集序列图像，基于尺度不变特征提取算法提取摄像机采集到的序列图像前后两帧中的空间特征点信息；

步骤3：根据步骤2获得的空间特征点信息进行图像初始匹配，得到初始匹配结果；依据空间特征点匹配信息量自适应调整步骤2中尺度不变特征提取算法的影响因子，获得至少7对匹配特征点；

步骤4：根据步骤3得到的初始匹配结果，基于两帧图像之间视差产生的对极几何约束，求解基础矩阵，通过对极几何约束剔除错误匹配，保留正确匹配特征点信息；

步骤5：根据步骤1标定获得的摄像机内参，结合步骤4中基础矩阵的求解结果，求解本质矩阵；利用奇异值分解，根据本质矩阵求解摄像机位姿变换的旋转矩阵和位移向量，并对所求旋转矩阵进行解算获得视觉位姿变换信息；

步骤6：利用惯导器件获得载体的惯性信息，将该惯性信息与步骤5获得的视觉位姿变换信息进行比较，保留视觉位姿变换信息中与惯导器件相近的数据作为正解，同时对摄像机坐标系与载体坐标系之间的安装误差进行修正，并将惯导与视觉导航获得的信息结合得到融合导航信息，同时获得载体本身的平移及转动速度；

步骤7：根据步骤6得到的融合导航信息，结合步骤4中保留的正确匹配特征点信息，利用步骤1中获得的投影关系进行空间特征点3D重构，从而获得准确的环境信息，完成载体在环境中的自主导航。

下面对本发明方法涉及的主要实现步骤作详细介绍。

(一)摄像机成像模型

空间特征点到图像成像主要涉及的坐标系有世界坐标系、摄像机坐标系、图像坐标系三类。图像经过畸变矫正后与空间特征点对应关系可用针孔模型近似表示，如图2所示：O_cx_cy_cz_c为摄像机坐标系，原点位于摄像机光心，O_cz_c平行于光轴；(u，v)表示以像素为单位的图像坐标系坐标，原点位于图像左上角；O₁为摄像机光心与图像平面的交点，像素坐标(u₀，v₀)；(x，y)表示以毫米为单位的图像坐标系坐标，每一个像素在x轴和y轴方向上的物理尺寸为dx和dy。摄像机坐标系与图像坐标系的关系如下：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{\mathrm{dx}}& 0& u_0\\ 0& \frac{1}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]---\left(1\right)$

摄像机坐标系到图像物理坐标系的转换关系如下：

$z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]---\left(2\right)$

式中f为摄像机焦距，结合式(1)、式(2)得到图像坐标系与摄像机坐标系的关系：

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\begin{array}{c}\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& 0& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\end{array}\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]---\left(3\right)$

世界坐标系与摄像机坐标系存在如下变换：

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(4\right)$

式中R_3×3和T_3×1分别为世界坐标系和摄像机坐标系之间的角度变换矩阵和位移变换矩阵，则空间中任意点到成像平面的投影可以用如下公式表示：

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\begin{array}{c}\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& 0& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\end{array}\left[\begin{array}{cc}{R}_{3\×3}& T_{3\×1}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_{3\×4}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]---\left(5\right)$

因为图像成像过程中会存在畸变，主要考虑径向和切向畸变，图像物理坐标的影响可以如下表示：

X_d＝(1+k₁R²+k₂R⁴)X+2k₃XY+k₄(R²+2X²)           (6)

Y_d＝(1+k₁R²+k₂R⁴)X+2k₄XY+k₃(R²+2X²)

(X_d，Y_d)表示经过畸变矫正的图像物理坐标系坐标，R²＝X²+Y²，k₁和k₂表示径向畸变参数，k₃和k₄表示切向畸变参数。

(二)基于尺度不变特征的目标初始匹配

本发明基于图像多尺度不变特征实现特征提取，整个流程的实现可分为如下四个部分：

(1)求取尺度空间极值。

首先基于高斯内核函数构建多尺度图像空间：

$L(x,y,\mathrm{\σ})=G(x,y,\mathrm{\σ})\⊗I(x,y)---\left(7\right)$

$G(x,y,\mathrm{\σ})=\left(\frac{1}{2\mathrm{\π}{\mathrm{\σ}}^2}{e}^{-(x^2+y^2)/2{\mathrm{\σ}}^2}\right)---\left(8\right)$

采用一组连续的高斯卷积核G(x，y，σ)与原图像I(x，y)卷积生成一系列尺度空间图像，相邻尺度图像求差生成DOG(Different ofGaussian)：

D(x，y，σ)＝L(x，y，kσ)-L(x，y，σ)                (9)

将DOG尺度空间各阶非最外层每点与本层及邻层的领域26点相比较求极值提取特征点。

(2)分配特征点方向。

在特征点邻域内采样，创建梯度直方图。直方图以每10度代表一个方向，共36个方向，选择直方图的主峰值作为特征点的主方向，量值达到主峰值80％的作为副方向，以增强匹配的鲁棒性。特征点梯度的模和方向分别如下：

$m(x,y)=\sqrt{{(L(x+1,y)-L(x-1,y))}^2+{(L(x,y+1)-L(x,y-1))}^2}---\left(10\right)$

θ(x，y)＝tan^-1((L(x，y+1)-L(x，y-1))/(L(x+1，y)-L(x-1，y)))        (11)

(3)特征描述符生成。

以特征点为中心16×16的邻域作为采样窗口，求取每个像素的梯度值和梯度方向。高斯加权求取每4*4小块在8个梯度方向的直方图(采样点与特征点使用相对方向以保持旋转不变性)，绘制每个梯度方向的累加值，形成一个种子点，由此每个特征点生成128维的特征描述符，将其归一化，以减少光照影响。邻域方向性信息联合的思想可增强算法的抗噪声能力，对于含有定位误差的特征匹配也提供了较好的容错性。

(4)初始特征匹配。

P_a{p^(a) ₁，p^(a) ₂...p^(a) _m}、P_b＝{p^(b) ₁，p^(b) ₂...p^(b) _n}分别为从前后两帧图像中提取的特征点集，以P_b中的特征点128维描述符每一同维数据与其均值的差方根大小依次为索引建立Kd-Tree，采用BBF搜索算法得到P_a特征点在Kd-Tree的近似k(k表示维数，这里k＝2)近邻。然后以Euclidean距离作为描述子的距离函数，按照distance-ratio准则进行匹配，确定匹配特征点。

(三)基于对极几何约束的基础矩阵求解

在获得特征点初始匹配结果后，摄像机的运动信息包含在前后图像的匹配特征点投影关系中，但是其中也包含了不少误匹配，为了提高匹配的正确性和精确度，采用多视几何的对极几何约束关系，对匹配特征点重新进行验证，剔除误匹配。

对极几何关系如图4所示，基线为连接两个摄像机光心O(O′)的直线；对极点e(e′)为基线与像平面的交点；对极平面为过基线与特定点M的平面；对极线是对极平面与图像平面的交线，M点投影的像素点m对应极线为l′＝Fm；某点在两像平面中的成像像素坐标满足如下的对极几何约束关系，即极线(epipolar line)约束：

m′^TFm＝0           (12)

其中F为3×3矩阵，即基础矩阵，秩为2，自由度为7，与摄像机的内外参数均有关。此约束是点到线的约束，可为一对多。

采用鲁棒性较好的随机抽样一致性算法思想来求解基础矩阵F。该方法利用RANSAC作为搜索引擎，并基于7点法的解算思想求解基础矩阵，具体如下：

(1)将所有特征点进行归一化处理；

(2)随机选取其中7组对应点，利用7点法求解F阵：

将公式(12)展开，可得：

x′xf₁₁+x′yf₁₂+x′f₁₃+y′xf₂₁+y′yf₂₂+y′f₂₃+xf₃₁+yf₃₂+f₃₃＝0      (13)

扩展到n组点匹配，可得到如下线性方程组：

$\mathrm{Af}=\left[\begin{array}{ccccccccc}{x_1}^{\′}{x}_1& {x_1}^{\′}{y}_1& {x_1}^{\′}& {y_1}^{\′}{x}_1& {y_1}^{\′}{y}_1& {y_1}^{\′}& x_1& y_1& 1\\ M& M& M& M& M& M& M& M& M\\ {x_n}^{\′}{x}_n& {x_n}^{\′}{y}_n& {x_n}^{\′}& {{y_n}^{\′}x}_n& {y_n}^{\′}{y}_n& {y_n}^{\′}& x_n& y_n& 1\end{array}\right]f=0---\left(14\right)$

利用SVD对A进行分解，得到两个对应零奇异值的两个向量f₁和f₂(分别对应F的两个解F₁和F₂)，两者张成A的零空间(null space)。然后利用det(F)＝0性质来解出F通解F＝αF₁+(1-α)F₂中的比例因子α，以确定所要估计的基础矩阵。由于det(F)＝0的约束是一个三次方程，可以得到3个解，如果某个解的模远小于其他两个解，则判定此解为保留解，否则3个解都要保留。

(3)通过上述计算的F阵如有3个解，计算匹配点对应极线的距离小于设定阈值的点作为内点，以内点数最多的解作为基础矩阵；

(4)由划为内点的点对重新估计基础矩阵F，用估计的F阵定义极线附近的搜索区域。

重复上述(2)～(4)步，进行反复迭代，直至特征点数目稳定为止，则得到最终的基础矩阵及内点信息，内点信息将保留作为后续的重构特征点。

(四)利用已知摄像机内参求解摄像机外参

获得的基础矩阵与摄像机的内、外参都有关，将特征点进行如下处理：

$\left[\begin{array}{c}\hat{u}\\ \hat{v}\\ 1\end{array}\right]={\left[\begin{array}{ccc}\frac{f}{\mathrm{dx}}& 0& u_0\\ 0& \frac{f}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right]}^{-1}\left[\begin{array}{c}u\\ v\\ 1\end{array}\right],$ 即 $\hat{m}=K^{-1}m---\left(15\right)$

则存在

E即为本质矩阵，由此可知：

E＝K′^TFK，且E＝[t]_×R          (16)

本质矩阵E的秩为2，自由度为5，R为3×3的旋转矩阵，[t]_×为平移矢量t＝(t_x，t_y，t_z)^T的反对称阵(skew-symmetric matrix)。E只与摄像机的外参有关，与摄像机内参无关。

利用SVD对本质矩阵进行分解E＝Udiag(k，k，0)V^T，利用如下结论(忽略符号)：

[t]_×＝UZU^T    R＝UWV^T或者R＝UW^TV^T            (17)

其中：

$W=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$ $Z=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]$

由上述公式可利用E阵得到的旋转矩阵R和位移向量T。

旋转矩阵R满足RR^T＝R^TR＝I_3×3，模为1。位移解为量纲。对于可能存在的多组解，可考虑通过在多幅序列图像下求得的一系列解中选择合理的、稳定变换的解作为正确结果。同时考虑后续工作需对外部特征点进行环境的构建，所以采用实际物理成像的可实现性对解算结果进行剔除，即在前后两个摄像机坐标系中，z_i和z_i′必同为正，由此存在着两个约束条件：

1)z_i和z_i′同号，根据针孔模型的坐标变换两边同时叉乘t，即可得

可知：

(t×m_i′)^T·(Em_i)＞0

z_i和z_i′为正，即任意平面上正确的对应点进行3维重构需满足z_i，z_i′＞0。

在获得旋转矩阵R后，根据图2所示的载体坐标系，由于坐标变换具有不可逆性，按照先绕Z轴ψ，再绕X轴θ，最后绕Y轴γ(单位°)的其欧拉角变换，坐标转换矩阵可唯一确定如下：

$C_{c2}^{c1}=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

(8)

$=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]=R$

通过上述公式反解即可求得摄像机运动的各姿态角变化。

(五)MEMS惯性器件结合视觉信息获得载体位姿信息

基于惯性信息可独立获得载体本身的绝对位移信息及角度变化量，通过视觉信息也可获得角度变化信息及位移信息(量纲)，但视觉获得的信息容易存在多解。

利用惯性器件获得的位姿信息作为基础参考信息，将视觉信息获得的位姿变化与惯导信息进行对比，保留有效的视觉解算信息。

角度：将视觉解算的角度(经安装误差修正)与惯性器件测量的角度进行比较，设置经验阈值，如果有解小于该阈值，则认为该视觉解算的角度信息有效，将有效的视觉角度信息保留，通过安装误差的补偿修正后与惯导信息进行加权处理，否则权值设置为零：

$\mathrm{\θ}=\frac{q_0{\mathrm{\θ}}_{\mathrm{INS}}+q_1{\mathrm{\θ}}_{\mathrm{vision}}}{q_0+q_1}---\left(19\right)$

位移：视觉信息获得的位移量为各个方向的量纲，即知各个方向的位移比例关系，以比例与惯性器件获得的信息进行对比，如比例关系满足阈值，则认为通过视觉信息解算得到的位移信息有效，可按比例关系修正惯导信息中位移信息的绝对值。考虑算法的简易性、实用性，通常可直接利用惯导信息的位移信息。

(六)安装误差补偿修正

本发明方法将摄像机固定安装在载体上，采用前视，如图3(a)所示。若摄像机光心与载体中心不重合，则存在安装误差，如图3(b)所示，绕相对于载体坐标系Z、X、Y轴的角度误差为(Δψ，Δθ，Δγ)，位移误差为(Δt_x，Δt_y，Δt_z)。

在获得摄像机坐标系的运动方程即旋转矩阵和位移向量后，需对安装误差进行修正补偿，根据空间一点在不同坐标系下的坐标位置关系，可利用如下步骤对安装误差进行修正：

(1)某一时刻空间中一点从摄像机坐标系到载体坐标系的坐标转换关系为：

$\left[\begin{array}{c}{x}_{b1}\\ y_{b1}\\ z_{b1}\end{array}\right]=C_c^b\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ {\mathrm{\Δt}}_z\end{array}\right]---\left(20\right)$

其中

为摄像机坐标系到载体坐标系的变换矩阵，具体形式为：

$C_c^b=\left[\begin{array}{ccc}\cos \mathrm{\Δ\γ}\cos \mathrm{\Δ\ψ}-\sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\ψ}+\sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& -\sin \mathrm{\Δ\γ}\cos \mathrm{\Δ\θ}\\ -\cos \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\θ}\\ \sin \mathrm{\Δ\γ}+\cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\ψ}-\cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\γ}\cos \mathrm{\Δ\θ}\end{array}\right]$

(2)某一时刻与下一时刻空间中一点在前后两个摄像机坐标系下的转换关系为：

$\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\end{array}\right]=C_{c2}^{c1}\left[\begin{array}{c}{x}_{c2}\\ y_{c2}\\ z_{c2}\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]---\left(21\right)$

其中

即求解的摄像机旋转矩阵，[t_x t_y t_z]^T为求解的摄像机平移量。

(3)某一时刻空间中一点从载体坐标系到摄像机坐标系的坐标转换关系为：

$\left[\begin{array}{c}{x}_{c2}\\ y_{c2}\\ z_{c2}\end{array}\right]=C_b^c\left[\begin{array}{c}{x}_{b2}\\ y_{b2}\\ z_{b2}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ {\mathrm{\Δt}}_z\end{array}\right]---\left(22\right)$

其中为载体坐标系到摄像机坐标系的变换矩阵，满足

(4)利用上述(1)～(3)，可得：

$\left[\begin{array}{c}{x}_{b1}\\ y_{b1}\\ z_{b1}\end{array}\right]=C_c^b\{\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]*[C_b^c\left[\begin{array}{c}{x}_{b2}\\ y_{b2}\\ z_{b2}\end{array}\right]-\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ {\mathrm{\Δt}}_z\end{array}\right]]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]\}+\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ {\mathrm{\Δt}}_z\end{array}\right]---\left(23\right)$

经化简，可得最终的安装误差修正公式：

$\left[\begin{array}{c}{x}_{b1}\\ y_{b1}\\ z_{b1}\end{array}\right]=C_c^b\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]C_b^c\left[\begin{array}{c}{x}_{b2}\\ y_{b2}\\ z_{b2}\end{array}\right]+(I-C_c^b\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right])\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ {\mathrm{\Δt}}_z\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]---\left(24\right)$

(七)利用摄像机位姿变化信息与对应特征点重构环境特征点

在完成摄像机位姿信息变换求解，结合计算基础矩阵保留的内点，根据摄像机投影模型，可利用匹配特征点进行特征点的3D重构。

根据世界坐标系在摄像机坐标系下的投影矩阵公式，即式(5)，对应特征点的投影关系如下：

$Z_{c1}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]$ $Z_{c2}=\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right]---\left(25\right)$

其中[u₁，v₁，1]^T和[u₂，v₂，1]^T分别为前后两幅图像中对应点的像素齐次坐标；[X，Y，Z，1]^T为特征点在世界坐标系下的齐次坐标。假设世界坐标系与前一时刻摄像机坐标系重合，则M₁＝K[I|0]，M₂＝K[R|T]。

对式(25)的左式进行展开，可得：

Z_c1u₁＝m₁₁X+m₁₂Y+m₁₃Z+m₁₄

Z_c1v₁＝m₂₁X+m₂₂Y+m₂₃Z+m₂₄       (26)

Z_c1＝m₃₁X+m₃₂Y+m₃₃Z+m₃₄

将式(26)中的第一式除以第三式，第二式除以第三式可消去Z_c1，依此方法将式(25)左右两式进行简化处理，消除Z_c1和Z_c2，得到关于X，Y，Z的四个线性方程：

$(u_1{m}_{31}^1-m_{11}^1)X+(u_1{m}_{32}^1-m_{12}^1)Y+(u_1{m}_{33}^1-m_{13}^1)Z=m_{14}^1-u_1{m}_{34}^1$

$(v_1{m}_{31}^1-m_{21}^1)X+(v_1{m}_{32}^1-m_{22}^1)Y+(v_1{m}_{33}^1-m_{23}^1)Z=m_{24}^1-v_1{m}_{34}^1$

(27)

$(u_2{m}_{31}^2-m_{11}^2)X+(u_2{m}_{32}^2-m_{12}^2)Y+(u_2{m}_{33}^2-m_{13}^2)Z=m_{14}^2-u_2{m}_{34}^2$

$(v_2{m}_{31}^2-m_{21}^2)X+(v_2{m}_{32}^2-m_{22}^2)Y+(v_2{m}_{33}^2-m_{23}^2)Z=m_{24}^1-v_2{m}_{34}^2$

上述线性关系有3个变量4个方程，由于像素点与摄像机坐标原点的射线必定相交，所以方程一定存在唯一解。而在实际应用当中，由于数据总是有噪声的，所以采用最小二乘法求解X，Y，Z，从而完成特征点在摄像机坐标系下的坐标还原。

完成特征点在前一时刻摄像机坐标系的坐标位置后，考虑到安装误差，根据式(20)即可获得特征点在载体前一时刻载体坐标系下的空间坐标位置。

至此，获得特征点相对载体坐标系的坐标位置，即构建了环境信息。通过每一步迭代，对自身位姿状态进行迭代的同时，不断感知环境信息，进行构建环境地图，为最终实现自主导航获取有用的位姿信息及地图信息。

Claims (3)

1.一种用于室内环境的单目视觉/惯性全自主导航方法，其特征在于：

该方法包括如下步骤：

步骤1：对载体上的摄像机进行内参标定，获得空间特征点从世界坐标系到图像坐标系的投影关系，并对摄像机的畸变进行非线性优化；

步骤2：利用摄像机采集序列图像，基于尺度不变特征提取算法提取摄像机采集到的序列图像前后两帧中的空间特征点信息；

步骤3：根据步骤2获得的空间特征点信息进行图像初始匹配，得到初始匹配结果；依据空间特征点匹配信息量自适应调整步骤2中尺度不变特征提取算法的影响因子，获得至少7对匹配特征点；

步骤4：根据步骤3得到的初始匹配结果，基于两帧图像之间视差产生的对极几何约束，求解基础矩阵，通过对极几何约束剔除错误匹配，保留正确匹配特征点信息；

步骤5：根据步骤1标定获得的摄像机内参，结合步骤4中基础矩阵的求解结果，求解本质矩阵；利用奇异值分解，根据本质矩阵求解摄像机位姿变换的旋转矩阵和位移向量，并对所求旋转矩阵进行解算获得视觉位姿变换信息；

步骤6：利用惯性器件获得载体的惯性导航信息，将该惯性导航信息与步骤5获得的视觉位姿变换信息进行比较，保留视觉位姿变换信息中与惯性器件相近的数据作为正解，同时对摄像机坐标系与载体坐标系之间的安装误差进行修正，并将惯导与视觉导航获得的信息结合得到融合导航信息，同时获得载体本身的平移及转动速度；

步骤7：根据步骤6得到的融合导航信息，结合步骤4中保留的正确匹配特征点信息，利用步骤1中获得的投影关系进行空间特征点3D重构，从而获得准确的环境信息，完成载体在环境中的自主导航。

2.根据权利要求1所述的一种用于室内环境的单目视觉/惯性全自主导航方法，其特征在于：所述步骤3中图像初始匹配的内容为：

利用图像的多尺度不变特性，获得空间特征点位置，并对每个空间特征点生成相应的特征描述向量，根据最近/次近邻规则获得初始匹配结果。

3.根据权利要求1所述的一种用于室内环境的单目视觉/惯性全自主导航方法，其特征在于：所述步骤6中对摄像机坐标系与载体坐标系之间的安装误差进行修正通过下式进行：

$\left[\begin{array}{c}{x}_{b1}\\ y_{b1}\\ z_{b1}\end{array}\right]=C_b^b\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]C_b^c\left[\begin{array}{c}{x}_{b2}\\ y_{b2}\\ z_{b2}\end{array}\right]+\left(\begin{array}{c}I-C_c^b\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\end{array}\right)\left[\begin{array}{c}{\mathrm{\Δt}}_x\\ {\mathrm{\Δt}}_y\\ \mathrm{\Δ}{t}_z\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]$

式中：下标b表示载体坐标系；下标c表示摄像机坐标系；

[x_b1 y_b1 z_b1]^T和[x_b2 y_b2 z_b2]^T分别表示空间某一点在前后两时刻载体坐标系中的坐标位置； $\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ 为前后两时刻摄像机坐标系的旋转关系矩阵；[t_x t_y t_z]^T表示前后两时刻摄像机坐标系的位移矩阵；[Δt_x Δt_y Δt_z]^T表示载体坐标系到摄像机坐标系的平移向量，即安装误差的平移量；

为摄像机坐标系到载体坐标系的变换矩阵，具体形式为：

$C_c^b=\left[\begin{array}{ccc}\cos \mathrm{\Δ\γ}\cos \mathrm{\Δ\ψ}-\sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\ψ}+\sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& -\sin \mathrm{\Δ\γ}\cos \mathrm{\Δ\θ}\\ -\cos \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\θ}\\ \sin \mathrm{\Δ\γ}+\cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\sin \mathrm{\Δ\ψ}& \sin \mathrm{\Δ\γ}\sin \mathrm{\Δ\ψ}-\cos \mathrm{\Δ\γ}\sin \mathrm{\Δ\θ}\cos \mathrm{\Δ\ψ}& \cos \mathrm{\Δ\γ}\cos \mathrm{\Δ\θ}\end{array}\right]$

其中(Δψ，Δθ，Δγ)为依次绕Z轴、X轴、Y轴方向的安装角误差，且

Images