视觉SLAM(simultaneous localization and mapping, SLAM)是指移动载体在未知环境中，通过获取自身携带的视觉传感器信息，创建与环境一致的地图，同时确定自身在地图中的位置^[1-3]。该技术不仅在机器人导航、任务规划和远程操作等领域具有重要研究意义，也在计算机游戏、虚拟现实和增强现实等领域具有广阔的应用前景^[4-7]。研究视觉SLAM问题常用的相机主要有深度相机^[8]、单目相机^[9]和双目相机^[10]，其中深度相机受光照条件影响苛刻，难以在户外大场景中应用；单目相机无法恢复尺度信息，应用价值有限；双目相机可计算像素深度，因此在学术和工业领域得到了广泛应用。

视觉SLAM可通过是否使用图像特征分为特征法和直接法两类^[11]，且上述2种方法在实际应用中各有优劣。特征法^[12-13]通过匹配图像特征，构建相邻帧之间的对应关系，并通过最小化重投影误差(re-projection error)对相机运动和地图结构进行优化。基于特征法的视觉SLAM系统鲁棒性强，即使在较大帧间运动时也可正常工作；但特征匹配操作需消耗大量时间，且特征法仅针对图像中少量特征点进行运算，忽略了其他像素信息，无法实现稠密建图。直接法^[14-15]仅考虑像素灰度信息，通过最小化光度误差(photometricerror)恢复相机运动和地图结构。基于直接法的视觉SLAM系统无需进行特征匹配操作，计算效率高，可使用图像中的所有像素信息进行稠密或半稠密建图；但直接法基于灰度不变假设，鲁棒性较差，且稠密或半稠密建图需对大量像素进行运算，CPU无法实时运行，需借助GPU加速实现。

本文以设计高精度、高适应性的双目视觉SLAM系统为目的，选择基于特征法的双目视觉SLAM系统作为研究对象。在众多基于特征法的双目视觉SLAM系统中，双目ORB-SLAM^[16]是目前最完善易用的实时视觉SLAM系统之一^[2]。然而，双目ORB-SLAM算法跟踪模块仅解算最新一帧图像对应的状态量，且精度有限，需通过回环检测模块对其进行修正。若实际运动不包含回环路径，状态估计结果将产生漂移，且随系统运行时间缓慢增大。本文对双目ORB-SLAM算法进行了改进，提出了一种基于滑窗非线性优化的双目视觉SLAM算法，通过引入滑窗思想，解算最新多帧图像对应的系统状态，同时采用了边缘化策略将其余状态量转换为先验测量信息，从而在保证算法实时性的基础上提高了不包含回环路径时系统的精确性。

1 算法框架

本文算法运行原理图如图1所示，主要分为3个并行线程：1) 特征提取和跟踪线程，主要负责提取和匹配图像特征，如图1中红框部分所示；2) 状态量估计线程，主要负责视觉特征点重投影误差计算，状态量非线性优化以及边缘化处理，如图1中蓝框部分所示；3) 建图线程，主要负责采用线性三角化和光束平差优化构建增量式地图，如图1中绿框部分所示。

2 图像特征跟踪算法

双目视觉SLAM需对左右目图像以及相邻帧图像进行特征点检测与跟踪，由于图像间畸变微小，本文采用导向性FAST特征加旋转BRIEF描述子(oriented FAST and rotated BRIEF, ORB)^[17] 特征实现特征跟踪，结果如图2所示。

本文图像特征跟踪具体实现步骤如下：首先，以原始图像为基准构建尺度因子为1.2的8层图像金字塔，在金字塔各层图像上提取基于加速分割测试的特征(features from accelerated sagment test, FAST)^[18]；其次，计算各特征点的方向，构建ORB特征描述子；最后，将左右目图像与相邻帧图像进行匹配，构建特征点间的匹配对应关系。

3 滑窗双目视觉SLAM系统状态估计

