基于毫米波雷达的车辆运动轨迹跟踪方法和系统技术方案

本发明专利技术公开了一种基于毫米波雷达的车辆运动轨迹跟踪方法和系统，毫米波雷达实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度，其中跟踪方法包括：1、建立车辆状态转移方程和状态观测方程；2、获取车辆初始状态；设置粒子群并初始化每个粒子的状态；3、根据当前时刻毫米波雷达获取到的车辆观测量对粒子群进行第一次更新；4、对粒子群进行重新采样做第二次更新；5、根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻车辆的位置和速度；6、跳转至步骤3进行下一时刻的车辆跟踪。该方法可以显著降低车辆观测数据中具有随机性和间歇性野值对车辆状态估计的影响，具有较强的抗奇异值能力。

【技术实现步骤摘要】
基于毫米波雷达的车辆运动轨迹跟踪方法和系统
本专利技术属于目标跟踪定位
，具体涉及一种车辆行驶中的跟踪方法和系统。
技术介绍
随着汽车工业的飞速发展和人民生活水平的提高，汽车已然成为生活中必不可少的一部分，因此日益增加的汽车数量所带来的安全问题就越来越受到人们的关注。为了帮助人们判断行车情况，同时也为自动驾驶护航，高级驾驶辅助系统(AdvancedDrivingAssistantSystem，ADAS)被引入大众的视野。ADAS利用安装在车上各式各样的传感器(如：毫米波雷达、激光雷达、单\双目摄像头以及卫星导航)收集环境信息以分析实时环境，其中车载毫米波雷达不易受到目标表面形状和颜色的影响，也不受天时天候的阻碍，具有环境适应性强，探测性能稳定的特点，是汽车安全技术的研究热点。在车辆A上安装车载毫米波雷达，对毫米波雷达发射波及回波的处理可以获取车辆A周围其他车辆相对于车辆A的大致距离、径向速度和方位角，在复杂的环境中如何利用这三个观测量准确跟踪目标车辆的行进轨迹是一个极富挑战性的研究课题。考虑到现有毫米波器件、集成技术和相关的信号处理和解决方案尚且没有完全成熟，现有目标跟踪相关技术存在以下问题：传统的目标跟踪一般用卡尔曼滤波实现，这一滤波算法只有针对线性模型且在观测噪声和状态转移噪声均服从高斯分布的时候才是最优的。大多数应用中针对非线性模型采用扩展卡尔曼滤波算法实现目标跟踪，即在短时间内将模型视为线性模型，然后利用线性卡尔曼滤波算法解决。该方法在做局部线性化时损失了一定的精度，尤其对于非线性度很强的模型，这样的算法往往跟踪效果不佳。而且，传感器的观测噪声往往不服从高斯分布，其中一种典型的情况就是观测量中存在奇异值(或者叫野值)，其分布不满足高斯分布，这样的奇异值可能来源于实时情景中的目标遮挡或车辆突发抖动，针对这种情况，传统滤波算法的跟踪效果较差。以上所述非线性模型的处理和非高斯观测噪声的存在都对目标跟踪算法提出了巨大的挑战。
技术实现思路
专利技术目的：针对现有技术中存在的问题，本专利技术提供了一种基于毫米波雷达的车辆运动轨迹跟踪方法，该方法可以显著降低车辆观测数据中具有随机性和间歇性野值对车辆状态估计的影响，具有较强的抗奇异值能力。技术方案：本专利技术一方面公开了一种基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；包括：(1)建立车辆状态转移方程：其中为k时刻车辆的状态量，xk、yk表示k时刻车辆相对于雷达的横、纵坐标，表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，Δt表示毫米波雷达的观测时间间隔，sk为状态转移噪声，服从零均值、协方差为Q的高斯分布，即建立车辆状态观测方程：其中rk、ak、vk分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；h(·)为观测函数；ok为观测噪声，服从零均值、协方差为R/Wk的高斯分布，即R为观测噪声协方差常数矩阵；Wk为观测噪声协方差权重矩阵，Wk＝diag([wk，1，wk，2，wk，3])，每一权值wk，m均服从Gamma分布，即(2)给定车辆初始状态的期望<Φ0>；产生包含S个粒子的集合，所述粒子集中第s个粒子在第k时刻的状态用S表示；以<Φ0>为均值，Q为协方差的高斯分布随机初始化粒子群中每一个粒子的状态为及其权重为初始时刻表示为k＝0；(3)令k＝k+1，根据k时刻毫米波雷达获取到的观测量zk，第一次更新粒子群，更新后第s个粒子的状态是一个以为均值、Q为协方差的高斯变量，其权重满足：其中为第s个粒子在(k-1)时刻完成第二次更新后的权重，为观测量zk的概率密度函数；(4)对粒子群进行重新采样完成第二次更新，更新后粒子的状态为权重为(5)根据第二次更新后的粒子权重和状态，计算当前k时刻车辆的状态估计<Φk>，得到当前时刻k车辆相对于毫米波雷达的位置和速度；(6)跳转至步骤(3)进行下一时刻的车辆跟踪。将k时刻观测噪声协方差权重矩阵Wk视为时变参数矩阵是，所述步骤(3)可以采用如下步骤对粒子权重做第一次更新：(3.1)将k时刻观测噪声协方差权重矩阵Wk视为时变参数矩阵，用EM算法估算Wk的估计值<Wk>：计算wk，m的估算值：其中δi，m表示向量(zi-h(θi))的第m个元素，Rm，m代表矩阵R对角线上的第m个元素；则有：<Wk>＝diag([<wk，1>，<wk，2>，<wk，3>])；(3.2)观测量zk的概率密度函数为：(3.3)粒子权重第一次更新的递推公式为：将Wk视为与θk相互独立的变量时，所述步骤(3)可以采用如下步骤对粒子权重做第一次更新：(3.1′)将Wk视为变量，与θk相互独立，则观测量zk的概率密度函数的计算式为：其中λm为矩阵R-1对角线上的第m个元素，等于向量的第m个元素，zk，m为观测量zk的第m个元素；m＝1，2，3；(3.2′)粒子权重第一次更新的递推公式为：所述步骤(4)具体包括：(4.1)对粒子权重做累积和：构建累积和序列ψk，ψk＝[ψk，0，ψk，1，...，ψk，S]，其中ψk，0＝0，ψk，S＝1；ψk中相邻两个元素构成一个区间，总共有S个区间；(4.2)在0到1之间产生随机数r，得到参考量(4.3)构建等差数列D＝[d1，d2，…，dS]，其中的第s个元素可以表示为：统计数列D中的每一个元素ds，位于ψk所构成的S个区间中的区间序号数，记为Is，s＝1，...，S，构成序列I＝[I1，I2，…，IS]；(4.4)根据序列I对粒子群进行重采样，并更新粒子权重；所述第s个粒子重采样后的状态为：权重为：所述步骤(5)采用如下方式计算当前时刻k车辆相对于毫米波雷达的状态估计<Φk>：<Φk>中的元素依次为当前时刻k车辆相对于毫米波雷达的横、纵坐标和横、纵向速度。本专利技术还公开了实现上述车辆运动轨迹跟踪方法的系统，包括：车辆跟踪模型建立模块，用于建立车辆状态转移方程和状态观测方法；初始化模块，用于获取车辆的初始状态，设置粒子群中粒子的初始状态；粒子群第一次更新模块，用于根据当前时刻k毫米波雷达获取到的车辆观测量zk对粒子群进行第一次更新；粒子群第二次更新模块，用于对粒子群进行重新采样做第二次更新；车辆状态估计模块，用于根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计，得到当前时刻k车辆的位置和速度。有益效果：与现有技术相比，本专利技术公开的车辆运动轨迹跟踪方法具有以下优点：1、本专利技术通过对车辆状态观测方程中的观测噪声所服从的高斯本文档来自技高网...

