CN104180818A - 一种单目视觉里程计算装置 - Google Patents

Abstract

本发明公开了一种单目视觉里程计算装置，包括：特征检测与关联模块1，用于在每一个时刻获取新的图像时，在图像中检测GFtT特征点，跟踪所述GFtT特征点在下一个时刻中图像的位置，从而获得一定时间内的GFtT特征点关联集合并输出给位姿估计模块；位姿估计模块2，用于根据所述特征检测与关联模块输出的GFtT特征点关联集合，利用基于车辆动力学模型约束的位姿估计算法进行建模从而得到相邻时刻之间的相对位姿变换；本发明具有较高的效率和精度，适合于智能车辆的实时定位导航需求；可用于存在大量运动障碍物的复杂环境，不受到静态场景假设的限制，能够满足智能车辆在复杂城市道路环境下的使用需求。

Description

一种单目视觉里程计算装置

技术领域

本发明涉及智能车辆视觉定位导航领域，尤其涉及一种单目视觉里程计算装置。

背景技术

定位导航技术主要用于确定智能车辆行驶的位置和方向，是保证智能车辆完成自主行为的重要前提。在没有已知地图、GPS信号被遮挡等无法获取环境位置信息的情况下，智能车辆在运动过程中的自定位能力具有非常重要的意义。随着计算机视觉技术的发展，视觉定位方法广泛应用于地面、空中、水下及行星表面等各种工作环境下，同时也越来越多地被用来进行地面移动车辆的自定位和定向估计。

视觉方法仅仅依赖于车载相机采集到的图像信息，基于相机模型与视觉几何学模型等来计算车体本身的6自由度运动。这种基于图像信息的自定位定向方法类似于传统里程计的航迹推算，因此称为视觉里程计。首先，视觉传感器所提供的丰富的感知信息，既可以满足车辆的自主定位要求，又能够同时为其他重要的任务提供辅助信息，如目标检测、避障等。其次，视觉传感器相对其他传感器来说成本较低，且体积小，在布置上更为灵活。另外，基于视觉的定位和定向估计还可以独立于地形以及地形-轮胎接触特性，如不受轮胎打滑的影响等。

视觉里程计应具有较高的精度和效率，满足智能车辆的实时定位需求，并且能够用于存在动态障碍物或是行驶条件恶劣的复杂场景。当前国内的视觉里程计研究仍然局限于小型地面移动机器人、仿真分析或者是较为简单的室内环境实验，试验距离局限在百米数量级，面向大范围复杂环境的陆地智能车辆实时应用还有待进一步的研究。此外纵观国内外的研究情况，车辆运动特性的约束在视觉里程计系统中的应用没有深入的研究，并且受到严格的平面场景假设的限制。然而在实际的车辆运动过程中，由于相机是固连于簧上质量，即使是在平坦道路上行驶，车体的俯仰与侧倾变化也是不可避免的。

发明内容

鉴于上述的分析，本发明旨在提供一种单目视觉里程计算装置，用以解决现有视觉里程计计算存在的局限性问题。

本发明的目的主要是通过以下技术方案实现的：

本发明提供了一种单目视觉里程计算装置，包括：

特征检测与关联模块1，用于在每一个时刻获取新的图像时，在图像中检测GFtT特征点，跟踪所述GFtT特征点在下一个时刻中图像的位置，从而获得一定时间内的GFtT特征点关联集合并输出给位姿估计模块；

位姿估计模块2，用于根据所述特征检测与关联模块输出的GFtT特征点关联集合，利用基于车辆动力学模型约束的位姿估计算法进行建模从而得到相邻时刻之间的相对位姿变换。

进一步地，所述特征检测与关联模块具体包括：GFtT特征点检测单元11和帧间LK跟踪单元12，其中，

GFtT特征点检测单元11，在每一个时刻获取新的图像时，在图像中检测GFtT特征点，并将检测到的GFtT特征点输出给帧间LK跟踪单元12；

帧间LK跟踪单元12，对于接收到的GFtT特征点，用LK跟踪方法跟踪其在下一个时刻中图像的位置，从而获得一定时间内的特征点关联集合并输出给位姿估计模块2。

进一步地，所述特征检测与关联模块还包括：

补充检测单元13，对于检测到GFtT特征点的图像，将图像划分为多个栅格，对每一个栅格，若其中已有GFtT特征点的数量超过预定阈值，则在本栅格不再进行特征点检测；反之在本栅格中进行补充检测。

