一种智能交通系统中多车辆协作定位方法与流程

本发明涉及无线定位技术领域，特别是指一种智能交通系统中多车辆协作定位方法。

背景技术：

近年来，随着车辆变得更加智能和自动化，在智能交通系统中，各种安全相关的应用，如交通状况的实时估计，碰撞警告系统，车道偏离警告系统等，这些都是为了提高驾驶的效率和安全性，从而减少了车辆碰撞事故。而这些安全应用程序主要依赖于本地交通网络提供的车辆位置信息。车辆导航技术包括全球定位系统(gps)、全球卫星导航系统(glonass)，伽利略和beidou系统(bds)，它们可以为车辆用户提供位置信息。gps是车辆定位中最常用的定位设备之一。然而，众所周知，gps信号受到不同来源的噪音和退化以及在复杂的环境下信号的暂时丢失，而且gps卫星可见度估计不足，这使得gps不能在所有情况下提供精确的位置信息。我们用于汽车应用的低成本gps接收机导航系统遭受着低精度和频繁的信号中断问题。通常情况下，gps标称的精度约为10m，这对于车辆主动安全系统来说误差太大。

提高自我定位准确性的最常用方法之一是使用其他嵌入式信息源，结合导航数据，通过数据融合，获取更准确的位置估计。目前常用的提高gps性能的技术有基于卡尔曼滤波(kalmanfiltering)的方法。基于卡尔曼滤波的方法主要思想是通过滤波减小gps伪距误差，但这种方法并没有结合周边车辆的位置信息，在智能交通系统中所提供的定位精度不高。

技术实现要素：

本发明要解决的技术问题是提供一种智能交通系统中多车辆协作定位方法，以解决现有技术所存在的定位精度低的问题。

为解决上述技术问题，本发明实施例提供一种智能交通系统中多车辆协作定位方法，包括：

获取系统中目标车辆和相邻车辆在当前时刻的位置观测值，所述相邻车辆为与所述目标车辆相邻的车辆；

获取所述目标车辆在当前时刻的运动观测值；

根据获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值；

获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程；

根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程；

将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值。

进一步地，所述获取系统中目标车辆和相邻车辆在当前时刻的位置观测值包括：

获取k时刻目标车辆x0的位置观测值x0k＝[x0ky0kθ0k]^t；

其中，k时刻表示当前时刻，t表示转置，x0k表示k时刻目标车辆x0在x轴坐标，y0k表示k时刻目标车辆x0在y轴坐标，θ0k表示k时刻目标车辆x0运动方向与x轴所形成的夹角；

获取k时刻相邻车辆xj的位置观测值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)；

其中，n表示相邻车辆的数目，xjk表示k时刻相邻车辆xj在x轴坐标，yjk表示k时刻相邻车辆xj在y轴坐标，θjk表示k时刻相邻车辆xj运动方向与x轴所形成的夹角。

进一步地，所述获取所述目标车辆在当前时刻的运动观测值包括：

获取k时刻目标车辆x0的运动观测值u0k＝[v0ka0kφ0k]^t；

其中，v0k表示k时刻目标车辆x0的速度，a0k表示k时刻目标车辆x0的加速度，φ0k表示k时刻目标车辆x0的转向角。

进一步地，所述根据获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值包括：

根据目标车辆x0在k时刻的位置观测值x0k和运动观测值u0k以及目标车辆x0的运动模型，计算目标车辆x0在k+1时刻的位置测量值x0(k+1)＝f(x0k,u0k)，其中，f(x0k,u0k)表示目标车辆x0的运动模型的离散运动方程；

根据f(x0k,u0k)，得到f(x0k,u0k)关于位置观测值x0k的雅可比矩阵为：

根据f(x0k,u0k)，得到f(x0k,u0k)关于运动观测值u0k的雅可比矩阵bu0k为：

进一步地，所述获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程包括：

获取所述相邻车辆的数目n；

根据获取的相邻车辆的数目n，结合获取的目标车辆在k时刻的位置观测值x0k和运动观测值u0k，构造k时刻包含所述目标车辆和相邻车辆的整个系统的系统状态系统输入则系统的运动方程为xk+1＝f(xk,uk)；其中，t表示矩阵转置；

