CN110631589B - 一种实时修正定位轨迹的方法 - Google Patents

Abstract

本发明涉及定位导航技术领域，公开了一种实时修正定位轨迹的方法：采集得到待定节点的第一位置信息和确定节点的位置信息；判断所述待定节点对应的运动数值是否超出第一阈值，若超出，则根据确定节点的运动数值拟定得到所述待定节点的第二位置信息，以所述第二位置信息作为所述待定节点的最终位置信息，对定位轨迹进行更新；所述运动数值包括速度或加速度；所述定位轨迹包括待定节点和确定节点。

Description

一种实时修正定位轨迹的方法

技术领域

本发明涉及定位导航技术领域，特别是涉及一种实时修正定位轨迹的方法。

背景技术

目前市面上的定位轨迹修正一般采用三种方式。第一、采用滤波算法抑制轨迹偏离，主要有：跳点过滤算法、最小二乘法滤波、卡尔曼滤波、粒子滤波等，缺点在于：单一的滤波算法有着固定的应用场景，例如跳点过滤算法、最小二乘法滤波应用于事后滤波，在实时滤波场景下，不能同时满足实时获取和优化轨迹的需求。卡尔曼滤波和粒子滤波受历史定位数据的影响，在周边环境多变，如空旷与遮挡环境切换，会造成严重的滤波定位失真。第二、根据已知道路数据，将定位轨迹投影在道路曲线上。问题在于：依赖于道路数据，没有道路数据时不能很好的实现轨迹修正。定位数据投影到道路上，会损失真实的定位信息，在真实前进轨迹存在拐弯、曲折情况时无法反映出来，算法应用场景受限。第三、轨迹拟合法，该方法多结合滤波算法抽取质量较高的定位数据，然后拟合轨迹，或者在有效定位数据中间做插值。该方法同样应用于事后滤波，不能实时获取准确的定位结果。

目前，市场上重点主要是放在如何提高定位轨迹的质量、定位精度等内容上，较少见到相关资料在不依赖道路信息的情况下，对实时定位轨迹进行修正这一问题进行探讨研究。

发明内容

为了解决上述的至少一个技术问题，本发明提供了一种实时修正定位轨迹的方法，可以通过结合定位节点对应的速度、加速度和运动方向，修正定位轨迹，利用卡尔曼滤波，进一步对定位轨迹进行修正；同时，本发明技术方案不依赖于道路数据，可应用于实时滤波场景。

为了实现上述技术效果，本发明一方面公开了：一种实时修正定位轨迹的方法，包括以下步骤：采集得到待定节点的第一位置信息和确定节点的位置信息；判断所述待定节点对应的运动数值是否超出第一阈值，若超出，则根据确定节点的运动数值拟定得到所述待定节点的第二位置信息，以所述第二位置信息作为所述待定节点的最终位置信息，对定位轨迹进行更新；所述运动数值包括速度或加速度；所述定位轨迹包括待定节点和确定节点。

优选地，拟定得到所述待定节点的第二位置信息包括以下步骤：根据最接近所述待定节点的至少三个所述确定节点，利用最小二乘法拟合得到直线方程；根据所述直线方程、至少三个所述确定节点对应的速度计算出的平均速度以及采集频率，生成位于所述确定节点沿直线方向的正前方的所述第二位置信息。

优选地，根据所述直线方程的斜率，推算得到所述待定节点的第二位置信息。

优选地，根据所述第二位置信息和所述确定节点的位置信息，对定位轨迹执行滤波，得到进一步修正后的定位轨迹。

优选地，对定位轨迹执行滤波，得到进一步修正后的定位轨迹包括以下步骤：根据预设的滤波倍数值，将所述待定节点的第一位置信息对应的点及所述待定节点最近的一个所述确定节点连接成第一直线，将所述确定节点拟合成第二直线，计算得到所述第一直线和所述第二直线的夹角，当所述夹角大于第二阈值时，增大所述滤波倍数值；将所述待定节点的第二位置信息作为观测值，所述待定节点最近的确定节点的位置信息作为预测值，对所述确定节点和所述待定节点执行卡尔曼滤波，得到所述的进一步修正后的定位轨迹。

优选地，对定位轨迹执行滤波，得到进一步修正后的定位轨迹还包括以下步骤：计算得到所述定位轨迹对应的平均速度v1，当所述平均速度v1小于第三阈值时，增大所述滤波倍数值，当所述平均速度v1大于第四阈值时，将所述滤波倍数值设定为固定值。

