CN109781118A - 一种无人车的位置跟踪方法 - Google Patents

Abstract

本发明提供了一种无人车的位置跟踪方法，利用非线性高斯系统状态的滤波方程进行数据滤波，并通过非线性状态函数处理进行状态预测和状态估计。本发明不仅可以对目标进行连续跟踪，也可预测目标运动模型和下一步的运动状态，在复杂环境中选择更有效的跟踪模态；并从统计量入手，使用简单可求的采样点代入非线性函数中得到非线性函数值点集来逼近状态真实值的后验分布，即提高了精度又降低了计算的复杂度。

Description

一种无人车的位置跟踪方法

技术领域

本发明涉及无人驾驶技术领域，具体的说是一种无人车的位置跟踪方法。

背景技术

在车辆已经成为人们主要的代步工具的今天，车辆的智能管理逐渐成为交通的关键问题，车辆数量不断增长，随之而来的交通拥堵问题对人们生活出行带来了很大的不方便，而重复的路网建设等方式早已满足不了人们对现代化交通建设的要求，同时还有许多遗留的成熟建设问题，从整体城市规划到具体某条道路的建设，对其进行大改造是不现实的，因此在这种条件的限制下，解决交通问题的重点就放在了对道路交通系统的管理上。

无人车就是智能交通系统(ITS)中一个很好的应用，不仅可以使用车辆定位监控系统来完成合理分布车辆、指挥调度等功能，从而合理分布车辆，而且可以随时获得车辆在三维空间位置、速度的精确信息。无人车在进行量产之前需要攻克环境感知、路径规划、决策控制等关键技术，其中以安全为第一核心要素，所以在道路上实现对无人车的跟踪与轨迹的准确预测就显得尤为重要。无人车感知系统由多种传感器合作实现，包括智能网联与自动驾驶，如今无人车的发展以多传感器信息的融合为大趋势，目的就是防止单一定位源产生定位死角对无人车的安全驾驶带来安全隐患。

常用导航源——GPS接收机给出的定位信息在卫星颗数数量足够并可见的情况下精度足够，若信号因某些原因丢失或者因强度不够被阻挡，就会导致GPS不能给出载体的精确位置、速度信息，为载体的精确定位带来了技术困难。再由于实际路况与无人车机动的复杂性，单一定位源总是显得不足够，选择一个尽量符合目标实际的运动模型更显得尤为重要。

发明内容

为了解决上述技术问题，本发明提供了一种无人车的位置跟踪方法，能够对车辆的位置和速度进行自适应跟踪和预测。

为达到上述目的，本发明通过以下技术方案来具体实现：

一种无人车的位置跟踪方法，包括：

步骤一、接收包括被跟踪车辆的高度信息在内的实时数据，对实时数据匹配预设了至少加速度阈值和航向角便宜的实施参数的机动模型，然后利用机动模型设定的马尔科夫转移概率组成概率矩阵；

步骤二、利用非线性高斯系统状态的滤波方程(1)对步骤一获得的数据进行滤波；

步骤三、利用UT变换对步骤二获得的数据进行处理以获得具有与系统状态分布相同的均值和协方差的Sigma采样点；

步骤四、根据步骤三获得的Sigma采样点，通过非线性状态函数处理以对被跟踪车辆进行状态预测；

步骤五、根据步骤四获得的状态预测提供的初值及步骤三获得的Sigma采样点，对下一个时间点进行状态估计；

步骤六、采用似然函数确定更新后的模型概率，并输出交互；

步骤七、将被跟踪车辆的3自由度平移信息、3自由度的旋转信息与协方差最终输出，频率与IMU输出保持一致。

步骤二的式(1)中w_k和v_k均为互不相关的高斯白噪声，它们的统计特性为

式中，δ_kj为狄利克雷函数，状态初始值x₀与ω_k、v_k彼此相互独立，且服从高斯分布。根据最小方差估计准则及系统噪声和量测噪声的高斯白噪声性质可知量测值Z^k-1与ω_k、v_k互不相关，所以

基于量测信息Z^k-1的非线性状态预测和输出预测表示如式(4)；

假设p(x_k|Z^k-1)服从高斯分布，均值和协方差分别为和P_k|k-1，整理得到噪声相关条件下的非线性高斯系统的最优一步状态预测递推公式为式(5)；