根据雅可比矩阵得到系统的运动方程f(xk,uk)关于系统状态xk的雅可比矩阵ak为：

根据雅可比矩阵得到系统的运动方程f(xk,uk)关于系统输入uk的雅可比矩阵bk为：

进一步地，所述根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程包括：

根据获取的k时刻目标车辆x0的位置观测值x0k和相邻车辆xj的位置观测值xjk，计算k时刻目标车辆x0和相邻车辆xj之间的相对位置信息zjk；

根据计算得到的k时刻目标车辆x0和相邻车辆xj之间的相对位置信息zjk，构造系统的观测方程zk＝[z1kz2k…zjk…z(n-1)kznk]^t,(j＝1,2,…n)，其中，n表示相邻车辆的数目；其中，所述zjk表示为：

其中，hj(·)表示相对位置信息的计算公式，djk为目标车辆x0与相邻车辆xj之间的相对距离，为目标车辆x0与相邻车辆xj之间的相对方位角；

根据zjk＝hj(x0k,xjk)，目标车辆x0相对于zjk＝hj(x0k,xjk)的雅可比矩阵hjk为：

根据雅可比矩阵hjk，得到观测方程zk的雅可比矩阵hk为：

进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值包括：

将系统状态xk的雅可比矩阵ak、系统输入uk的雅可比矩阵bk、观测方程zk的雅可比矩阵hk以及系统的运动方程xk+1和观测方程zk代入到扩展卡尔曼滤波中，得到目标车辆x0的位置估计值

进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值包括：

s1、根据系统状态xk的雅可比矩阵ak和系统输入uk的雅可比矩阵bk，计算k+1时刻系统状态协方差的预测值其中，pk为k时刻系统状态协方差，q为位置观测值误差协方差，t为矩阵转置，ts为采样周期；

s2、根据观测方程zk的雅可比矩阵hk和计算得到的k+1时刻系统状态协方差的预测值计算k+1时刻卡尔曼滤波增益其中，r为观测方程zk的误差协方差；

s3、根据计算得到的k+1时刻卡尔曼滤波增益kk+1，计算k+1时刻系统状态协方差其中，i为单位矩阵；

s4、根据系统状态xk的雅可比矩阵ak和系统输入uk的雅可比矩阵bk，计算k+1时刻系统状态预测值和系统观测方程预测值其中，

所述和分别表示为：

其中，n表示相邻车辆的数目；

其中，hj(·)表示相对位置信息的计算公式；

s5、根据得到的系统观测方程预测值判断目标车辆x0与相邻车辆xj之间的相对方位角预测值是否满足预设的判断公式，若满足，则确定k+1时刻系统状态估计值若不满足，则确定k+1时刻系统状态估计值为：

其中，zk+1为k+1时刻系统观测方程的观测值；

s6、根据k+1时刻系统状态估计值计算k+1时刻目标车辆位置估计值为：

进一步地，所述预设的判断公式表示为：

其中，θthresh为预设的阈值。

进一步地，所述

其中，为目标车辆x0与相邻车辆xj之间的相对方位角的方差。

本发明的上述技术方案的有益效果如下：

上述方案中，通过获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值；获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程；根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程；将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值；这样，通过将目标车辆的位置观测值与相邻车辆的位置观测值相结合，计算目标车辆与相邻车辆之间的相对位置信息，基于计算得到的目标车辆与相邻车辆之间的相对位置信息，利用扩展卡尔曼滤波进行滤波后，估计出目标车辆的位置，实现协作定位，能够减少位置误差，从而提高车辆的定位精度。

附图说明

图1为本发明实施例提供的智能交通系统中多车辆协作定位方法的流程示意图；

图2为本发明实施例提供的智能交通系统多车辆系统模型示意图；

图3为本发明实施例测量的位置误差与gps测量的位置误差对比示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明针对现有的定位精度低的问题，提供一种智能交通系统中多车辆协作定位方法。

如图1所示，本发明实施例提供的智能交通系统中多车辆协作定位方法，包括：

s101，获取系统中目标车辆和相邻车辆在当前时刻的位置观测值，所述相邻车辆为与所述目标车辆相邻的车辆；

