阅读说明：本技术 一种基于双目序列图像的车辆位置与速度估计方法 (Vehicle position and speed estimation method based on binocular sequence images ) 是由 钟赫冉 耿可可 殷国栋 刘能 范世硕 于 2020-05-29 设计创作，主要内容包括：本发明公开了一种基于双目序列图像的车辆位置与速度估计方法,包括以下步骤：S1.利用ZED双目摄像头,获取深度图和点云图；S2.通过KNN算法实现背景减除,辨识序列图像中的运动目标和静止背景环境；S3.检测运动目标边缘点,绘制位于运动目标上的矩形识别框,实时跟踪运动目标；S4.去除光影等误差因素导致的非运动目标上出现的矩形识别框；S5.计算锁定在运动目标上的有效矩形识别框的中心点的像素坐标；S6.获得点的(X,Y,Z)三维空间坐标；S7.利用卡尔曼滤波算法,由上一帧和当前帧的三维空间坐标,对下一时刻的位置和速度进行估算。本发明算法原理简单,计算复杂度低,适用于仿真技术领域,用于无人驾驶车辆对同向或对向车辆的位置速度进行检测。(The invention discloses a vehicle position and speed estimation method based on binocular sequence images, which comprises the following steps of: s1, acquiring a depth map and a point cloud map by using a ZED binocular camera; s2, background subtraction is achieved through a KNN algorithm, and a moving target and a static background environment in the sequence image are identified; s3, detecting edge points of the moving target, drawing a rectangular identification frame positioned on the moving target, and tracking the moving target in real time; s4, removing a rectangular identification frame appearing on the non-moving target caused by error factors such as light and shadow and the like; s5, calculating the pixel coordinate of the central point of the effective rectangular identification frame locked on the moving target; s6, obtaining (X, Y, Z) three-dimensional space coordinates of the points; and S7, estimating the position and the speed of the next moment by using a Kalman filtering algorithm according to the three-dimensional space coordinates of the previous frame and the current frame. The method has the advantages of simple algorithm principle and low calculation complexity, is suitable for the technical field of simulation, and is used for detecting the position and the speed of the unmanned vehicle in the same direction or opposite direction.)

一种基于双目序列图像的车辆位置与速度估计方法

技术领域

本发明涉及智能汽车定位的技术领域，具体涉及一种基于双目序列图像的车辆位置与速度估计方法。

技术背景

车辆的位置与速度的测算与预测，在无人操控领域有重要意义。其中，利用双目摄像头获取的序列图像来进行位置和速度的测算和预测，同时保证预测值的相对准确性，是一种较为新颖的途径，也能够较好的控制使用成本。基于双目图像序列的车辆速度与位置估计，考虑使用双目摄像机获取的序列图像，借助KNN算法实现背景减除并进行相关优化调整，获取相关数值后利用卡尔曼滤波算法进行处理计算。即利用实时图像的前后两帧的特征点的位置变化来预测下一时刻的位置和速度。

目前，已有的比较成熟的车速检测技术有地上磁感应线圈检测、红外激光射线车速检测、雷达车速检测和基于视频的车速检测等。由于视频的检测速度相对较快、通过视频能同时获取的信息量大、检测技术成本低等特点而受到关注。基于视频的检测方法，需要通过视频帧获取相关运动目标，检测其位置坐标信息，进而计算出速度。比较成熟的有Madasu和Hanmandlu等人在2010年提出的一种基于运动方程和KLT特征点跟踪的车辆测速方法，通过这个方法能获得车辆的位置信息和速度信息。

在类似的视频检测方法中，较为常用的是单目摄像头，通过单目摄像头读取实时视频，通过对已知的相机状态和前后两帧的二维图像序列进行对比，获得运动目标的位置变化。但此种方式不能直接获得三维立体坐标，使用上仍有一定的局限。

发明内容

为了降低传统以激光雷达或组合导航系统为主的车载定位系统的成本，本发明提出了一种基于双目序列图像的车辆位置与速度估计方法，包括以下步骤：