【技术保护点】
1.基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；其特征在于，包括：/n(1)建立车辆状态转移方程：/n

【技术特征摘要】
1.基于毫米波雷达的车辆运动轨迹跟踪方法，所述毫米波雷达用于实时获取车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；其特征在于，包括：
(1)建立车辆状态转移方程：



其中为k时刻车辆的状态量，xk、yk表示k时刻车辆相对于雷达的横、纵坐标，表示k时刻车辆相对于雷达的横、纵向速度；f(·)为状态转移函数，A为状态转移矩阵，△t表示毫米波雷达的观测时间间隔，sk为状态转移噪声，服从零均值、协方差为Q的高斯分布，即
建立车辆状态观测方程：



其中rk、ak、vk分别为k时刻毫米波雷达获取的车辆与雷达的距离、车辆相对于雷达的方位角和径向速度；h(·)为观测函数；ok为观测噪声，服从零均值、协方差为R/Wk的高斯分布，即R为观测噪声协方差常数矩阵；Wk为观测噪声协方差权重矩阵，Wk＝diag([wk,1,wk,2,wk,3])，每一权值wk,m均服从Gamma分布，即
(2)给定车辆初始状态的期望<Φ0>；产生包含S个粒子的集合，所述粒子集中第s个粒子在第k时刻的状态用表示；以<Φ0>为均值，Q为协方差的高斯分布随机初始化粒子群中每一个粒子的状态及其权重为初始时刻表示为k＝0；
(3)令k＝k+1，根据k时刻毫米波雷达获取到的观测量zk，第一次更新粒子群，更新后第s个粒子的状态是一个以为均值、Q为协方差的高斯变量，相应的权重满足：
其中为第s个粒子在(k-1)时刻完成第二次更新后的权重，为观测量zk的概率密度函数；
(4)对粒子群进行重新采样完成第二次更新，更新后粒子的状态为权重为
(5)根据第二次更新后的粒子权重和状态，计算当前时刻k车辆的状态估计<Φk>，得到当前时刻k车辆相对于毫米波雷达的位置和速度；
(6)跳转至步骤(3)进行下一时刻的车辆跟踪。


