CN112258547B - 基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法 - Google Patents

Abstract

本发明属于一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，包括基于逆透视投影变换的车辆三维轨迹坐标求解、基于跟车模型的车辆运动坐标预测、联合逆透视投影变换和跟车模型的车辆三维轨迹坐标优化校正三部分，本发明结合逆透视投影变换方法并引入交通仿真中的跟车模型，实现了一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化重建，能自动进行结果修正、实现车辆轨迹重建。

Description

基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法

技术领域

本发明属于交通仿真和机器视觉领域，尤其涉及一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法。

背景技术

城市路网内大量的监控摄像头收集了海量的城市交通场景视频数据。如何根据这些二维视频数据实现场景中车辆三维运动轨迹的重建，对市内交通场景重建、交通预测推演等具有重要的意义。

逆透视投影变换方法可以根据二维视频图像数据实现场景内车辆的三维坐标轨迹重建。虽然该方法具有真实性高、实时性好的特点，但是一方面，遮挡问题对计算结果的影响非常大，受遮挡车辆的坐标丢失严重；另一方面，路网内摄像头通常安装在道路一侧，采集的视频数据中，远离当前道路侧的车辆坐标轨迹三维重建误差大，甚至出现后一帧车辆位置前移等和实际情况背离的情形。

发明内容

本发明主要解决的技术问题是提供一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，解决受遮挡车辆坐标丢失严重、车辆坐标轨迹三维重建误差大的问题。

本发明的目的是这样实现的：一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，包括以下步骤：

逆透视投影变换计算获得车辆的三维轨迹坐标；由逆透视投影变换公式获取逆透视投影变换参数，由所述逆透视投影变换参数结合视频图像内车辆的图像坐标，获取视频图像内车辆的三维轨迹坐标；

跟车模型计算获得车辆的预测位置坐标；获取视频图像内车辆的状态信息，由所述状态信息基于跟车模型计算获取视频图像内车辆的预测位置坐标；

卡尔曼增益矩阵计算获得最优位置坐标；由卡尔曼增益矩阵对所述三维轨迹坐标与所述预测位置坐标的结合进行优化，然后结合所述预测位置坐标获取车辆的最优位置坐标。

优选的，选取视频图像中具有已知长度的参照物，由所述参照物的图像坐标以及所述参照物的长度获取所述逆透视投影变换参数。

优选的，使用局部搜索算法迭代求解所述逆透视投影变换参数，使所述逆透视投影变换参数的误差收敛至小于1e-9。

优选的，将所述三维轨迹坐标作为控制点，采用贝塞尔曲线拟合所述控制点获得车道线曲线，用于所述跟车模型的计算。

优选的，所述状态信息包括有速度信息。

优选的，由视频图像中车辆的三维坐标获取噪声协方差，由视频图像中车辆的所述预测位置坐标，获取预测误差协方差矩阵，由所述噪声协方差和所述预测误差协方差矩阵获取卡尔曼增益矩阵。

优选的，所述噪声协方差为：

式中：R为噪声协方差，N₀为初始时刻视频图像内车辆的总数，j为初始时刻视频图像内的第j个车辆，Z_j(t)为初始时刻视频图像内第j辆车的三维坐标，

为初始时刻视频图像内所有车辆三维坐标Z_j(t)的平均值，T表示矩阵转置。

优选的，所述预测误差协方差矩阵为：

式中：P^f(t+1)为t+1时刻的预测误差协方差矩阵，N_t+1为t+1时刻视频图像内所有车辆的总数，i表示视频图像内的第i个车辆,X_i(t+1)为t+1时刻视频图像内第i辆车的预测位置坐标，

为t+1时刻视频图像内所有车辆的预测位置坐标X_i(t+1)的平均值。T表示矩阵转置。

优选的，所述卡尔曼增益矩阵为：

K(t+1)＝P^f(t+1)(P^f(t+1)+R)^-1

式中：K(t+1)为t+1时刻的卡尔曼增益矩阵，P^f(t+1)为t+1时刻视频图像内的所述预测误差协方差矩阵，R为噪声协方差。