S1.利用ZED双目摄像头，连续采集视觉范围内序列图像，获取深度图和点云图；

所述的ZED双目摄像头可以完整地获取运动物体的几何信息。传统上用于车辆测速的摄像头一般为单目摄像头，虽然获取运动物体信息的方式与双目摄像头类似，但获取到的信息极其有限。因为图像深度信息的缺失，单目摄像头仅仅能获取图像坐标系中(x，y)坐标信息，而不能得到某一运动物体，或某一特征点在三维坐标系下的(x，y，z)三维坐标，因此在速度和位置以及距离方面的计算和预测上有很大的局限性，对摄像头的摆放角度等也有要求。

双目摄像头采取的双目立体视觉可以解决这个问题，利用摄像头处建立的三维立体坐标系，完整地记录运动物体的三维位置信息，对于后面利用卡尔曼滤波法对位置和速度进行的预测和计算的需求有很明显的帮助作用。

双目立体视觉对于目标点坐标的测量源于视差原理，首先建立以摄像头中心点为原点的三维立体坐标系，确定两摄像头中心距离b。之后对两个摄像头中获取的图像进行匹配，获取到同一特征点M在两个相机坐标系中的投影M₁和M₂的坐标值(x₁，y₁)和(x₂，y₂)，通过数据计算后即可获取该特征点在摄像头坐标系下的三维坐标(x，y，z)

通过ZED双目摄像头获取的三维坐标统一储存在ZED内置的点云类Point cloud之中，可以自由选定该坐标系是基于左目还是右目建立而成的(默认为右目)。同时，也可实现三维坐标系与图像坐标系的转化，即：

通过左目摄像头获取的图像得到的动态目标在图像坐标系中的(x，y)坐标，代入三维点云图的检索函数中，即可完成坐标系转换并返回左目摄像头坐标系中该目标点的(x，y，z)坐标，用于卡尔曼滤波中的代数计算。

S2.通过KNN算法实现背景减除，辨识序列图像中的运动目标和静止背景环境；

K最近邻算法(KNN)属于数据挖掘分类技术，其特点在于：根据某一样本的最近邻样本所属类别来判断该样本所属类别，其中所选择的邻居已经是经过正确分类后的样本。

KNN算法在背景建模时的原理如下：预测一个坐标(x，y)下的特征值类型，根据与其距离最近的K个点的特征值类别来决定自身的类别。首先根据距离公式

计算目标点与相邻点的距离，再通过K值筛选用于比较的点，最后决定目标点的类型。

KNN算法准确率很高，效果稳定，并且思路简单，过程清晰。尽管其有不存在训练过程以及计算过程复杂的问题，但是其计算过程的复杂性对于样本容量小的摄像头像素点而言并非有明显影响，可以忽略，而基于其分类能力优越的特性，将其运用在移动物体和背景的分类区分上十分合适，遂采用KNN算法。

鉴于KNN算法属于机器学习范畴，在程序刚开始运行的前几帧中，算法要根据图像进行适应训练，直到找到合适的背景后才会自动分离背景与移动物体。不过由于适应训练的时间十分短暂，去除掉适应过程中的数据处理内容对后续功能并无明显影响，因此该部分对于运动物体的追踪以及之后的坐标解算和卡尔曼滤波数据处理过程没有影响，仍可效果很好地完成背景消除建模追踪运动目标的任务。

背景减除算法是十分经典的运动目标检测手段，其通过背景建模的方式获取静止场景下的背景模型，再藉由当前帧下的图像特征与之前保存的背景模型进行差分运算，将得到的区域作为运动区域运动目标加以存储，从而完成运动物体的识别和跟踪。背景减除算法的原理是

(x，y)—图像坐标系中对应像素点的坐标值，f_c(x，y)—当前帧下(x，y)坐标处像素点的特征值，f_b(x，y)—背景建模上(x，y)坐标处像素点的特征值，T_h—设定的用于判断运动物体的阈值，M_Object(x，y)—当前帧与背景差分图像进行黑白二值化和相应处理后得到的图像。当判断到像素点特征值与背景点特征值的差值大于阈值时，即判定该点为运动物体，反之判定为背景点。