其中

当状态预测值和协方差P_k+1|k已知，获得新的量测z_k+1后，进行量测更新

选取为状态变量，其中φ_E,φ_N,φ_U为数学平台失准角，δ_vE,δ_vN,δ_vU为载体的东北天向速度误差，δL,δλ,δh为载体纬经度、高度误差，ε_x,ε_y,ε_z,▽_x,▽_y,▽_z为陀螺与加计的随机漂移；选取[Δl,x,y,z,h]为观测值，其中Δl为里程计观测的一定时间间隔内的位移信息，x,y,z为激光雷达给出的三维空间信息，h为气压表观测到的相对高度信息。

步骤四中，由和P_k|k-1计算Sigma点ξ_i,k|k-1，通过非线性状态函数f、h传播后的γ、χ计算后验均值和后验自协方差和后验互协方差

将式(7)代入式(5)可得k+1时刻最优一步状态预测和误差协方差P_k+1|k。

步骤五中，由P_k+1|k计算Sigma点ξ_i,k+1|k，如步骤4.2中计算出k+1时刻的后验均值、后验自协方差和后验互协方差，代入式(6)得到k+1时刻最优滤波状态估计

步骤六中，模型j的似然函数如式(8)。

其中

S_j(k)＝H(k)P_j(k|k-1)H(k)^T+R(k) (10)

模型j的概率如式(11)

基于模型的概率，对每个滤波器的估计结果加权合并，得到总的状态估计值与总的协方差估计值，其中权重就是该时刻模型对目标运动状态描述正确的概率。

本发明具有较强的关于模型参数失配的鲁棒性，较低的关于系统量测噪声及初始状态统计特性的敏感性，并且有极强的关于突变状态的跟踪能力，并且在滤波器达到稳定状态时可以保持这种能力。

本发明可以给出位置与速度的传感器有GPS天线与里程计，因为里程计不存在信号被遮挡的问题，所以系统在信号丢失的状态下有很强的跟踪能力。

本发明不仅可以对目标进行连续跟踪，也可预测目标运动模型和下一步的运动状态，在复杂环境中选择更有效的跟踪模态。

本发明从统计量入手，使用简单可求的采样点代入非线性函数中得到非线性函数值点集来逼近状态真实值的后验分布，即提高了精度又降低了计算的复杂度。

本发明采用的滤波稳定性和收敛性强，可靠性高，便于对无人车状态的有效实时跟踪。

附图说明

下面根据附图和实施例对本发明作进一步详细说明。

图1无人车跟踪系统结构图。

图2多模交互结构图。

图3非线性滤波算法框图。

图4多模交互算法框图。

具体实施方式

本发明实施例提供一种多模型交互智能滤波的无人车跟踪系统，包括：IMU惯导、车端天线、高度气压表、里程计、GIS电子地图、激光雷达、数据融合模块、数据输出模块。其中，作为传感器的IMU、天线、气压表、里程计和激光雷达与辅助手段GIS电子地图共同电连接数据融合模块，数据融合模块电连接数据输出模块，为无人车的决策执行做基础。整个系统由车载计算机提供电源，通过GPRS网络通信，系统结构示意如图1所示。

所述的IMU惯导与车端天线作为提供组合导航信息源的主要模块可以进行优势互补，组合导航作为常见的民用导航方式，考虑成本与精度的折中选取，GPS/INS组合方式使用的比较多。

所述的高度气压表在传统二维平面地图定位中增加了一个维度即高度方向的定位，适用于立交桥、高架等实际路况中。高度气压表体积小、精度高、可以对高度进行实时计算。

所述的里程计传感器可以提供无人车的位移信息。

所述的气压高度表与里程计的组合方式与相应融合算法的配合解算可以解决无人车桥上桥下的位置信息混乱的问题，理论依据为如今的GPS解算后的高度误差一般在几十米的量级之内，而城市中的立交高度差别不会在十米的量级之上。

所述的GIS电子地图作为一种车道级地理信息系统，可以将车辆的位置和GIS数据中已知的路网联系起来，当达到预设的某一条件或多种条件时，就确定了车辆在地图中的精确位置，为多传感器融合后的车辆位置进行比较精确的纠正。