基于特征点的SLAM算法通常把状态估计问题构建为以视觉特征点重投影误差为代价函数的非线性优化模型，通过引入滑窗思想，本文构建了新的代价函数，并采用高斯牛顿法迭代地求解最优的状态变量。如图3所示，滑窗双目视觉SLAM系统代价函数可表示为

${{J}}({{\chi }}) = {\sum\limits_{k = 1}^n {\sum\limits_{i = 1}^m {{{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})^{\rm{T}}} } }{{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }}) + {{{J}}_{{\rm{mar}}}}({{\chi }})$

式中： $\;{{\chi }}$ 为滑窗中的待优化的状态变量集合， $\;{{\chi }} =$ $ [{{{x}}_1},{{{x}}_2}, \cdots ,{{{x}}_n},{{p}}_{{f_1}}^w,{{p}}_{{f_2}}^w, \cdots ,{{p}}_{{f_m}}^w]$ ； ${{{x}}_k} = [{{p}}_k^w,{{{R}}_{wk}}]$ 包含了滑窗中第k帧相机坐标系相对世界坐标系的位置 ${{p}}_k^w$ 和姿态 ${{{R}}_{wk}}$ ； ${{p}}_{{f_i}}^w$ 为视觉特征点 ${f_i}$ 在世界坐标系中的三维坐标；n为滑窗中图像的帧数；m为滑窗中可观测到的视觉特征点的个数； ${{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})$ 为视觉特征点 ${f_i}$ 在滑窗中的第k帧图像中的重投影误差； ${{{J}}_{{\rm{mar}}}}({{\chi }})$ 为边缘化产生的先验约束信息。

3.1 视觉特征点重投影误差模型

如图4所示，通过相机投影模型，将视觉特征点 ${f_i}$ 在世界坐标系中的三维坐标转化为第k帧像素坐标系下的像素坐标，并计算该值与视觉量测所得匹配点像素坐标间的差异，二者的差值即为视觉特征点 ${f_i}$ 在第k帧像素坐标系中的重投影误差。

视觉特征点 ${f_i}$ 在第k帧像素坐标系中的重投影误差可表示为

$\begin{array}{c} {{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }}) = \left[ \begin{array}{l} {{r}}({\hat{ l}}_{{f_i}}^{{c_{k,1}}},{{\chi }})\\ {{r}}({\hat{ l}}_{{f_i}}^{{c_{k,2}}},{{\chi }}) \end{array} \right]=\\ \left[ \begin{array}{l} {{{\pi}}}({{R}}_{{\rm{wk}}}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)){\rm{ - }}{\hat{ l}}_{{f_i}}^{{c_{k,1}}}\\ {{{\pi}}} ({{R}}_{{c_{k,2}}{c_{k,1}}}^{}({{R}}_{{\rm{wk}}}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)) + {{p}}_{{c_{k,1}}}^{{c_{k,2}}}){\rm{ - }}{\hat{ l}}_{{f_i}}^{{c_{k,2}}} \end{array} \right] \end{array}$

式中： ${\hat{ l}}_{{f_i}}^{{c_{k,1}}}$ 和 ${\hat{ l}}_{{f_i}}^{{c_{k,2}}}$ 分别为视觉特征点 ${f_i}$ 在第k帧左、右目像素坐标系中的测量值，由图像特征跟踪获得； ${{R}}_{{c_{k,2}}{c_{k,1}}}^{}$ 和 ${{p}}_{{c_{k,1}}}^{{c_{k,2}}}$ 为左目相机坐标系到右目相机坐标系的旋转矩阵和平移向量，通过相机标定获得；记 ${{p}}_{{f_i}}^{{c_{k,1}}}$ = ${{\rm{[}}x_{{f_i}}^{{c_{k,1}}}{\rm{,}}y_{{f_i}}^{{c_{k,1}}},z_{{f_i}}^{{c_{k,1}}}{\rm{]}}^{\rm{T}}}$ = ${{R}}_{wk}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)$ 为视觉特征点 ${f_i}$ 在第k帧左目相机坐标系中的三维坐标； ${{p}}_{{f_i}}^{{c_{k,2}}}$ = ${{\rm{[}}x_{{f_i}}^{{c_{k,2}}}{\rm{,}}y_{{f_i}}^{{c_{k,2}}},z_{{f_i}}^{{c_{k,2}}}{\rm{]}}^{\rm{T}}}$ = ${{R}}_{{c_{k,2}}{c_{k,1}}}^{}({{R}}_{wk}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)) + {{p}}_{{c_{k,1}}}^{{c_{k,2}}}$ 为视觉特征点 ${f_i}$ 在第k帧右目相机坐标系中的三维坐标； ${{\pi}}( \cdot )$ 表示从相机坐标系通过内参数矩阵转换到像素坐标系的函数；误差项 ${{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})$ 相对于状态量 ${{R}}_{wk}^{}$ ， ${{p}}_k^w$ ， ${{p}}_{{f_i}}^w$ 的雅克比矩阵分别为

