CN103395435A - 一种高精度高速列车实时定位系统方法 - Google Patents

Abstract

本发明公开了一种高精度高速列车实时定位系统方法，步骤如下：1）初始化步骤：1.1）列车初始位置

1.2）初始化中需要设定粒子的个数N，并且初始化每一个粒子的坐标；1.3）初始化系统噪声Q和观测噪声R；2）采集里程计信息；3）根据列车运动模型预测列车位置；4）记录新特征点位置；5）根据特征点位置计算粒子权重；6）计算列车位置，加权平均。本方法通过激光雷达探测列车两旁的反射物（电线杆，隧道中的墙壁）等，通过反射物的位置修正列车自身位置，从而最大限度降低累积误差对于列车定位的影响。

Description

一种高精度高速列车实时定位系统方法

技术领域

本发明涉及一种列车定位方法，尤其涉及一种高精度高速列车实时定位系统方法。

背景技术

我国高速列车的运行速度已经达到300Km/h。在列车运行过程中，精确的列车定位对于列车调度，车距控制都有很大的帮助。列车的定位结果直接关系到各个列车的安全。目前，列车定位主要是依靠GPS，车载里程计以及车载惯性导航器件完成，如图1所示。但是这样的定位方式存在诸多问题。

GPS可以在空旷的地方达到高精度的定位（<10m），但是如果列车进入隧道、行驶在山间或者楼宇之间，GPS信号会受到很严重的干扰，使得定位信息非常不准确。而山间，隧道又是列车行驶过程中较为危险的路段，因此在这些路段获得高精度的定位非常重要。

在GPS信号不准确的时候可以采用车载里程计或惯性导航器对列车位置进行补偿。但二者也有各自的缺陷。车载里程计通过记录车轮旋转圈数加之车轮半径信息得到定位结果，但列车轮常常会发生打滑，定位结果会随着时间的增加急剧递增。加装惯性导航器的成本非常高，并且惯性导航器也不能消除累积误差，车辆的定位误差还是会随着时间的增加而不断递增。

发明内容

针对现有技术中存在的上述不足，本发明提供了一种可用以全天候、任意环境下、实时列车位置估计的高精度高速列车实时定位系统方法。

为了解决上述技术问题，本发明采用了如下技术方案：

一种高精度高速列车实时定位系统方法，该方法包括如下步骤：

1）初始化步骤：

1.1）列车初始位置

该列车的初始位置由2维坐标系下的列车位置(x₀，y₀)^T和2维坐标系下的列车朝向角θ组成，T为转置符号；

1.2）初始化中需要设定粒子的个数N，并且初始化每一个粒子的坐标这里[i]为第i个粒子表示的列车位置；每个粒子表征一个列车位置和朝向角；

1.3）初始化系统噪声Q和观测噪声R，系统噪声Q值表示里程计的误差，观测噪声R值表示激光雷达的观测误差；

2）采集里程计信息：

使用车轮里程计信息v_k和电子罗盘方向θ_k，k为时刻；

3）根据列车运动模型预测列车位置：

列车运动模型为：

$s_k^{\left[i\right]}=f(s_{k-1}^{\left[i\right]},u_k,n)=s_{k-1}^{\left[i\right]}+\left[\begin{array}{c}(v+\mathrm{\&delta;v})\&CenterDot;\cos \left({\mathrm{\&theta;}}_k\right)\\ (v+\mathrm{\&delta;v})\&CenterDot;\sin \left({\mathrm{\&theta;}}_k\right)\\ {\mathrm{\&theta;}}_v+\mathrm{\&delta;\&theta;}\end{array}\right]\&CenterDot;\mathrm{dt}$

其中：

表示k时刻第i个粒子估计的列车位置；u_k＝(v_k，θ_k)^T为k时刻的里程计信息；n＝(δv，δθ)^T为噪声，服从均值为0，方差为Q的正态分布；

表示k-1时刻第i个粒子估计的列车位置；v表示列车运行速度；dv和dq分别表示列车速度的误差和列车运行角度的误差；θ_v列车当前运动的方向；dt表示采样时间；