所述的数据融合模块融合惯导、GPS、点云和位移四种定位信息，惯导型号为IMU-200A，GPS数据采集板卡为K700，传感器与数据融合模块采用无线传输方式，无线传输模块型号为WG-8010，所述数据融合模块与数据输出模块电连接采用CPCI连接器，采用RS232串行通信连接，所述里程计无需外部信号输入，不存在类似GNSS天线有信号遮挡的问题。

所述IMU模块经惯导解算后为数据融合提供误差修正项与协方差预设项。

所述数据融合模块使用非线性滤波算法融合IMU、GNSS天线、里程计、气压高度表输出信息，其中IMU输出3自由度角运动(ω_x,ω_y,ω_z)与3自由度线运动(a_x,a_y,a_z)，GNSS天线输出经度(L)、纬度(λ)和速度(υ)信息，里程计给出位移(l)信息，气压高度表给出高度(h)信息，激光雷达与GIS电子地图给出高精度点云信息，融合给出滤波后估计值。

所述的数据输出模块输出6自由度的位姿信息与协方差矩阵，其中包括3自由度的平移信息与3自由度的旋转信息。

本发明实施例提供一种多模型交互智能滤波的无人车跟踪方法，其特征在于，包括：

步骤1：安装传感器，完成无人车跟踪系统的布置。

步骤2：采集传感器与电子地图的实时数据，输送至中心处理器。

步骤3：多模型(IMM)交互。

步骤3.1：定义无人车的机动模型，如框图2所示。根据无人车全局运动的统计特性，以典型参数设定无人车机动等级由低到高的四种模型。

步骤3.2：根据预设机动模型设定相应实时参数，包括加速度阈值与航向角偏移。

步骤3.3：输入交互模型，设定交互模型相对应的马尔科夫转移概率组成概率矩阵，此矩阵控制模型转换。

步骤3.4：非线性滤波算法设计。匹配混合后机动模型。在跟踪的初始阶段，从非机动模型开始进行时间与量测的更新，完成阶段内的量测更新。对于无人车的智能跟踪需考虑实际噪声环境，所以本发明采用的非线性滤波算法为噪声相关条件下的最优滤波，如框图3所示。传统Sigma点强跟踪卡尔曼滤波(SPKF)不仅在噪声相关条件下会失效，而且噪声的统计特性也很大可能是未知或不准确的，这就造成了SPKF滤波精度下降甚至发散。本发明基于最小均方误差估计准则，给出了非线性高斯系统状态的滤波方程与最优一步预测和最优滤波估计，滤波方程如式(1)，其统计特性为(2)，最优一步预测如式(4)，最优滤波估计如式(5)。

式中，w_k和v_k均为互不相关的高斯白噪声，它们的统计特性为

式中，δ_kj为狄利克雷函数，状态初始值x₀与ω_k、v_k彼此相互独立，且服从高斯分布。根据最小方差估计准则及系统噪声和量测噪声的高斯白噪声性质可知量测值Z^k-1与ω_k、v_k互不相关，所以

基于量测信息Z^k-1的非线性状态预测和输出预测表示如式(4)。

假设p(x_k|Z^k-1)服从高斯分布，均值和协方差分别为和P_k|k-1，整理得到噪声相关条件下的非线性高斯系统的最优一步状态预测递推公式为式(5)。

其中

当状态预测值和协方差P_k+1|k已知，获得新的量测z_k+1后，进行量测更新

本发明选取为状态变量，其中φ_E,φ_N,φ_U为数学平台失准角，为载体的东北天向速度误差，δL,δλ,δh为载体纬经度、高度误差，ε_x,ε_y,ε_z,▽_x,▽_y,▽_z为陀螺与加计的随机漂移；选取[Δl,x,y,z,h]为观测值，其中Δl为里程计观测的一定时间间隔内的位移信息，x,y,z为激光雷达给出的三维空间信息，h为气压表观测到的相对高度信息。