$\frac{{\partial {{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})}}{{\partial {{{\theta }}_{{{wk}}}}}} = \left[ \begin{gathered} \frac{{\partial ({ {{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,1}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,1}}}}}{[{{R}}_{{{wk}}}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)]_ \times } \\ \frac{{\partial ({{{\pi}}}({{p}}_{{f_i}}^{{c_{k,2}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,2}}}}}{{R}}_{{c_{k,2}}{c_{k,1}}}^{}{[{{R}}_{wk}^{ - 1}({{p}}_{{f_i}}^w - {{p}}_k^w)]_ \times } \end{gathered} \right]$

$\frac{{\partial {{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})}}{{\partial {{p}}_k^w}} = \left[ \begin{gathered} {\rm{ - }}\frac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,1}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,1}}}}}{{R}}_{wk}^{ - 1} \\ {\rm{ - }}\frac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,2}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,2}}}}}{{R}}_{{c_{k,2}}{c_{k,1}}}^{}{{R}}_{wk}^{ - 1} \end{gathered} \right]$

$\frac{{\partial {{r}}({\hat{ l}}_{{f_i}}^{{c_k}},{{\chi }})}}{{\partial {{p}}_{{f_i}}^w}} = \left[ \begin{gathered} \frac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,1}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,1}}}}}{{R}}_{wk}^{ - 1} \\ \frac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,2}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,2}}}}}{{R}}_{{c_{k,2}}{c_{k,1}}}^{}{{R}}_{wk}^{ - 1} \end{gathered} \right]$

式中： ${{{\theta }}_{wk}}$ 为与 ${{R}}_{wk}^{}$ 对应的旋转向量； ${[{{p}}]_ \times }$ 为由三维向量 ${{p}} = {[{p_x},{p_y},{p_z}]^{\rm{T}}}$ 构建的反对称矩阵； $\dfrac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,1}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,1}}}}}$ 为 ${{{\pi}}}({{p}}_{{f_i}}^{{c_{k,1}}})$ 相对于 ${{p}}_{{f_i}}^{{c_{k,1}}}$ 的雅克比矩阵，即

$\dfrac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,1}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,1}}}}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {{\alpha _{x,1}}\dfrac{1}{{z_{{f_i}}^{{c_{k,1}}}}}}&0&{{\rm{ - }}{\alpha _{x,1}}\dfrac{{x_{{f_i}}^{{c_{k,1}}}}}{{{{(z_{{f_i}}^{{c_{k,1}}})}^2}}}}\\ 0&{{\alpha _{y,1}}\dfrac{1}{{z_{{f_i}}^{{c_{k,1}}}}}}&{{\rm{ - }}{\alpha _{y,1}}\dfrac{{y_{{f_i}}^{{c_{k,1}}}}}{{{{(z_{{f_i}}^{{c_{k,1}}})}^2}}}} \end{array}} \right]$

