基于无人扫地车融合导航输出结果平滑处理方法及系统技术方案

本发明专利技术涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统，方法包括：采用卡尔曼滤波算法将GPS原始数据、里程计数据和IMU惯导数据进行融合获得第二融合定位数据；采用卡尔曼滤波算法将激光雷达数据、里程计数据和IMU惯导数据进行融合获得第一融合定位数据；基于数据队列中相邻的第一融合定位数据和第二融合定位数据之间的差计算均值；利用滑动窗将切换点前n/2个融合定位数据减去均值，并替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并替换数据队列中切换点后n/2个融合定位数据，就保证了数据队列中融合定位数据组成的曲线是平滑的，防止车辆出现晃动。防止车辆出现晃动。防止车辆出现晃动。

【技术实现步骤摘要】
基于无人扫地车融合导航输出结果平滑处理方法及系统


[0001]本专利技术涉及数据平滑处理
，特别是涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统。

技术介绍

[0002]在低速无人扫地车多传感器融合导航中，由于不同传感器在融合过程中存在偏差，如果偏差较大，且符合定位结果，则在直接输出中就会出现车辆的严重晃动。比如gps和激光雷达同时发布位置信息坐标x、y、z和航向信息，当以gps为主时，在路过高楼之间时，gps信号丢失，需要切换到以激光雷达定位，但是由于激光雷达定位技术的限制，其与gps坐标系会稍微有点偏差，这样在切换的过程中车定位坐标会有一个跳变，导致车辆出现晃动。另外，现有技术方案并未公开如何解决上述晃动，进而实现平滑处理，因此本领域亟需解决因切换传感器而导致的车辆晃动问题。

技术实现思路

[0003]本专利技术的目的是提供一种基于无人扫地车融合导航输出结果平滑处理方法及系统，以实现导航输出结果平滑处理。
[0004]为实现上述目的，本专利技术提供了一种基于无人扫地车融合导航输出结果平滑处理方法，所述方法包括：
[0005]步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
[0006]步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
[0007]步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
[0008]步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
[0009]步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
[0010]步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
[0011]可选地，所述方法还包括：
[0012]步骤S7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
[0013]可选地，所述基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
[0014]步骤S51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
[0015]步骤S52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
[0016]可选地，所述将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
[0017]根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
[0018]本专利技术还公开一种基于无人扫地车融合导航输出结果平滑处理系统，所述系统包括：
[0019]获取模块，用于获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
[0020]第二融合定位数据确定模块，用于以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
[0021]第一融合定位数据确定模块，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
[0022]数据存入模块，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
[0023]均值计算模块，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
[0024]平滑处理模块，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
[0025]可选地，所述系统还包括：
[0026]数据更新模块，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
[0027]可选地，所述均值计算模块，具体包括：
[0028]偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
[0029]判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
[0030]可选地，所述数据存入模块，具体包括：
[0031]根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
[0032]根据本专利技术提供的具体实施例，本专利技术公开了以下技术效果：
[0033]本专利技术涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统，方法包括：采用卡尔曼滤波算法将GPS原始数据、里程计数据和IMU惯导数据进行融合获得第二融合定位数据；采本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述方法包括：步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。2.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述方法还包括：步骤S7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。3.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：步骤S51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；步骤S52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。4.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束...

【专利技术属性】
技术研发人员：杨海松，
申请(专利权)人：上海于万科技有限公司，
类型：发明
国别省市：