2.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(3)的步骤为：
(3.1)将k时刻观测噪声协方差权重矩阵Wk视为时变参数矩阵，用EM算法估算Wk的估计值<Wk>：
计算wk,m的估算值：其中δi,m表示向量(zi-h(θi))的第m个元素，Rm,m代表矩阵R对角线上的第m个元素；则有：
<Wk>＝diag([<wk,1>,<wk,2>,<wk,3>])；
(3.2)观测量zk的概率密度函数为：



(3.3)粒子权重第一次更新的递推公式为：





3.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(3)的步骤为：
(3.1')将Wk视为变量，与θk相互独立，则观测量zk的概率密度函数的计算式为：



其中λm为矩阵R-1对角线上的第m个元素，等于向量的第m个元素，zk,m为观测量zk的第m个元素；m＝1,2,3；
(3.2')粒子权重第一次更新的递推公式为：





4.根据权利要求1所述的车辆运动轨迹跟踪方法，其特征在于，所述步骤(4)具体包括：
(4.1)对粒子权重做累积和：



构建累积和序列ψk，ψk＝[ψk,0,ψk,1,...,ψk,S]，其中ψk,0＝0，ψk,S＝1；ψk中相邻两个元素构成一个区间，总共有S个区间；
(4.2)在0到1之间产生随机数r，得到参考量
(4.3)构建等差数列D＝[d1,d2,…,dS]，其中的第s个元素可以表示为：统计数列D中的每一个元素ds，位于ψk所构成的S个区间中的区间序号数，记为Is，s＝1,...,...

【专利技术属性】
技术研发人员：武其松，赖志超，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏;32