$\dfrac{{\partial ({{{\pi}}}({{p}}_{{f_i}}^{{c_{k,2}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,2}}}}}$ 为 ${{{\pi}}}({{p}}_{{f_i}}^{{c_{k,2}}})$ 相对于 ${{p}}_{{f_i}}^{{c_{k,2}}}$ 的雅克比矩阵，即

$\dfrac{{\partial ({{{{\pi}}}}({{p}}_{{f_i}}^{{c_{k,2}}}))}}{{\partial {{p}}_{{f_i}}^{{c_{k,2}}}}}{\rm{ = }}\left[ {\begin{array}{*{20}{c}} {{\alpha _{x,2}}\dfrac{1}{{z_{{f_i}}^{{c_{k,2}}}}}}&0&{{\rm{ - }}{\alpha _{x,2}}\dfrac{{x_{{f_i}}^{{c_{k,2}}}}}{{{{(z_{{f_i}}^{{c_{k,2}}})}^2}}}}\\ 0&{{\alpha _{y,2}}\dfrac{1}{{z_{{f_i}}^{{c_{k,1}}}}}}&{{\rm{ - }}{\alpha _{y,2}}\dfrac{{y_{{f_i}}^{{c_{k,2}}}}}{{{{(z_{{f_i}}^{{c_{k,2}}})}^2}}}} \end{array}} \right]$

式中： ${\alpha _{x,1}}$ 和 ${\alpha _{y,1}}$ 为左目相机x轴和y轴的焦距； ${\alpha _{x,2}}$ 和 ${\alpha _{y,2}}$ 为右目相机x轴和y轴的焦距。

状态量 ${{R}}_{{\rm{wk}}}^{}$ ， ${{p}}_k^w$ 和 ${{p}}_{{f_i}}^w$ 的更新方程为（其中箭头表示将右侧的更新量够赋值给左侧变量）

$\left\{ \begin{array}{l} {{{R}}_{{\rm{wk}}}} \leftarrow {{{R}}_{{\rm{wk}}}}{\rm{exp}}(\delta {{{\theta }}_{{\rm{wk}}}}) \\ {{p}}_k^w \leftarrow {{p}}_k^w{\rm{ + }}\delta {{p}}_k^w \\ {{p}}_{{f_i}}^w \leftarrow {{p}}_{{f_i}}^w{\rm{ + }}\delta {{p}}_{{f_i}}^w \end{array} \right.$

式中 ${\rm{exp}}(\delta {{{\theta }}_{wc}}){\rm{ = }}\exp ({{\rm{[}}\delta {{{\theta }}_{{\rm{wc}}}}{\rm{]}}_ \times }) \approx {{{I}}_{3 \times 3}} + {{\rm{[}}\delta {{{\theta }}_{{\rm{wc}}}}{\rm{]}}_ \times }$ 。

3.2 状态量边缘化

在视觉SLAM系统中，随着运行时间推移，由视觉特征点三维坐标与相机位姿构成的状态变量数目将显著增加，算法复杂度将随状态量数目增加而增大。如果求解全状态SLAM问题将无法满足实时性需求；如果仅求解最新一帧图像对应的状态量将忽略各状态量间的关联，显著降低了SLAM系统的精度。本文引入了滑窗思想，有效限制了待求解状态量数目，采用状态量边缘化策略，且将其余状态量转换为待求解状态量的先验观测信息，有效实现了算法复杂度和精确性间的平衡。

3.2.1 边缘化原理

采用高斯牛顿法求解状态增量的公式为

${{H}}\Delta {{\chi }} = {{b}}$

(1)

式中： ${{H}}{\rm{ = }}{{{J}}^{\rm{T}}}{{J}}$ ； ${{b}} = - {{{J}}^{\rm{T}}}{{r}}$ ； ${{J}}$ 为误差项 ${{r}}$ 的雅克比阵； $\Delta \;{{\chi }}$ 为求解的状态增量。假设 ${\;{{\chi }}_\mu }$ 为 $\;{{\chi }}$ 中待边缘化的状态量； ${\;{{\chi }}_\lambda }$ 为 $\;{{\chi }}$ 中与 ${\;{{\chi }}_\mu }$ 存在误差关联的状态量； ${\;{{\chi }}_p}$ 为 $\;{{\chi }}$ 中除去 ${\;{{\chi }}_\mu }$ 和 ${\;{{\chi }}_\lambda }$ 剩余的状态量，即 $\;{{\chi }} = [{\;{{\chi }}_\mu },{\;{{\chi }}_\lambda },{\;{{\chi }}_p}]$ 。由于 ${\;{{\chi }}_p}$ 相对于 ${\;{{\chi }}_\mu }$ 独立，可采用其子问题简化式(1)，可得

$\left[ {\begin{array}{*{20}{c}} {{{{H}}_{{{\mu \mu }} }}}&{{{{H}}_{{{\mu \lambda }} }}}\\ {{{{H}}_{{{\lambda \mu }} }}}&{{{{H}}_{\lambda \lambda }}} \end{array}} \right]\left[ \begin{array}{l} \Delta {{{\chi }}_\mu }\\ \Delta {{{\chi }}_\lambda } \end{array} \right] = \left[ \begin{array}{l} {{{b}}_\mu }\\ {{{b}}_\lambda } \end{array} \right]$

(2)

采用舒尔补(schur complement)^[19]原理简化式(2)，得

$\left[ {\begin{array}{*{20}{c}} {{{{H}}_{{{\mu \mu }} }}}&{{{{H}}_{{{\mu \lambda }} }}}\\ 0&{{{H}}_{_{{{\lambda \lambda }} }}^ * } \end{array}} \right]\left[ \begin{array}{l} \Delta {{{\chi }}_\mu }\\ \Delta {{{\chi }}_\lambda } \end{array} \right] = \left[ \begin{array}{l} {{{b}}_\mu }\\ {{b}}_{_\lambda }^ * \end{array} \right]$

式中： ${{H}}_{{{\lambda \lambda }} }^ * = {{{H}}_{{{\lambda \lambda }} }} - {{{H}}_{{{\lambda \mu }} }}{{H}}_{{{\mu \mu }}}^{}{{{H}}_{{{\mu \lambda }} }}$ ， ${{b}}_\lambda ^ * = {{{b}}_\lambda } - {{{H}}_{{{\lambda \mu }} }}{{H}}_{{{\mu \mu }} }^{ - 1}{{{b}}_\mu }$ 。

求解方程 ${{H}}_{_{{\lambda \lambda } }}^ * \Delta {{{\chi }}_\lambda }{\rm{ = }}{{b}}_{_\lambda }^ * $ ，可得增量 $\Delta {{{\chi }}_\lambda }$ 值，进而可更新状态量 ${{{\chi }}_\lambda }$ 。由于增量 $\Delta {{{\chi }}_\mu }$ 未求解，将无法更新状态量 ${{{\chi }}_\mu }$ ，需采用最初估计雅克比矩阵(first estimates jacobians)思想^[20]，将线性化求解 ${{H}}$ 矩阵的过程始终固定在初始点，即 ${{H}}_{_{{{λλ}} }}^ * $ 值不变，而 ${{b}}_{_\lambda }^ * $ 将采用下式更新

${{b}}_\lambda ^ * \leftarrow {{b}}_\lambda ^ * {\rm{ + }}\frac{{\partial {{b}}_\lambda ^ * }}{{\partial {{{\chi }}_\lambda }}}\Delta {{{\chi }}_\lambda }{\rm{ = }}{{b}}_\lambda ^ * {\rm{ - }}{{H}}_{{{\lambda\lambda}} }^ * \Delta {{{\chi }}_\lambda }$

3.2.2 边缘化在滑窗双目视觉SLAM中的应用

固定滑窗内图像帧数目为n，本文边缘化策略的如下：如果滑窗内第n−1帧为关键帧，将边缘化滑窗内第1个关键帧对应状态量 ${{{x}}_1}$ 及与其相关的视觉测量；如果第n−1帧为非关键帧，将直接删除第n−1帧对应状态量 ${{{x}}_{n - 1}}$ 及与其相关的视觉测量，此时采用了删除策略，并未进行边缘化处理，其目的是为了保持滑窗内系统的稀疏性。采用何种策略取决于滑窗内第n−1帧是否为关键帧，如满足下述条件之一即为关键帧：

1) 第n−1帧跟踪到的特征点数目大于60个；

2) 跟踪到的特征点中50%为未知三维坐标；