3.1）是否获得激光雷达信息，激光雷达安装在列车的最前方，用于探测列车前面的环境；

3.2）判断是否有环境特征点：

每个粒子有机器人的位置信息，存储环境特征信息；环境特征可以用点表示；激光雷达的信息为z＝ρv^T，其中ρ表示激光雷达到特征点的距离，v为特征点与列车运行方向的夹角；

3.3）判断环境特征点是否被观测过

用激光雷达观测的值和列车位置估算该特征点是否被观测过；

4）记录新特征点位置：

记录新特征点的位置和方差，返回步骤2）进行计算；新特征点的位置和方差由

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})$

${\mathrm{\&Sigma;}}_{n_k}^{\left[i\right]}={\left[{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T{R}^{-1}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right]}^{-1}$

其中

$z_{n_k}={\left[\begin{array}{c}\mathrm{\&rho;}\\ \mathrm{\&upsi;}\end{array}\right]}_{n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k})=\left[\begin{array}{c}\sqrt{{(x_{n_k}-x_k^{\left[i\right]})}^2+{(y_{n_k}-y_k^{\left[i\right]})}^2}\\ {\tan }^{-1}\left(\frac{y_{n_k}-y_k^{\left[i\right]}}{x_{n_k}-x_k^{\left[i\right]}}\right)-\mathrm{\&theta;}\end{array}\right]$

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})=\left[\begin{array}{c}{x}_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\sin ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\\ x_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\cos ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\end{array}\right]$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{s}h(s,\mathrm{\&theta;}){|}_{s=s_{k|k-1}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

公式中：表示观测方程

的逆，μ^[i]为列车位置和观测信息计算环境中特征点的位置；

为

的雅克比矩阵；

表示k时刻第[i]个粒子估计的列车位置，

为第n_k个特征点激光雷达返回信息；h表示观测方程，即通过列车的位置和环境特征点的位置计算激光雷达返回信息的方程；

表示估计结果的协方差；R为观测误差，即激光雷达传感器的误差；ρ_k表示列车位置到特征点的距离；μ_i表示特征点在空间中的位置；

表示k时刻第i个粒子的估计列车的角度；

激光雷达的观测角度；

为计算雅克比矩阵的运算符号；x_iy_i表示第i个特征点的x方向坐标和y方向坐标；

5）根据特征点位置计算粒子权重：

当第n_k个特征点被重新观测到，对每一个粒子的位姿

进行更新；计算每一个粒子的权重并更新特征点位置；

${\hat{z}}_{k,n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]})$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{\mathrm{\&theta;}}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$H_{s,n_k}^{\left[i\right]}={\&dtri;}_{s}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$S_f^{\left[i\right]}=H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\&CenterDot;{\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T+R$

$K_{k,n_k}^{\left[i\right]}={\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;H_{s,n_k}^{\left[i\right]}\&CenterDot;{\left(S_f^{\left[i\right]}\right)}^{-1}$

${\mathrm{\&mu;}}_{n_k,k}^{\left[i\right]}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}+K_{k,n_k}^{\left[i\right]}(z_{n_k,k}-{\hat{z}}_{n_k,k}^{\left[m\right]})$

${\mathrm{\&Sigma;}}_k^{\left[i\right]}=(I-K_{k,n_t}^{\left[i\right]}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}){\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}$

$w_k^{\left[i\right]}=\frac{1}{\sqrt{2\mathrm{\&pi;}{S}_f^{\left[i\right]}}}\exp \{-\frac{1}{2}{(z_k-{\hat{z}}_{n_k,k})}^T{\left(S_f^{\left[i\right]}\right)}^{-1}(z_k-{\hat{z}}_{n_k,k})\}$

公式中：

表示k时刻第i个粒子估计的特征点位置；

为以中间变量，通常被称作卡尔曼增益；

列车对第n_k个特征点的观测结果；

对第n_k个特征点的观测值的估计结果；I为单位矩阵；表示k-1时刻第i个粒子估计的特征点位置；

