CN112611377A - 一种用于小车室外导航的状态预测方法、装置及存储介质 - Google Patents

Abstract

本发明涉及一种用于小车室外导航的状态预测方法、装置及计算机可读存储介质，所述小车室外导航的状态预测方法包括以下步骤：获取小车的航向角、转向角及前向移动速度和横向侧滑速度，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型；确定小车的观测向量，根据观测向量判断小车的位置变化是否超过设定阈值，若是，则根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标；根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量。本发明所述的用于小车室外导航的状态预测方法，降低了室外导航中状态预测的时间复杂度。

Description

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

技术领域

本发明涉及小车室外导航技术领域，尤其涉及一种用于小车室外导航的状态预测方法、装置及计算机可读存储介质。

背景技术

移动小车室外定位导航一直是智能交通领域的关注热点，GPS是目前主流的实现小车室外定位的依赖工具，因为此方法有非常突出的优点，即位置信息直接在绝对帧中给出，而且不需要增加额外的设备。

GPS接收机在实际应用中存在较大定位误差，不能满足室外导航的定位精度需求，此外，在导航过程中可能存在信号接收机被树木或者建筑物等障碍物遮挡的情况，导致GPS信号强度降低甚至信号中断，使得小车无法实现定位，影响导航的精度。

现有的室外导航定位算法大都采用卡尔曼滤波算法进行传感器信息融合，主要考虑如何提高滤波算法的精准性，而缺乏对算法的时间复杂度进行分析；一般的，在树木或者建筑物等障碍物遮挡的环境下，经常发生GPS信号强度低甚至中断的情况，导致GPS定位数据无法及时更新，考虑如何保证室外导航的精准性，增加其环境适应性是必要的，但是导航的时间消耗同样值得关注，减少状态预测的计算量，降低状态预测的时间复杂度可以使室外导航任务更加高效的完成。

发明内容

有鉴于此，有必要提供一种用于小车室外导航的状态预测方法、装置及计算机可读存储介质，用以解决现有技术中室外导航中状态预测的时间复杂度较大的问题。

本发明提供了一种用于小车室外导航的状态预测方法，包括以下步骤：

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

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

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

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

其中，ψ为航向角，δ为转向角，V_x和V_y分别为小车的前向移动速度和横向侧滑速度，

分别为地球坐标系下的经度值和纬度值的微分，

为偏航率，L为车头的GPS信号接收机与车中心的轴向距离。

进一步地，所述小车的观测向量，具体包括，

其中，E_T ^GPS、N_T ^GPS和ψ^GPS分别为小车上的GPS信号接收机接收到的经度坐标、纬度坐标和航向角，

为偏航率，V_X ^odom为小车前向移动速度。

进一步地，根据所述预测模型及观测向量判断小车的位置变化是否超过设定阈值，具体包括，利用小车的位置变化判定公式确定小车的位置变化是否超过设定阈值，所述小车的位置变化判定公式为

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

为滤波估计值。

进一步地，根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标，具体包括，若信号接收机获取的GPS信号值准确，则以GPS信号值确定小车当前的经度坐标和纬度坐标，若信号接收机获取的GPS信号值不准确，则以GPS信号估计模型确定小车当前的经度坐标和纬度坐标，所述GPS信号估计模型为

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

进一步地，所述用于小车室外导航的状态预测方法还包括，根据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信号值准确。

进一步地，根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量，具体包括，根据所述小车当前的经度坐标、纬度坐标及预测模型获取状态估计方程模型及观测方程模型，进行滤波预测及滤波矫正，获取小车的的预测状态向量。

进一步地，所述状态估计方程模型及观测方程模型分为

其中，E_T、N_T、V_X、ψ、

g_b、ψ_b、O_b分别为经度坐标、纬度坐标、前向移动速度、航向角、偏航率、陀螺仪偏差、航向角偏差、里程计偏差，ω、v均为噪声向量。

本发明还提供了一种用于小车室外导航的状态预测装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如上述任一技术方案所述的用于小车室外导航的状态预测方法。

本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机该程序被处理器执行时，实现如上述任一技术方案所述的用于小车室外导航的状态预测方法。

与现有技术相比，本发明的有益效果包括：通过获取小车的航向角、转向角及前向移动速度和横向侧滑速度，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型；确定小车的观测向量，根据观测向量判断小车的位置变化是否超过设定阈值，若是，则根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标；根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量；降低了室外导航中状态预测的时间复杂度。

附图说明

图1为本发明提供的用于小车室外导航的状态预测方法的流程示意图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

实施例1

本发明实施例提供了一种用于小车室外导航的状态预测方法，其流程示意图，如图1所示，所述方法以下步骤：

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

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

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

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

其中，ψ为航向角，δ为转向角，V_x和V_y分别为小车的前向移动速度和横向侧滑速度，

分别为地球坐标系下的经度值和纬度值的微分，

为偏航率，L为车头的GPS信号接收机与车中心的轴向距离；

需要说明的是，

描述了小车在地球坐标系下的位置变化情况；具体实施例时，小车的状态向量为

和

分别为经度坐标、纬度坐标和航向角的估计值，

表示小车的前向移动速度的估计值，

为偏航率的估计值，

为陀螺仪偏差的估计值，

为航向角的估计值，

为里程计偏差的估计值；

优选的，所述小车的观测向量，具体包括，

其中，E_T ^GPS、N_T ^GPS和ψ^GPS分别为小车上的GPS信号接收机接收到的经度坐标、纬度坐标和航向角，

为偏航率，V_X ^odom为小车前向移动速度；

优选的，根据所述预测模型及观测向量判断小车的位置变化是否超过设定阈值，具体包括，利用小车的位置变化判定公式确定小车的位置变化是否超过设定阈值，所述小车的位置变化判定公式为

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