步骤3.1：Sigma采样点选取。本着为了更好的近似随机变量经过非线性变换后的统计特性和Sigma点集应该尽可能近似更高阶数的中心矩的原则，本发明选择UT变换Sigma采样点策略。UT变换作为一种计算随机变量经过非线性变换后统计特性的方法，它通过设置Sigma采样点分布和权值逼近样本非线性变换参量的矩，变换后Sigma点具有与系统状态分布相同的均值和协方差。采用UT变换代替求解非线性函数雅克比矩阵，计算状态后验均值和协方差。

步骤3.5：状态预测。

由和P_k|k-1计算Sigma点ξ_i,k|k-1，通过非线性状态函数f、h传播后的γ、χ计算后验均值和后验自协方差和后验互协方差

将式(7)代入式(5)可得k+1时刻最优一步状态预测和误差协方差P_k+1|k。

步骤3.6：状态估计。

由P_k+1|k计算Sigma点ξ_i,k+1|k，如步骤4.2中计算出k+1时刻的后验均值、后验自协方差和后验互协方差，代入式(6)得到k+1时刻最优滤波状态估计P_k+1。

综上，噪声相关条件下的SPKF具有两个计算回路，状态预测与状态估计，其中状态预测可独立进行，而状态估计需要状态预测提供初值。

步骤4：采用似然函数确定更新后的模型概率，最后输出交互，如框图4所示。

模型j的似然函数如式(8)。

其中

S_j(k)＝H(k)P_j(k|k-1)H(k)^T+R(k) (10)

模型j的概率如式(11)

基于模型的概率，对每个滤波器的估计结果加权合并，得到总的状态估计值与总的协方差估计值，其中权重就是该时刻模型对目标运动状态描述正确的概率。

步骤5：数据输出。将无人车的3自由度平移信息、3自由度的旋转信息与协方差最终输出，频率与IMU输出保持一致。

所述步骤2中，GPS接收机数据包括：经度、纬度、高度、东向速度、北向速度和天向速度，频率为1Hz，IMU输出数据包括3自由度陀螺信息、3自由度加速度计信息，频率为100Hz。

所述步骤3中，因为噪声与实际环境与运动状态相关，预设IMM模型所使用的典型参数为系统的扰动噪声。

所述步骤3中，IMM滤波中马尔科夫链状态转移概率的选取实际上就是相当于模型状态方程的状态转移矩阵，直接影响模型误差和模型概率估计的准确性。算法具有模块化的特点，在无人车运动规律较为清楚时，可以自适应选取精度高的模型，当以无人车运动规律不时很清楚时，选取一般的模型保证跟踪的鲁棒性。

所述步骤3中，噪声条件下的非线性滤波算法考虑了噪声实际存在的情况，摒弃了线性近似非线性的步骤，直接将点集代入非线性状态估计函数中得到状态估计值的点集。

[1]如图1所示，本发明所研究的基于多模型交互智能滤波的无人车跟踪系统，核心技术是融合不同冗余度的定位信息，跟踪系统中包括：车端天线、IMU惯导、气压高度表、里程计、激光雷达与GIS电子地图等传感器器件或信息端，其中车端天线接收1Hz的GPS信息，与IMU组合提供无人车的位置与速度信息，并辅以里程计与气压高度表实现无人车在三维平面内的智能跟踪，再结合激光雷达与GIS电子地图实现无人车的精确定位，有更好的跟踪效果。

[2]本发明所研究的基于多模型交互智能滤波的无人车跟踪方法，提出了一种非线性滤波的智能跟踪算法，是在噪声条件下推导出的最优滤波估计，以经典Kalman算法为基础，采用Sigma点强跟踪滤波，辅以多模态交互，实现无人车复杂环境下的连续有效跟踪。多模态交互的非线性滤波算法框图如图2。首先根据全局统计特性设定可能的机动模型与相匹配的马尔科夫转移概率，接着放大局部统计特性设定匹配模型和匹配概率，再使用似然函数更新模型概率，最后滤波融合估计后输出交互，框图如图4。

[3]本发明所研究的基于多模型交互智能滤波的无人车跟踪系统，IMM引入多个目标运动模型，具有自适应的特点，有效地对各个模型的概率进行调整。IMM交互输入后经滤波更新模型概率反馈至滤波并输出交互，其中各子滤波器并行(每个模型匹配一个子滤波器)，初始交互经统计特性设定，滤波开始后根据似然函数更新模型概率，加权概率输出融合后交互，从而实现在复杂环境下对运动目标的跟踪。