进一步地，所述位姿估计模块2具体包括：

动力学模型预估单元21，利用根据车辆动力学微分方程计算质心侧偏角，将其表达为横摆角速率，侧向加速度以及横摆角加速度的线性组合；根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模；用离线辨识获得的参数以及建模得到的结果，结合特征点关联集合中的二对特征关联对，求解用于数据关联中假设生成的内数据；

最大似然估计器22，根据内层计算得到的内数据以及运动参数的优胜解，对优胜解进行最大似然估计从而得到最终解。

进一步地，利用根据车辆动力学微分方程计算质心侧偏角，将其表达为横摆角速率，侧向加速度以及横摆角加速度的线性组合为：

$\mathrm{\β}=l_R\frac{\mathrm{\ω}}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}}\·{\mathrm{\ω}}_r-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r,$ 其中l＝l_R+l_F为车辆轴距，β表示质心侧偏角，l_F，l_R分别表示前、后轴中心到质心的距离；k_R为后轴侧偏刚度，m为整车质量，I_y为车体绕竖直y轴的转动惯量；v_long代表纵向车速，ω_r代表横摆角速率，代表横摆角加速度，a_lat表示侧向加速度。

进一步地，根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模的结果为

$C\·\left[\begin{array}{c}\mathrm{\ω}\\ \mathrm{\γ}\end{array}\right]=b$

其中， $C=\left[\begin{array}{c}{C}^1\\ C^2\end{array}\right]$ 是2×2矩阵，满足Cⁱ＝[ⁱx_k-1 ⁱy_kb₀-(ⁱy_k-1-ⁱy_k)b₁+ⁱy_k ⁱy_k-1 ⁱy_kb₀-ⁱx_k+b₀]，c1表示 $(\frac{l_R}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}})\·\mathrm{dt},$ c2表示 $\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r;$ b是2×1向量，满足b(i)＝ⁱx_k-1 ⁱy_k-ⁱx_k ⁱy_k-1+ⁱy_k-1b₀-ⁱy_kb₀；x_k-1、y_k-1、x_k、y_k都是特征点的归一化到球面坐标系的图像坐标，即满足上标i表示特征关联的序号。

进一步地，求解用于数据关联中假设生成的内数据的过程包括：

为点关联i＝1:N-1,j＝i+1:N，构建形如上的模型并求解模型参数横摆角变化量^ijω及俯仰角变化量^ijγ；

以一定的分辨率和范围构建二维直方图Hist，用离散化^ijω的^ijγ和进行直方图投票；

选择票数最高的模型解{ω^*,γ^*}＝argmax{Hist}作为优胜解，其中，ω^*表示最优横摆角变化量，γ^*表示最优俯仰角变化量；

根据上述线性组合和辨识参数计算最优质心侧偏角β^*，并计算模型估计运动参数旋转矩阵和平移向量；

$R^m=\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]t^m=\left[\begin{array}{c}\sin {\mathrm{\β}}^{*}\\ 0\\ \cos {\mathrm{\β}}^{*}\end{array}\right]$

用模型估计R^m和t^m对特征关联进行运动基线三角化，得到无尺度的三维坐标估计并计算重投影误差；重投影误差大于设定阈值的，判定为外数据，剩下的即为内数据。

进一步地，所述最大似然估计器22具体用于，

对单目位姿估计来说，通过假设图像点测量误差满足高斯白噪声模型，采用林温伯格-麦考特(L-M)迭代优化的最大似然估计器来优化已知基础矩阵F，其中已知基础矩阵 $F\≡K_1^{-T}{\left[t\right]}_{\×}{\mathrm{RK}}_0^{-1};$

根据已有优胜解 和分别对应t^m和R^m，计算最大似然的步骤如下：

选择已知相机矩阵P_k-1＝[I|0]以及其中极点e_k是从初始估计中计算得到；

利用特征关联角化获得对三维位置的估计

获得与初始估计一致的校正关联为

用L-M方法最小化代价函数

$\underset{i}{\mathrm{\Σ}}d{(\stackrel{\‾}{q}_{k-1}^i,\hat{q}_{k-1}^i)}^2+d{(\stackrel{\‾}{q}_k^i,\hat{q}_k^i)}^2$

获得最优解F^*，用已知的分解和判定方法获得最优模型估计旋转矩阵R^*和平移向量t^*。