表示对q求雅克比矩阵；

表示对s求雅克比矩阵;

为一中间变量符号；表示k时刻第i个粒子；表示k时刻对第n_k个特征点的估计结果；

6）计算列车位置，加权平均：

${\hat{s}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left[i\right]}\&CenterDot;s_k^{\left[i\right]}$

表示k时刻对列车位置的估计结果。

与现有技术相比，本发明具有如下优点：

1、本方法通过激光雷达探测列车两旁的反射物（电线杆，隧道中的墙壁）等，通过反射物的位置修正列车自身位置，从而最大限度降低累积误差对于列车定位的影响。

2、通过车载激光雷达探测传感器信息，并且环境特征点（电线杆的圆心，周围树木的圆心，隧道墙壁的拐角等）。在通过观测过的环境特征点信息，修正自身位置，减少累积误差。本方法中使用多个粒子对列车位置进行估计，使用粒子的加权平均值获得列车位置的最优估计结果。

附图说明

图1为现有技术中采用车载里程计或惯性导航器对列车进行定位的流程图；

图2为采用本发明的方法进行列车定位的框图；

图3为采用本发明的方法进行列车定位的流程图；

图4为k时刻列车车头位置的示意图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步详细地描述。

一种高精度高速列车实时定位系统方法，该方法（如图2所示）通过车载激光雷达探测传感器信息，并且环境特征点（电线杆的圆心，周围树木的圆心，隧道墙壁的拐角等）。在通过观测过的环境特征点信息，修正自身位置，减少累积误差。本方法中使用多个粒子对列车位置进行估计，使用粒子的加权平均值获得列车位置的最优估计结果。

该方法包括如下步骤（如图3所示）：

1）初始化步骤：

1.1）列车初始位置

该列车的初始位置由2维坐标系下的列车位置(x₀，y₀)^T和2维坐标系下的列车朝向角θ组成，T为转置符号；在应用过程中可以计算列车的3维坐标系位置和朝向角。

1.2）初始化中需要设定粒子的个数N，并且初始化每一个粒子的坐标

这里[i]为第i个粒子表示的列车位置；每个粒子表征一个列车位置和朝向角。

1.3）初始化系统噪声Q和观测噪声R，系统噪声Q值表示里程计的误差，观测噪声R值表示激光雷达的观测误差；这两个噪声值将用于以后的运算中。

2）采集里程计信息：

使用车轮里程计信息v_k和电子罗盘方向θ_k，k为时刻。

3）根据列车运动模型预测列车位置：

列车运动模型为：

$s_k^{\left[i\right]}=f(s_{k-1}^{\left[i\right]},u_k,n)=s_{k-1}^{\left[i\right]}+\left[\begin{array}{c}(v+\mathrm{\&delta;v})\&CenterDot;\cos \left({\mathrm{\&theta;}}_k\right)\\ (v+\mathrm{\&delta;v})\&CenterDot;\sin \left({\mathrm{\&theta;}}_k\right)\\ {\mathrm{\&theta;}}_v+\mathrm{\&delta;\&theta;}\end{array}\right]\&CenterDot;\mathrm{dt}$

其中：

表示k时刻第i个粒子估计的列车位置；u_k＝(v_k，θ_k)^T为k时刻的里程计信息；n＝(δv，δθ)^T为噪声，服从均值为0，方差为Q的正态分布；

表示k-1时刻第i个粒子估计的列车位置；v表示列车运行速度；dv和dq分别表示列车速度的误差和列车运行角度的误差；θ_v列车当前运动的方向；dt表示采样时间；

在计算中需要随机产生N（这里表示是N个系统噪声Q和观测噪声R：这里共N个粒子。其中，每一个粒子使用一组均值为0，方差为Q的系统噪声和一组均值为0，方差为R的观测噪声）噪声值，用于每一粒子的估计中。图4中实现所画为k时刻列车车头位置，靠它最近的为前一时刻的列车车头位置。“丁”字形表示粒子位置。k-1时刻，假定粒子位置与列车车头位置相同，则k时刻粒子估计位置会分布在列车真实位置附近。

