一种定位轨迹信息的处理方法和车载导航装置

技术领域

本申请实施例涉及车载组合导航领域，尤指一种定位轨迹信息的处理方法和车载导航装置。

背景技术

全球导航卫星系统(Global Navigation Satellite System,GNSS)能够提供全球范围内载体的位置、速度和时间信息，定位误差不随时间发散，能够满足长时间、高精度定位需求。但是，GNSS信号经常因复杂城市环境干扰而中断，系统抗干扰能力较差。近年，随着微机电系统的快速发展，由三轴加速度计和三轴陀螺仪组成的低成本惯性测量装置(Inertial Measurement Unit,IMU)在民用领域得到广泛应用。GNSS/IMU低成本组合导航系统，成为目前车载导航系统的重要组成部分。但低成本IMU的定位误差因较大的测量噪声与器件零偏较高等原因，精度INS发散更加迅速，如何提高低成本GNSS/IMU组合导航系统的性能成为车载导航系统发展的关键。

GNSS/IMU车载导航系统在卫星信号可用时，对IMU和ODO(Odometry，里程计)的相关误差状态进行训练；当卫星信号失锁时，利用车辆运动约束模型完成导航定位精度的维持。车辆运动模型约束又称为非完整性约束(Non-Holonomic Constraint，NHC)，是指车辆在陆地行驶时车辆载体坐标系v系的右向与上向的速度为0，利用车辆运动的速度特征作为约束可以对惯导的更新进行修正。

对于监控等后装用户的应用场景而言，卫星信号易受到干扰，信号质量较低，无法有效提供里程计信息，在卫星信号可用时对惯导相关误差状态训练不足，在卫星信号不可用的情况下仅仅依靠NHC对惯导进行更新修正，虽然可以有效维持车辆载体的航向精度和俯仰精度，抑制横向和上向定位偏差，但对前向定位误差没有任何约束。当车辆载体长时间GNSS拒止环境，例如长隧道或长时间地库行驶时，不可避免出现定位轨迹跳跃或回折的问题，无法满足监控等应用的需求。

发明内容

为了解决上述任一技术问题，本申请实施例提供了一种定位轨迹信息的处理方法和车载导航装置。

为了达到本申请实施例目的，本申请实施例提供了一种定位轨迹信息的处理方法，应用于集成GNSS和IMU的车载组合导航装置，所述方法包括：

获取第i个历元的定位结果和第i-1个历元的定位结果，其中i为大于等于2的整数；

根据所述第i个历元的定位结果和所述第i-1个历元的定位结果，得到第i个历元的车辆的位移矢量；

根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常。

一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上文所述的方法。

一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上文所述的方法。

一种车载导航装置，包括：

GNSS模块；

IMU；

定位轨迹信息的处理模块，与所述GNSS模块和所述IMU相连，用于实上文所述的方法。

上述技术方案中的一个技术方案具有如下优点或有益效果：

通过获取第i个历元的定位结果和第i-1个历元的定位结果，根据所述第i个历元的定位结果和所述第i-1个历元的定位结果，得到第i个历元的车辆的位移矢量，并根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常，实现对定位轨迹信息中异常的定位结果的检测。

本申请实施例的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请实施例而了解。本申请实施例的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本申请实施例技术方案的进一步理解，并且构成说明书的一部分，与本申请实施例的实施例一起用于解释本申请实施例的技术方案，并不构成对本申请实施例技术方案的限制。

图1为本申请实施例提供的定位轨迹信息的处理方法的流程图；

图2本申请实施例提供的车载组合导航系统的定位轨迹信息中的处理方法的流程图；

图3(a)为本申请实施例提供的定位轨迹跳跃的处理示意图；

图3(b)为本申请实施例提供的定位轨迹跳跃的处理方法示意图；

图4(a)为本申请实施例提供的定位轨迹回折的处理示意图；

图4(b)为本申请实施例提供的定位轨迹回折的处理方法示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚明白，下文中将结合附图对本申请实施例的实施例进行详细说明。需要说明的是，在不冲突的情况下，本申请实施例中的实施例及实施例中的特征可以相互任意组合。

图1为本申请实施例提供的定位轨迹信息的处理方法的流程图。如图1所示，图1所示方法应用于集成GNSS和IMU的车载组合导航装置，所述方法包括：

步骤101、获取第i个历元的定位结果和第i-1个历元的定位结果，其中i为大于等于2的整数；

步骤102、根据所述第i个历元的定位结果和所述第i-1个历元的定位结果，得到第i个历元的车辆的位移矢量；

步骤103、根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常。

本申请实施例提供的方法，通过获取第i个历元的定位结果和第i-1个历元的定位结果，根据所述第i个历元的定位结果和所述第i-1个历元的定位结果，得到第i个历元的车辆的位移矢量，并根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常，实现对定位轨迹信息中异常的定位结果的检测。

下面对本申请实施例提供的方法进行说明：

在一个示例性实施例中，所述根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常，包括：

根据所述第i个历元的车辆的位移矢量，确定自第i-1个历元至第i个历元内所述车辆的行驶距离，根据所述车辆的行驶距离与预设的正常行驶距离的判断条件，确定所述车辆的定位轨迹信息是否发生跳跃；或者，

确定所述第i个历元的车辆的位移矢量的方向，根据所述位移矢量的方向与所述车载组合导航装置指示的行驶航向，确定所述车辆的定位轨迹信息是否发生回折。

组合导航定位轨迹异常可以分为两种情况：

(1)定位轨迹跳跃，

在一个示例性实施例中，根据所述车辆的行驶距离与预设的正常行驶距离的判断条件，确定所述车辆的定位轨迹信息是否发生跳跃，包括：

如果所述车辆的行驶距离满足所述正常行驶距离的判断条件，则确定所述车辆的定位轨迹信息没有发生跳跃；如果所述车辆的行驶距离不满足所述正常行驶距离的判断条件，则确定所述车辆的定位轨迹信息发生跳跃。

在一个示例性实施例中，所述正常行驶距离的判断条件是根据所述车辆的最大行驶距离确定的，其中：

所述最大行驶距离是根据所述车辆当前行驶航向的最大行驶速度以及自第i-1个历元至第i个历元之间的时长确定的。

在上述示例性实施例中，如果相邻的两个定位点的行驶距离超过预设的正常行驶距离，则表示车辆的行驶速度不符合正常车辆行驶速度，则可以判定为定位轨迹出现跳跃；

其中，该正常行驶距离可以根据车辆当前行驶航向在1个历元时长内所能行驶的最大行驶距离来确定，该最大行驶距离可以通过该车辆的最大行驶速度来计算。

可以将定位频率设置为1Hz，则1个历元时长为1s。

(2)定位轨迹回折

如果相邻的两个定位点的位移矢量方向与车辆行驶方向相反时，可以判定为定位轨迹回折。

基于上述内容，利用当前历元的定位结果和上一个历元的定位结果，构建车辆在第i个历元的位移矢量，通过该位移矢量对组合导航定位轨迹跳跃或回折进行判定，得到当前历元的定位结果是否异常的结果。

在一个示例性实施例中，所述根据所述位移矢量的方向与所述车载组合导航装置指示的行驶航向，确定所述车辆的定位轨迹信息是否发生回折，包括：

判断所述第i个历元的车辆的位移矢量的长度是否大于预设的长度阈值；

如果所述位移矢量的长度大于所述长度阈值，则判断所述位移矢量的方向与所述车载组合导航装置指示的行驶航向的夹角的角度是否符合预设的角度判断条件；

如果所述夹角的角度符合所述角度判断条件，则确定所述车辆的定位轨迹信息发生回折。

在判断位移矢量的方向与行驶航向是否相同时，利用长度阈值来确定位移矢量的长度来确定所述位移矢量的长度是否足够长，以保证夹角的角度的计算精确性，再利用角度判断条件对夹角的角度的大小进行判断，来确定位移矢量的方向与行驶航向是否相同，得到车辆的定位轨迹信息是否发生回折的结果。

其中，长度阈值可以根据1个历元时长内车辆行驶的距离来确定。例如，设置该长度阈值为5米。

其中，角度判断条件可以为采用角度的具体数值来表示判断条件，例如，设置的角度判断条件为大于80度；或者，用于表示角度大小的参数来表示判断条件，该参数可以为正弦值sin、余弦值cos，例如，设置的角度判断条件为余弦值小于0.18.

在一个示例性实施例中，所述根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常之后，所述方法还包括：

在确定所述第i个历元的定位结果异常后，对得到的定位轨迹信息进行平滑处理，得到处理后的定位轨迹信息；

输出所述处理后的定位轨迹信息。

通过对出现异常的定位结果进行处理，保证系统定位轨迹能够连续、平滑，更加匹配车辆行驶的真实轨迹。

在一个示例性实施例中，所述在确定所述第i个历元的定位结果异常后，在对得到的定位轨迹信息进行平滑处理之前，所述方法还包括：

如果所述车辆的定位轨迹信息发生回折，则检测所述车辆是否在执行倒车操作，得到检测结果；

如果所述检测结果为所述车辆未在执行倒车操作，则对得到的定位轨迹信息进行平滑处理。

考虑正常的倒车行驶会导致轨迹回折，通过将车辆的倒车操作作为定位轨迹异常的判断条件，保证判断的准确。

可以利用前向速度作为倒车操作的判断依据，如果前向速度小于预设的速度阈值，则表示车辆在执行倒车行为，例如，该速度阈值为-1m/s。

在一个示例性实施例中，所述对得到的定位轨迹信息进行平滑处理，包括：

对所述第i个历元的定位结果进行修正，并根据修正后的所述第i个历元的定位结果与第i+1个历元的定位结果，确定第i+1个历元的定位结果是否异常；

如果第i+1个历元的定位结果正常，则确定平滑处理操作结束；

如果第i+1个历元的定位结果异常，对所述第i+1个历元的定位结果进行修正，以此类推，直到在下一个历元的定位结果正常为止或者所处理的历元总时长达到预设的时间阈值。

对于定位轨迹跳跃或定位轨迹回折的情况，从上一个历元的定位结果出发，在对当前历元的定位结果进行修正后，得到正常定位结果，保存正常的定位结果并继续进行定位结果的异常检测，直到所检测的历元的定位结果正常为止。

对于定位轨迹跳跃或定位轨迹回折的情况，可以锁定当前历元的定位结果，对前后历元的位移矢量方向进行持续判定，直到位移方向矢量方向与车辆行驶方向一致，解除锁定，避免输出轨迹回折。由于轨迹回折平滑策略中会将输出的定位结果进行锁点，因此需要添加锁点时间的限制，如果时长达到预设的时间阈值的前提下都判断为轨迹回折，则恢复正常位置输出。

在一个示例性实施例中，所述对所述第i个历元的定位结果进行修正，包括：

如果所述车辆的定位轨迹信息发生跳跃，则以第i-1个历元的定位结果为起点，沿所述第i个历元的车辆的位移矢量的方向，按照预设的最大行驶距离，确定第i个历元对应的修正后的定位结果；

在一个示例性实施例中，所述对所述第i个历元的定位结果进行修正，包括：

如果所述车辆的定位轨迹信息发生回折，将第i-1个历元的定位结果作为所述第i个历元的车辆的定位结果。

如果判定当前历元的定位结果出现跳跃，则根据设定的最快行驶速度，从上一个历元的定位结果出发，通过航位推算计算获得当前历元的正常定位结果并输出，然后保存航位推算的定位结果并继续进行轨迹跳跃的判定，在一定的时间内以最快行驶速度快速趋近真实位置，保证输出的定位轨迹平滑且不存在超出最快行驶速度的跳跃。

如果判定当前历元的定位结果出现回折，则锁定当前历元的定位结果，对前后历元的位移矢量方向进行持续判定，直到位移方向矢量方向与车辆行驶方向一致，解除锁定，避免输出轨迹回折。

基于上述方案，可以准确判断GNSS/IMU车载导航系统的定位轨迹异常，然后通过平滑策略保证定位轨迹的输出不会出现跳跃和回折的问题。

下面以实施例对本申请实施例提供的方法进行说明：

本申请实施例提供的方法，用于解决基于低成本的GNSS/IMU的车载组合导航系统在长时间GNSS拒止环境，例如长隧道或长时间地库行驶时，定位轨迹偏长、偏短或轨迹扭转的问题。为了解决低成本GNSS/IMU车载组合导航系统在长时间卫星拒止环境下定位轨迹跳跃或回折的问题，提供一种定位轨迹跳跃、回折的检测和轨迹输出平滑的方案。

实施例一

图2本申请实施例提供的车载组合导航系统的定位轨迹信息中的处理方法的流程图。如图2所示，所述方法包括：

步骤201、获取当前历元的组合导航定位结果；

步骤202、利用当前历元和上一历元的定位结果，构建位移矢量；

如果定位频率为1Hz，则当前历元和上一历元的时长为1s，则构建1s内的位移矢量。

步骤203、利用所述位移矢量，执行定位轨迹异常判决；

如果定位轨迹异常，且为定位轨迹跳跃，则执行步骤204；

如果定位轨迹异常，且为定位轨迹回折，则执行步骤205；

如果所述定位轨迹信息为组合导航装置的正常定位轨迹，则执行步骤206；

步骤204，以按照车辆行驶的最快速度，按照上一历元的定位结果进行航位推算，得到修正后的当前历元的定位结果，再执行步骤206；

步骤205，以按照车辆行驶的最快速度，按照上一历元的定位结果进行航位推算，得到修正后的当前历元的定位结果，再执行步骤206；

步骤206、保存当前历元的定位结果作为下一个历元的定位结果；

步骤207、输出定位结果。

本申请实施例提供的方法，通过对定位轨迹信息的异常检测，对存在异常的定位结果进行修正，保证输出正常的定位轨迹信息。

实施例二

图3(a)为本申请实施例提供的定位轨迹跳跃的处理示意图。如图3(a)所示，组合定位轨迹为A

图3(b)为本申请实施例提供的定位轨迹跳跃的处理方法示意图。如图3(b)所示，所述方法包括：

步骤301、根据第i个历元和第i-1个历元之间的位移矢量大小，判别第i个历元的定位轨迹是否出现跳跃；

如果第i个历元的定位轨迹出现跳跃，则步骤302；

如果第i个历元的定位轨迹未出现跳跃，则步骤303；

以组合定位轨迹为A

步骤302、修正第i个历元的定位结果，再执行步骤303；

从A

步骤303、将第i个历元的定位结果保存并输出；

步骤304、更新i的取值为i+1；

步骤305、判断更新后的i的取值是否大于组合轨迹的定位点的最大值；

如果i+1大于定位点的最大值，则表示对组合轨迹信息的处理完成，否则，继续对定位点是否出现跳跃进行检测。

通过对i的取值的更新，可以触发根据第i个历元和第i+1个历元之间的位移矢量大小，继续判别第i+1个历元的定位轨迹出现跳跃；

以图3所示场景为例进行说明：

在i＝2时，完成位置A

若

如果

由于

反之，在i＝3时，如果

本申请实施例提供的方法，通过对定位轨迹跳跃的检测，对存在异常的定位结果进行修正，保证输出正常的定位轨迹信息。

实施例三：

图4(a)为本申请实施例提供的定位轨迹回折的处理示意图。如图4(a)所示，判别指标为相邻时刻的位移矢量方向与组合导航指示的行驶航向是否一致。图4(a)中的组合定位轨迹为A