本发明有益效果如下：

本发明具有较高的效率和精度，适合于智能车辆的实时定位导航需求；可用于存在大量运动障碍物的复杂环境，不受到静态场景假设的限制，能够满足智能车辆在复杂城市道路环境下的使用需求。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

图1为本发明实施例所述系统的结构示意图；

图2为本发明实施例中，动态栅格模板示意图；

图3为透视相机球面坐标系的原理示意图。

具体实施方式

本发明实施例所述方法是应用于单目视觉系统、能够获得车速信息的场合，基于单轨动力学模型与合理线性近似的动力学预估横摆角和俯仰角分量。通过考虑轮胎的侧偏特性，推导了质心侧偏角与横摆角变化量之间的关系，从而获得了更准确的质心侧偏角估计；通过引入俯仰角变化量引起的旋转矩阵分量，并考虑车体俯仰角变化的实际范围，进行合理的线性近似，获得了高效且精确的初始解。

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

如图1所以，图1为本发明实施例所述系统的结构示意图，该系统具体可以包括：特征检测与关联模块1和位姿估计模块2，其中，该特征检测与关联模块具体包括：GFtT特征点检测单元11、帧间LK跟踪单元12以及补充检测单元13，以下对这各个模块的功能予以详细说明。

特征检测与关联模块1，主要负责在每一个时刻获取新的图像时，在图像中检测GFtT特征点(Good Features to Track，易于跟踪的特征点)，用LK(Lucas-Kanade,卢卡斯，卡纳德)跟踪方法跟踪其在下一个时刻中图像的位置，从而获得一定时间内的特征点关联集合并输出给位姿估计模块。

该特征检测与关联模块具体包括：GFtT特征点检测单元11、帧间LK跟踪单元12以及补充检测单元13，其中，

特征点检测单元11，在每一个时刻获取新的图像时，在图像中检测GFtT特征点，并将检测到的GFtT特征点输出给帧间LK跟踪单元12；即特征点检测单元11的输入是图像序列，输出是特征关联集合i＝1:N，k为当前时刻，N为特征关联的数量；

帧间LK跟踪单元12，对于接收到的GFtT特征点，用LK跟踪方法跟踪其在下一个时刻中图像的位置，从而获得一定时间内的特征点关联集合并输出给位姿估计模块2；

进一步地，由于特征检测与关联模块1采用GFtT特征点结合LK跟踪用于视觉里程计系统的帧间匹配，纯跟踪的方法会导致留存的点数量越来越少，为了克服这一缺陷维持可用特征关联的数量，因此特征检测与关联模块还包括：

补充检测单元13，对于当前帧已经检测到或跟踪到的特征点，将图像划分为一定数量的小栅格，对每一个栅格中，若已有特征的数量超过某阈值(根据实际应用得到的经验值)，则在本栅格位置不再进行特征点检测；反之在本栅格中需进行补充检测。

位姿估计模块2，主要负责根据特征检测与关联模块输出的特征关联集合，利用基于车辆动力学模型约束的位姿估计算法(MYP算法)恢复得到相邻时刻之间的相对位姿变换，所谓相对位姿变换即旋转矩阵R和平移向量t。不同于传统的视觉里程计系统仅将相机的运动视为无约束的6自由度运动，本发明实施例中考虑轮式车辆的动力学模型，附加考虑车体俯仰角位姿变化，推导出全新的适用于车载单目视觉里程计系统的位姿估计算法。MYP算法采用的是单轨自行车模型，该模型包含了前轮和后轮，将车辆处理为自行车并描述了车辆3自由度的运动(二维平面位置和横摆角速率)。

以下将对位姿估计模块2的计算过程进行具体介绍，为了便于理解，首先对相关的计算基础知识进行简要介绍。

相邻时刻k-1,k的两个相机位置通过刚体坐标变换关联，满足如下形式：

$T_{k-1,k}=\left[\begin{array}{cc}{R}_{k-1,k}& t_{k-1,k}\\ 0& 1\end{array}\right]---(a.1)$

其中R_k-1,k∈SO(3)是旋转矩阵，是平移向量。集合T_0,…,M＝{T_0,1,…,T_M-1,M}包含了所有的总帧数为M的序列运动。最终，相机位姿的集合C_0,…,_M＝{C₀,…,C_M}包含了相机相对于初始时刻k＝0时的变换。当前位姿C_M可以通过累积所有的变换T_k-1,k(k＝1,...,M)得到