3.1）是否获得激光雷达信息，激光雷达安装在列车的最前方，用于探测列车前面的环境；

3.2）判断是否有环境特征点：

计算中每个粒子有机器人的位置信息，还需要存储环境特征信息。环境特征可以用点表示，如电线杆的圆心，周围树木的圆心，隧道墙壁的拐角等。激光雷达的信息为z＝ρv^T，其中ρ表示激光雷达到特征点的距离，v为特征点与列车运行方向的夹角。

3.3）判断环境特征点是否被观测过

用激光雷达观测的值和列车位置估算该特征点是否被观测过。

4）记录新特征点位置：

记录新特征点的位置和方差，返回步骤2）进行计算；新特征点的位置和方差由

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})$

${\mathrm{\&Sigma;}}_{n_k}^{\left[i\right]}={\left[{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T{R}^{-1}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right]}^{-1}$

其中

$z_{n_k}={\left[\begin{array}{c}\mathrm{\&rho;}\\ \mathrm{\&upsi;}\end{array}\right]}_{n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k})=\left[\begin{array}{c}\sqrt{{(x_{n_k}-x_k^{\left[i\right]})}^2+{(y_{n_k}-y_k^{\left[i\right]})}^2}\\ {\tan }^{-1}\left(\frac{y_{n_k}-y_k^{\left[i\right]}}{x_{n_k}-x_k^{\left[i\right]}}\right)-\mathrm{\&theta;}\end{array}\right]$

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})=\left[\begin{array}{c}{x}_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\sin ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\\ x_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\cos ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\end{array}\right]$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{s}h(s,\mathrm{\&theta;}){|}_{s=s_{k|k-1}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

公式中：

表示观测方程

的逆，μ^[i]为列车位置和观测信息计算环境中特征点的位置；

为

的雅克比矩阵；每一个时刻，不是所有的特征点都可以被观测到，当前以第n_k个特征点被观测到为例介绍该专利算法。其中，

表示k时刻第[i]个粒子估计的列车位置，

为第n_k个特征点激光雷达返回信息；h表示观测方程，即通过列车的位置和环境特征点的位置计算激光雷达返回信息的方程；表示估计结果的协方差；R为观测误差，即激光雷达传感器的误差；ρ_k表示列车位置到特征点的距离；μ_i表示特征点在空间中的位置；

表示k时刻第i个粒子的估计列车的角度；

激光雷达的观测角度；

为计算雅克比矩阵的运算符号；x_iy_i表示第i个特征点的x方向坐标和y方向坐标；

5）根据特征点位置计算粒子权重：

当第n_k个特征点被重新观测到，对每一个粒子的位姿

进行更新；计算每一个粒子的权重并更新特征点位置；

${\hat{z}}_{k,n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]})$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{\mathrm{\&theta;}}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$H_{s,n_k}^{\left[i\right]}={\&dtri;}_{s}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$S_f^{\left[i\right]}=H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\&CenterDot;{\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T+R$

$K_{k,n_k}^{\left[i\right]}={\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;H_{s,n_k}^{\left[i\right]}\&CenterDot;{\left(S_f^{\left[i\right]}\right)}^{-1}$

${\mathrm{\&mu;}}_{n_k,k}^{\left[i\right]}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}+K_{k,n_k}^{\left[i\right]}(z_{n_k,k}-{\hat{z}}_{n_k,k}^{\left[m\right]})$

${\mathrm{\&Sigma;}}_k^{\left[i\right]}=(I-K_{k,n_t}^{\left[i\right]}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}){\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}$

$w_k^{\left[i\right]}=\frac{1}{\sqrt{2\mathrm{\&pi;}{S}_f^{\left[i\right]}}}\exp \{-\frac{1}{2}{(z_k-{\hat{z}}_{n_k,k})}^T{\left(S_f^{\left[i\right]}\right)}^{-1}(z_k-{\hat{z}}_{n_k,k})\}$

