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

权利要求书

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所述的基于逆透视投影变换和跟车模型的车辆三维轨迹优化方法，其特征在于，将视频图像中不同时刻的车辆的最优位置坐标按时刻顺序依次连接，获得车辆的最优轨迹。