为滤波估计值；

一个优选实施例中，ε、σ分别为4和8，当时，小车的位置变化超过设定阈值，再根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标；

优选的，根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标，具体包括，若信号接收机获取的GPS信号值准确，则以GPS信号值确定小车当前的经度坐标和纬度坐标，若信号接收机获取的GPS信号值不准确，则以GPS信号估计模型确定小车当前的经度坐标和纬度坐标，所述GPS信号估计模型为

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

优选的，所述用于小车室外导航的状态预测方法还包括，根据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信号值准确；

具体实施时，当λ_k＝1、r_k＝1成立时，则传感器向KF(卡尔曼滤波)估计器发送信息，使KF估计器工作，KF估计器根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量。

优选的，根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量，具体包括，根据所述小车当前的经度坐标、纬度坐标及预测模型获取状态估计方程模型及观测方程模型，进行滤波预测及滤波矫正，获取小车的的预测状态向量。

优选的，所述状态估计方程模型及观测方程模型分为

其中，E_T、N_T、V_X、ψ、

g_b、ψ_b、O_b分别为经度坐标、纬度坐标、前向移动速度、航向角、偏航率、陀螺仪偏差、航向角偏差、里程计偏差，ω、v均为噪声向量；

一个具体实施例中，通过预测模型对状态向量以及误差协方差矩阵进行滤波预测，滤波预测过程表示为

P′_k＝AP_k-1A^T+Q_ω

将传感器测量信息通过滤波算法与预测信息融合，计算出卡尔曼增益和最终的状态估计信息，即滤波矫正，滤波矫正过程为

K_k＝P′_kH^T(HP′_kH^T+(1-γ_k)R_υ+(1-λ_k)σ²I)^-1

其中，A、H、Q_ω、X_k、R_υ、

P_k、

K_k、

分别为状态转移矩阵、测量矩阵、过程估计误差协方差矩、状态向量真实值、测量误差协方差矩阵、状态向量卡尔曼滤波估计值、卡尔曼误差协方差矩阵、状态向量预测值、预测误差协方差矩阵、卡尔曼增益、测量余量；状态向量预测值即为小车的的预测状态向量；

需要说明的是，X_k为k时刻的小车状态信息，Z_k为由车载传感器测得的小车状态信息，

为卡尔曼滤波估计值，引入λ_k确保估计状态信息和测量状态信息只有在小车的状态变化超过设定阈值时才被传递到KF估计器，利用γ_k及时的使能GPS信号估计器，使KF滤波过程不受GPS信号中断的影响，λ_k和γ_k代表算法中的触发事件，是基于事件卡尔曼滤波算法的关键；最后，将估计信息和测量信息传到KF估计器进行卡尔曼滤波迭代计算，给出小车的最新状态信息估计向量

一个具体实施例中，以导航的精准度和整体时间消耗作为评价指标，在进行室外导航之前要为小车提供目标点的经、纬度坐标以及航向角信息，导航开始之后记录整个导航过程的耗时，作为本发明实施例所述用于小车室外导航的状态预测方法的时间评价指标，构建位置精准度评价模型如下，

e_ψ＝|ψ^goal-ψ|

C＝sin(N_T ^goal)sin(N_T)cos(E_T ^goal-E_T)+cos(N_T ^goal)cos(N_T)

其中，所述e_p和e_ψ分别代表位置误差和航向角误差，其中ψ^goal、N_T ^goal和E_T ^goal分别代表目标点的航向角、纬度坐标和经度坐标，相应的ψ、N_T和E_T表示当前小车的航向角、纬度坐标和经度坐标。

实施例2

本发明实施例提供了一种用于小车室外导航的状态预测装置，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如实施例1所述的用于小车室外导航的状态预测方法。

实施例3

本发明提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机该程序被处理器执行时，实现如实施例1所述的用于小车室外导航的状态预测方法。

本发明所述的用于小车室外导航的状态预测方法、装置及计算机可读存储介质，通过获取小车的航向角、转向角及前向移动速度和横向侧滑速度，根据航向角、转向角及前向移动速度和横向侧滑速度，建立描述小车在地球坐标系下的位置变化的预测模型；确定小车的观测向量，根据观测向量判断小车的位置变化是否超过设定阈值，若是，则根据所述信号接收机获取的GPS信号值是否准确，确定小车当前的经度坐标和纬度坐标；根据所述小车当前的经度坐标、纬度坐标及预测模型，获取小车的的预测状态向量；降低了室外导航中状态预测的时间复杂度；

本发明所述技术方案将事件触发机制加入KF算法，提供了模拟GPS信号的GPS信号估计器，将小车的状态变化作为KF的触发事件，当状态偏差超过一定阈值时，KF估计器响应并给出新的状态估计，即使GPS信号中断发生，GPS信号估计器为KF估计器提供连续可用的位置信息使滤波过程不受影响；从而在保证室外导航的精度的同时，降低算法的时间复杂度，减少完成导航任务所需的时间。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

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

8.根据权利要求7所述的用于小车室外导航的状态预测方法，其特征在于，其特征在于，所述状态估计方程模型及观测方程模型分为

其中，E_T、N_T、V_X、ψ、

g_b、ψ_b、O_b分别为经度坐标、纬度坐标、前向移动速度、航向角、偏航率、陀螺仪偏差、航向角偏差、里程计偏差，ω、v均为噪声向量。

9.一种用于小车室外导航的状态预测装置，其特征在于，包括处理器以及存储器，所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现如权利要求1-8任一所述的用于小车室外导航的状态预测方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机该程序被处理器执行时，实现如权利要求1-8任一所述的用于小车室外导航的状态预测方法。

Images