公式中：每一个时刻，不是所有的特征点都可以被观测到，当前以第n_k个特征点被观测到为例介绍该专利算法。表示k时刻第i个粒子估计的特征点位置；

为以中间变量，通常被称作卡尔曼增益；

列车对第n_k个特征点的观测结果；

对第n_k个特征点的观测值的估计结果；I为单位矩阵；表示k-1时刻第i个粒子估计的特征点位置；

表示对q求雅克比矩阵；

表示对s求雅克比矩阵;

为一中间变量符号；表示k时刻第i个粒子；

表示k时刻对第n_k个特征点的估计结果。

6）计算列车位置，加权平均：

${\hat{s}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left[i\right]}\&CenterDot;s_k^{\left[i\right]}$

表示k时刻对列车位置的估计结果，

然后进行重新采集。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (1)

1.一种高精度高速列车实时定位系统方法，其特征在于，该方法包括如下步骤：

1）初始化步骤：

1.1）列车初始位置

该列车的初始位置由2维坐标系下的列车位置(x₀，y₀)^T和2维坐标系下的列车朝向角θ组成，T为转置符号；

1.2）初始化中需要设定粒子的个数N，并且初始化每一个粒子的坐标这里[i]为第i个粒子表示的列车位置；每个粒子表征一个列车位置和朝向角；

1.3）初始化系统噪声Q和观测噪声R，系统噪声Q值表示里程计的误差，观测噪声R值表示激光雷达的观测误差；

2）采集里程计信息：

使用车轮里程计信息v_k和电子罗盘方向θ_k，k为时刻；

3）根据列车运动模型预测列车位置：

列车运动模型为：

$s_k^{\left[i\right]}=f(s_{k-1}^{\left[i\right]},u_k,n)=s_{k-1}^{\left[i\right]}+\left[\begin{array}{c}(v+\mathrm{\&delta;v})\&CenterDot;\cos \left({\mathrm{\&theta;}}_k\right)\\ (v+\mathrm{\&delta;v})\&CenterDot;\sin \left({\mathrm{\&theta;}}_k\right)\\ {\mathrm{\&theta;}}_v+\mathrm{\&delta;\&theta;}\end{array}\right]\&CenterDot;\mathrm{dt}$

其中：

表示k时刻第i个粒子估计的列车位置；u_k＝(v_k，θ_k)^T为k时刻的里程计信息；n＝（δv，δθ)^T为噪声，服从均值为0，方差为Q的正态分布；

表示k-1时刻第i个粒子估计的列车位置；v表示列车运行速度；dv和dq分别表示列车速度的误差和列车运行角度的误差；θ_v列车当前运动的方向；dt表示采样时间；

3.1）是否获得激光雷达信息，激光雷达安装在列车的最前方，用于探测列车前面的环境；

3.2）判断是否有环境特征点：

每个粒子有机器人的位置信息，存储环境特征信息；环境特征可以用点表示；激光雷达的信息为z＝ρv^T，其中ρ表示激光雷达到特征点的距离，v为特征点与列车运行方向的夹角；

3.3）判断环境特征点是否被观测过

用激光雷达观测的值和列车位置估算该特征点是否被观测过；

4）记录新特征点位置：

记录新特征点的位置和方差，返回步骤2）进行计算；新特征点的位置和方差由

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})$

${\mathrm{\&Sigma;}}_{n_k}^{\left[i\right]}={\left[{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T{R}^{-1}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right]}^{-1}$

其中

$z_{n_k}={\left[\begin{array}{c}\mathrm{\&rho;}\\ \mathrm{\&upsi;}\end{array}\right]}_{n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k})=\left[\begin{array}{c}\sqrt{{(x_{n_k}-x_k^{\left[i\right]})}^2+{(y_{n_k}-y_k^{\left[i\right]})}^2}\\ {\tan }^{-1}\left(\frac{y_{n_k}-y_k^{\left[i\right]}}{x_{n_k}-x_k^{\left[i\right]}}\right)-\mathrm{\&theta;}\end{array}\right]$

