一种多传感器融合定位轨迹平滑方法

技术领域

本发明涉及定位技术领域，具体地说，本发明涉及一种多传感器融合定位轨迹平滑方法。

背景技术

在基于滤波器的多传感器定位方案中，IMU用于位置预测，位置观测有多种定位方式可以选择，一般在室外定位使用RTK定位作为位置观测，在室内定位使用激光雷达定位作为位置观测。

现有技术在位置观测切换时，不同的定位方式可能出现定位不一致问题，它们之间经常有较大的位置跳变，滤波器若直接融合这类位置观测，那么融合后定位轨迹会变得不平滑，定位位置在观测切换后出现较大的抖动。

发明内容

本发明提供一种多传感器融合定位轨迹平滑方法，以解决上述背景技术中存在的技术问题。

为了实现上述目的，本发明采取的技术方案为：一种多传感器融合定位轨迹平滑方法，具体包括以下步骤，

步骤S1，计算位置差ΔP，设P

步骤S2，计算位置平滑量P

步骤S3，补偿位置观测值，将平滑量P

步骤S4，在滤波器中融合P’

步骤S5，重复步骤S2到步骤S4，直至满足平滑退出条件，完成从观测A到观测B的切换。

优选的，所述步骤S2，计算位置平滑量P

步骤S2.1，计算位置差的二范数||ΔP||，计算公式根据位置坐标的表示形式确定，假设位置坐标使用平面坐标，则计算公式可以确定为：

步骤S2.2，位置差阈值判断；若||ΔP||

步骤S2.3，根据位置平滑速率k，计算位置平滑用时

步骤S2.4，计算位置平滑因子K＝F(t,T)。

优选的，所述步骤S2.1计算公式中，P

优选的，所述步骤S2.2中P

优选的，所述步骤S2.3式中t为从平滑开始时刻到当前时刻的时间差，F为平滑因子计算函数，它根据t和T计算出平滑因子K。

优选的，所述设计F函数需要考虑实际使用场景的限制。

采用以上技术方案的有益效果是：

1、本发明的方法能够处理切换位置观测时出现的定位不一致问题，实现从一种位置观测平滑过渡到另一种位置观测；

2、本发明的方法能够使融合后定位轨迹变得平滑，有效的避免定位位置在观测切换后出现较大的抖动的问题。

3、本发明的方法降低了融合定位对位置观测一致性的要求，能够提高定位的可靠性和可用性。

附图说明

图1为本发明算法流程图；

图2为本发明轨迹平滑示意图；

图3为本发明函数参考设计曲线图；

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，目的是帮助本领域的技术人员对本发明的构思、技术方案有更完整、准确和深入的理解，并有助于其实施。

如图1至图3所示，本发明是一种多传感器融合定位轨迹平滑方法，能够处理切换位置观测时出现的定位不一致问题，实现从一种位置观测平滑过渡到另一种位置观测。

具体包括以下步骤，

步骤S1，计算位置差ΔP，设P

步骤S2，计算位置平滑量P

步骤S3，补偿位置观测值，将平滑量P

步骤S4，在滤波器中融合P’

步骤S5，重复步骤S2到步骤S4，直至满足平滑退出条件，完成从观测A到观测B的切换。

步骤S2，计算位置平滑量P

步骤S2.1，计算位置差的二范数||ΔP||，计算公式根据位置坐标的表示形式确定，假设位置坐标使用平面坐标，则计算公式可以确定为：

步骤S2.2，位置差阈值判断；若||ΔP||

步骤S2.3，根据位置平滑速率k，计算位置平滑用时

步骤S2.4，计算位置平滑因子K＝F(t,T)。

步骤S2.1计算公式中，P

步骤S2.2中P

步骤S2.3式中t为从平滑开始时刻到当前时刻的时间差，F为平滑因子计算函数，它根据t和T计算出平滑因子K，设计F函数需要考虑实际使用场景的限制。

以下用具体实施例对具体工作方式进行阐述：

实施例1：

如图2所示，当位置观测准备从观测A切换到观测B时，本方法首先计算定位观测值B与融合定位之间的位置差，然后根据平滑策略计算平滑量，并将平滑量补偿到定位观测值B，最后在滤波器中融合补偿过的定位观测值B，从而实现从观测A平滑切换到观测B

以上结合附图对本发明进行了示例性描述，显然，本发明具体实现并不受上述方式的限制，只要是采用了本发明的方法构思和技术方案进行的各种非实质性的改进；或未经改进，将本发明的上述构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。