[发明专利]一种用于小车室外导航的状态预测方法、装置及存储介质

权利要求书

1.一种用于小车室外导航的状态预测方法，其特征在于，包括以下步骤：

获取小车的航向角、转向角及前向移动速度和横向侧滑速度，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型；

确定小车的观测向量，根据观测向量判断小车的位置变化是否超过设定阈值，若是，则根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标；

根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量。

2.根据权利要求1所述的用于小车室外导航的状态预测方法，其特征在于，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型，具体包括，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型

其中，ψ为航向角，δ为转向角，V_x和V_y分别为小车的前向移动速度和横向侧滑速度，分别为地球坐标系下的经度值和纬度值的微分，为偏航率，L为车头的GPS信号接收机与车中心的轴向距离。

3.根据权利要求1所述的用于小车室外导航的状态预测方法，其特征在于，所述小车的观测向量，具体包括，其中，E_T^GPS、N_T^GPS和ψ^GPS分别为小车上的GPS信号接收机接收到的经度坐标、纬度坐标和航向角，为偏航率，V_X^odom为小车前向移动速度。

4.根据权利要求3所述的用于小车室外导航的状态预测方法，其特征在于，根据所述预测模型及观测向量判断小车的位置变化是否超过设定阈值，具体包括，利用小车的位置变化判定公式确定小车的位置变化是否超过设定阈值，所述小车的位置变化判定公式为

当λ_k＝1时，小车的位置变化超过设定阈值，当λ_k＝0时，小车的位置变化未超过设定阈值，Z_k、Z_k-1为k、k-1时刻的观测向量，ε、σ分别为第一设定阈值、第二设定阈值，ξ＝[1 1 1 00]，为滤波估计值。

5.根据权利要求1所述的用于小车室外导航的状态预测方法，其特征在于，根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标，具体包括，若信号接收机获取的GPS信号值准确，则以GPS信号值确定小车当前的经度坐标和纬度坐标，若信号接收机获取的GPS信号值不准确，则以GPS信号估计模型确定小车当前的经度坐标和纬度坐标，所述GPS信号估计模型为

其中，μ＝[0 0 0 0 1 0]，ω＝[0 0 1 0 0 0]，E_T、N_T分别为小车当前的经度坐标和纬度坐标。

6.根据权利要求5所述的用于小车室外导航的状态预测方法，其特征在于，还包括，根据GPS信号值判断公式判断信号接收机获取的GPS信号值是否准确，所述GPS信号值判断公式为

其中，α＝[1 0 0 0 0]、β＝[0 1 0 0 0]、θ＝[0 0 1 0 0]，当r_k＝1时，则信号接收机获取的GPS信号值不准确，当r_k＝0，则信号接收机获取的GPS信号值准确。

7.根据权利要求6所述的用于小车室外导航的状态预测方法，其特征在于，根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量，具体包括，根据所述小车当前的经度坐标、纬度坐标及预测模型获取状态估计方程模型及观测方程模型，进行滤波预测及滤波矫正，获取小车的的预测状态向量。