一种基于单目视觉和惯性融合的车辆场景动态性分析方法与流程

1.本发明涉及计算机视觉技术以及数据处理和信息融合技术，具体是一种基于单目视觉惯性融合的车辆场景动态性分析方法。


背景技术：

2.近年来，移动机器人和计算机视觉技术大量应用在无人车辅助驾驶系统中。固定在无人车内的单目相机采集的前方区域含有丰富的车辆场景信息，能够辅助导航系统感知周围的环境和载体自身所处的位置。以特斯拉自动驾驶系统为例，系统能够通过单目视觉判断前方车辆距离载体自身的距离，并以此为依据估计自身的轨迹并规划出合适的路径。然而现有的视觉感知导航系统存在如下两个问题：1)在单目相机的视野区域内，如果载体目标占据图像的大量区域，并且大多数匹配点对集中在前方移动车辆区域的时候，单目视觉系统难以区分图像内的静态特征点和动态特征点，进而无法做出载体运动的准确估计；2)现有的单目视觉导航系统没有感知周围车辆的动态性功能，致使系统做出错误的操作，比如变向、并道、超车从而造成追尾、撞车等交通事故。
3.与单目视觉系统的外视野感知类型不同，惯性测量单元(inertialmeasurement unit,imu)属于内置传感器，能够采集载体自身的运动信息，比如加速度、角速度，通过对采集量的积分来估计载体的短时运动信息，能够弥补单目视觉系统无法有效区分图像静态特征点和动态特征点的不足。单目视觉和 imu的有效融合能够准确感知视野内的静态场景和移动车辆，并能够准确的估计出载体当前的位姿状态，提高了无人车车载导航系统的可靠性和安全性。


技术实现要素：