背景消除建模能够分离运动物体与背景特征点，从而获取需要的部分(在本项目中需要获取运动物体的点云坐标)，本项目利用KNN算法实现背景消除在C++中通过apply函数得以实现，并在经过黑白二值化、腐蚀、膨胀的去噪处理后，即可获得比较完整的运动目标和背景分离的图像(运动物体以白色，背景以黑色显示)。

S3.检测运动目标边缘点，绘制位于运动目标上的矩形识别框，实时跟踪运动目标；

通过OPENCV库中的Find Contours轮廓检测函数对图像进行分析，并可通过rectangle函数将运动目标可视化处理，即可实现对运动物体的实时追踪。

S4.设置矩形识别框筛选机制，去除光影等误差因素导致的非运动目标上出现的矩形识别框；

通过设定筛选函数筛选识别范围，保证运动目标识别准确无误。

S5.计算锁定在运动目标上的有效矩形识别框的中心点的像素坐标X′、Y′；

将可视化图像基于图像坐标系的中心点的像素坐标(x，y)保存。

S6.利用所得的X′、Y′并借助ZED点云坐标，获取对应的真实坐标X、Y和深度坐标Z，即获得点的(X，Y，Z)三维空间坐标；

将像素坐标(x，y)通过S1步骤所述的坐标转换方式转化到摄像头三维坐标(x，y，z)。

S7.利用卡尔曼滤波算法，由上一帧和当前帧的三维空间坐标，对下一时刻的位置和速度进行估算。

卡尔曼滤波算法核心思想在于预测加测量反馈，它由两部分组成，第一部分是线性系统状态预测方程，第二部分是线性系统观测方程

线性系统状态预测方程可表示为：

x_k＝Ax_k-1+Bu_k-1+ω_k-1

其中：p(ω)～N(0，Q)

方程中x_k和x_k-1表示k和(k-1)时刻的状态真值，u_k-1表示(k-1)时刻的控制量，ω_k-1表示过程激励噪声，A表示状态转移系数矩阵(n×n阶)，B表示可选的控制输入的增益矩阵，Q表示过程激励噪声的协方差矩阵。

线性系统观测方程可表示为：

z_k＝Hx_k+v_k

其中：p(v)～N(0，R)

方程中z_k为k时刻的观测值，v_k为观测噪声，H表示量测系数矩阵(m×n阶矩阵)，R表示测量噪声协方差矩阵。

要想得到最优的估计，那么必须知道k时刻的后验估计协方差先验估计协方差。

k时刻的后验估计协方差表达式为：

可以将k时刻的后验估计用先验估计与卡尔曼增益来表示：

从而可以得到：

k时刻的先验估计协方差表达式为:

由以上两式可以得到：

把k时刻的后验估计协方差矩阵的迹记为T[P_k]，因此有：

P_k对角线元素的和即为均方差。均方差对未知量K求导,令导函数等于0，就能确定卡尔曼增益K的值，完成模型的最小均方差估计，从而使后验估计的误差很小，更加逼近状态的真值。

求最小均方差确定卡尔曼增益的表达式：

可以看出，越大，K_k越大，即先验估计误差较大，需要更加信任测量反馈，模型自动加大卡尔曼增益来进行更准确的估计；为0，K_k为0，即先验估计没有任何误差，只需要通过预测就能够得到准确的后验估计，无需测量的反馈，模型自动将测量反馈的权值设为0。

因此可以得到卡尔曼滤波算法最终方程：

(1)时间更新方程

向前推算状态变量：

向前推算误差协方差：

(2)状态更新方程

计算卡尔曼增益：

由观测变量z_k更新估计：

更新误差协方差：

本发明的有益效果为：