优选地，计算得到所述第一直线和所述第二直线的夹角包括以下步骤：将所述确定节点的位置信息和所述待定节点的第一位置信息转换为笛卡尔坐标信息，通过向量夹角计算所述第一直线和所述第二直线的夹角；所述确定节点的位置信息和所述待定节点的第一位置信息为经纬度信息。

优选地，对定位轨迹执行滤波，得到进一步修正后的定位轨迹包括以下步骤：当所述待测节点第一位置对应的移动速度小于第三阈值且所述第一直线和第二直线的夹角大于第二阈值时，增大所述滤波倍数值；当所述待测节点对应的移动速度大于第四阈值且所述第一直线和第二直线的夹角小于第二阈值是时，减小所述滤波倍数值。

优选地，将所述修正后的定位轨迹数据发送到显示设备，供所述显示设备对所述定位轨迹进行显示。

另一方面，本发明还提供了一种计算机介质，所述计算机介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述的修正定位轨迹的方法。

技术效果：本发明提供一种实时修正定位轨迹的方法，结合定位节点对应的速度、加速度和运动方向，实时修正定位轨迹，调整卡尔曼滤波的参数，解决环境变化时卡尔曼滤波定位失真问题，可应用于实时滤波场景。同时，本发明技术方案不依赖于道路数据，保留了轨迹的转弯、曲折情况，能够更好的适应复杂环境。

附图说明

为更好地理解本发明的技术方案，可参考下列的、用于对现有技术或实施例进行辅助说明的附图。这些附图将对现有技术或本发明部分实施例中，涉及到的产品或方法有选择地进行展示。这些附图的基本信息如下：

图1为一个实施例中，一种实时修正定位轨迹的方法示意图。

具体实施方式

下文将对本发明涉及的技术手段或技术效果作进一步的展开描述，显然，所提供的实施例仅是本发明的部分实施方式，而并非全部。基于本发明中的实施例以及图文的明示或暗示，本领域技术人员在没有作出创造性劳动的前提下所能获得的所有其他实施例，都将在本发明保护的范围之内。

本发明提供了一种实时修正定位轨迹的方法，如图1所示，包括以下步骤：采集得到待定节点的第一位置信息和确定节点的位置信息；判断所述待定节点对应的运动数值是否超出第一阈值，若超出，则根据确定节点的运动数值拟定得到所述待定节点的第二位置信息，以所述第二位置信息作为所述待定节点的最终位置信息，对定位轨迹进行更新；所述运动数值包括速度或加速度；所述定位轨迹包括待定节点和确定节点。

所述采集指的是通过具备定位功能的定位模组或定位设备，同卫星定位导航系统建立同步通信，获得所述待定节点的第一位置信息；在一些实施例中，也可以通过将所述待定节点接入定位基站CORS(Continuously Operating Reference Stations)网，以及用于数据传输的蓝牙设备、用于识别运动方向的摄像设备等辅助设备，获得所述待定节点的第一位置信息。

所述待定节点指的是处于采集状态下的目标点在运动过程中位置信息，在定位应用场景下，正在通过定位装置或者定位系统解算获取位置信息的目标点，由于定位装置或定位系统存在大气误差、设备精度等误差以及解算过程也会涉及到对大气误差、设备误差等误差的处理，因此此时的目标点处于待定状态；通过定位设备或定位系统解算获取得到的位置信息即是第一位置信息；已经确定了位置信息的节点即是确定节点。

所述位置信息一般而言指的是通过卫星导航系统获得的待定节点的经纬度信息，也可以是通过CORS网获得的二维或三维坐标信息。

在定位应用场景中，对目标点的定位是基于定位设备或定位系统的频率，以该频率对目标点进行定位解算得到所述待定节点的位置信息和对应的运动数值，因此在该频率对应的每个单位时间点获得一个待定节点的位置信息也就是所述待定节点的第一位置信息；在定位过程中，目标点处于实时运动状态，通过卫星导航系统及CORS网均可获得在每个单位时间点目标点的运动数值(速度和加速度)和运动方位，由于系统误差及解算误差的存在，因此需要在每个单位时间点，判断所述第一位置信息对应的速度或加速度是否在预设的任一运动数值即速度或加速度的第一阈值内。

当所述第一位置信息对应的速度或加速度在预设的任一运动数值即速度或加速度的第一阈值内时，则保留该第一位置信息，进行下一个单位时间点的结算。