s102，获取所述目标车辆在当前时刻的运动观测值；

s103，根据获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值；

s104，获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程；

s105，根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程；

s106，将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值。

本发明实施例所述的智能交通系统中多车辆协作定位方法，通过获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值；获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程；根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程；将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值；这样，通过将目标车辆的位置观测值与相邻车辆的位置观测值相结合，计算目标车辆与相邻车辆之间的相对位置信息，基于计算得到的目标车辆与相邻车辆之间的相对位置信息，利用扩展卡尔曼滤波进行滤波后，估计出目标车辆的位置，实现协作定位，能够减少位置误差，从而提高车辆的定位精度。

本发明实施例所述的智能交通系统中多车辆协作定位方法，在低信噪比环境下，测量结果比gps测量结果更加准确，且扩展卡尔曼滤波运算速度快，有助于提高车辆的定位精度。且在实际应用中，可以根据实际情况自适应地选择相邻车辆个数。在智能交通系统中，目标车辆周围的相邻车辆是不断变化的，本发明实施例所述的智能交通系统中多车辆协作定位方法可自主选择目标车辆周围的相邻车辆的个数。

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述获取系统中目标车辆和相邻车辆在当前时刻的位置观测值包括：

获取k时刻目标车辆x0的位置观测值x0k＝[x0ky0kθ0k]^t；

其中，k时刻表示当前时刻，t表示转置，x0k表示k时刻目标车辆x0在x轴坐标，y0k表示k时刻目标车辆x0在y轴坐标，θ0k表示k时刻目标车辆x0运动方向与x轴所形成的夹角；

获取k时刻相邻车辆xj的位置观测值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)；

其中，n表示相邻车辆的数目，xjk表示k时刻相邻车辆xj在x轴坐标，yjk表示k时刻相邻车辆xj在y轴坐标，θjk表示k时刻相邻车辆xj运动方向与x轴所形成的夹角。

本实施例中，目标车辆x0可以通过车载导航(例如，gps设备或北斗设备)获取k时刻目标车辆x0的位置观测值x0k＝[x0ky0kθ0k]^t；同时，目标车辆x0可以通过短程距离通信(dedicatedshort-rangecommunication，dsrc)获取k时刻相邻车辆xj的位置观测值xjk＝[xjkyjkθjk]^t，(j＝1,2…n)，开支小。

本实施例中，θk表示k时刻目标车辆x0运动方向与x轴所形成的夹角，即k时刻目标车辆x0的方位角；θjk表示k时刻相邻车辆xj运动方向与x轴所形成的夹角，即k时刻相邻车辆xj的方位角。

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述获取所述目标车辆在当前时刻的运动观测值包括：

获取k时刻目标车辆x0的运动观测值u0k＝[v0ka0kφ0k]^t；

其中，v0k表示k时刻目标车辆x0的速度，a0k表示k时刻目标车辆x0的加速度，φ0k表示k时刻目标车辆x0的转向角。

本实施例中，可以通过车载传感器获取k时刻目标车辆x0的运动观测值u0k＝[v0ka0kφ0k]^t，其中，所述车载传感器可以包括但不限于：加速度传感器，速度传感器。

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述根据获取的目标车辆在当前时刻的位置观测值和运动观测值，计算所述目标车辆在下一时刻的位置测量值包括：

根据目标车辆x0在k时刻的位置观测值x0k和运动观测值u0k以及目标车辆x0的运动模型，计算目标车辆x0在k+1时刻的位置测量值x0(k+1)＝f(x0k,u0k)，其中，f(x0k,u0k)表示目标车辆x0的运动模型的离散运动方程；

根据f(x0k,u0k)，得到f(x0k,u0k)关于位置观测值x0k的雅可比矩阵为：

根据f(x0k,u0k)，得到f(x0k,u0k)关于运动观测值u0k的雅可比矩阵为：

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述获取所述相邻车辆的数目，并结合获取的目标车辆在当前时刻的位置观测值和运动观测值，以及计算得到的所述目标车辆在下一时刻的位置测量值，构造系统的运动方程包括：

获取所述相邻车辆的数目n；