$C_M=\underset{k}{\overset{M}{\mathrm{\Π}}}{T}_{k-1,k}=T_{k-1,k}\·C_{M-1}---(a.2)$

C₀是相机在时刻k＝0的位姿。

视觉里程计的基础是两视角视觉几何学。对于三维点Q和它的图像二维投影点q，有3×4矩阵P满足上划线表示齐次表达,～表示这是一个定义直到尺度的等式，即与是共线的向量但不一定长度相等，该矩阵P称为相机矩阵。每一个视角下的相机矩阵P都可分解为P＝K[R|t]，式中K是相机内参数矩阵，而R和t是相机运动的旋转矩阵与平移向量。对于点Q及其在两个不同视角下的二维投影点q₀与q₁，令第一个视角是初始状态，即R₀＝I³,t₀＝0，其中I³为3×3单位矩阵。两个视角的相机矩阵分别为P₀＝K₀[I³|0]，P₁＝K₁[R|t]。

令[t]_×为由平移向量t的元素构成的螺旋对称矩阵，形如

${\left[t\right]}_{\×}\≡\left[\begin{array}{ccc}0& -t\left(3\right)& t\left(2\right)\\ t\left(3\right)& 0& -t\left(1\right)\\ -t\left(2\right)& t\left(1\right)& 0\end{array}\right]---(a.3)$

得到基础矩阵

$F\≡K_1^{-T}{\left[t\right]}_{\×}{\mathrm{RK}}_0^{-1}---(a.4)$

通过基础矩阵就可以表达共面性方程，也称对极约束方程

${\stackrel{\‾}{q}}_1^T\·F\·{\stackrel{\‾}{q}}_0=0---(a.5)$

对于一个已经标定的相机，内参数矩阵K是已知且假设恒定的，即K₀＝K₁＝K，对分别左乘逆标定矩阵K^-1，这一过程是对图像点归一化到单元球面的过程，得到

则(a.5)式的对极约束将变为

式中E＝[t]_×R称为本征矩阵。

一般的视觉里程计计算过程从特征点关联中求解E然后从中分解得到R与t。令E的奇异值分解为E＝U diag(1,1,0)V^T，则四个可能的因式化分解E＝[t]_×R形如：

(a)  R＝UWV^T  t＝+U⁽³⁾

(b)  R＝UWV^T  t＝-U⁽³⁾

(a.7)

(c)  R＝UW^TV^T  t＝+U⁽³⁾

(d)  R＝UW^TV^T  t＝-U⁽³⁾

其中 $W=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$ 为正交矩阵，U⁽³⁾代表矩阵U的第3行。

在实际的执行中，应该对所有图像点进行三角化重建，获得在两个视角下深度值t(3)都大于零的重建三维点最多的解为真实解。

上述位姿估计模块2具体包括：动力学模型预估单元21以及最大似然估计器22，其中，

动力学模型预估单元21，主要利用根据车辆动力学微分方程计算质心侧偏角，将其表达为横摆角速率，侧向加速度以及横摆角加速度的线性组合；根据上述线性组合对视觉里程相机模型的旋转矩阵与平移向量进行建模；利用离线辨识获得的参数以及建模得到的线性方程组，结合特征点关联集合中的二对特征关联对，求解用于数据关联中假设生成的内数据；

上述动力学模型预估单元21主要完成的计算过程如下：

(1)利用根据车辆动力学微分方程计算质心侧偏角β，将其表达为横摆角速率ω_r，侧向加速度以及横摆角加速度的线性组合。

考虑车辆动力学微分方程

$\begin{array}{c}(k_F+k_R)\mathrm{\β}+\frac{1}{v_{\mathrm{long}}}(l_F{k}_F-l_R{k}_R){\mathrm{\ω}}_r-k_F\mathrm{\δ}=m({\stackrel{\·}{v}}_{\mathrm{lat}}+v_{\mathrm{long}}{\mathrm{\ω}}_r)\\ (l_F{k}_R-l_R{k}_R)\mathrm{\β}+\frac{1}{v_{\mathrm{long}}}(l_F^2{k}_F+l_R^2{k}_R){\mathrm{\ω}}_r-l_F{k}_F\mathrm{\δ}=I_y{\stackrel{\·}{\mathrm{\ω}}}_r\end{array}---\left(1\right)$