优选的，将视频图像中不同时刻的车辆的最优位置坐标按时刻顺序依次连接，获得车辆的最优轨迹。

本发明使用简单、可操作性强，使用常规单目摄像头获得的单幅图像使用逆透视投影变求解整个图像中道路内物体的三维坐标，避免因遮挡而使车辆坐标丢失的情况。本发明结合逆透视投影变换方法并引入交通仿真中的跟车模型，实现了一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化重建，能自动进行结果修正、实现车辆轨迹重建。

附图说明

图1是本发明基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法一实施例的流程图。

具体实施方式

为了便于理解本发明，下面结合附图和具体实施例，对本发明进行更详细的说明。附图中给出了本发明的较佳的实施例。但是，本发明可以以许多不同的形式来实现，并不限于本说明书所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

需要说明的是，除非另有定义，本说明书所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是用于限制本发明。本说明书所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。

图1显示了本发明基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，包括以下步骤：

逆透视投影变换计算获得车辆的三维轨迹坐标S10；由逆透视投影变换公式获取逆透视投影变换参数，由所述逆透视投影变换参数结合视频图像内车辆的图像坐标，获取视频图像内车辆的三维轨迹坐标；

跟车模型计算获得车辆的预测位置坐标S20；获取视频图像内车辆的状态信息，由所述状态信息基于跟车模型计算获取视频图像内车辆的预测位置坐标；

卡尔曼增益矩阵计算获得最优位置坐标S30；由卡尔曼增益矩阵对所述三维轨迹坐标与所述预测位置坐标的结合进行优化，然后结合所述预测位置坐标获取车辆的最优位置坐标。

本发明结合逆透视投影变换方法并引入交通仿真中的跟车模型，实现了一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化重建，能自动进行结果修正、实现车辆轨迹重建。

进一步的，所述的逆透视投影变换公式为：

其中，x_G、y_G、z_G为某点所在视频图像的图像坐标；x_C、y_C、z_C为该点在以地面为xy平面、以竖直向上为z轴的世界坐标系下的三维坐标；h为坐标变换中平移距离；d为投影点坐标；K为图像坐标系到世界坐标系的单位转化比例，(h,d,θ,K)为所述逆透视投影变换中的未知量。

进一步的，为了确定上述逆透视投影变换公式中的未知量(h,d,θ,K)，接下来从视频图像中选取若干参照物，根据参照物所在视频图像的图像坐标以及其世界坐标系下的三维坐标为变换前后坐标，采用局部搜索算法迭代求解逆透视投影变换参数(h,d,θ,K)。

进一步的，首先标记参照物图像坐标和参照物国家标准中三维实际长度。参照物的选取要求为：数量不少于4个、实际大小确定唯一、任意两个参照物之间的间距不小于整幅图像区域的1/5、参考物在现实世界中应当有一定夹角，以垂直为宜。

进一步的，根据参照物两端点的图像坐标，结合对应参照物国家标准中的三维实际长度得到一组样本集：

(x_1n y_1n x_2n y_2nl_n)n∈[1,N]

其中，x_1n、y_1n、x_2n、y_2n为参照物n两端点图像坐标，其中图像坐标系以图像中心点为原点，图像正右方向为x轴正方向，图像正上方向为y轴正方向，l_n为国家标准中参照物n两端点的三维实际长度。N表示参照物的数量。

进一步的，使用局部搜索算法迭代求解逆透视投影变换参数(h,d,θ,K)，直至误差收敛至一定范围内，优选误差收敛至小于1e-9。

逆透视投影变换参数(h,d,θ,K)得出后，结合视频图像内车辆的图像坐标，由逆透视投影变换公式获得视频图像内任一车辆的三维坐标(x_c y_c z_c)。

将视频图像中时刻t为0时的时刻作为初始时刻。将初始视频图像内车辆的总数记为N₀，初始时刻视频图像内的某一车辆记为j，初始时刻视频图像内第j辆车的三维坐标记为Z_j(t)。进一步的，由获得的三维坐标Z_j(t)获取噪声协方差，即：