当所述第一位置信息对应的速度或加速度超出预设的任一运动数值即速度或加速度的第一阈值时，则说明系统误差或者解算误差过大，该位置信息与真实位置信息偏差过大而失真，不能保留该第一位置信息。

所述第一阈值指的是任一运动数值即速度或加速度的第一阈值，一般而言可以根据不同的目标点设置不同的第一阈值，如果目标点是人或动物，则一般可以将速度的第一阈值设置在50公里每小时或60公里每小时，将加速度设置在0.5G或者0.4G；如果目标点是陆地交通工具，由于交通法规的限制，则一般可以将速度的第一阈值设置在150公里每小时或180公里每小时，将加速度设置在0.7G或者1G；如果是轨道交通工具，则一般可以将速度的第一阈值设置在450公里每小时或460公里每小时，将加速度设置在0.8G或者1G，本技术领域技术人员可以根据实际情况对第一阈值进行不同的设置，以确保获得的第一位置信息是合理的。

第二位置信息指的是当采集得到的所述第一位置信息超出预设的运动数值(速度或加速度)的第一阈值设定的范围，此时第一位置信息明显失真，不符合实际，需要将采集得到的所述第一位置信息删除，根据节点的运行轨迹，在运动数值(速度或加速度)的第一阈值内拟定一个第二位置信息作为该待定节点的位置信息，以拟定的第二位置信息作为该所述待定节点的最终位置信息。确定了所述待定节点的第二位置信息后，此时的待定节点更新为确定节点，同时对定位轨迹进行更新。定位轨迹是根据采集装置的工作频率，采集得到的确定节点和待定节点组成，所述待定节点更新为确定节点即是对定位轨迹的更新，依上所述，当所述待定节点的第一位置信息在预设的运动数值(速度或加速度)的第一阈值设定的范围内，该第一位置信息有效，所述待定节点更新为确定节点；当所述待定节点的第一位置信息超出预设的运动数值(速度或加速度)的第一阈值设定的范围，删除该第一位置信息，拟定一个第二位置信息作为该待定节点的位置信息，以拟定的第二位置信息作为该所述待定节点的最终位置信息，所述待定节点更新为确定节点。

在一些实施例中，拟定得到所述待定节点的第二位置信息包括以下步骤：根据最接近所述待定节点的至少三个所述确定节点，利用最小二乘法拟合得到直线方程；根据所述直线方程、至少三个所述确定节点对应的速度计算出的平均速度以及采集频率，生成位于所述确定节点沿直线方向的正前方的所述第二位置信息。

所述拟定的方式是在沿定位轨迹的运动方向，在轨迹的运动方向的前方在运动数值(速度或加速度)的第一阈值设定的范围内拟定一个符合定位轨迹运动规律的第二位置信息。

一般而言，两个点相连可以形成一条直线，在定位过程中，如果仅用两个确定节点拟合一条直线的话，如果一个点出现运动数值在第一阈值内的误差时，会存在不可避免的定位偏移，导致后续的待定节点第二位置信息的确定出现进一步的误差；以三个确定节点的位置信息拟合形成的直线，在技术效果上更接近实际情况，可以克服个别确定节点出现运动数值第一阈值内的误差带来的定位轨迹偏移；在实际操作过程中，拟合定位轨迹的直线，一般选择3到10个确定节点的位置，为了更好的操作便利性和精准性，可以选择5-7个确定节点的位置。

确定定位轨迹的运动方向需要根据所述待定节点的至少三个所述确定节点形成的直线方向以上述所述的至少三个确定节点对应的速度计算出的平均速度以及用于采集所述待定节点的采集装置或采集系统的采集频率，根据所述平均速度和采集频率即在所述直线方向在轨迹的运动方向的前方计算出第二位置信息，由于该拟定方法是根据确定节点对应的速度计算出的平均速度，因此不需要依赖道路信息，能够很好的适应环境复杂变化的应用场景。

所述最小二乘法又称最小平方法是一种数学优化方法，通过对所述确定节点的位置信息最小化误差的平方和待定节点第二位置信息的最佳函数匹配。利用最小二乘法可以简便地求得待定节点第二位置信息。

在一些实施例中，根据所述直线方程的斜率，推算得到所述待定节点的第二位置信息。

根据距离所述待定节点的至少三个所述确定节点形成的直线的斜率，推算所述待定节点的第二位置信息。