[4]如图3所示，本发明所研究的基于多模型交互智能滤波的无人车跟踪方法，使用非线性滤波进行IMM的融合估计，核心在UT变换，按照规则在原状态分布中选取采样点，约束条件为采样点的均值和协方差等于原状态分布的均值和协方差。整个滤波分为两个回路，将Sigma点集分别代入到非线性函数中，通过点集分别进行状态预测与状态估计，其中状态预测是独立计算的，可以离线进行，而状态估计回路依赖于状态预测回路，如此得到的非线性变换后估计值的均值与协方差精度最少有Taylor展开式的2阶精度。

最后应说明的是：以上所述仅为发明的优选实施例而已，并不用于限制发明，尽管参照前述实施例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在发明的保护范围之内。

Claims (5)

1.一种无人车的位置跟踪方法，包括：

步骤一、接收包括被跟踪车辆的高度信息在内的实时数据，对实时数据匹配预设了至少加速度阈值和航向角便宜的实施参数的机动模型，然后利用机动模型设定的马尔科夫转移概率组成概率矩阵；

步骤二、利用非线性高斯系统状态的滤波方程(1)对步骤一获得的数据进行滤波；

步骤三、利用UT变换对步骤二获得的数据进行处理以获得具有与系统状态分布相同的均值和协方差的Sigma采样点；

步骤四、根据步骤三获得的Sigma采样点，通过非线性状态函数处理以对被跟踪车辆进行状态预测；

步骤五、根据步骤四获得的状态预测提供的初值及步骤三获得的Sigma采样点，对下一个时间点进行状态估计；

步骤六、采用似然函数确定更新后的模型概率，并输出交互；

步骤七、将被跟踪车辆的3自由度平移信息、3自由度的旋转信息与协方差最终输出，频率与IMU输出保持一致。

2.如权利要求1所述的方法，其特征在于，

步骤二的式(1)中w_k和v_k均为互不相关的高斯白噪声，它们的统计特性为

式中，δ_kj为狄利克雷函数，状态初始值x₀与ω_k、v_k彼此相互独立，且服从高斯分布。根据最小方差估计准则及系统噪声和量测噪声的高斯白噪声性质可知量测值Z^k-1与ω_k、v_k互不相关，所以

基于量测信息Z^k-1的非线性状态预测和输出预测表示如式(4)；

假设p(x_k|Z^k-1)服从高斯分布，均值和协方差分别为和P_k|k-1，整理得到噪声相关条件下的非线性高斯系统的最优一步状态预测递推公式为式(5)；

其中

当状态预测值和协方差P_k+1|k已知，获得新的量测z_k+1后，进行量测更新

选取为状态变量，其中φ_E,φ_N,φ_U为数学平台失准角，为载体的东北天向速度误差，δL,δλ,δh为载体纬经度、高度误差，ε_x,ε_y,ε_z,为陀螺与加计的随机漂移；选取[Δl,x,y,z,h]为观测值，其中Δl为里程计观测的一定时间间隔内的位移信息，x,y,z为激光雷达给出的三维空间信息，h为气压表观测到的相对高度信息。

3.如权利要求2所述的方法，其特征在于，

步骤四中，由和P_k|k-1计算Sigma点ξ_i,k|k-1，通过非线性状态函数f、h传播后的γ、χ计算后验均值和后验自协方差和后验互协方差

将式(7)代入式(5)可得k+1时刻最优一步状态预测和误差协方差P_k+1|k。

4.如权利要求3所述的方法，其特征在于，

步骤五中，由P_k+1|k计算Sigma点ξ_i,k+1|k，如步骤4.2中计算出k+1时刻的后验均值、后验自协方差和后验互协方差，代入式(6)得到k+1时刻最优滤波状态估计P_k+1。

5.如权利要求4所述的方法，其特征在于，

步骤六中，模型j的似然函数如式(8)。

其中

S_j(k)＝H(k)P_j(k|k-1)H(k)^T+R(k) (10)

模型j的概率如式(11)

基于模型的概率，对每个滤波器的估计结果加权合并，得到总的状态估计值与总的协方差估计值，其中权重就是该时刻模型对目标运动状态描述正确的概率。