其中，l_F，l_R分别表示前、后轴中心到质心的距离，为可测量的车体已知几何参数；k_F，k_R分别为前、后轴侧偏刚度，m为整车质量，I_y为车体绕竖直y轴的转动惯量；v_long代表纵向车速，v_lat代表横向车速，β为车体质心侧偏角，ω_r代表横摆角速率。为了消去无法测量且不需要的前轮偏角值δ，用式(1)的上式乘以l_F减去下式，得到

$(l_F+l_R)k_R\mathrm{\β}-\frac{l_R}{v_{\mathrm{long}}}(l_F+l_R)k_R{\mathrm{\ω}}_r=m\·l_F\·{\stackrel{\·}{v}}_{\mathrm{lat}}+m\·l_F\·v_{\mathrm{long}}\·{\mathrm{\ω}}_r-I_y{\stackrel{\·}{\mathrm{\ω}}}_r---\left(2\right)$

质心侧偏角β可以表达为横摆角速率ω_r，侧向加速度(也即a_lat)以及横摆角加速度的线性组合，即

$\mathrm{\β}=l_R\frac{\mathrm{\ω}}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}}\·{\mathrm{\ω}}_r-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r---\left(3\right)$

其中l＝l_R+l_F为车辆轴距。

(2)据此对旋转矩阵R¹与平移向量t¹进行建模，其中

$R^1=\left[\begin{array}{ccc}\cos \mathrm{\ω}& 0& -\sin \mathrm{\ω}\\ 0& 1& 0\\ \sin \mathrm{\ω}& 0& \cos \mathrm{\ω}\end{array}\right],t^1=\mathrm{\ρ}\left[\begin{array}{c}\sin \mathrm{\β}\\ 0\\ \cos \mathrm{\β}\end{array}\right]---\left(4\right)$

其中，ω表示横摆角变化量，β表示车体质心侧偏角。

建模过程如下：

对旋转矩阵的建模则只需对R¹右乘一个俯仰变换矩阵R_γ，

$R_{\mathrm{\γ}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]$

由于俯仰角变化量γ较小，简化令sinγ≈γ，cosγ≈1，最终得到模型估计为

$R^m=\left[\begin{array}{ccc}\cos \mathrm{\ω}& 0& -\sin \mathrm{\ω}\\ 0& 1& -\mathrm{\γ}\\ \sin \mathrm{\ω}& \mathrm{\γ}\cos \mathrm{\ω}& \cos \mathrm{\ω}\end{array}\right],t^m=\left[\begin{array}{c}\sin \mathrm{\β}\\ 0\\ \cos \mathrm{\β}\end{array}\right]---\left(5\right)$

得到简化表达本征矩阵形如

$E=\left[\begin{array}{ccc}0& -1& \mathrm{\γ}\\ \cos \mathrm{\ω}-\tan \mathrm{\β}\sin \mathrm{\ω}& -\mathrm{\γ}(\sin \mathrm{\ω}+\cos \mathrm{\ω}\tan \mathrm{\β})& -(\sin \mathrm{\ω}+\cos \mathrm{\ω}\tan \mathrm{\β})\\ 0& \tan \mathrm{\β}& -\mathrm{\γ}\tan \mathrm{\β}\end{array}\right]---\left(6\right)$

根据对极约束有

x_k-1y_k(cosω-tanβsinω)-x_ky_k-1-y_k-1y_kγ(sinω+cosωtanβ)  (7)

+y_k-1tanβ+x_kγ-y_k(sinω+cosωtanβ)-γtanβ＝0

式中x_k-1、y_k-1、x_k、y_k都是特征点的归一化到球面坐标系的图像坐标，即满足其中球面坐标系如图3所示，图3展示了球面坐标系的原理。对于标定透视相机，任何一点都可以映射到单元球面上。图3中图像点Q归一化到了以相机中心G为球心的单元球面上。这些向量称为图像点在单元球面上的归一化坐标。

如式(7)所示，方程(7)包含了复杂的三角函数因此难以直接计算ω。根据来自仿真数据和实车实验数据的观察，每100毫秒横摆角变化量ω和俯仰角变化量γ都较小，采用三角函数的泰勒展开对其进行近似，包括