针对车辆位置与速度的估计，考虑使用双目摄像机获取的序列图像，借助KNN算法实现背景减除并进行相关优化调整，获取相关数值后利用卡尔曼滤波算法进行处理计算。即利用实时图像的前后两帧的特征点的位置变化来预测下一时刻的位置和速度。通过实验证明，本发明所提提出的预测方法在一定范围内有较高的可靠性。本发明算法原理简单，计算复杂度低，且利用双目摄像头的使用成本低，不仅适用于仿真技术领域，还能在真实环境在实现对车辆速度和位置计算和预测，可用于无人驾驶车辆对同向或对向车辆的位置速度进行检测，也可用于路口车辆测速等实际场景，有很好的应用前景。

附图说明

图1为本发明的算法流程图；

图2(a)为本发明的实验小车三维空间位置的真实值与预测值对比图(X方向)；

图2(b)为本发明的实验小车三维空间位置的真实值与预测值对比图(Y方向)；

图2(c)为本发明的实验小车三维空间位置的真实值与预测值对比图(Z方向)；

图3(a)为本发明的实验小车三维方向速度的真实值与预测值对比图(X方向)；

图3(b)为本发明的实验小车三维方向速度的真实值与预测值对比图(Y方向)；

图3(c)为本发明的实验小车三维方向速度的真实值与预测值对比图(Z方向)；

图4为本发明的双目摄像头的工作原理图。

具体实施例

下面结合附图和

具体实施方式

，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明的总体设计思路是：利用ZED双目相机连续采集序列图像，并获得深度图与点云图，通过KNN算法实现背景减除(BSM)，用矩形识别框锁定运动目标，实现对运动车辆的精准跟踪，并获得运动目标上点的三维空间坐标，获得车辆的实时运动状态，并利用卡尔曼滤波算法实现对车辆即将出现的运动状态进行较为准确的预测。

如图所示，本实施例公开了一种基于双目序列图像的车辆位置与速度估计方法，本发明的实施例包含以下步骤：

S1.利用ZED双目摄像头，连续采集视觉范围内序列图像，获取深度图和点云图

实验小车从摄像头前面***而过，ZED双目摄像头持续获取图像序列，经过处理计算后，通过ZED双目摄像头获取的三维坐标统一储存在ZED内置的点云类Point cloud之中，可以自由选定该坐标系是基于左目还是右目建立而成的(默认为右目)。同时实现三维坐标系与图像坐标系的转化，通过左目摄像头获取的图像得到的动态目标在图像坐标系中的(x，y)坐标，代入三维点云图的检索函数中，完成坐标系转换并返回左目摄像头坐标系中该目标点的(x，y，z)坐标，用于卡尔曼滤波中的代数计算。

S2.通过KNN算法实现背景减除，辨识序列图像中的运动目标和静止背景环境；

利用KNN算法实现背景消除，通过apply函数实现背景与运动小车的分离，进行黑白二值化、腐蚀、膨胀的去噪处理后，通过背景建模的方式获取静止场景下的背景模型，再藉由当前帧下的图像特征与之前保存的背景模型进行差分运算，将得到的区域作为运动区域运动目标加以存储，从而完成运动物体的识别和跟踪，获得比较完整的运动目标和背景分离的图像。

通过KNN算法实现背景减除，采用背景减除法进行运动物体的识别，背景减除法，即背景消除建模，利用图像分割模式的高斯混合模型或机器学习的KNN算法得以实现，

KNN算法在背景建模时，预测一个坐标(x，y)下的特征值类型，根据与其距离最近的K个点的特征值类别来决定自身的类别，首先根据距离公式

计算目标点与相邻点的距离，再通过K值筛选用于比较的点，最后决定目标点的类型；

背景减除算法具体是

(x，y)—图像坐标系中对应像素点的坐标值，f_c(x，y)—当前帧下(x，y)坐标处像素点的特征值，f_b(x，y)—背景建模上(x，y)坐标处像素点的特征值，T_h—设定的用于判断运动物体的阈值，M_object(x，y)—当前帧与背景差分图像进行黑白二值化和相应处理后得到的图像；当判断到像素点特征值与背景点特征值的差值大于阈值时，即判定该点为运动物体，反之判定为背景点。