式中：R为噪声协方差，N₀为初始时刻视频图像内车辆的总数，j为初始时刻视频图像内的第j个车辆，Z_j(t)为初始时刻视频图像内第j辆车的三维坐标，

为初始时刻视频图像内所有车辆三维坐标Z_j(t)的平均值，T表示矩阵转置。

同样的，逆透视投影变换参数(h,d,θ,K)得出后，结合视频图像内车辆的图像坐标，由逆透视投影变换公式可以获得任一时刻视频图像内任一车辆的三维坐标(x_c y_c z_c)。以t时刻表示视频图像的当前时刻，以t-1时刻表示视频图像当前时刻的上一时刻。以t+1时刻表示视频图像当前时刻的下一时刻。将t时刻视频图像内车辆的总数记为N_t，将视频图像内的某一车辆记为i，t时刻视频图像内第i辆车的三维坐标作为三维轨迹坐标，记为Z_i(t)，t∈[0,t_total]，其中t_total表示视频图像的总时长，同样的，将t+1时刻视频图像内车辆的总数量记为N_t+1，t+1时刻视频图像内第i辆车的三维坐标作为三维轨迹坐标，记为Z_i(t+1)。

进一步的，将所述三维轨迹坐标Z_i(t)作为控制点，采用贝塞尔曲线拟合控制点获得车道线曲线，用于接下来跟车模型求解。

进一步的，获取视频图像中车辆的状态信息，状态信息包括速度信息，通过跟车模型由t-1时刻视频图像内车辆的速度信息获得t时刻内车辆的速度信息，即：

V_i(t)＝M(V_i(t-1))

式中：V_i(t)为标量，表示t时刻视频图像内第i辆车的速度信息，V_i(t-1)为t-1时刻视频图像内第i辆车的速度信息；i表示视频图像内的第i个车辆,M为跟车模型算子。

进一步的，所述跟车模型算子为：

式中：v表示速度，s表示前车与后车两辆车辆之间的车间距，Δv表示和前车的速度差，v₀表示期望车速，δ表示加速系数，

表示最小期望间距，T表示安全时距，a表示最大加速度，b表示期望减速度，s₀分别表示安全车距。v₀，δ，T，a，b，s₀等参数可以进行常规选择给出。

进一步的，由t时刻内车辆的状态信息获得t+1时刻车辆的预测位置坐标为：

式中：X_i(t+1)为t+1时刻第i辆车的预测位置坐标，S为位置更新算子，是位置坐标的函数，即S(X_i(t)),X_i(t)表示t时刻第i辆车的预测位置坐标，V_i(t)为t时刻第i辆车的速度信息；

表示第i辆车所在的车道线曲线位置处的切向方向,表示视频图像内的第i辆车。

由获得的t+1时刻视频图像内第i辆车的预测位置坐标X_i(t+1)，获取预测误差协方差矩阵。预测误差协方差矩阵为：

式中：P^f(t+1)为t+1时刻的预测误差协方差矩阵，N_t+1为t+1时刻视频图像内所有车辆的总数，i表示视频图像内的第i个车辆,X_i(t+1)为t+1时刻视频图像内第i辆车的预测位置坐标，

为t+1时刻视频图像内所有车辆的预测位置坐标X_i(t+1)的平均值。T表示矩阵转置。

进一步的，由t+1时刻的预测误差协方差矩阵P^f(t+1)和噪声协方差R获得t+1时刻的卡尔曼增益矩阵，所述卡尔曼增益矩阵为：

K(t+1)＝P^f(t+1)(P^f(t+1)+R)^-1

式中：K(t+1)为t+1时刻的卡尔曼增益矩阵，P^f(t+1)为t+1时刻视频图像内的所述预测误差协方差矩阵，R为噪声协方差。

由上述可知，逆透视投影变换获得了t+1时刻视频图像内第i辆车的三维轨迹坐标Z_i(t+1)。跟车模型获得了t+1时刻视频图像内第i辆车的预测位置坐标X_i(t+1)。由获得三维坐标Z_j(t)获得了噪声协方差R，由预测位置坐标X_i(t+1)获得了t+1时刻的预测误差协方差矩阵P^f(t+1)，由噪声协方差R和t+1时刻的预测误差协方差矩阵P^f(t+1)获得了t+1时刻的卡尔曼增益矩阵K(t+1)。