$\begin{array}{cc}\sin \mathrm{\ω}\≈\mathrm{\ω}& \cos \mathrm{\ω}\≈1-\frac{{\mathrm{\ω}}^2}{2}\\ \tan \mathrm{\β}=\tan (c_1\mathrm{\ω}+c_2)\≈c_1\mathrm{\ω}+c_2+\frac{{(c_1\mathrm{\ω}+c_2)}^3}{3}\≈c_2+\frac{c_2^3}{3}+c_1(1+c_2^2)\mathrm{\ω}+c_1^2{c}_2{\mathrm{\ω}}^2& \end{array}$

其中，c1表示公式(3)中c2表示公式(3)中的dt是两帧图像之间采集的时间间隔，在视觉里程计系统中是已知参数，一般为定值。

再令 $b_0=c_2+\frac{c_2^3}{3},b_1=c_1(1+c_2^2),b_2=c_1^2{c}_2,$ 即有tanβ＝b₀+b₁ω+b₂ω²。

将泰勒近似带入方程(7)，最终得到线性方程组

$C\·\left[\begin{array}{c}\mathrm{\ω}\\ \mathrm{\γ}\end{array}\right]=b---\left(9\right)$

式中 $C=\left[\begin{array}{c}{C}^1\\ C^2\end{array}\right]$ 是2×2矩阵，满足Cⁱ＝[ⁱx_k-1 ⁱy_kb₀-(ⁱy_k-1-ⁱy_k)b₁+ⁱy_k ⁱy_k-1 ⁱy_kb₀-ⁱx_k+b₀]c1表示 $(\frac{l_R}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}})\·\mathrm{dt},$ c2表示 $\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r,;$ b是2×1向量，满足b(i)＝ⁱx_k-1 ⁱy_k-ⁱx_k ⁱy_k-1+ⁱy_k-1b₀-ⁱy_kb₀，且上标i表示特征关联的序号。

发明的算法中涉及到的参数包括整车质量m，后轮侧偏刚度k_R以及车体绕竖直轴转动惯量I_y。实际应用中可以通过获取的真实车体位姿数据来对它们进行离线辨识。定义最小化目标函数为模型输出与测量值之间的误差平方和，令表示在时刻k测量到的质心侧偏角，定义系数矩阵A满足，

$A(k,1)=l\·{\widetilde{\mathrm{\β}}}_i-l_R\·{\stackrel{\·}{\mathrm{\ω}}}_r\left(k\right)/v_{\mathrm{long}}\left(k\right)$

$A(k,2)=-l_F\·({\stackrel{\·}{v}}_{\mathrm{lat}}\left(k\right)+v_{\mathrm{long}}\left(k\right)\·{\stackrel{\·}{\mathrm{\ω}}}_r\left(k\right))---\left(10\right)$

$A(k,3)={\stackrel{\·}{v}}_{\mathrm{lat}}\left(k\right)$

从系数矩阵A中可以求解最小二乘意义上的解m/k_R和I_y/k_R。即对系数矩阵进行奇异值分解A＝U·S·V^T，则有m/k_R＝V(2,3)/V(1,3)，I_y/k_R＝V(3,3)/V(1,3)。在实际的应用中，只需要这两个比值，而非m，k_R和I_y的绝对数值。

(3)利用离线辨识获得的参数(前文提到的质量m，后轮侧偏刚度k_R和绕竖直轴惯量I_y)以及推导得到的线性方程组(9)，结合二对特征关联对就足以求解用于数据关联中假设生成的内数据，具体过程如下：

为了避免流行的概率方法造成的不确定性，本发明实施例中采用二维直方图投票法来获得优胜解并移除外数据，得到内数据。

考虑共N对点关联i＝1:N，直方图投票法的具体执行步骤如下：

(1)为点关联i＝1:N-1,j＝i+1:N，构建形如式(9)的线性方程组并求解模型参数横摆角变化量^ijω及俯仰角变化量^ijγ；

(2)以一定的分辨率和范围(本发明实施例中采用0.05度，横摆角变化量范围±10度，俯仰角变化量±3度)构建二维直方图Hist，用离散化的^ijω和^ijγ进行直方图投票；

(3)选择票数最高的模型解{ω^*,γ^*}＝argmax{Hist}，即优胜解；

(4)根据式(3)和辨识参数等计算质心侧偏角β^*，并计算模型估计运动参数旋转矩阵和平移向量

$R^m=\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]t^m=\left[\begin{array}{c}\sin {\mathrm{\β}}^{*}\\ 0\\ \cos {\mathrm{\β}}^{*}\end{array}\right]---\left(11\right)$