利用KNN算法实现背景消除在C++中通过apply函数得以实现，并在经过黑白二值化、腐蚀、膨胀的去噪处理后，即可获得比较完整的运动目标和背景分离的图像。

S3.检测运动目标边缘点，绘制位于运动目标上的矩形识别框，实时跟踪运动目标；

通过OPENCV库中的Find Contours轮廓检测函数对图像进行分析，通过rectangle函数将运动目标可视化处理，实现对运动小车的实时追踪效果。

S4.设置矩形识别框筛选机制，去除光影等误差因素导致的非运动目标上出现的矩形识别框；

通过设定筛选函数筛选识别范围，保证识别框准确无误地套选在运动小车轮廓之上，无明显偏差和错误。

S5.计算锁定在运动目标上的有效矩形识别框的中心点的像素坐标X′、Y′；

将可视化小车追踪图像基于图像坐标系的中心点的像素坐标(x，y)保存，用于之后获取三维坐标。

S6.利用所得的X′、Y′并借助ZED点云坐标，获取对应的真实坐标X、Y和深度坐标Z，即获得点的(X，Y，Z)三维空间坐标；

将运动小车中心像素坐标(x，y)通过S1步骤所述的坐标转换方式转化到摄像头坐标系下的小车位置真实三维坐标(x，y，z)。

S7.利用卡尔曼滤波算法，由上一帧和当前帧的三维空间坐标，对下一时刻的位置和速度进行估算。

即可利用卡尔曼滤波算法对下一时刻的位置和速度进行估算；

卡尔曼滤波算法由两部分组成，第一部分是线性系统状态预测方程，第二部分是线性系统观测方程

线性系统状态预测方程可表示为：

x_k＝Ax_k-1+Bu_k-1+ω_k-1

其中：p(ω)～N(0，Q)

方程中x_k和x_k-1表示k和(k-1)时刻的状态真值，u_k-1表示(k-1)时刻的控制量，ω_k-1表示过程激励噪声，A表示状态转移系数矩阵(n×n阶)，B表示可选的控制输入的增益矩阵，Q表示过程激励噪声的协方差矩阵；

线性系统观测方程可表示为：

z_k＝Hx_k+v_k

其中：p(v)～N(0，R)

方程中z_k为k时刻的观测值，v_k为观测噪声，H表示量测系数矩阵(m×n阶矩阵)，R表示测量噪声协方差矩阵。

k时刻的后验估计协方差表达式为：

将k时刻的后验估计用先验估计与卡尔曼增益来表示：

从而可以得到：

k时刻的先验估计协方差表达式为:

由以上两式可以得到：

我们把k时刻的后验估计协方差矩阵的迹记为T[Pk]，因此有：

P_k对角线元素的和即为均方差。均方差对未知量K求导,令导函数等于0，就能确定卡尔曼增益K的值，完成模型的最小均方差估计，从而使后验估计的误差很小，更加逼近状态的真值。

求最小均方差确定卡尔曼增益的表达式：

利用卡尔曼滤波算法最终方程：

(1)时间更新方程

向前推算状态变量：

向前推算误差协方差：

(2)状态更新方程

计算卡尔曼增益：

由观测变量z_k更新估计：

更新误差协方差：

带入每一帧的真实坐标值、测量速度值与时间间隔，对小车下一时刻的位置和速度进行预测，并且保存预测数据，与下一时刻计算的真实数据进行比对和后验修正。

如图4，双目立体视觉对于目标点坐标的测量源于视差原理，首先建立以摄像头中心点为原点的三维立体坐标系，确定两摄像头中心距离b。之后对两个摄像头中获取的图像进行匹配，获取到同一特征点M在两个相机坐标系中的投影M₁和M₂的坐标值(x₁，y₁)和(x₂，y₂)，通过数据计算后即可获取该特征点在摄像头坐标系下的三维坐标(x，y，z)。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。