在一些实施例中，根据所述第二位置信息和所述确定节点的位置信息，对定位轨迹执行滤波，得到进一步修正后的定位轨迹。

针对上述实施例，将定位轨迹的待定节点和确定节点，用线段连接起来，将点状定位轨迹先拟合成线性轨迹，在对线性轨迹执行卡尔曼滤波，将线性定位轨迹进一步修正为直线定位轨迹，达到更符合视觉识别的定位轨迹效果。

在一些实施例中，对定位轨迹执行滤波，得到进一步修正后的定位轨迹包括以下步骤：根据预设的滤波倍数值，将所述待定节点的第一位置信息对应的点及所述待定节点最近的一个所述确定节点连接成第一直线，将所述确定节点拟合成第二直线，计算得到所述第一直线和所述第二直线的夹角，当所述夹角大于第二阈值时，增大所述滤波倍数值；将所述待定节点的第二位置信息作为观测值，所述待定节点最近的确定节点的位置信息作为预测值，对所述确定节点和所述待定节点执行卡尔曼滤波，得到所述的进一步修正后的定位轨迹。

针对上述实施例，一般而言，本领域技术人员根据技术需要，可以将滤波器的滤波倍数值设置在3-9之间；根据所有的确定节点，将距离待测节点最近的确定节点和距离待测节点最远的确定节点连接拟合成为第二直线，将距离待测节点最近的确定节点和待测节点连接成为第一直线，所述第一直线与第二直线相交，形成一个夹角，本领域技术人员根据技术需要，可以将所述夹角的第二阈值设置在30-90度之间；当夹角大于设置的所述第二阈值时，增大滤波器的滤波倍数值，一般而言，根据技术效果，可以将滤波倍数值逐步或直接增大到设置的滤波倍数值的双倍，也可以缓慢的调节增大。

在一些实施例中，对定位轨迹执行滤波，得到进一步修正后的定位轨迹还包括以下步骤：计算得到所述定位轨迹对应的平均速度v1，当所述平均速度v1小于第三阈值时，增大所述滤波倍数值，当所述平均速度v1大于第四阈值时，将所述滤波倍数值设定为固定值。

针对上述实施例，计算所述定位轨迹对应的平均速度V1指的是根据待测节点和所有的确定节点对应的速度计算得到对应的平均速度V1；一般情况下，本领域技术人员根据技术需要和技术效果，可以将第三阈值设置在0.1米每秒到0.5米每秒之间，可以将第四阈值设置在5米每秒到8米每秒之间，当所述平均速度V1小于第三阈值时，根据技术效果，可以将滤波倍数值逐步或直接增大到设置的滤波倍数值的双倍，也可以缓慢的调节增大；当所述平均速度V1大于第四阈值时，将所述滤波倍数值设定为固定值，一般而言所述固定值设定为2或者3。

在一些实施例中，计算得到所述第一直线和所述第二直线的夹角进一步包括：将所述确定节点的位置信息和所述待定节点的第一位置信息转换为笛卡尔坐标信息，通过向量夹角计算所述第一直线和所述第二直线的夹角；所述确定节点的位置信息和所述待定节点的第一位置信息为经纬度信息。

针对上述实施例，所述笛卡尔坐标系就是直角坐标系和斜角坐标系的统称，相交于原点的两条数轴，构成了平面放射坐标系，将所述确定节点的位置的经纬度信息和所述待定节点的第一位置的经纬度信息转换后放入笛卡尔坐标中，或者笛卡尔坐标平面信息，所述第一直线为第一向量，所述第二直线为第二向量，所述第一直线和所述第二直线的夹角为向量夹角，向量计算夹角即可计算得到所述第一直线和所述第二直线的夹角。

在一些实施例中，对定位轨迹执行滤波，得到进一步修正后的定位轨迹包括以下步骤：当所述待测节点第一位置对应的移动速度小于第三阈值且所述第一直线和第二直线的夹角大于第二阈值时，增大所述滤波倍数值；当所述待测节点对应的移动速度大于第四阈值且所述第一直线和第二直线的夹角小于第二阈值是时，减小所述滤波倍数值。

针对上述实施例，所述待测节点第一位置对应的移动速度较低，指的是移动速度小于第三阈值时；所述待测节点第一位置对应的移动速度较高，指的是移动速度大于第四阈值时。

在一些实施例中，将所述修正后的定位轨迹数据发送到显示设备，供所述显示设备对所述定位轨迹进行显示。

针对上述实施例，述所显示设备可以是移动通讯终端的显示装置也可以是计算机的显示装置，用于将对定位轨迹的采集和修正过程以可视化的图像形式显示出来。

另一方面，本发明还提供了一种计算机介质，所述计算机介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述所述的修正定位轨迹的方法。

在符合本领域技术人员的知识和能力水平范围内，本文提及的各种实施例或者技术特征在不冲突的情况下，可以相互组合而作为另外一些可选实施例，这些并未被一一罗列出来的、由有限数量的技术特征组合形成的有限数量的可选实施例，仍属于本发明揭露的技术范围内，亦是本领域技术人员结合附图和上文所能理解或推断而得出的。

最后再次强调，上文所列举的实施例，为本发明较为典型的、较佳实施例，仅用于详细说明、解释本发明的技术方案，以便于读者理解，并不用以限制本发明的保护范围或者应用。

因此，在本发明的精神和原则之内所作的任何修改、等同替换、改进等而获得的技术方案，都应被涵盖在本发明的保护范围之内。

Claims (8)

1.一种实时修正定位轨迹的方法，其特征在于：包括以下步骤：

采集得到待定节点的第一位置信息和确定节点的位置信息；

判断所述待定节点对应的运动数值是否超出第一阈值，若超出，则根据确定节点的运动数值拟定得到所述待定节点的第二位置信息，以所述第二位置信息作为所述待定节点的最终位置信息，对定位轨迹进行更新；

所述运动数值包括速度或加速度；

所述定位轨迹包括待定节点和确定节点；

根据所述第二位置信息和所述确定节点的位置信息，对定位轨迹执行滤波，得到进一步修正后的定位轨迹。

根据预设的滤波倍数值，将所述待定节点的第一位置信息对应的点及所述待定节点最近的一个所述确定节点连接成第一直线，将所述确定节点拟合成第二直线，计算得到所述第一直线和所述第二直线的夹角，当所述夹角大于第二阈值时，增大所述滤波倍数值；

将所述待定节点的第二位置信息作为观测值，所述待定节点最近的确定节点的位置信息作为预测值，对所述确定节点和所述待定节点执行卡尔曼滤波，得到所述的进一步修正后的定位轨迹。

2.根据权利要求1所述修正定位轨迹的方法，其特征在于，拟定得到所述待定节点的第二位置信息包括以下步骤：

根据最接近所述待定节点的至少三个所述确定节点，利用最小二乘法拟合得到直线方程；

根据所述直线方程、至少三个所述确定节点对应的速度计算出的平均速度以及采集频率，生成位于所述确定节点沿直线方向的正前方的所述第二位置信息。

3.根据权利要求2所述修正定位轨迹的方法，其特征在于：

根据所述直线方程的斜率，推算得到所述待定节点的第二位置信息。

4.根据权利要求1所述修正定位轨迹的方法，其特征在于，对定位轨迹执行滤波，得到进一步修正后的定位轨迹还包括以下步骤：

计算得到所述定位轨迹对应的平均速度v1，当所述平均速度v1小于第三阈值时，增大所述滤波倍数值，当所述平均速度v1大于第四阈值时，将所述滤波倍数值设定为固定值。

5.根据权利要求1所述修正定位轨迹的方法，其特征在于，计算得到所述第一直线和所述第二直线的夹角包括以下步骤：

将所述确定节点的位置信息和所述待定节点的第一位置信息转换为笛卡尔坐标信息，通过向量夹角计算所述第一直线和所述第二直线的夹角；

所述确定节点的位置信息和所述待定节点的第一位置信息为经纬度信息。

6.根据权利要求1所述修正定位轨迹的方法，其特征在于，对定位轨迹执行滤波，得到进一步修正后的定位轨迹包括以下步骤：

当待测节点第一位置对应的移动速度小于第三阈值且所述第一直线和第二直线的夹角大于第二阈值时，增大所述滤波倍数值；当待测节点对应的移动速度大于第四阈值且所述第一直线和第二直线的夹角小于第二阈值是时，减小所述滤波倍数值。

7.根据权利要求1所述修正定位轨迹的方法，其特征在于：

将所述修正后的定位轨迹数据发送到显示设备，供所述显示设备对所述定位轨迹进行显示。

8.一种计算机介质，其特征在于，所述计算机介质上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1至7任一项所述的修正定位轨迹的方法。

Images