根据获取的相邻车辆的数目n，结合获取的目标车辆在k时刻的位置观测值x0k和运动观测值u0k，构造k时刻包含所述目标车辆和相邻车辆的整个系统的系统状态系统输入则系统的运动方程为xk+1＝f(xk,uk)；其中，t表示矩阵转置；

根据雅可比矩阵得到系统的运动方程f(xk,uk)关于系统状态xk的雅可比矩阵ak为：

根据雅可比矩阵得到系统的运动方程f(xk,uk)关于系统输入uk的雅可比矩阵bk为：

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述根据获取的目标车辆和相邻车辆在当前时刻的位置观测值，计算所述目标车辆与相邻车辆之间的相对位置信息，并根据相邻车辆的数目，构造系统的观测方程包括：

根据获取的k时刻目标车辆x0的位置观测值x0k和相邻车辆xj的位置观测值xjk，计算k时刻目标车辆x0和相邻车辆xj之间的相对位置信息zjk；

根据计算得到的k时刻目标车辆x0和相邻车辆xj之间的相对位置信息zjk，构造系统的观测方程zk＝[z1kz2k…zjk…z(n-1)kznk]^t,(j＝1,2,…n)，其中，n表示相邻车辆的数目；其中，所述zjk表示为：

其中，hj(·)表示相对位置信息的计算公式，djk为目标车辆x0与相邻车辆xj之间的相对距离，为目标车辆x0与相邻车辆xj之间的相对方位角；

根据zjk＝hj(x0k,xjk)，目标车辆x0相对于zjk＝hj(x0k,xjk)的雅可比矩阵hjk为：

根据雅可比矩阵hjk，得到观测方程zk的雅可比矩阵hk为：

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值包括：

将系统状态xk的雅可比矩阵ak、系统输入uk的雅可比矩阵bk、观测方程zk的雅可比矩阵hk以及系统的运动方程xk+1和观测方程zk代入到扩展卡尔曼滤波中，得到目标车辆x0的位置估计值

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述将系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值包括：

s1、根据系统状态xk的雅可比矩阵ak和系统输入uk的雅可比矩阵bk，计算k+1时刻系统状态协方差的预测值其中，pk为k时刻系统状态协方差，q为位置观测值误差协方差，t为矩阵转置，ts为采样周期；

s2、根据观测方程zk的雅可比矩阵hk和计算得到的k+1时刻系统状态协方差的预测值计算k+1时刻卡尔曼滤波增益其中，r为观测方程zk的误差协方差；

s3、根据计算得到的k+1时刻卡尔曼滤波增益kk+1，计算k+1时刻系统状态协方差其中，i为单位矩阵；

s4、根据系统状态xk的雅可比矩阵ak和系统输入uk的雅可比矩阵bk，计算k+1时刻系统状态预测值和系统观测方程预测值其中，

所述和分别表示为：

其中，n表示相邻车辆的数目；

其中，hj(·)表示相对位置信息的计算公式；

s5、根据得到的系统观测方程预测值判断目标车辆x0与相邻车辆xj之间的相对方位角预测值是否满足预设的判断公式，若满足，则确定k+1时刻系统状态估计值若不满足，则确定k+1时刻系统状态估计值为：

其中，zk+1为k+1时刻系统观测方程的观测值；

s6、根据k+1时刻系统状态估计值计算k+1时刻目标车辆位置估计值为：

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述预设的判断公式表示为：

其中，θthresh为预设的阈值。

在s5中，可以通过预设的判断公式判断目标车辆x0与相邻车辆xj之间的相对方位角预测值是否接近若接近则保留系统状态预测值即k+1时刻系统状态估计值若不接近，则k+1时刻系统状态估计值为：

在前述智能交通系统中多车辆协作定位方法的具体实施方式中，进一步地，所述

其中，为目标车辆x0与相邻车辆xj之间的相对方位角的方差。

如图2所示，以具体的例子对本实施例所述的智能交通系统中多车辆协作定位方法进行详细说明，并使用matlab仿真平台，对本实施例所述的智能交通系统中多车辆协作定位方法的性能进行仿真分析：

步骤1，如图2所示，考虑到智能交通系统中一个目标车辆x0和四个相邻车辆x1,x2,x3,x4，行驶在道路上，车辆x0,x1,x2,x3,x4通过自身gps设备分别接收到k时刻各自的位置观测值；