由此即可将t+1时刻的卡尔曼增益矩阵K(t+1)与t+1时刻视频图像内第i辆车的三维轨迹坐标Z_i(t+1)和t+1时刻视频图像内第i辆车的预测位置坐标X_i(t+1)相结合，获取t+1时刻视频图像内第i辆车的最优位置坐标。即最优位置坐标为：

式中：

为t+1时刻视频图像内第i辆车的最优位置坐标，X_i(t+1)为t+1时刻视频图像内第i辆车的预测位置坐标，K(t+1)为t+1时刻的卡尔曼增益矩阵，Z_i(t+1)为t+1时刻视频图像内第i辆车的三维轨迹坐标。

由此即可获得视频图像内多个不同时刻的第i辆车的最优位置坐标，将这些不同时刻的第i辆车的最优位置坐标按时刻顺序依次连接，即可获得第i辆车的最优轨迹。从而完成对车辆的三维轨迹优化。

本发明结合逆透视投影变换方法并引入交通仿真中的跟车模型，实现了一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化重建，能自动进行结果修正、实现车辆轨迹重建。

以上所述仅为本发明的优选实施例而已，并不用以限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，其特征在于，包括以下步骤：

逆透视投影变换计算获得车辆的三维轨迹坐标；由逆透视投影变换公式获取逆透视投影变换参数，选取视频图像中具有已知长度的参照物，由所述参照物的图像坐标以及所述参照物的长度获取所述逆透视投影变换参数；使用局部搜索算法迭代求解所述逆透视投影变换参数，使所述逆透视投影变换参数的误差收敛至小于1e-9；由所述逆透视投影变换参数结合视频图像内车辆的图像坐标，获取视频图像内车辆的三维轨迹坐标；将所述三维轨迹坐标作为控制点，采用贝塞尔曲线拟合所述控制点获得车道线曲线，用于跟车模型的计算；

跟车模型计算获得车辆的预测位置坐标；获取视频图像内车辆的状态信息，所述状态信息包括有速度信息；由所述状态信息基于跟车模型计算获取视频图像内车辆的预测位置坐标；

由视频图像中车辆的三维坐标获取噪声协方差，由视频图像中车辆的所述预测位置坐标，获取预测误差协方差矩阵，由所述噪声协方差和所述预测误差协方差矩阵获取卡尔曼增益矩阵；

所述噪声协方差为：

式中：R为噪声协方差，N₀为初始时刻视频图像内车辆的总数，j为初始时刻视频图像内的第j个车辆，Z_j(t)为初始时刻视频图像内第j辆车的三维坐标，

为初始时刻视频图像内所有车辆三维坐标Z_j(t)的平均值，T表示矩阵转置；

卡尔曼增益矩阵计算获得最优位置坐标；由卡尔曼增益矩阵对所述三维轨迹坐标与所述预测位置坐标的结合进行优化，然后结合所述预测位置坐标获取车辆的最优位置坐标。

2.根据权利要求1所述的基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，其特征在于，所述预测误差协方差矩阵为：

式中：P^f(t+1)为t+1时刻的预测误差协方差矩阵，N_t+1为t+1时刻视频图像内所有车辆的总数，i表示视频图像内的第i个车辆,X_i(t+1)为t+1时刻视频图像内第i辆车的预测位置坐标，

为t+1时刻视频图像内所有车辆的预测位置坐标X_i(t+1)的平均值，T表示矩阵转置。

3.根据权利要求2所述的基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，其特征在于，所述卡尔曼增益矩阵为：

K(t+1)＝P^f(t+1)(P^f(t+1)+R)^-1

式中：K(t+1)为t+1时刻的卡尔曼增益矩阵，P^f(t+1)为t+1时刻视频图像内的所述预测误差协方差矩阵，R为噪声协方差。

4.根据权利要求3所述的基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，其特征在于，将视频图像中不同时刻的车辆的最优位置坐标按时刻顺序依次连接，获得车辆的最优轨迹。

Images