4.针对现有无人车单目视觉导航系统无法有效区分视野内的静态场景和动态场景，本发明提出一种基于单目视觉惯性融合的车辆场景动态性分析方法。该方法充分利用imu的方向估计信息，辅助视觉系统区分图像间的静态特征点对。同时采用可变形部件检测模型(deformable part based model,dpm)来确定车辆区域内的动态特征点对。在此基础上，利用静态特征点对和imu的测量值，采用目标函数优化的方法实现载体自身的姿态估计；同时利用动态特征点对来估计车辆场景的动态性。本方法提高了导航系统对车辆场景动态性分析的准确性和鲁棒性，可用于无人车的视觉导航系统中。
5.本发明目的是提供一种基于单目视觉惯性融合的车辆场景动态性分析方法。
6.本发明为实现上述目的所采用的技术方案是：
7.一种基于单目视觉和惯性融合的车辆场景动态性分析方法，包括以下步骤：
8.基于增益自适应互补滤波器的imu方向估计，得到imu的方向信息
9.基于orb的特征点检测，根据图像间特征点描述子的相似度，得到相邻图像间特征点匹配对，进而得到载体的姿态角直方图；
10.基于dpm的图像车辆区域检测，确定图像内的机动车区域；
11.基于相对熵的imu辅助视觉静态特征点提取：利用imu的方向信息以及特征点描述子之间的特征点匹配对提取静态特征点；
12.基于图像静态特征点和惯性测量值的载体自主运动估计，得到载体位姿；
13.基于姿态角直方图投票的动态特征点提取；
14.基于动态特征点集的场景动态性分析，得到图像中机动车辆数目、机动车区域所占图像的比例和图像中动态特征点所占的比例。
15.基于增益自适应互补滤波器的imu方向估计，包含以下步骤：
16.1.1)计算陀螺仪的方向估计，
17.1.1.1)求解t δt时刻描述imu方向的四元数的变化率
[0018][0019]
其中，四元数中的q1、q2、q3分量分别对应旋转轴在x、y、z方向的投影， q0分量对应imu围绕旋转轴旋转的角度；符号表示四元数乘积，表示t时刻imu陀螺仪的四元数估计值，ω
t δt
表示t δt时刻陀螺仪的输出值。
[0020]
1.1.2)求解t δt时刻四元数的估计值
[0021][0022]
1.1.3)归一化处理四元数估计值，得出t δt时刻由陀螺仪估计出的四元数值
[0023][0024]
1.2)计算加速计与磁力计的方向估计值首先确定初始时刻的加速度值z
α
和磁场强度值z
m
，再根据t δt时刻加速计和磁力计的输出值α(t δt)和m(t δt)，以最小化估计值误差为目标，建立目标函数：
[0025][0026]
其中，和分别表示t δt时刻加速计和磁力计估计值的误差，计算过程为：
[0027][0028][0029]
其中，为t δt时刻由加速计与磁力计解算的方向估计值，δt为imu 采样时间间隔；
[0030]
1.3)基于互补滤波算法的方向估计，联合t δt时刻陀螺仪的方向估计值以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法求解imu坐标系下imu的方向信息
[0031][0032]
其中，k
t δt
表示t δt时刻自适应的增益系数，表示t δt时刻的方向估计信息；
[0033]
1.4)imu姿态估计，根据imu估计出的四元数得到载体的旋转角度θ
imu
：
[0034][0035]
所述基于orb的特征点检测，并进行相邻图像间的特征点匹配包括以下步骤：
[0036]
2.1)对于相邻图像中的每一帧，基于orb的特征点检测，得到每一帧图像的特征点，并计算对应的特征点描述子；
[0037]
2.2)相邻图像间的特征点匹配
[0038]
通过计算相邻图像特征点描述子之间的hamming距离实现相邻帧间图像间特征点的匹配，得到匹配的特征点对。
[0039]
所述基于相对熵的imu辅助视觉静态特征点提取包括以下步骤：
[0040]
3.1)建立基于对极几何和非完整性约束的统计直方图；
[0041]
根据步骤2.2)，得到相邻图像间一系列匹配的特征点对 {(p1,p
′1),(p2,p
′2),
…
(p
n
,p
′
n
)}，n为匹配点对数，p
n
、p
′
n
分别表示图像i1,i2间的匹配点对，描述载体位姿的旋转矩阵r和平移向量t用旋转角度θ来表示：
[0042][0043]
根据计算机视觉的两视图对极几何关系，给定匹配点对(p,p
′
)，可得到匹配点对的外极线约束模型：
[0044]
p
′
[t]
×
rp＝0
[0045]
其中p＝(p
x
，p
y
，p
z
)和p
′
＝(p
′
x
,p
′
y
,p
′
z
)分别表示世界坐标系中的点p，在相机坐标系下的归一化坐标；
[0046]
根据(p,p
′
)进而得到载体运动的角度θ：
[0047][0048]
相邻帧图像间特征点匹配对集合{(p1,p
′1),(p2,p
′2),
…
(p
n
,p
′
n
)}转化为与之对应的姿态角集合{θ1,θ2,
…
,θ
n
}，进而构建出关于θ的统计直方图。
[0049]
3.2)基于相对熵的静态特征点提取：
[0050]
根据步骤1.4)，使用均值为μ
imu
,方差为σ
imu2
的高斯分布n(μ
imu
,σ
imu2
)来描述随机变量θ
imu
；根据步骤3.1)，得到载体姿态角θ的统计直方图，用混合高斯模型来描述，与imu估计值一致的视觉静态特征点集合用相对熵模型来描述：
[0051][0052]
其中，p
imu
表示随机变量θ
imu
的概率密度函数；p
j
表示关于载体姿态角的第 j组高斯混合模型的概率密度函数，d
re
(
·
)表示度量随机变量p
imu
和p
j
的相对熵函数；
[0053]
与imu估计值一致的姿态角集合所对应的高斯分布模型为：
[0054][0055]
根据θ
*
可得到相邻图像间的静态特征点集合:
[0056]
{(p1,p
′1),(p2,p
′2),
…
(p
m
,p
′
m
)}。
[0057]
所述基于图像静态特征点集进行惯性测量值的载体自主运动估计包括以下步骤：
[0058]
5.1)定义待优化的状态变量
[0059]
待优化的变量包括位置向量旋转矩阵速度向量加速度偏差和角速度偏差其中小标w表示世界坐标系；小标b表示基坐标系；小标 t表示时间；
[0060]
5.2)待优化的状态变量可通过最小化如下的目标函数的得到：
[0061][0062]
其中，e
c
(i1,i2)表示静态特征点在相邻图像i1和i2间的重投影误差；e
i
(i1,i2) 表示imu的惯性误差项；s
c
和s
i
分别表示各误差项的权重系数；
[0063]
关于静态特征点间的重投影误差项定义如下：
[0064][0065]
其中和分别表示第j个静态匹配点对在图像i1和i2中的位置(单位是像素)，π(
·
)表示图像内部的重投影函数，表示关于第j个静态匹配点的信息矩阵；
[0066]
imu的惯性误差项e
i
(i1,i2)定义如下：
[0067][0068]
其中和分别表示在[i1,i2]时间间隔内，惯性测量值的位置误差项、速度误差项和旋转误差项；和分别表示在[i1,i2]时间间隔内惯性测量值的加速度偏差项和角速度偏差项。分别表示关于位移、速度、旋转、加速度偏差和角速度变差的信息矩阵；上小标t表示矩阵的偏置。
[0069]
各项偏差模型定义如下：
[0070][0071][0072][0073][0074][0075]
其中，常量g表示重力加速度，δt表示图像[i1,i2]的时间间隔；和分别表示在时刻i1处，惯性坐标系相对于世界坐标系的旋转矩阵和平移向量；和分别表示在时刻i2处，惯性坐标系相对于世界坐标系的旋转矩阵和平移向量；和分别表示世界坐标系下，惯性系统在i2和i1时刻的速度；和分别表示时间间隔[i1,i2]内系统的期望位移量、期望速度偏差和期望旋转量，和分别表示在i1时刻惯性测量值的加速度偏差项和角速度偏差项；和分别表示在i2时刻惯性测量值的加速度偏差项和角速度偏差项；表示旋转矩阵的转置矩阵。
[0076]
所述基于直方图投票的动态特征点提取包括以下步骤：
[0077]
6.1)基于区域相关性的相邻图像间车辆区域匹配
[0078]
6.1.1)计算图像i1的车辆区域质心位置和对应的区域面积同理，计算图像i2的车辆区域质心位置和对应的区域面积
[0079]
6.1.2)根据车辆区域的质心间近邻约束和区域面积的相关性来完成图像i1和i2的车辆区域匹配，匹配模型如下：
[0080][0081]
其中，w1表示质心间近邻约束部分的权重系数；w2表示区域面积相关性的权重系数，a和b分别表示图像i1和i2中待匹配的车辆区域序号；
[0082]
6.2)匹配区域内的动态特征点提取；
[0083]
6.2.1)构建车辆区域内的姿态角直方图；
[0084]
6.2.2)使用直方图投票方法提取出动态特征点集合。
[0085]
所述基于动态特征点集的场景动态性分析包括以下步骤：
[0086]
7.1)估计车辆区域占据整幅图像的比例
[0087]
第i1帧图像存在k块车辆场景区域，分别是{r1,r2,
…
,r
k
}，而其车辆区域的面积分别为则在i1帧图像中车辆区域占据整幅图像的比例为：
[0088][0089]
其中area(i1)表示i1图像的面积，单位是像素；
[0090]
7.2)估计动态特征点匹配对与整幅图像间特征点匹配对的比例
[0091]
假定第i1帧图像中车辆区域内的动态特征点数量分别是而总共的特征点数目为n(i1)，则在i1帧图像中动态特征点匹配对与整个特征点匹配对的比例为：
[0092][0093]
本发明具有以下有益效果及优点：
[0094]
1.本发明方法利用imu的方向估计信息，结合着载体的非完整性约束模型，来提取图像间的静态特征点集合，用来估计载体自身的运动。
[0095]
2.本发明利用可变形部件检测模型，来检测图像中的车辆区域，同时利用区域内的质心位置和车辆区域面积，来匹配图像帧间的车辆区域，来实现前方车辆的动态性分析。
附图说明
[0096]
图1为发明方法原理图。
具体实施方式
[0097]
下面结合附图及实例对本发明做进一步的详细说明。
[0098]
本发明主要分为三部分，图1所示为本发明方法原理图，具体实现过程如下所述。
[0099]
步骤1：基于增益自适应互补滤波器的imu方向估计。
[0100]
imu包含三个主要的传感器：三轴陀螺仪、三轴加速计和三轴磁力计。imu 的姿态估计包括三类传感器的方向估计，并对其估计值进行融合。
[0101]
步骤1.1：计算陀螺仪的方向估计。
[0102]
步骤1.1.1：求解t δt时刻描述imu方向的四元数求解t δt时刻描述imu方向的四元数的变化率(也称导数)公式如下：
[0103][0104]
其中，四元数中的q1、q2、q3分量分别对应旋转轴在x、y、z方向的投影， q0分量对应imu围绕旋转轴旋转的角度；符号表示四元数乘积，表示t时刻imu陀螺仪的四
元数估计值，ω
t δt
表示t δt时刻陀螺仪的输出值。
[0105]
步骤1.1.2：求解t δt时刻四元数的估计值
[0106][0107]
步骤1.1.3：归一化处理四元数估计值，得出t δt时刻由陀螺仪估计出的四元数值
[0108][0109]
步骤1.2：计算加速计与磁力计的方向估计。
[0110]
通过高斯牛顿的迭代优化算法计算t δt时刻加速计与磁力计的方向估计值首先确定初始时刻的加速度值z
α
和磁场强度值z
m
，再根据t时刻加速计和磁力计的输出值α(t δt)和m(t δt)，以最小化估计值误差为目标，建立目标函数：
[0111][0112]
其中，和分别表示由加速计和磁力计估计值的误差，计算过程为：
[0113][0114][0115]
步骤1.3：基于互补滤波算法的方向估计。
[0116]
联合t δt时刻陀螺仪的方向估计值和以及加速计和磁力计的方向估计值采用增益自适应互补滤波算法求解imu坐标系下imu的方向信息
[0117][0118]
其中k
t
表示自适应的增益系数，表示t时刻的方向估计信息。
[0119]
步骤1.4:imu姿态估计。
[0120]
根据imu估计出的四元数得到载体的旋转角度θ
imu
：
[0121][0122]
步骤2：基于相对熵的imu辅助视觉静态特征点提取
[0123]
步骤2.1：基于orb的特征点检测
[0124]
步骤2.2：相邻图像间的特征点匹配
[0125]
通过计算特征点描述子之间的欧氏距离实现相邻帧间特征点的匹配，如果与的比值小于预定义的阈值δ
d
，即则接受这一对匹配点；其中，对于图像中的某个特征点p，其特征点描述子记为d
p
＝[x1,x2,
…
x
32
]
t
；待匹配图像中与p点对应的第一近邻点和第二近邻点分别记为p1和p2；所述第一近邻点和第二近邻点分别指待匹配
图像的特征点中，与p点描述子汉明距离最近的描述子的特征点和次近的描述子的特征点；p1和p2的特征点描述子分别记为p点与p1和p2的特征点描述子之间的汉明距离和可表示为分别表示为：
[0126][0127][0128]
步骤2.3：建立基于对极几何和非完整性约束的统计直方图
[0129]
根据步骤2.2，可得到相邻图像间一系列特征点匹配对 {(p1,p
′1),(p2,p
′2),
…
(p
n
,p
′
n
)}，其中(p
j
,p
′
j
)表示第j(1≤j≤n)组匹配点对，下小标n表示匹配点对的个数。利用地面载体的非完整性约束，在不考虑平移尺度因素的前提下，描述载体位姿的旋转矩阵r和平移向量t可用旋转角度θ来表示：
[0130][0131]
根据计算机视觉的两视图对极几何关系，给定匹配点对(p,p
′
)，可得到匹配点对的外极线约束模型：
[0132]
p
′
[t]
×
rp＝0
[0133]
其中p＝(p
x
，p
y
，p
z
)和p
′
＝(p
′
x
,p
′
y
,p
′
z
)分别表示物理世界中点p，在相机坐标系下的归一化坐标。
[0134]
利用对极几何约束模型和地面载体的非完整性约束模型，可估计出描述载体运动的角度θ：
[0135][0136]
图像相邻帧间特征点匹配对集合{(p1,p
′1),(p2,p
′2),
…
(p
n
,p
′
n
)}可转化为与之对应的姿态角集合{θ1,θ2,
…
,θ
n
}，进而可构建出关于θ的统计直方图。
[0137]
步骤2.4：基于相对熵的静态特征点提取：
[0138]
根据步骤1.4，根据imu的姿态估计角期望值θ
imu
和姿态估计角方差σ
imu2
，可得到关于imu姿态估计角的随机变量θ
i
的高斯分布模型n(θ
imu
,σ
imu2
)。根据步骤2.3，由视觉获得的载体姿态角θ
c
的统计直方图，可用混合高斯模型来描述，与imu估计值一致的视觉静态特征点集合可用相对熵模型来描述：
[0139][0140]
其中，p
imu
表示随机变量θ
i
的概率密度函数；随机变量p
j
表示关于载体姿态角的第j组高斯混合模型的概率密度函数；d
re
(
·
)表示度量随机变量p
imu
和p
j
的相对熵函数。
[0141]
与imu估计值一致的姿态角集合所对应的高斯分布模型为：
[0142][0143]
其中下小标m表示混合高斯模型的数目。
[0144]
根据θ
*
可得到相邻图像间的静态特征点集合(inlier group): {(p1,p
′1),(p2,p
′2),
…
(p
m
,p
′
m
)}
[0145]
步骤3：基于dpm的图像车辆区域检测
[0146]
步骤3.1：将待检测的车辆目标抽象成车辆特征描述子
[0147]
步骤3.1.1：将样本中车辆区域分别归类成前视图、侧视图和后视图三类数据集；
[0148]
步骤3.1.2：将待检测的车辆目标描述成一个由根滤波器和一系列的部件滤波器f＝{f1,f2,
…
,f
n
}生成的车辆特征描述子；
[0149]
步骤3.1.3：针对车辆区域的数据集，分别提取根滤波器的梯度直方图 {r
f
,r
s
,r
r
}作为根滤波器的特征描述子，其中下标{f,s,r}分别代表车辆的前视图、左视图和后视图；
[0150]
步骤3.1.4：针对车辆区域的数据集，分别提取前视图、侧视图和后视图三个视角下的部件滤波器梯度直方图f
f
＝{f
f1
,f
f2
,
…
,f
fn
}， f
x
＝{f
s1
,f
s2
,
…
,f
sn
}，f
r
＝{f
r1
,f
r2
,
…
,f
rn
}作为部件滤波器的特征描述子；
[0151]
步骤3.1.5：根据根滤波器的特征描述子和部件滤波器的特征描述子，构造待检测的目标函数
[0152][0153]
其中和r
j
分别表示第j个视角下的根滤波器梯度直方图特征和对应的权重系数；和f
ji
分别表示第j个视角下第i个部件的滤波器梯度直方图特征和对应的权重系数；d
ji
表示j视角下第i个部件模板相对于根模板的几何形变系数；表示j视角下第i个部件的几何形变特征。
[0154]
步骤3.2：利用svm(支持向量机)生成车辆检测器
[0155]
步骤3.2.1：正则化训练样本集的尺寸；
[0156]
步骤3.2.2：初始化根滤波器和部件滤波器的特征描述子的滑动窗口大小，块大小，直方图的bin数目；
[0157]
步骤3.2.3：将正负例的特征描述子样本送入到svm分类器中，训练分类器的模型参数。
[0158]
步骤4：基于直方图投票的动态特征点提取
[0159]
步骤4.1：基于区域相关性的相邻图像间车辆区域匹配
[0160]
步骤4.1.1：计算图像i1的车辆区域质心位置和对应的区域面积同理，计算图像i2的车辆区域质心位置和对应
的区域面积
[0161]
步骤4.1.2：根据车辆区域的质心间近邻约束和区域面积的相关性来完成图像i1和i2的车辆区域匹配，匹配模型如下：
[0162][0163]
其中w1表示质心间近邻约束部分的权重系数；w2表示区域面积相关性的权重系数。
[0164]
步骤4.2：匹配区域内的动态特征点提取
[0165]
步骤4.2.1：构建车辆区域内的姿态角直方图；
[0166]
步骤4.2.2：使用直方图投票方法提取出动态特征点集合。
[0167]
步骤5：基于静态特征点和imu紧耦合的载体自主运动估计
[0168]
步骤5.1：定义待优化的状态变量
[0169]
待优化的变量包括位置向量旋转矩阵速度向量加速度偏差和角速度偏差其中小标w表示世界坐标系；小标b表示基坐标系；小标 t表示时间。
[0170]
待优化的状态变量可通过最小化如下的目标函数来实现：
[0171][0172]
其中，e
c
(i1,i2)表示静态特征点在图像i1和i2间的重投影误差；e
i
(i1,i2)表示 imu的惯性误差项；s
c
和s
i
分别表示各误差项的权重系数。
[0173]
关于静态特征点间的重投影误差项定义如下：
[0174][0175]
其中和分别表示第j个静态匹配点对在图像i1和i2中的位置(单位是像素)，π(
·
)表示图像内部的重投影函数表示关于第j个静态匹配点的信息矩阵。
[0176]
imu的惯性误差项e
i
(i1,i2)定义如下：
[0177][0178]
其中和分别表示在[i1,i2]时间间隔内，惯性测量值的位置误差项、速度误差项和旋转误差项；和分别表示在[i1,i2]时间间隔内惯性测量值的加速度偏差项和角速度偏差项。分别表示关于位
移、速度、旋转、加速度偏差和角速度变差的信息矩阵。
[0179]
各项偏差模型定义如下：
[0180][0181][0182][0183][0184][0185]
其中，常量g表示重力加速度，δt表示图像[i1,i2]的时间间隔；和分别表示在时刻i1处，惯性坐标系相对于世界坐标系的旋转矩阵和平移向量；和分别表示在时刻i2处，惯性坐标系相对于世界坐标系的旋转矩阵和平移向量；和分别表示世界坐标系下，惯性系统在i2和i1时刻的速度；和分别表示时间间隔[i1,i2]内系统的期望位移量、期望速度偏差和期望旋转量，和分别表示在i1时刻惯性测量值的加速度偏差项和角速度偏差项；和分别表示在i2时刻惯性测量值的加速度偏差项和角速度偏差项，表示旋转矩阵的转置矩阵。
[0186]
步骤6：基于动态特征点的场景动态性分析
[0187]
步骤6.1：估计车辆区域占据整幅图像的比例
[0188]
假定第i1帧图像存在k块车辆场景区域，分别是{r1,r2,
…
,r
k
}，而其车辆区域的面积分别为则在i1帧图像中车辆区域占据整幅图像的比例为：
[0189][0190]
其中area(i1)表示i1图像的面积，单位是像素。
[0191]
步骤6.2：估计动态特征点匹配对与整个特征点匹配对的比例
[0192]
本专利中，动态特征点对代表图像中车辆区域的匹配点对。假定第i1帧图像车辆区域内的动态特征点数量分别是而总共的特征点数目为 n(i1)，则在i1帧图像中动态特征点匹配对与整个特征点匹配对的比例为：
[0193]
[0194]
图像中动态特征点匹配对与整个特征点匹配对的比例用于分析图像中对应机动车辆的相对运动情况，用于估计前方场景中机动车的相对位置和姿态。
[0195]
根据车载相机采集的图像分析出图像中前方机动车的数目、车辆区域所占整幅图像的比例、动态特征点等，用于场景动态性分析。