CN104180818B - 一种单目视觉里程计算装置 - Google Patents
一种单目视觉里程计算装置 Download PDFInfo
- Publication number
- CN104180818B CN104180818B CN201410394767.7A CN201410394767A CN104180818B CN 104180818 B CN104180818 B CN 104180818B CN 201410394767 A CN201410394767 A CN 201410394767A CN 104180818 B CN104180818 B CN 104180818B
- Authority
- CN
- China
- Prior art keywords
- gftt
- feature
- image
- matrix
- yaw
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004364 calculation method Methods 0.000 title abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 28
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 53
- 238000000034 method Methods 0.000 claims description 26
- 239000013598 vector Substances 0.000 claims description 23
- 230000000007 visual effect Effects 0.000 claims description 22
- 230000033001 locomotion Effects 0.000 claims description 19
- 238000007476 Maximum Likelihood Methods 0.000 claims description 17
- 238000013519 translation Methods 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 15
- 230000009466 transformation Effects 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 230000003068 static effect Effects 0.000 abstract description 2
- 230000010354 integration Effects 0.000 abstract 1
- 230000008569 process Effects 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000000354 decomposition reaction Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000036039 immunity Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
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,根据内层计算得到的内数据以及运动参数的优胜解,对优胜解进行最大似然估计从而得到最终解。
进一步地,利用根据车辆动力学微分方程计算质心侧偏角,将其表达为横摆角速率,侧向加速度以及横摆角加速度的线性组合为:
其中l=lR+lF为车辆轴距,β表示质心侧偏角,lF,lR分别表示前、后轴中心到质心的距离;kR为后轴侧偏刚度,m为整车质量,Iy为车体绕竖直y轴的转动惯量;vlong代表纵向车速,ωr代表横摆角速率,代表横摆角加速度,alat表示侧向加速度。
进一步地,根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模的结果为
其中,是2×2矩阵,满足Ci=[ixk-1 iykb0-(iyk-1-iyk)b1+iyk iyk-1 iykb0-ixk+b0],c1表示c2表示b是2×1向量,满足b(i)=ixk-1 iyk-ixk iyk-1+iyk-1b0-iykb0;xk-1、yk-1、xk、yk都是特征点的归一化到球面坐标系的图像坐标,即满足上标i表示特征关联的序号。
进一步地,求解用于数据关联中假设生成的内数据的过程包括:
为点关联i=1:N-1,j=i+1:N,构建形如上的模型并求解模型参数横摆角变化量ijω及俯仰角变化量ijγ;
以一定的分辨率和范围构建二维直方图Hist,用离散化ijω的ijγ和进行直方图投票;
选择票数最高的模型解{ω*,γ*}=argmax{Hist}作为优胜解,其中,ω*表示最优横摆角变化量,γ*表示最优俯仰角变化量;
根据上述线性组合和辨识参数计算最优质心侧偏角β*,并计算模型估计运动参数旋转矩阵和平移向量;
用模型估计Rm和tm对特征关联进行运动基线三角化,得到无尺度的三维坐标估计并计算重投影误差;重投影误差大于设定阈值的,判定为外数据,剩下的即为内数据。
进一步地,所述最大似然估计器22具体用于,
对单目位姿估计来说,通过假设图像点测量误差满足高斯白噪声模型,采用林温伯格-麦考特(L-M)迭代优化的最大似然估计器来优化已知基础矩阵F,其中已知基础矩阵
根据已有优胜解 和分别对应tm和Rm,计算最大似然的步骤如下:
选择已知相机矩阵Pk-1=[I|0]以及其中极点ek是从初始估计中计算得到;
利用特征关联角化获得对三维位置的估计
获得与初始估计一致的校正关联为
用L-M方法最小化代价函数
获得最优解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的两个相机位置通过刚体坐标变换关联,满足如下形式:
其中Rk-1,k∈SO(3)是旋转矩阵,是平移向量。集合T0,…,M={T0,1,…,TM-1,M}包含了所有的总帧数为M的序列运动。最终,相机位姿的集合C0,…,M={C0,…,CM}包含了相机相对于初始时刻k=0时的变换。当前位姿CM可以通过累积所有的变换Tk-1,k(k=1,...,M)得到
C0是相机在时刻k=0的位姿。
视觉里程计的基础是两视角视觉几何学。对于三维点Q和它的图像二维投影点q,有3×4矩阵P满足上划线表示齐次表达,~表示这是一个定义直到尺度的等式,即与是共线的向量但不一定长度相等,该矩阵P称为相机矩阵。每一个视角下的相机矩阵P都可分解为P=K[R|t],式中K是相机内参数矩阵,而R和t是相机运动的旋转矩阵与平移向量。对于点Q及其在两个不同视角下的二维投影点q0与q1,令第一个视角是初始状态,即R0=I3,t0=0,其中I3为3×3单位矩阵。两个视角的相机矩阵分别为P0=K0[I3|0],P1=K1[R|t]。
令[t]×为由平移向量t的元素构成的螺旋对称矩阵,形如
得到基础矩阵
通过基础矩阵就可以表达共面性方程,也称对极约束方程
对于一个已经标定的相机,内参数矩阵K是已知且假设恒定的,即K0=K1=K,对分别左乘逆标定矩阵K-1,这一过程是对图像点归一化到单元球面的过程,得到
则(a.5)式的对极约束将变为
式中E=[t]×R称为本征矩阵。
一般的视觉里程计计算过程从特征点关联中求解E然后从中分解得到R与t。令E的奇异值分解为E=U diag(1,1,0)VT,则四个可能的因式化分解E=[t]×R形如:
(a) R=UWVT t=+U(3)
(b) R=UWVT t=-U(3)
(a.7)
(c) R=UWTVT t=+U(3)
(d) R=UWTVT t=-U(3)
其中为正交矩阵,U(3)代表矩阵U的第3行。
在实际的执行中,应该对所有图像点进行三角化重建,获得在两个视角下深度值t(3)都大于零的重建三维点最多的解为真实解。
上述位姿估计模块2具体包括:动力学模型预估单元21以及最大似然估计器22,其中,
动力学模型预估单元21,主要利用根据车辆动力学微分方程计算质心侧偏角,将其表达为横摆角速率,侧向加速度以及横摆角加速度的线性组合;根据上述线性组合对视觉里程相机模型的旋转矩阵与平移向量进行建模;利用离线辨识获得的参数以及建模得到的线性方程组,结合特征点关联集合中的二对特征关联对,求解用于数据关联中假设生成的内数据;
上述动力学模型预估单元21主要完成的计算过程如下:
(1)利用根据车辆动力学微分方程计算质心侧偏角β,将其表达为横摆角速率ωr,侧向加速度以及横摆角加速度的线性组合。
考虑车辆动力学微分方程
其中,lF,lR分别表示前、后轴中心到质心的距离,为可测量的车体已知几何参数;kF,kR分别为前、后轴侧偏刚度,m为整车质量,Iy为车体绕竖直y轴的转动惯量;vlong代表纵向车速,vlat代表横向车速,β为车体质心侧偏角,ωr代表横摆角速率。为了消去无法测量且不需要的前轮偏角值δ,用式(1)的上式乘以lF减去下式,得到
质心侧偏角β可以表达为横摆角速率ωr,侧向加速度(也即alat)以及横摆角加速度的线性组合,即
其中l=lR+lF为车辆轴距。
(2)据此对旋转矩阵R1与平移向量t1进行建模,其中
其中,ω表示横摆角变化量,β表示车体质心侧偏角。
建模过程如下:
对旋转矩阵的建模则只需对R1右乘一个俯仰变换矩阵Rγ,
由于俯仰角变化量γ较小,简化令sinγ≈γ,cosγ≈1,最终得到模型估计为
得到简化表达本征矩阵形如
根据对极约束有
xk-1yk(cosω-tanβsinω)-xkyk-1-yk-1ykγ(sinω+cosωtanβ) (7)
+yk-1tanβ+xkγ-yk(sinω+cosωtanβ)-γtanβ=0
式中xk-1、yk-1、xk、yk都是特征点的归一化到球面坐标系的图像坐标,即满足其中球面坐标系如图3所示,图3展示了球面坐标系的原理。对于标定透视相机,任何一点都可以映射到单元球面上。图3中图像点Q归一化到了以相机中心G为球心的单元球面上。这些向量称为图像点在单元球面上的归一化坐标。
如式(7)所示,方程(7)包含了复杂的三角函数因此难以直接计算ω。根据来自仿真数据和实车实验数据的观察,每100毫秒横摆角变化量ω和俯仰角变化量γ都较小,采用三角函数的泰勒展开对其进行近似,包括
其中,c1表示公式(3)中c2表示公式(3)中的dt是两帧图像之间采集的时间间隔,在视觉里程计***中是已知参数,一般为定值。
再令即有tanβ=b0+b1ω+b2ω2。
将泰勒近似带入方程(7),最终得到线性方程组
式中是2×2矩阵,满足Ci=[ixk-1 iykb0-(iyk-1-iyk)b1+iyk iyk-1 iykb0-ixk+b0]c1表示c2表示b是2×1向量,满足b(i)=ixk-1 iyk-ixk iyk-1+iyk-1b0-iykb0,且上标i表示特征关联的序号。
发明的算法中涉及到的参数包括整车质量m,后轮侧偏刚度kR以及车体绕竖直轴转动惯量Iy。实际应用中可以通过获取的真实车***姿数据来对它们进行离线辨识。定义最小化目标函数为模型输出与测量值之间的误差平方和,令表示在时刻k测量到的质心侧偏角,定义系数矩阵A满足,
从系数矩阵A中可以求解最小二乘意义上的解m/kR和Iy/kR。即对系数矩阵进行奇异值分解A=U·S·VT,则有m/kR=V(2,3)/V(1,3),Iy/kR=V(3,3)/V(1,3)。在实际的应用中,只需要这两个比值,而非m,kR和Iy的绝对数值。
(3)利用离线辨识获得的参数(前文提到的质量m,后轮侧偏刚度kR和绕竖直轴惯量Iy)以及推导得到的线性方程组(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)和辨识参数等计算质心侧偏角β*,并计算模型估计运动参数旋转矩阵和平移向量
(5)用模型估计Rm和tm对特征关联进行运动基线三角化,得到无尺度的三维坐标估计并计算重投影误差;重投影误差大于设定阈值的,判定为外数据,剩下的即为内数据。
最大似然估计器22,根据内层计算得到的内数据以及运动参数的优胜解,对优胜解进行最大似然估计从而得到最终解,其主要计算过程如下:
对单目位姿估计来说,通过假设图像点测量误差满足高斯白噪声模型,采用林温伯格-麦考特(L-M)迭代优化的最大似然估计器来优化计算基础矩阵F,其中基础矩阵
根据已有初始解计算最大似然值,该初始解就是根据内层估计的优胜解(票数最高的模型解推导得到的模型估计运动参数旋转矩阵和平移向量)式(11)计算的,K是标定参数据矩阵已知,和分别对应式(11)中的tm和Rm,主要计算步骤如下:
a.选择相机矩阵Pk-1=[I|0]以及其中极点ek是从初始估计中计算得到。
b.利用特征关联三角化获得对三维位置的估计
c.获得与初始估计一致的校正关联为
d.用L-M方法最小化代价函数
e.获得最优解F*,用前面(a.7)中描述的分解和判定方法获得最优模型估计旋转矩阵R*和平移向量t*。
综上所述,本发明实施例提供了一种单目视觉里程计算装置,提出了基于单轨动力学模型与合理线性近似的动力学预估横摆角和俯仰角分量的视觉里程计算法,称为MYP算法。通过充分考虑轮胎的侧偏特性、车速等重要的车辆状态信息,结合车辆单轨动力学模型,推导了质心侧偏角与横摆角变化量之间的关系,能够正确捕捉到车载相机运动的主分量,即横摆角变化量、质心侧偏角和俯仰角变化量。能够应对城市环境中各种不同类型的动态障碍物目标,实现精确的自主定位定向,能够满足智能车辆在复杂城市道路环境下的定位导航需求。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求书的保护范围为准。
Claims (5)
1.一种单目视觉里程计算装置,其特征在于,包括:
特征检测与关联模块1,用于在每一个时刻获取新的图像时,在图像中检测GFtT特征点,跟踪所述GFtT特征点在下一个时刻中图像的位置,从而获得一定时间内的GFtT特征点关联集合并输出给位姿估计模块;所述GFtT特征点是指易于跟踪的特征点;
位姿估计模块2,用于根据所述特征检测与关联模块输出的GFtT特征点关联集合,利用基于车辆动力学模型约束的位姿估计算法进行建模从而得到相邻时刻之间的相对位姿变换;
所述特征检测与关联模块1具体包括:GFtT特征点检测单元11和帧间LK跟踪单元12,其中,
GFtT特征点检测单元11,在每一个时刻获取新的图像时,在图像中检测GFtT特征点,并将检测到的GFtT特征点输出给帧间LK跟踪单元12;
帧间LK跟踪单元12,对于接收到的GFtT特征点,用LK跟踪方法跟踪其在下一个时刻中图像的位置,从而获得一定时间内的特征点关联集合并输出给位姿估计模块2;所述LK是指Lucas-Kanade,即卢卡斯·卡纳德。
补充检测单元13,对于检测到GFtT特征点的图像,将图像划分为多个栅格,对每一个栅格,若其中已有GFtT特征点的数量超过预定阈值,则在本栅格不再进行特征点检测;反之在本栅格中进行补充检测;
所述位姿估计模块2具体包括:
动力学模型预估单元21,利用根据车辆动力学微分方程计算质心侧偏角,将其表达为横摆角速率,侧向加速度以及横摆角加速度的线性组合;根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模;用离线辨识获得的参数以及建模得到的结果,结合特征点关联集合中的二对特征关联对,求解用于数据关联中假设生成的内数据;所述离线辨识获得的参数是指整车质量m,后轮侧偏刚度kR和绕竖直轴惯量Iy;
最大似然估计器22,根据已有初始解计算最大似然值,该初始解就是根据内层估计的优胜解,对优胜解进行最大似然估计从而得到最终解;其中,K是标定参数据矩阵已知,和分别对应所述旋转矩阵与平移向量。
2.根据权利要求1所述的装置,其特征在于,利用根据车辆动力学微分方程计算质心侧偏角,将其表达为横摆角速率,侧向加速度以及横摆角加速度的线性组合为:
其中l=lR+lF为车辆轴距,β表示质心侧偏角,lF,lR分别表示前、后轴中心到质心的距离;kR为后轴侧偏刚度,m为整车质量,Iy为车体绕竖直y轴的转动惯量;vlong代表纵向车速,ωr代表横摆角速率,代表横摆角加速度,alat表示侧向加速度,ω表示横摆角变化量。
3.根据权利要求2所述的装置,其特征在于,根据上述线性组合对视觉里程计相机模型的旋转矩阵与平移向量进行建模的结果为
1
其中,是2×2矩阵,满足c1表示c2表示b是2×1向量,满足xk-1、yk-1、xk、yk都是特征点的归一化到球面坐标系的图像坐标,即满足上标i表示特征关联的序号。
4.根据权利要求3所述的装置,其特征在于,求解用于数据关联中假设生成的内数据的过程包括:
为点关联i=1:N-1,j=i+1:N,构建形如公式的模型并求解模型参数横摆角变化量ijω及俯仰角变化量ijγ;
以一定的分辨率和范围构建二维直方图Hist,用离散化的ijω和ijγ进行直方图投票;
选择票数最高的模型解{ω*,γ*}=argmax{Hist}作为优胜解,其中,ω*表示最优横摆角变化量,γ*表示最优俯仰角变化量;
根据上述线性组合和辨识参数计算最优质心侧偏角β*,并计算模型估计运动参数旋转矩阵和平移向量;
用模型估计Rm和tm对特征关联进行运动基线三角化,得到无尺度的三维坐标估计并计算重投影误差;重投影误差大于设定阈值的,判定为外数据,剩下的即为内数据。
5.根据权利要求4所述的装置,其特征在于,所述最大似然估计器22具体用于,
对单目位姿估计来说,通过假设图像点测量误差满足高斯白噪声模型,采用林温伯格-麦考特迭代优化的最大似然估计器来优化已知基础矩阵F,其中已知基础矩阵
根据已有优胜解K是标定参数据矩阵,和分别对应tm和Rm,计算最大似然的步骤如下:
选择已知相机矩阵Pk-1=[I|0]以及其中极点ek是从初始估计中计算得到;
利用特征关联和获得对三维位置的估计
获得与初始估计一致的校正关联为
用林温伯格-麦考特迭代最小化代价函数
获得最优解F*,根据最优解F*进行最大似然估计,得到旋转矩阵R*和平移向量t*。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410394767.7A CN104180818B (zh) | 2014-08-12 | 2014-08-12 | 一种单目视觉里程计算装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410394767.7A CN104180818B (zh) | 2014-08-12 | 2014-08-12 | 一种单目视觉里程计算装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104180818A CN104180818A (zh) | 2014-12-03 |
CN104180818B true CN104180818B (zh) | 2017-08-11 |
Family
ID=51962052
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410394767.7A Active CN104180818B (zh) | 2014-08-12 | 2014-08-12 | 一种单目视觉里程计算装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104180818B (zh) |
Families Citing this family (19)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105374043B (zh) * | 2015-12-02 | 2017-04-05 | 福州华鹰重工机械有限公司 | 视觉里程计背景过滤方法及装置 |
CN105973265B (zh) * | 2016-05-19 | 2019-03-19 | 杭州申昊科技股份有限公司 | 一种基于激光扫描传感器的里程估计方法 |
CN105976402A (zh) * | 2016-05-26 | 2016-09-28 | 同济大学 | 一种单目视觉里程计的真实尺度获取方法 |
CN107248171B (zh) * | 2017-05-17 | 2020-07-28 | 同济大学 | 一种基于三角剖分的单目视觉里程计尺度恢复方法 |
CN107229063A (zh) * | 2017-06-26 | 2017-10-03 | 奇瑞汽车股份有限公司 | 一种基于gnss和视觉里程计融合的无人驾驶汽车导航定位精度矫正方法 |
CN109238281B (zh) * | 2017-07-10 | 2020-09-29 | 南京原觉信息科技有限公司 | 基于图像螺旋线的视觉导航及避障方法 |
CN107907131B (zh) * | 2017-11-10 | 2019-12-13 | 珊口(上海)智能科技有限公司 | 定位***、方法及所适用的机器人 |
US10436590B2 (en) | 2017-11-10 | 2019-10-08 | Ankobot (Shanghai) Smart Technologies Co., Ltd. | Localization system and method, and robot using the same |
CN108151713A (zh) * | 2017-12-13 | 2018-06-12 | 南京航空航天大学 | 一种单目vo快速位姿估计方法 |
WO2020030272A1 (en) * | 2018-08-09 | 2020-02-13 | Abb Schweiz Ag | Method for estimating a wrench |
CN109887029A (zh) * | 2019-01-17 | 2019-06-14 | 江苏大学 | 一种基于图像颜色特征的单目视觉里程测量方法 |
CN110017817B (zh) * | 2019-01-24 | 2021-09-14 | 中国煤炭科工集团太原研究院有限公司 | 一种基于顶板特征的煤矿巷道导航定位方法和装置 |
CN110412596A (zh) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | 一种基于图像信息和激光点云的机器人定位方法 |
DE102019210670A1 (de) * | 2019-07-18 | 2021-01-21 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Betreiben eines Bremssystems, Computerprogramm und Computerprogramm-Produkt, Bremssystem |
CN112154479A (zh) * | 2019-09-29 | 2020-12-29 | 深圳市大疆创新科技有限公司 | 提取特征点的方法、可移动平台及存储介质 |
CN110766024B (zh) * | 2019-10-08 | 2023-05-23 | 湖北工业大学 | 基于深度学习的视觉里程计特征点提取方法及视觉里程计 |
CN111950370B (zh) * | 2020-07-10 | 2022-08-26 | 重庆邮电大学 | 动态环境下线拓展视觉里程计方法 |
CN114047358A (zh) | 2021-11-15 | 2022-02-15 | 中国计量科学研究院 | 一种基于单目视觉的线角振动校准方法 |
CN115222961B (zh) * | 2022-09-19 | 2022-11-25 | 成都信息工程大学 | 一种用于影像基础矩阵不确定性的评估方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101067557A (zh) * | 2007-07-03 | 2007-11-07 | 北京控制工程研究所 | 适用于自主移动车辆的环境感知的单目视觉导航方法 |
CN101441769A (zh) * | 2008-12-11 | 2009-05-27 | 上海交通大学 | 单目摄像机实时视觉定位方法 |
CN101456418A (zh) * | 2007-12-12 | 2009-06-17 | 现代自动车株式会社 | 用于控制车辆的侧向稳定性的方法和装置 |
CN101681168A (zh) * | 2007-05-31 | 2010-03-24 | 迪尔公司 | 利用基于视觉的调整引导车辆的方法和*** |
CN101839721A (zh) * | 2010-03-12 | 2010-09-22 | 西安电子科技大学 | 自主交会对接中的视觉导航方法 |
CN102221358A (zh) * | 2011-03-23 | 2011-10-19 | 中国人民解放军国防科学技术大学 | 基于逆透视投影变换的单目视觉定位方法 |
CN102435188A (zh) * | 2011-09-15 | 2012-05-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
CN102577349A (zh) * | 2009-07-07 | 2012-07-11 | 天宝导航有限公司 | 基于图像的表面追踪 |
WO2013029020A1 (en) * | 2011-08-25 | 2013-02-28 | James Chia-Ming Liu | Portals: registered objects as virtualized, personalized displays |
-
2014
- 2014-08-12 CN CN201410394767.7A patent/CN104180818B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101681168A (zh) * | 2007-05-31 | 2010-03-24 | 迪尔公司 | 利用基于视觉的调整引导车辆的方法和*** |
CN101067557A (zh) * | 2007-07-03 | 2007-11-07 | 北京控制工程研究所 | 适用于自主移动车辆的环境感知的单目视觉导航方法 |
CN101456418A (zh) * | 2007-12-12 | 2009-06-17 | 现代自动车株式会社 | 用于控制车辆的侧向稳定性的方法和装置 |
CN101441769A (zh) * | 2008-12-11 | 2009-05-27 | 上海交通大学 | 单目摄像机实时视觉定位方法 |
CN102577349A (zh) * | 2009-07-07 | 2012-07-11 | 天宝导航有限公司 | 基于图像的表面追踪 |
CN101839721A (zh) * | 2010-03-12 | 2010-09-22 | 西安电子科技大学 | 自主交会对接中的视觉导航方法 |
CN102221358A (zh) * | 2011-03-23 | 2011-10-19 | 中国人民解放军国防科学技术大学 | 基于逆透视投影变换的单目视觉定位方法 |
WO2013029020A1 (en) * | 2011-08-25 | 2013-02-28 | James Chia-Ming Liu | Portals: registered objects as virtualized, personalized displays |
CN102435188A (zh) * | 2011-09-15 | 2012-05-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
Non-Patent Citations (5)
Title |
---|
《Design of a road vehicle detection system based on monocular vision》;Wang Hai等;《Journal of Southeast University(English Edition)》;20110630;第27卷(第2期);169-173 * |
《基于单目视觉的前方车辆探测方法》;施树明等;《农业机械学报》;20040731;第35卷(第4期);5-8 * |
《基于单目视觉的智能车辆导航算法研究》;王庆军;《山西电子技术》;20130430(第2期);87-88,94 * |
《基于运动微分约束的无人车辆纵横向协同规划算法的研究》;姜岩等;《自动化学报》;20131231;第39卷(第12期);2013-2019,2020 * |
《智能车辆视觉里程计算法研究进展》;江燕华等;《兵工学报》;20120228;第33卷(第2期);214-220 * |
Also Published As
Publication number | Publication date |
---|---|
CN104180818A (zh) | 2014-12-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104180818B (zh) | 一种单目视觉里程计算装置 | |
CN112083725B (zh) | 一种面向自动驾驶车辆的结构共用多传感器融合定位*** | |
CN108051002B (zh) | 基于惯性测量辅助视觉的运输车空间定位方法及*** | |
Scaramuzza et al. | Absolute scale in structure from motion from a single vehicle mounted camera by exploiting nonholonomic constraints | |
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
JP5991952B2 (ja) | 周囲状況に対するカメラの姿勢を求めるための方法 | |
CN111263960B (zh) | 用于更新高清晰度地图的设备和方法 | |
CN113819914A (zh) | 一种地图构建方法及装置 | |
JP2018506768A (ja) | サラウンドビュー画像から自動車のエゴモーションを推定するための方法および装置 | |
CN112669354B (zh) | 一种基于车辆非完整约束的多相机运动状态估计方法 | |
CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
CN111862673A (zh) | 基于顶视图的停车场车辆自定位及地图构建方法 | |
Lagisetty et al. | Object detection and obstacle avoidance for mobile robot using stereo camera | |
CN112258409A (zh) | 一种用于无人驾驶的单目相机绝对尺度恢复方法及装置 | |
JP7173471B2 (ja) | 3次元位置推定装置及びプログラム | |
CN114234967B (zh) | 一种基于多传感器融合的六足机器人定位方法 | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
Parra-Tsunekawa et al. | A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles | |
CN112862818B (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
CN111260725B (zh) | 一种面向动态环境的轮速计辅助的视觉里程计方法 | |
CN113450411B (zh) | 一种基于方差分量估计理论的实时自身位姿计算方法 | |
CN111145267A (zh) | 基于imu辅助的360度全景视图多相机标定方法 | |
Zhu et al. | Monocular visual odometry and dense 3d reconstruction for on-road vehicles | |
Cai et al. | A vision-based road surface slope estimation algorithm for mobile service robots in indoor environments | |
Mihalyi et al. | Uncertainty estimation of ar-marker poses for graph-slam optimization in 3d object model generation with rgbd data |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |