一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法技术

本发明专利技术公开了一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，该方法包括以下步骤：步骤1：对导航轨迹中的轨迹点进行去噪处理；步骤2：对导航轨迹中的缺失点进行补全处理；步骤3：采用卡尔曼滤波对经过去噪和补全后的导航轨迹点，进行偏移矫正；使用卡尔曼滤波算法，将服从高斯分布的偏移点很好地矫正到合理的范围内，通过对历史导航轨迹的偏移点矫正，使得用户定位设备回传的导航轨迹更接近于真实的行车轨迹，提高了历史导航轨迹的可用性。

A Kalman filter-based correction method for vehicular navigation trajectory point offset

【技术实现步骤摘要】
一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法
本专利技术属于路径导航规划领域，特别涉及一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法。
技术介绍
卫星导航是现在使用非常普遍的一种技术，其可在全球范围内提供经度、纬度、海拨高度、速度、航向、时间等导航信息,通过卫星导航可以方便的引导驾驶员到达目的地。导航的核心技术是定位技术，现在主要通过GPS系统或者北斗系统获得数据。对于导航路径规划，需要研究用户导航的历史轨迹以给用户提供更好的出行建议。在用户导航的历史轨迹点中，因为大气层电离层延迟效应，卫星时钟误差，卫星轨道误差以及城市建筑群引起的多径效应会造成GPS定位不准，会导致一些用户导航的轨迹点偏离实际位置的距离比较远，对于这些偏移的点，需要矫正其位置，才能使得历史导航轨迹更接近于用户的行车轨迹，便于对用户的历史轨迹特征进行进一步的研究。目前，国内外有大量关于在导航过程中实时定位偏移矫正的研究，以便及时修正实时导航方案，但是却极少数关注历史导航轨迹点的偏移矫正。对于用户导航的历史轨迹，由于用户定位设备的限制，会存在一些轨迹点偏离了实际位置。在对于用户历史轨迹特征的研究中，如果存在一些点的位置偏离实际位置比较远，将无法准确知道用户行驶轨迹，从而会影响到对于导航轨迹特征的研究。
技术实现思路
本专利技术提供一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，通过使用卡尔曼滤波算法，将服从高斯分布的偏移点很好地矫正到合理的范围内，同时，对于正常点不会引起太远距离的偏移，很好地还原了用户导航历史轨迹，为基于导航轨迹特征的研究提供了很大的帮助。一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，包括以下步骤：步骤1：对导航轨迹中的轨迹点进行去噪处理；步骤2：对导航轨迹中的缺失点进行补全处理；步骤3：采用卡尔曼滤波对经过去噪和补全后的导航轨迹点，进行偏移矫正；步骤3.1：获取前一个状态的轨迹点的最优估计位置，利用导航系统过程模型预测当前状态的轨迹点的位置，并获取导航系统在当前状态的轨迹点预测位置的协方差；步骤3.2：利用导航系统在当前状态的轨迹点预测位置的协方差，计算导航系统在当前状态的轨迹点预测位置的卡尔曼增益；步骤3.3：基于导航系统在当前轨迹点预测位置及其卡尔曼增益，以及当前状态的轨迹点的位置测量值，采用卡尔曼滤波最优估计，获取当前状态的轨迹点的最优估计位置，以当前状态的轨迹点的最优估计位置作为矫正后的位置；其中，导航系统在初始状态下，导航轨迹的第一个轨迹点的最优估计位置值为第一个轨迹点的位置测量值；导航系统在初始状态下，导航轨迹的第一个轨迹点的协方差取值为1。进一步地，所述利用导航系统过程模型预测当前状态的轨迹点的位置按照以下公式进行计算获得预测值；X(k|k-1)＝AX(k-1|k-1)其中，X(k|k-1)表示以前一个状态k-1的轨迹点的最优估计位置X(k-1|k-1)预测得到的当前状态k的轨迹点的位置预测值，A表示导航系统增益，取值为1；所述获取导航系统在当前状态的轨迹点预测位置的协方差，按照以下公式计算获得：P(k|k-1)＝AP(k-1|k-1)A'+Q其中，P(k|k-1)和P(k-1|k-1)分别表示X(k|k-1)和X(k-1|k-1)对应的协方差，当k-1＝0时，P(k-1|k-1)表示初始点协方差，即导航系统的初始协方差P(0|0)＝1；当k-1>0，P(k-1|k-1)＝(I-Kg(k-1)H)P(k-1|k-2)；A'＝A＝1，Q表示导航系统的过程协方差，取值为1×10-5，I为单位矩阵。进一步地，所述采用卡尔曼滤波最优估计，获取当前状态的轨迹点的最优估计位置，采用以下公式计算获得：X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))Kg(k)＝P(k|k-1)H'/(HP(k|k-1)H'+R)其中，Kg(k)表示卡尔曼增益，Z(k)表示在状态k时轨迹点的测量值，H表示导航系统状态对于导航系统在测量值的增益，取值为1。进一步地，所述对导航轨迹中的轨迹点进行去噪处理的过程如下：步骤1.1：对导航轨迹中的轨迹点进行速度区段划分；对导航轨迹中，将属于相同速度区段的连续轨迹点所在区段进行速度区段划分，每个速度区段中至少包括5个连续轨迹点；轨迹点速度区段包括低速区段[0～10)m/s，中速区段[20～30)m/s，高速区段[0～10m/s；步骤1.2：计算每个速度区段中相邻轨迹点之间的距离，判断各速度区段中的相邻轨迹点之间的距离是否超过预设的正常最大距离，若超过，则认为相邻轨迹点中的第二个轨迹点为噪声点，将噪声点删除；在不存在轨迹点缺失的情况下：低速区段，采样时间相隔1s的两个点之间的正常最大距离为20m；中速区段，采样时间相隔1s的两个点之间的正常最大距离为前一个点的瞬时速度的3倍；高速区段，采样时间相隔1s的两个点之间的正常最大距离为前一个点的瞬时速度的2倍；在存在轨迹点缺失的情况下：以相邻两个点的采样时间差乘以所在速度区段在不存在轨迹点缺失的情况下相应的最大距离得到的乘积，作为相邻两点之间的预设的正常最大距离。进一步地，所述对导航轨迹中的缺失点进行补全处理的过程如下：步骤2.1：轨迹点缺失程度检测；从导航轨迹起始点开始依次遍历每个点的时间戳信息，若后一个轨迹点的采样时刻与前一个轨迹点的采样时刻超过采样时间间隔，则存在缺失点，统计后一个轨迹点后面的连续30个轨迹点中的缺失时间，若缺失时间超过10s，则以后一个轨迹点开始的连续30个点为缺失严重轨迹，删除该轨迹段，否则，继续寻找下一个缺失点；步骤2.2：基于分段线性插值的导航轨迹点缺失补全；从导航轨迹起始点开始依次遍历每个点的时间戳信息，找到所有有缺失的位置，将某一缺失位置区间记为[xi,xi+1]；在缺失位置区间[xi,xi+1]前后两个轨迹点的采样时间差为n(s)，其中0<n≤10且n为整数；将缺失位置区间[xi,xi+1]进行n等分，补全点位置横坐标分别缺失位置区间的1/n，2/n，3/n，...，(n-1)/n等分点的位置，依次代入以下一次线性方程式计算出纵坐标；其中，(x,y)表示补全点的坐标，(xi,yi)和(xi+1,yi+1)分别表示缺失位置取点的前后两个轨迹点的坐标。有益效果本专利技术提供了一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，使用卡尔曼滤波算法，将服从高斯分布的偏移点很好地矫正到合理的范围内，通过对历史导航轨迹的偏移点矫正，使得用户定位设备回传的导航轨迹更接近于真实的行车轨迹，提高了历史导航轨迹的可用性。相对于现有技术而言，具有以下几点优点：1.本专利技术采用卡尔曼滤波，使用简单的递推算法，所需的数据存储量较小，便于用计算机进行实时处理；2.该方案可以使得导航偏移点被矫正，同时不会使正常点造成远距离偏移，很好地还原了用户行驶轨迹；3.本专利技术通过对导航轨迹点的偏移矫正，使得用户历史轨迹的特征共性凸显出来，使得用户历史导航轨迹有更高的可用性。附图说明图1是基于卡尔曼滤波的车载导航轨迹偏移矫正的流程框图；图2是导航轨迹按速度分区段示意图；图3是在对应速度区段的噪声点示意图；图4是导航轨迹点缺失的线性插值补全示意图；图5是卡尔曼滤波前后经纬度变化曲线图；图6是卡尔曼滤波前局部导航轨迹示意图；图7是卡尔曼本文档来自技高网...

【技术保护点】
1.一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，其特征在于，包括以下步骤：步骤1：对导航轨迹中的轨迹点进行去噪处理；步骤2：对导航轨迹中的缺失点进行补全处理；步骤3：采用卡尔曼滤波对经过去噪和补全后的导航轨迹点，进行偏移矫正；步骤3.1：获取前一个状态的轨迹点的最优估计位置，利用导航系统过程模型预测当前状态的轨迹点的位置，并获取导航系统在当前状态的轨迹点预测位置的协方差；步骤3.2：利用导航系统在当前状态的轨迹点预测位置的协方差，计算导航系统在当前状态的轨迹点预测位置的卡尔曼增益；步骤3.3：基于导航系统在当前轨迹点预测位置及其卡尔曼增益，以及当前状态的轨迹点的位置测量值，采用卡尔曼滤波最优估计，获取当前状态的轨迹点的最优估计位置，以当前状态的轨迹点的最优估计位置作为矫正后的位置；其中，导航系统在初始状态下，导航轨迹的第一个轨迹点的最优估计位置值为第一个轨迹点的位置测量值；导航系统在初始状态下，导航轨迹的第一个轨迹点的协方差取值为1。

【技术特征摘要】
1.一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法，其特征在于，包括以下步骤：步骤1：对导航轨迹中的轨迹点进行去噪处理；步骤2：对导航轨迹中的缺失点进行补全处理；步骤3：采用卡尔曼滤波对经过去噪和补全后的导航轨迹点，进行偏移矫正；步骤3.1：获取前一个状态的轨迹点的最优估计位置，利用导航系统过程模型预测当前状态的轨迹点的位置，并获取导航系统在当前状态的轨迹点预测位置的协方差；步骤3.2：利用导航系统在当前状态的轨迹点预测位置的协方差，计算导航系统在当前状态的轨迹点预测位置的卡尔曼增益；步骤3.3：基于导航系统在当前轨迹点预测位置及其卡尔曼增益，以及当前状态的轨迹点的位置测量值，采用卡尔曼滤波最优估计，获取当前状态的轨迹点的最优估计位置，以当前状态的轨迹点的最优估计位置作为矫正后的位置；其中，导航系统在初始状态下，导航轨迹的第一个轨迹点的最优估计位置值为第一个轨迹点的位置测量值；导航系统在初始状态下，导航轨迹的第一个轨迹点的协方差取值为1。2.根据权利要求1所述的方法，其特征在于，所述利用导航系统过程模型预测当前状态的轨迹点的位置按照以下公式进行计算获得预测值；X(k|k-1)＝AX(k-1|k-1)其中，X(k|k-1)表示以前一个状态k-1的轨迹点的最优估计位置X(k-1|k-1)预测得到的当前状态k的轨迹点的位置预测值，A表示导航系统增益，取值为1；所述获取导航系统在当前状态的轨迹点预测位置的协方差，按照以下公式计算获得：P(k|k-1)＝AP(k-1|k-1)A'+Q其中，P(k|k-1)和P(k-1|k-1)分别表示X(k|k-1)和X(k-1|k-1)对应的协方差，当k-1＝0时，P(k-1|k-1)表示初始点协方差，即导航系统的初始协方差P(0|0)＝1；当k-1>0，P(k-1|k-1)＝(I-Kg(k-1)H)P(k-1|k-2)；A'＝A＝1，Q表示导航系统的过程协方差，取值为1×10-5，I为单位矩阵。3.根据权利要求2所述的方法，其特征在于，所述采用卡尔曼滤波最优估计，获取当前状态的轨迹点的最优估计位置，采用以下公式计算获得：X(k|k)＝X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))Kg(k)＝P(k|k-1)H'/(HP(k|k-1)H'+R)其中，Kg(k)表示卡尔曼增益，Z(k)表示在状态k时轨迹点的测...

【专利技术属性】
技术研发人员：肖晓明，吴箫，
申请(专利权)人：中南大学，
类型：发明
国别省市：湖南,43