[发明专利]一种高精度高速列车实时定位系统方法

权利要求书

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）根据列车运动模型预测列车位置：

列车运动模型为：

sk[i]=f(sk-1[i],uk,n)=sk-1[i]+(v+δv)·cos(θk)(v+δv)·sin(θk)θv+δθ·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）进行计算；新特征点的位置和方差由

μ[i]=h-1(sk[i],znk)]]>

Σnk[i]=[(Hθ,nk[i])TR-1Hθ,nk[i]]-1]]>

其中

znk=ρυnk=h(sk[i],μnk)=(xnk-xk[i])2+(ynk-yk[i])2tan-1(ynk-yk[i]xnk-xk[i])-θ]]>

μ[i]=h-1(sk[i],znk)=xk[i]+ρnk·sin(θk[i]+υnk)xk[i]+ρnk·cos(θk[i]+υnk)]]>

Hθ,nk[i]=▿sh(s,θ)|s=sk|k-1[i],θ=μnk,k-1[i]]]>

公式中：表示观测方程的逆，μ^[i]为列车位置和观测信息计算环境中特征点的位置；为的雅克比矩阵；表示k时刻第[i]个粒子估计的列车位置，为第n_k个特征点激光雷达返回信息；h表示通过列车的位置和环境特征点的位置计算激光雷达返回信息的方程；表示估计结果的协方差；R为激光雷达传感器的误差；ρ_k表示列车位置到特征点的距离；μ_i表示特征点在空间中的位置；表示k时刻第i个粒子的估计列车的角度；激光雷达的观测角度；为计算雅克比矩阵的运算符号；x_iy_i表示第i个特征点的x方向坐标和y方向坐标；

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

当第n_k个特征点被重新观测到，对每一个粒子的位姿进行更新；计算每一个粒子的权重并更新特征点位置；

z^k,nk=h(sk[i],μnk,k-1[i])]]>

Hθ,nk[i]=▿θh(s,θ)|s=sk|[i],θ=μnk,k-1[i]]]>

Hs,nk[i]=▿sh(s,θ)|s=sk|[i],θ=μnk,k-1[i]]]>

Sf[i]=Hθ,nk[i]·Σnk,k-1[i]·(Hθ,nk[i])T+R]]>

Kk,nk[i]=Σnk,k-1[i]·Hs,nk[i]·(Sf[i])-1]]>

μnk,k[i]=μnk,k-1[i]+Kk,nk[i](znk,k-z^nk,k[m])]]>

Σk[i]=(I-Kk,nt[i]Hθ,nk[i])Σnk,k-1[i]]]>

wk[i]=12πSf[i]exp{-12(zk-z^nk,k)T(Sf[i])-1(zk-z^nk,k)}]]>

公式中：表示k时刻第i个粒子估计的特征点位置；为以中间变量；列车对第n_k个特征点的观测结果；对第n_k个特征点的观测值的估计结果；I为单位矩阵；表示k-1时刻第i个粒子估计的特征点位置；表示对q求雅克比矩阵；表示对s求雅克比矩阵;为一中间变量符号；表示k时刻第i个粒子；表示k时刻对第n_k个特征点的估计结果；

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

s^k=Σi=1Nwk[i]·sk[i]]]>

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