(5)用模型估计R^m和t^m对特征关联进行运动基线三角化，得到无尺度的三维坐标估计并计算重投影误差；重投影误差大于设定阈值的，判定为外数据，剩下的即为内数据。

最大似然估计器22，根据内层计算得到的内数据以及运动参数的优胜解，对优胜解进行最大似然估计从而得到最终解，其主要计算过程如下：

对单目位姿估计来说，通过假设图像点测量误差满足高斯白噪声模型，采用林温伯格-麦考特(L-M)迭代优化的最大似然估计器来优化计算基础矩阵F，其中基础矩阵 $F\≡K_1^{-T}{\left[t\right]}_{\×}{\mathrm{RK}}_0^{-1};$

根据已有初始解计算最大似然值，该初始解就是根据内层估计的优胜解(票数最高的模型解推导得到的模型估计运动参数旋转矩阵和平移向量)式(11)计算的，K是标定参数据矩阵已知，和分别对应式(11)中的t^m和R^m，主要计算步骤如下：

a.选择相机矩阵P_k-1＝[I|0]以及其中极点e_k是从初始估计中计算得到。

b.利用特征关联三角化获得对三维位置的估计

c.获得与初始估计一致的校正关联为

d.用L-M方法最小化代价函数

$\underset{i}{\mathrm{\Σ}}d{(\stackrel{\‾}{q}_{k-1}^i,\hat{q}_{k-1}^i)}^2+d{(\stackrel{\‾}{q}_k^i,\hat{q}_k^i)}^2---\left(12\right)$

e.获得最优解F^*，用前面(a.7)中描述的分解和判定方法获得最优模型估计旋转矩阵R^*和平移向量t^*。

综上所述，本发明实施例提供了一种单目视觉里程计算装置，提出了基于单轨动力学模型与合理线性近似的动力学预估横摆角和俯仰角分量的视觉里程计算法，称为MYP算法。通过充分考虑轮胎的侧偏特性、车速等重要的车辆状态信息，结合车辆单轨动力学模型，推导了质心侧偏角与横摆角变化量之间的关系，能够正确捕捉到车载相机运动的主分量，即横摆角变化量、质心侧偏角和俯仰角变化量。能够应对城市环境中各种不同类型的动态障碍物目标，实现精确的自主定位定向，能够满足智能车辆在复杂城市道路环境下的定位导航需求。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (8)

1.一种单目视觉里程计算装置，其特征在于，包括：

特征检测与关联模块1，用于在每一个时刻获取新的图像时，在图像中检测GFtT特征点，跟踪所述GFtT特征点在下一个时刻中图像的位置，从而获得一定时间内的GFtT特征点关联集合并输出给位姿估计模块；

位姿估计模块2，用于根据所述特征检测与关联模块输出的GFtT特征点关联集合，利用基于车辆动力学模型约束的位姿估计算法进行建模从而得到相邻时刻之间的相对位姿变换。

2.根据权利要求1所述的装置，其特征在于，所述特征检测与关联模块1具体包括：GFtT特征点检测单元11和帧间LK跟踪单元12，其中，

GFtT特征点检测单元11，在每一个时刻获取新的图像时，在图像中检测GFtT特征点，并将检测到的GFtT特征点输出给帧间LK跟踪单元12；

帧间LK跟踪单元12，对于接收到的GFtT特征点，用LK跟踪方法跟踪其在下一个时刻中图像的位置，从而获得一定时间内的特征点关联集合并输出给位姿估计模块2。

3.根据权利要求2所述的装置，其特征在于，所述特征检测与关联模块1还包括：

补充检测单元13，对于检测到GFtT特征点的图像，将图像划分为多个栅格，对每一个栅格，若其中已有GFtT特征点的数量超过预定阈值，则在本栅格不再进行特征点检测；反之在本栅格中进行补充检测。

4.根据权利要求1到3中任意一项所述的装置，其特征在于，所述位姿估计模块2具体包括：

动力学模型预估单元21，利用根据车辆动力学微分方程计算质心侧偏角，将其表达为横摆角速率，侧向加速度以及横摆角加速度的线性组合；根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模；用离线辨识获得的参数以及建模得到的结果，结合特征点关联集合中的二对特征关联对，求解用于数据关联中假设生成的内数据；