${\mathrm{\&mu;}}^{\left[i\right]}=h^{-1}(s_k^{\left[i\right]},z_{n_k})=\left[\begin{array}{c}{x}_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\sin ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\\ x_k^{\left[i\right]}+{\mathrm{\&rho;}}_{n_k}\&CenterDot;\cos ({\mathrm{\&theta;}}_k^{\left[i\right]}+{\mathrm{\&upsi;}}_{n_k})\end{array}\right]$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{s}h(s,\mathrm{\&theta;}){|}_{s=s_{k|k-1}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

公式中：

表示观测方程

的逆，μ^[i]为列车位置和观测信息计算环境中特征点的位置；

为

的雅克比矩阵；

表示k时刻第[i]个粒子估计的列车位置，为第n_k个特征点激光雷达返回信息；h表示通过列车的位置和环境特征点的位置计算激光雷达返回信息的方程；

表示估计结果的协方差；R为激光雷达传感器的误差；ρ_k表示列车位置到特征点的距离；μ_i表示特征点在空间中的位置；表示k时刻第i个粒子的估计列车的角度；

激光雷达的观测角度；

为计算雅克比矩阵的运算符号；x_iy_i表示第i个特征点的x方向坐标和y方向坐标；

5）根据特征点位置计算粒子权重：

当第n_k个特征点被重新观测到，对每一个粒子的位姿

进行更新；计算每一个粒子的权重并更新特征点位置；

${\hat{z}}_{k,n_k}=h(s_k^{\left[i\right]},{\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]})$

$H_{\mathrm{\&theta;},n_k}^{\left[i\right]}={\&dtri;}_{\mathrm{\&theta;}}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$H_{s,n_k}^{\left[i\right]}={\&dtri;}_{s}h{(s,\mathrm{\&theta;})|}_{s=s_{k|}^{\left[i\right]},\mathrm{\&theta;}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}}$

$S_f^{\left[i\right]}=H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\&CenterDot;{\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;{\left(H_{\mathrm{\&theta;},n_k}^{\left[i\right]}\right)}^T+R$

$K_{k,n_k}^{\left[i\right]}={\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}\&CenterDot;H_{s,n_k}^{\left[i\right]}\&CenterDot;{\left(S_f^{\left[i\right]}\right)}^{-1}$

${\mathrm{\&mu;}}_{n_k,k}^{\left[i\right]}={\mathrm{\&mu;}}_{n_k,k-1}^{\left[i\right]}+K_{k,n_k}^{\left[i\right]}(z_{n_k,k}-{\hat{z}}_{n_k,k}^{\left[m\right]})$

${\mathrm{\&Sigma;}}_k^{\left[i\right]}=(I-K_{k,n_t}^{\left[i\right]}{H}_{\mathrm{\&theta;},n_k}^{\left[i\right]}){\mathrm{\&Sigma;}}_{n_k,k-1}^{\left[i\right]}$

$w_k^{\left[i\right]}=\frac{1}{\sqrt{2\mathrm{\&pi;}{S}_f^{\left[i\right]}}}\exp \{-\frac{1}{2}{(z_k-{\hat{z}}_{n_k,k})}^T{\left(S_f^{\left[i\right]}\right)}^{-1}(z_k-{\hat{z}}_{n_k,k})\}$

公式中：

表示k时刻第i个粒子估计的特征点位置；

为以中间变量；

列车对第n_k个特征点的观测结果；

对第n_k个特征点的观测值的估计结果；I为单位矩阵；

表示k-1时刻第i个粒子估计的特征点位置；

表示对q求雅克比矩阵；

表示对s求雅克比矩阵;

为一中间变量符号；

表示k时刻第i个粒子；

表示k时刻对第n_k个特征点的估计结果；

6）计算列车位置，加权平均：

${\hat{s}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left[i\right]}\&CenterDot;s_k^{\left[i\right]}$

表示k时刻对列车位置的估计结果。

Images