一种基于current-ellipse模型的车辆动态跟踪方法与流程

本发明涉及车辆动态跟踪
技术领域：
，尤其涉及到一种基于current-ellipse模型的车辆动态跟踪方法。
背景技术：
：车辆的跟踪或定位是构建车联网，实现智能交通，促进社会经济发展的重要环节，长期以来，受到了研究人员的广泛关注。目前，如gps、北斗和伽利略等全球导航卫星系统(globalnavigationsatellitesystem，gnss)可以提供精确到米的车辆定位应用。然而，现实中会常常遇到当车辆行驶在较为封闭的区域时，gnss信号的接收和发送受到了阻碍，导致无法满足车辆跟踪或定位的精度需求。虽然采用基于激光雷达(如采用velodyne-64和velodyne-128)的定位系统可以取得厘米级精度的定位效果。然而激光雷达设备比较昂贵，同时激光雷达传感器的使用也受到了如下雨、下雪和大雾等天气条件的限制；基于视觉相机的定位方法成本较低，而定位的精度取决于获取图像的分辨率大小(分辨率越高，定位精度越高)，该方法极易受到光照条件的影响。另外，在车辆的跟踪系统中，确定车辆运动的数学模型也是极其重要的，一个基于合理的车辆运动数学模型的定位算法能比没有明确模型的定位算法更高效。根据不同的行驶情况，车辆的运动模型主要分为以下等级：强运动等级(如在高速公路中高速行驶的车辆)和弱运动等级(如使用巡航控制系统匀速行驶的车辆)。目前，最简单的两种车辆运动数学模型是恒速度模型(constant-velocity,cv)和恒加速度模型(constant-acceleration，ca)。cv模型假定目标车辆保持一个速度行驶，车辆的加速度比较小，以至于可以忽略；ca模型假定目标车辆拥有恒定的加速度。然而在在实际中，两种数学模型过于简单，都不适用于复杂的车辆行驶。r.a.singer在1970年提出了singer模型，在该模型中，假定目标车辆的加速度是一阶零均值的马尔科夫过程，该模型可以通过调整参数实现自适应，但模型本质上依然是先验模型，因为没有利用目标车辆的在线运动信息。实际强运动情况中，目标车辆的加速度平均值也明显是不为零的；zhou等人提出了“current”模型，该模型本质上是具有自适应非零加速度平均值的singer模型，采用的是改进的瑞利分布(rayleighdistribution)。与singer模型相比，“current”模型更适合描述车辆加速度的统计分布，但该模型依然需要改进，以适应强、弱和均匀的不同车辆运动等级。还有的是，在车辆跟踪过程中，根据目标车辆的运动数学模型采用何种信息融合算法也是极其重要的环节。当前，业界常用的是t2tf(tract-to-trackfusion)算法，该算法可以充分反映传感器的可靠性。此外，changjoolee等人提出了改进的t2tf算法；mas.等人利用加权线性最小二乘算法实现车辆的静态定位，取得了米级的精度；rabieer.等人使用粒子滤波来估计车辆位置，粒子滤波适用于非线性和非高斯的系统，然而在线性高斯系统中，上述几种算法均存在各自的缺点。技术实现要素：本发明的目的在于克服现有技术的不足，提供一种跟踪精度高的基于current-ellipse模型的车辆动态跟踪方法。为实现上述目的，本发明所提供的技术方案为：一种基于current-ellipse模型的车辆动态跟踪方法，包括以下步骤：s1、在gnss信号受限区域部署rsus(roadsideunits)车辆无线定位系统，获取目标跟踪车辆的运动观测数据；s2、以current模型为基础，改进得到目标跟踪车辆的current-ellipse运动数学模型；s3、在步骤s2改进得到的current-ellipse运动数学模型的基础上，对来自rsus的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，从而得到精确的目标跟踪车辆的运动数据。进一步地，所述步骤s2的具体过程如下：current模型使用rayleigh分布来描述车辆行驶加速度的分布，车辆的正向加速度的概率密度函数为：上式中，a为车辆加速度，amax为车辆的正向加速度上限；μ>0，为常数；相应的，根据概率密度函数表达式，计算出加速度的均值和方差：对于车辆的反向加速度，有：根据以下公式，确定正向加速度均值e[a]的取值范围：化简可得：因此，正向加速度的均值范围如下：对于反向加速度有：根据current模型，计算得出车辆加速度的协方差：由于当加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，描述加速度分布的概率密度函数失效，同时加速度的协方差也变得不准确；因此，对current模型进行改进，得到目标跟踪车辆的current-ellipse运动数学模型；具体改进如下：[(4-π)a-max/4,(4-π)amax/4]区间内，用椭圆曲线的一半来代替current模型的协方差曲线，该椭圆的顶点为：和该椭圆的长轴长度a为(4-π)amax/4，短轴长度b为(4-π)amax/6，即椭圆方程如下：在current-ellipse运动数学模型中，车辆加速度的协方差计算公式如下：其中，表示当前加速度的均值，进一步地，所述步骤s3的具体过程如下：s3-1、输入来自rsus的目标跟踪车辆观测数据：观测数据数n、加速度时间常数τ的倒数α、加速度上限αmax、加速度下限α-max；s3-2、初始化采样周期t、状态转移矩阵φ(k/k-1)和φ*(t)、观测矩阵h(k)、观测噪声的协方差矩阵r(k)、误差的协方差矩阵p(k-1)，零初始化预测结果predicted_result[n]；k＝1；s3-3、预测过程：1)先验状态估计：x(k/k-1)＝φ*(t)x(k-1)；2)基于current-ellipse模型计算协方差3)计算过程噪声的协方差矩阵q(k)；4)先验误差的协方差：p(k/k-1)＝φ(k/k-1)p(k-1)φt(k/k-1)+q(k)；s3-4、更新过程：5)优化卡尔曼增益：k(k)＝p(k/k-1)ht(k)[h(k)p(k/k-1)ht(k)+r(k)]-1；6)更新后的估计协方差：p(k)＝[1-k(k)h(k)]p(k/k-1)；7)更新后的状态估计：x(k)＝x(k/k-1)+k(k)[z(k)-h(k)x(k/k-1)]；8)预测结果predicted_result[k]＝x(k)；s3-5、判断k是否小于等于观测数据数n，若是，则k＝k+1，并返回步骤s3-3；若不是，则直接输出预测结果。与现有技术相比，本方案原理及优点如下：1.在封闭区域内引入rsus车辆无线定位系统，辅助目标车辆定位，使得在封闭区域获取比较精确的目标跟踪车辆的运动观测数据。2.基于rsus的车辆跟踪观测数据比较精确，但原始数据依然存在噪声，需要考虑目标跟踪车辆的运动数学模型并完成滤波处理，得到跟踪车辆的最优状态估计。当前经典的current数学模型可以在一定范围内，较好地反映目标车辆的运动状态。然而，当车辆的加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，current模型描述加速度分布的概率密度函数失效了；同时，基于模型计算的加速度协方差也变得不准确，从而影响后续滤波算法的使用。本方案对此以current模型为基础，改进得到current-ellipse模型，在[(4-π)a-max/4,(4-π)amax/4]区间内，用椭圆曲线的一半来代替current模型的协方差曲线，从下面的实验分析结果可以看出，采用本方案的current-ellipse模型，取得的车辆跟踪效果更佳。3.采用基于线性系统的自适应卡尔曼滤波算法，对原始观测数据进行处理，具有方法简单，结果精确，鲁棒性好等优点。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的服务作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1为本发明一种基于current-ellipse模型的车辆动态跟踪方法的原理流程图；图2为经典current模型和本发明所使用的current-ellipse模型的加速度协方差对比图；图3为本发明一种基于current-ellipse模型的车辆动态跟踪方法中自适应卡尔曼滤波的原理流程图；图4为车辆a实验结果的rsme数据对比图；图5为车辆b实验结果的rsme数据对比图；图6为车辆c实验结果的rsme数据对比图；图7为实验地点概况图；图8为对应图7中a,b,c地点的gnss信号强度示意图；图9为gnss跟踪结果示意图(颜色较浅的为实际的行走线路)；图10为采用本发明后的跟踪结果示意图(颜色较深的为实际的行走线路)。具体实施方式下面结合具体实施例对本发明作进一步说明：如图1所示，一种基于current-ellipse模型的车辆动态跟踪方法，包括以下步骤：s1、在gnss信号受限区域部署rsus车辆无线定位系统，获取目标跟踪车辆的运动观测数据；s2、以current模型为基础，改进得到目标跟踪车辆的current-ellipse运动数学模型；本步骤的具体过程如下：current模型使用rayleigh分布来描述车辆行驶加速度的分布，车辆的正向加速度的概率密度函数为：上式中，a为车辆加速度，amax为车辆的正向加速度上限；μ>0，为常数；相应的，根据概率密度函数表达式，计算出加速度的均值和方差：对于车辆的反向加速度，有：根据以下公式，确定正向加速度均值e[a]的取值范围：化简可得：因此，正向加速度的均值范围如下：同理，对于反向加速度有：综上，可以知道在current模型中，[(4-π)a-max/4,(4-π)amax/4]不是加速度均值e[a]的取值范围；故在实际情况中，当目标车辆的加速度均值处在该区间内时，该模型所使用的原概率密度函数就不再适用于精确描述车辆加速度的分布；此外，根据current模型，还可以计算出车辆加速度的协方差：上式中，为当前加速度的均值；当加速度均值处在[(4-π)a-max/4,(4-π)amax/4]范围时，描述加速度分布的概率密度函数失效了，同时加速度的协方差也变得不准确，从而影响了自适应卡尔曼滤波算法所需的加速度协方差矩阵的计算；对此，本实施例提出的改进current-ellipse模型的具体内容如下：如图2所示，是current模型和本实施例提出的current-ellipse模型的加速度协方差图；在[(4-π)a-max/4,(4-π)amax/4]区间内，本实施例采用椭圆曲线的一半来代替原来current模型的协方差曲线；该椭圆的顶点为：和该椭圆的长轴长度a为(4-π)amax/4，短轴长度b为(4-π)amax/6，因此，椭圆方程如下：综合上述内容，在本实施例提出的current-ellipse数学模型下，车辆加速度的协方差计算公式如下：其中，表示当前加速度的均值，s3、在步骤s2改进得到的current-ellipse运动数学模型的基础上，对来自rsus的目标跟踪车辆观测数据执行自适应卡尔曼滤波算法，得到精确的目标跟踪车辆的运动数据；本步骤中应用到：(1)状态方程基于current-ellipse运动数学模型，车辆加速度满足以下公式：其中，为加速度均值，设定为常数；为零均值加速度过程；w(t)为零均值高斯白噪声，τ为车辆加速度机动时间常数，令α＝1/τ；由于车辆的运动是二维的，取正东方向和正北方向的坐标轴，构造车辆的运动状态方程如下：上式中，xe，ve，ae分别表示车辆正东方向的位置，速度和加速度；xn，vn，an分别表示车辆正北方向的位置，速度和加速度；和分别代表两个方向的零均值高斯白噪声，它们各自的协方差分别为和参数αe＝1/τe，αn＝1/τn，其中和分别代表两个方向的机动时间常数；将采样周期设为t，对车辆的运动状态方程离散化，得到以下离散的状态方程：其中，x(k)＝[xe(k)ve(k)ae(k)xn(k)vn(k)an(k)]t，系统的状态转移矩阵为：φ(k/k-1)＝diag[φe(k/k-1),φn(k/k-1)]；控制输入矩阵u(k)＝diag(ue(k),un(k))，ue(k)和un(k)形式相同，可得：随后，得到过程噪声的协方差矩阵：q(k)＝diag(qe(k),qn(k))；此处，qe(k)和qn(k)形式相同，只是下标不同；(2)观测方程利用rsus车辆无线定位系统，可以得到目标车辆的跟踪观测值z(k)＝[eobs(k)nobs(k)]t，其中eobs和nobs分别代表了两个方向上的分量；观测值和系统的真实状态有以下关系：z(k)＝h(k)x(k)+v(k)；其中，观测矩阵能够将状态空间映射到观测空间；观测噪声v(k)＝[ve(k)vn(k)]t为零均值的高斯白噪声，观测噪声的协方差矩阵为：如图3所示，本步骤中执行自适应卡尔曼滤波算法的具体过程如下：s3-1、输入来自rsus的目标跟踪车辆观测数据：观测数据数n、加速度时间常数τ的倒数α、加速度上限αmax、加速度下限α-max；s3-2、初始化采样周期t、状态转移矩阵φ(k/k-1)和φ*(t)、观测矩阵h(k)、观测噪声的协方差矩阵r(k)、误差的协方差矩阵p(k-1)，零初始化预测结果predicted_result[n]；k＝1；s3-3、预测过程：1)先验状态估计：x(k/k-1)＝φ*(t)x(k-1)；2)基于current-ellipse模型计算协方差3)计算过程噪声的协方差矩阵q(k)；4)先验误差的协方差：p(k/k-1)＝φ(k/k-1)p(k-1)φt(k/k-1)+q(k)；s3-4、更新过程：5)优化卡尔曼增益：k(k)＝p(k/k-1)ht(k)[h(k)p(k/k-1)ht(k)+r(k)]-1；6)更新后的估计协方差：p(k)＝[1-k(k)h(k)]p(k/k-1)；7)更新后的状态估计：x(k)＝x(k/k-1)+k(k)[z(k)-h(k)x(k/k-1)]；8)预测结果predicted_result[k]＝x(k)；s3-5、判断k是否小于等于观测数据数n，若是，则k＝k+1，并返回步骤s3-3；若不是，则直接输出预测结果。本实施例中，current-ellipse模型下的车辆加速度均值可以先赋值为一个先验加速度预测值，例如和这样，目标跟踪车辆在例如正东方向上有以下计算过程：xe(k/k-1)＝xe(k-1)+tve(k-1)+t2ae(k-1)/2ve(k/k-1)＝ve(k-1)+tae(k-1)ae(k/k-1)＝ae(k-1)；因此，公式可以简化为：x(k/k-1)＝φ*(t)x(k-1)；其中，为了证明本实施例的有效性，下面进行实验分析：实验一，比较基于不同数学模型的车辆跟踪算法精度本次实验在同一直线路段的实验场地进行，使用3辆参数不同的实验车辆，利用rsus车辆无线定位系统获取目标跟踪车辆的运动数据，并比较基于current数学模型和本实施例的current-ellipse运动数学模型的算法的实验精度(实验真值由激光雷达传感器获取)。3辆汽车的参数如下：表1加速度参数车辆100公里加速时间最大加速度a9.8seconds2.83m/s2b3.5seconds7.93m/s2c4.5seconds6.17m/s2实验结果如图4-图6所示，根据三种条件下实验结果的均方根误差(rootmeansquareerror,rmse)分析可知，基于本实施例的current-ellipse模型的算法具有很高精度，与current模型相比，取得了更好地跟踪效果。实验二，本实施例方法的应用实例本次实验的实验地点为广东工业大学(广州大学城校区)的部分区域，如图7所示；该区域对应的gnss信号强度如图8所示。在该区域，本次实验对比了单纯使用gnss跟踪方法，单纯使用rsus静态定位和本实施例采用的动态跟踪方法。三种方法得到的车辆跟踪轨迹结果分别如图9-图10所示，从这些图分析，可以明显看出本实施例方法取得的效果最好，具有误差小，低漂移，跟踪精确的优点。以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。当前第1页12