最大似然估计器22，根据内层计算得到的内数据以及运动参数的优胜解，对优胜解进行最大似然估计从而得到最终解。

5.根据权利要求4所述的装置，其特征在于，利用根据车辆动力学微分方程计算质心侧偏角，将其表达为横摆角速率，侧向加速度以及横摆角加速度的线性组合为：

$\mathrm{\β}=l_R\frac{\mathrm{\ω}}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}}\·{\mathrm{\ω}}_r-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r,$ 其中l＝l_R+l_F为车辆轴距，β表示质心侧偏角，l_F，l_R分别表示前、后轴中心到质心的距离；k_R为后轴侧偏刚度，m为整车质量，I_y为车体绕竖直y轴的转动惯量；v_long代表纵向车速，ω_r代表横摆角速率，代表横摆角加速度，a_lat表示侧向加速度。

6.根据权利要求5所述的装置，其特征在于，根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模的结果为

$C\·\left[\begin{array}{c}\mathrm{\ω}\\ \mathrm{\γ}\end{array}\right]=b$

其中， $C=\left[\begin{array}{c}{C}^1\\ C^2\end{array}\right]$ 是2×2矩阵，满足Cⁱ＝[ⁱx_k-1 ⁱy_kb₀-(ⁱy_k-1-ⁱy_k)b₁+ⁱy_k ⁱy_k-1 ⁱy_kb₀-ⁱx_k+b₀]，c1表示 $(\frac{l_R}{v_{\mathrm{long}}}+\frac{m}{k_R}\·\frac{l_F}{l}\·v_{\mathrm{long}})\·\mathrm{dt},$ c2表示 $\frac{m}{k_R}\·\frac{l_F}{l}\·a_{\mathrm{lat}}-\frac{I_y}{l\·k_R}{\stackrel{\·}{\mathrm{\ω}}}_r;$ b是2×1向量，满足b(i)＝ⁱx_k-1 ⁱy_k-ⁱx_k ⁱy_k-1+ⁱy_k-1b₀-ⁱy_kb₀；x_k-1、y_k-1、x_k、y_k都是特征点的归一化到球面坐标系的图像坐标，即满足上标i表示特征关联的序号。

7.根据权利要求6所述的装置，其特征在于，求解用于数据关联中假设生成的内数据的过程包括：

为点关联i＝1:N-1,j＝i+1:N，构建形如上的模型并求解模型参数横摆角变化量^ijω及俯仰角变化量^ijγ；

以一定的分辨率和范围构建二维直方图Hist，用离散化^ijω的^ijγ和进行直方图投票；

选择票数最高的模型解{ω^*,γ^*}＝argmax{Hist}作为优胜解，其中，ω^*表示最优横摆角变化量，γ^*表示最优俯仰角变化量；

根据上述线性组合和辨识参数计算最优质心侧偏角β^*，并计算模型估计运动参数旋转矩阵和平移向量；

$R^m=\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]t^m=\left[\begin{array}{c}\sin {\mathrm{\β}}^{*}\\ 0\\ \cos {\mathrm{\β}}^{*}\end{array}\right]$

用模型估计R^m和t^m对特征关联进行运动基线三角化，得到无尺度的三维坐标估计并计算重投影误差；重投影误差大于设定阈值的，判定为外数据，剩下的即为内数据。

8.根据权利要求7所述的装置，其特征在于，所述最大似然估计器22具体用于，

对单目位姿估计来说，通过假设图像点测量误差满足高斯白噪声模型，采用林温伯格-麦考特(L-M)迭代优化的最大似然估计器来优化已知基础矩阵F，其中已知基础矩阵 $F\≡K_1^{-T}{\left[t\right]}_{\×}{\mathrm{RK}}_0^{-1};$

根据已有优胜解 和分别对应t^m和R^m，计算最大似然的步骤如下：

选择已知相机矩阵P_k-1＝[I|0]以及其中极点e_k是从初始估计中计算得到；

利用特征关联三角化获得对三维位置的估计

获得与初始估计一致的校正关联为

用L-M方法最小化代价函数

$\underset{i}{\mathrm{\Σ}}d{(\stackrel{\‾}{q}_{k-1}^i,\hat{q}_{k-1}^i)}^2+d{(\stackrel{\‾}{q}_k^i,\hat{q}_k^i)}^2$

获得最优解F^*，用已知的分解和判定方法获得最优模型估计旋转矩阵R^*和平移向量t^*。