图4(b)为本申请实施例提供的定位轨迹回折的处理方法示意图。如图4(b)所示，所述方法包括：

步骤401、根据第i个历元和第i-1个历元之间的位移矢量，判别出第i个历元的定位轨迹出现回折；

定位轨迹回折的判定条件为：第i个历元和第i-1个历元之间的位移矢量的长度大于长度阈值，且第i个历元和第i-1个历元之间的位移矢量的方向与姿态航向矢量之间的夹角满足角度判断条件；

如果第i个历元的定位轨迹出现回折，则步骤402；

如果第i个历元的定位轨迹未出现回折，则步骤403；

以组合定位轨迹为A

步骤402、修正第i个历元的定位结果，再执行步骤403；

修正后的第2个历元的定位结果为第1个历元的定位结果，即A

步骤403、将第i个历元的定位结果保存并输出。

步骤404、更新i的取值为i+1；

步骤405、判断i+1是否大于组合轨迹的定位点的最大值或者连续修正的定位点的个数是否达到预设的个数阈值；

如果i+1大于定位点的最大值，则表示对组合轨迹信息的处理完成；否则，继续对定位点是否出现回折进行检测。

如果连续修正的定位点个数大于个数阈值，则表示修正的定位点对应的总时长预设的时间长度，需停止修正；否则，继续对定位点是否出现回折进行检测。

通过对i的取值的更新，可以触发根据第i个历元和第i+1个历元之间的位移矢量大小，继续判别第i+1个历元的定位轨迹出现跳跃；

以图4(a)所示场景为例进行说明：

在i＝2时，完成位置A

若

判断定位结果A

继续判断定位结果A

继续判断定位结果A

反之，在i＝3时，若

由于平滑处理操作会将输出的定位结果进行锁点，因此需要对其进行时间的限制，如果连续20s都判断为定位轨迹回折，则恢复正常位置输出，并且采用上述判断条件无法区分倒车场景所造成的锁点，因此可以将车辆的前向速度加入判别依据，如果前向速度小于-1m/s，则不进行反折锁点，上述判断倒车行为的操作可以在上述例子中检测到定点结果A

本申请实施例提供的方法，通过对定位轨迹回折的检测，对存在异常的定位结果进行修正，保证输出正常的定位轨迹信息。

相较于相关技术中的车载组合导航系统，本申请实施例提供的方法通过定位轨迹异常判断，可以有效解决低成本GNSS/IMU车载组合导航系统在长时间卫星拒止环境下定位轨迹跳跃、回折的问题，保证系统定位轨迹能够连续、平滑，更加匹配车辆行驶的真实轨迹。

本申请实施例提供一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上文任一项中所述的方法。

本申请实施例提供一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上文任一项中所述的方法。

本申请实施例提供一种车载导航装置，包括：

GNSS模块；

IMU；

定位轨迹信息的处理模块，与所述GNSS模块和所述IMU相连，用于实现如上文任一项中所述的方法。

本申请实施例提供的装置，通过获取第i个历元的定位结果和第i-1个历元的定位结果，根据所述第i个历元的定位结果和所述第i-1个历元的定位结果，得到第i个历元的车辆的位移矢量，并根据所述第i个历元的车辆的位移矢量，确定所述第i个历元的定位结果是否异常，实现对定位轨迹信息中异常的定位结果的检测，并通过对定位轨迹信息的平滑处理，保证定位轨迹能够连续、平滑，更加匹配车辆行驶的真实轨迹。

本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。在硬件实施方式中，在以上描述中提及的功能模块/单元之间的划分不一定对应于物理组件的划分；例如，一个物理组件可以具有多个功能，或者一个功能或步骤可以由若干物理组件合作执行。某些组件或所有组件可以被实施为由处理器，如数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存储器技术、CD-ROM、数字多功能盘(DVD)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。