[发明专利]一种基于惯导/视觉里程计的列车定位方法及系统

权利要求书

1.一种基于惯导/视觉里程计的列车定位方法，包括：

获取传感器实时数据，其中，所述传感器实时数据包括惯性传感器数据和摄像机采集到的图像数据；

通过惯性传感器对所述惯性传感器数据进行解算，获取列车的运动信息；

通过视觉里程计对所述图像数据进行处理，获取列车的运动信息；

采用改进的无迹卡尔曼滤波算法对通过惯性传感器以及通过视觉里程计获取的列车的运动信息进行数据融合处理，获取列车的运动信息；

获取的列车的运动信息与线路数据库的信息进行匹配，当获取的列车的运动信息显示为进入公里标的捕获区域时，采集轨旁公里标图像，通过对采集到轨旁公里标图像进行处理确定当前列车的运动信息；

其中，在通过惯性传感器对所述惯性传感器数据进行解算，获取列车的运动信息的过程中，

(1)通过前一时刻列车的速度值与惯性传感器采集的陀螺仪与加速度计的数据，解算出当前时刻列车的速度值；

(2)通过前一时刻列车的位置值、前一时刻列车的速度值与当前时刻列车的速度值，计算地球参数，解算出当前时刻列车的位置值；

(3)通过当前时刻惯性传感器采集的陀螺仪与加速度计的数据，采用辅助重力信息修正陀螺仪误差，通过姿态四元数更新方程，计算当前时刻的姿态四元数与姿态角；

在采用改进的无迹卡尔曼滤波算法对通过惯性传感器获取的列车的运动信息以及通过视觉里程计获取的列车的运动信息进行数据融合处理，获取列车的位置信息的过程中，

(1)对于建立的非线性系统模型进行初始化

其中，为状态变量的初始值，P₀为协方差初始值；

(2)对于建立的非线性系统模型进行状态估计

1)计算sigma点

其中，χ_i|k-1为通过最小偏度单形采样策略获得的Sigma点；

2)时间传播方程

根据系统状态方程对Sigma点集中的每一个Sigma点进行f(·)非线性变换：

χ_i，k|k-1＝f(χ_i|k-1)

对变换后的点集进行加权，得到一步预测状态及方差

3)测量更新方程

根据测量方程对Sigma点集进行非线性变换：

γ_i，k|k-1＝h(χ_i，k|k-1)

进行加权处理得到预测值

计算系统量测输出变量的方差阵：

计算协方差P_xy，k：

其中，K表示计算滤波增益阵；

表示得到状态更新后的滤波值；

P_x，k求状态后验方差阵。

2.如权利要求1所述的一种基于惯导/视觉里程计的列车定位方法，其中，

建立的状态方程为：

其中，X为15维系统误差状态向量，F为一步预测状态转移矩阵，G为系统噪声矩阵，W为系统噪声；

X＝[δr^e δV^e ε^e d b]^T

其中，上标e表示在地固系下表示，δr^e为三维位置误差向量，δV^e为三维速度误差向量，ε^e为姿态误差向量，d为载体系下陀螺仪的三轴漂移向量，b为载体系下加速度计的三轴漂移向量；

建立的观测方程为：

ΔZ_k＝H_kX_k-H_k-1X_k-1+U_k

其中，ΔZ_k＝Z_k-Z_k-1，Z为观测向量，H为观测矩阵，U_k＝V_k-V_k-1，V为观测噪声；k表示第k步；其中，

H＝[0_3×3 I_3×3 0_3×9]

其中，0_n×m为n*m的零矩阵，I_n×m为n*m的单位阵。