3) 地图构建线程处于空闲状态。

4 滑窗双目视觉SLAM系统地图构建

视觉SLAM系统需根据相机运动状态以及观测到的特征点信息构建增量式地图。由于建图线程不受相机帧率限制，本文仅选用关键帧图像观测到的特征点构建地图。当滑窗内倒数第一帧图像为关键帧时，对该帧图像中未知三维坐标的匹配点，采用线性三角化方法，进行三维重建。记特征点 ${f_i}$ 在参考帧相机坐标系中的齐次坐标 ${{p}}_{{f_i}}^r{\rm{ = [}}x_{{f_i}}^r,y_{{f_i}}^r,z_{{f_i}}^r{\rm{,}}1{{\rm{]}}^{\rm{T}}}$ ，可得

$\left\{ \begin{gathered} {{{{\pi}}}^{ - 1}}({{l}}_{{f_i}}^c) \times {{{T}}_{cr}}{{p}}_{{f_i}}^r = 0 \\ {{{{\pi}}}^{ - 1}}({{l}}_{{f_i}}^r) \times {{p}}_{{f_i}}^r = 0 \\ \end{gathered} \right.$

(3)

式中： ${{{T}}_{{{cr}}}}{{ = }}\left[ {\begin{array}{*{20}{c}}{{{{R}}_{{{cr}}}}}&{{{p}}_r^c}\end{array}} \right]{{ = }}\left[ {\begin{array}{*{20}{c}}{{{R}}_{{{rc}}}^{ - 1}}&{{{ - }}{{R}}_{{{rc}}}^{ - 1}}\end{array}{{p}}_c^r} \right]$ ， ${{R}}_{{{rc}}}^{}$ 和 ${{p}}_c^r$ 分别为当前帧相机坐标系到参考帧相机坐标系的旋转矩阵和平移向量； ${{l}}_{{f_i}}^c{\rm{ = [}}u_{{f_i}}^c,v_{{f_i}}^c,1{{\rm{]}}^{\rm{T}}}$ 和 ${{l}}_{{f_i}}^r{\rm{ = [}}u_{{f_i}}^r,v_{{f_i}}^r,1{{\rm{]}}^{\rm{T}}}$ 分别为特征点 ${f_i}$ 在当前帧和参考帧像素坐标系中的齐次坐标； ${{{{{\pi}}}}^{{\rm{ - }}1}}( \cdot )$ 表示相机的逆投影变换，即

${{{{\pi}}}^{{\rm{ - }}1}}({{l}}_{{f_i}}^c){\rm{ = }}{\left[ {\begin{array}{*{20}{c}} {{\alpha _{x,1}}}&0&{{\beta _{x,1}}}\\ 0&{{\alpha _{y,1}}}&{{\beta _{y,1}}}\\ 0&0&1 \end{array}} \right]^{ - 1}}\left[ \begin{array}{l} u_{{f_i}}^c\\ v_{{f_i}}^c\\ 1 \end{array} \right]$

${{{{\pi}}}^{{\rm{ - }}1}}({{l}}_{{f_i}}^r){\rm{ = }}{\left[ {\begin{array}{*{20}{c}} {{\alpha _{x,1}}}&0&{{\beta _{x,1}}}\\ 0&{{\alpha _{y,1}}}&{{\beta _{y,1}}}\\ 0&0&1 \end{array}} \right]^{ - 1}}\left[ \begin{array}{l} u_{{f_i}}^r\\ v_{{f_i}}^r\\ 1 \end{array} \right]$

式中： ${\alpha _{x,1}}$ 和 ${\alpha _{y,1}}$ 分别表示左目相机x轴和y轴焦距；( $\;{\beta _{x,1}}$ , $\;{\beta _{y,1}}$ )表示左目相机主点像素坐标。由式(3)可得线性方程

$\left[ \begin{gathered} {[{{{{\pi}}}^{{{ - }}1}}({{l}}_{{f_i}}^c)]_1}{[{{{T}}_{{{cr}}}}]_3} - {[{{{T}}_{{{cr}}}}]_1} \\ {[{{{{\pi}}}^{{{ - }}1}}({{l}}_{{f_i}}^c)]_2}{[{{{T}}_{{{cr}}}}]_3} - {[{{{T}}_{{{cr}}}}]_2} \\ {[{{{{\pi}}}^{{{ - }}1}}({{l}}_{{f_i}}^r)]_1}{[{{{I}}_{4 \times 4}}]_3} - {[{{{I}}_{4 \times 4}}]_1} \\ {[{{{{\pi}}}^{{{ - }}1}}({{l}}_{{f_i}}^r)]_2}{[{{{I}}_{4 \times 4}}]_3} - {[{{{I}}_{4 \times 4}}]_2} \\ \end{gathered} \right]{{p}}_{{{{f}}_{{i}}}}^r = {{Ap}}_{{{{f}}_{{i}}}}^r{{ = }}{{0}}$

式中： ${[ \cdot ]_i}$ 表示矩阵 $[ \cdot ]$ 的第i行； ${{{I}}_{4 \times 4}}$ 表示4阶单位阵。对矩阵 ${{A}}$ 进行奇异值分解可得

${{A}} = {{{U}}_A}{{{\Sigma }}_A}{{V}}_A^{\rm{T}}$

式中： ${{{U}}_A}$ 和 ${{{V}}_A}$ 均为4阶方阵； ${{{\Sigma }}_A}$ 为 ${{A}}$ 矩阵奇异值组成的对角矩阵。将 ${{{V}}_A}$ 最后一列构成的列向量进行标准化处理，化简为 ${{\rm{[}}x_{{f_i}}^r,y_{{f_i}}^r,z_{{f_i}}^r,{\rm{1]}}^{\rm{T}}}$ 的齐次表达形式，即为向量 ${{p}}_{{f_i}}^r$ 的解。

5 实验测试与结果分析

选用欧洲机器人技术挑战赛(european robotics challenge, EuRoC)公开数据集^[21]中的MH_05_difficult数据文件，将本文算法和双目ORB-SLAM进行对比测试，评估本文算法的精确性和有效性，其中本文算法滑窗内图像帧数目设为6帧，算法运行平台为配备Intel Core i7处理器，内存为8 GB的笔记本电脑。实验测试结果如图5~12所示，实验结果包括轨迹曲线、三维地图、位置绝对误差分量和姿态绝对误差分量；位置绝对误差和姿态绝对误差的统计结果如表1所示。

从图5中可以看出，双目ORB-SLAM和本文算法都能有效估计载体位姿，且不发生显著漂移。从表1中可以看出，双目ORB-SLAM和本文算法的位置均方根误差分别为0.269 331和0.175 442 m，姿态均方根误差为1.591 809°和0.814 265°。由此可见，本文算法明显优于双目ORB-SLAM算法，其原因在于双目ORB-SLAM算法跟踪模块仅解算最新一帧图像对应的状态量，且精度有限；而本文通过引入滑窗思想，解算最新多帧图像对应的系统状态，同时采用了边缘化策略将其余状态量转换为先验测量信息，从而在保证算法实时性的基础上提高了系统的精确性。

6 结论

本文提出了一种基于滑窗非线性优化的双目视觉SLAM算法，主要包括：

1）在双目ORB-SLAM算法框架基础上，引入了滑窗思想，解算最新多帧图像对应的状态。

2）采用了边缘化策略将其余状态量转换为先验测量信息，从而保证了算法的精确性和实时性。

3）本文通过公开数据集测试实验，验证了算法的优越性。然而，本文双目视觉SLAM算法仅在PC(personal computer)端实现了实时定位与建图功能，未来可以考虑简化算法，将其移植到嵌入式系统或移动端应用中。