步骤2，目标车辆x0通过车载传感器获取k时刻自身的运动观测值u0k＝[v0ka0kφ0k]^t，设目标车辆做的是直线运动，在采样周期内可以看作是匀加速直线运动，即a0k为定值；

步骤3，根据目标车辆x0在k时刻的位置观测值和运动观测值，得出目标车辆在k+1时刻的位置观测值和运动观测值，即目标车辆x0的运动模型；具体的，步骤3可以包括：

3.1)目标车辆x0的运动模型的离散运动方程为：

其中，ts为采样周期。

3.2)根据离散运动方程f(x0k,u0k)，可得f(x0k,u0k)关于x0k的雅可比矩阵为：

3.3)根据离散运动方程f(x0k,u0k)，可得f(x0k,u0k)关于u0k的雅可比矩阵为：

步骤4，通过步骤3构造k时刻包含x0k与相邻车辆x1k,x2k,x3k,x4k的整个系统的系统状态为xk＝[x0kx0kx0kx0k]^t，系统输入为uk＝[u0ku0ku0ku0k]^t，则系统的运动方程为xk+1＝f(xk,uk)；具体的，步骤4可以包括：

4.1)根据步骤3的雅可比矩阵可得系统的运动方程f(xk,uk)关于系统状态xk的雅可比矩阵ak为：

4.2)根据步骤3的雅可比矩阵可得系统的运动方程f(xk,uk)关于系统输入uk的雅可比矩阵bk为：

步骤5：计算得到目标车辆与相邻车辆之间的相对位置信息，构造观测方程zk＝[z1kz2kz3kz4k]^t，具体的，步骤5可以包括：

5.1)k时刻目标车辆x0与相邻车辆xj之间的相对位置信息zjk为：

其中，djk为目标车辆x0与相邻车辆xj之间的相对距离，为目标车辆x0与相邻车辆xj之间的相对方位角；

5.2)目标车辆x0相对于zjk＝hj(x0k,xjk)的雅可比矩阵hjk为：

5.3)根据雅可比矩阵hjk可得观测方程zk的雅可比矩阵hk为：

步骤6，将步骤4得到的系统状态xk的雅可比矩阵ak、系统输入uk的雅可比矩阵bk和步骤5得到的观测方程zk的雅可比矩阵hk以及系统的运动方程和观测方程代入到扩展卡尔曼滤波中，得到目标车辆的位置估计值具体的，步骤6可以包括：

6.1)计算系统状态协方差的预测值

其中，pk为k时刻系统状态协方差，q为位置观测值误差协方差，ts为采样周期；

6.2)根据系统状态协方差的预测值计算k+1时刻卡尔曼滤波增益kk+1：

其中，r为观测方程zk的误差协方差，hk为观测方程zk的雅可比矩阵。

6.3)根据k+1时刻卡尔曼滤波增益kk+1得到k+1时刻系统状态协方差pk+1：

其中，i为单位矩阵，hk为观测方程zk的雅可比矩阵。

6.4)根据系统状态xk的雅可比矩阵ak和系统输入uk的雅可比矩阵bk计算系统状态预测值

其中，

和系统观测方程预测值

其中，

6.5)计算k+1时刻系统状态估计值根据6.4)得到的系统观测方程的预测值判断目标车辆x0与相邻车辆xj之间的相对方位角预测值是否接近若接近则保留系统状态预测值即若不接近，则

其中，kk+1为k+1时刻卡尔曼滤波增益，zk+1为k+1时刻系统观测方程的观测值，为k+1时刻系统状态预测值。

6.6)根据6.5)计算得到的可得k+1时刻目标车辆位置估计值为：

本实施例中，如图3所示，图3描述了本发明实施例测量的位置误差与gps测量的位置误差对比示意图，图3中的横坐标是采样时间，纵坐标是位置误差大小，该结果是在信噪比为3db下进行1000次的同条件重复实验得出的。由图3可知，本发明实施例提出的方法降低了位置误差，并给出了更准确的位置估计。这说明本发明实施例提出的协作定位方法是准确并且有效的，同时对比结果也表明本发明采用的方法性能优于gps测量的性能。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。