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

1.本发明涉及数据平滑处理技术领域，特别是涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统。


背景技术：

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


技术实现要素：

3.本发明的目的是提供一种基于无人扫地车融合导航输出结果平滑处理方法及系统，以实现导航输出结果平滑处理。
4.为实现上述目的，本发明提供了一种基于无人扫地车融合导航输出结果平滑处理方法，所述方法包括：
5.步骤s1：获取激光雷达数据、里程计数据、imu惯导数据和gps原始数据；
6.步骤s2：以gps原始数据为观测数据，采用卡尔曼滤波算法将所述gps原始数据、所述里程计数据和所述imu惯导数据进行融合，获得第二融合定位数据；
7.步骤s3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述imu惯导数据进行融合，获得第一融合定位数据；
8.步骤s4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
9.步骤s5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
10.步骤s6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
11.可选地，所述方法还包括：
12.步骤s7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
13.可选地，所述基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
14.步骤s51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
15.步骤s52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
16.可选地，所述将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
17.根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
18.本发明还公开一种基于无人扫地车融合导航输出结果平滑处理系统，所述系统包括：
19.获取模块，用于获取激光雷达数据、里程计数据、imu惯导数据和gps原始数据；
20.第二融合定位数据确定模块，用于以gps原始数据为观测数据，采用卡尔曼滤波算法将所述gps原始数据、所述里程计数据和所述imu惯导数据进行融合，获得第二融合定位数据；
21.第一融合定位数据确定模块，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述imu惯导数据进行融合，获得第一融合定位数据；
22.数据存入模块，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
23.均值计算模块，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
24.平滑处理模块，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
25.可选地，所述系统还包括：
26.数据更新模块，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
27.可选地，所述均值计算模块，具体包括：
28.偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
29.判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
30.可选地，所述数据存入模块，具体包括：
31.根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
32.根据本发明提供的具体实施例，本发明公开了以下技术效果：
33.本发明涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统，方法包括：采用卡尔曼滤波算法将gps原始数据、里程计数据和imu惯导数据进行融合获得第二融合定位数据；采用卡尔曼滤波算法将激光雷达数据、里程计数据和imu惯导数据进行融合获得第一融合定位数据；基于数据队列中相邻的第一融合定位数据和第二融合定位数据之间的差计算均值；利用滑动窗将切换点前n/2个融合定位数据减去均值，并替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并替换数据队列中切换点后n/2个融合定位数据，就保证了数据队列中融合定位数据组成的曲线是平滑的，防止车辆出现晃动。
附图说明
34.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
35.图1为本发明基于无人扫地车融合导航输出结果平滑处理方法流程图；
36.图2为本发明数据队列中存入的融合定位数据构成的曲线示意图；
37.图3为本发明求取均值示意图；
38.图4为本发明基于无人扫地车融合导航输出结果平滑处理系统结构图；
39.图5为原始没有进行平滑的定位曲线示意图；
40.图6为本发明进行平滑处理的定位曲线示意图。
具体实施方式
41.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
42.本发明的目的是提供一种基于无人扫地车融合导航输出结果平滑处理方法及系统，以实现导航输出结果平滑处理。
43.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
44.实施例1
45.如图1所示，本发明公开一种基于无人扫地车融合导航输出结果平滑处理方法，所述方法包括：
46.步骤s1：获取激光雷达数据、里程计数据、imu惯导数据和gps原始数据。
47.步骤s2：以gps原始数据为观测数据，采用卡尔曼滤波算法将所述gps原始数据、所述里程计数据和所述imu惯导数据进行融合，获得第二融合定位数据。
48.步骤s3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述imu惯导数据进行融合，获得第一融合定位数据。
49.步骤s4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中。
50.步骤s5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值。
51.步骤s6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
52.下面对各个步骤进行详细论述：
53.步骤s1：获取激光雷达数据、里程计数据、imu惯导数据和gps原始数据；所述imu惯导数据包括车辆加速度和角加速度信息，所述里程计数据为车辆的速度信息。
54.步骤s2：以gps原始数据为观测数据，采用卡尔曼滤波算法将所述gps原始数据、所述里程计数据和所述imu惯导数据进行融合，获得第二融合定位数据；初始时刻的位姿数据必然为gps原始数据；因为本发明设置gps优先级高于激光雷达。具体生成第二融合定位数据的方式与生成第一融合定位数据的方式相同，在此不再一一赘述。
55.步骤s3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述imu惯导数据进行融合，获得第一融合定位数据，具体包括：
56.步骤s31：采用ndt算法将所述激光雷达数据转换到84坐标系下，获得第一位姿数据；所述第一位姿数据包括航向角和位置信息。
57.步骤s32：根据当前时刻的位姿数据、所述里程计数据和所述imu惯导数据进行推算，获得预测位姿数据l。
58.步骤s33：根据l
′
＝fl u确定下一时刻的位姿数据，其中，l
′
表示下一时刻的位姿数据，f为系统状态方程，u为系统噪声。
59.步骤s34：根据w＝n-hl
′
确定测量余量，其中，w为测量余量，n为激光雷达数据通过ndt算法转换到84坐标系下获得第一位姿数据，h为测量矩阵。
60.步骤s35：根据计算卡尔曼增益，其中，p
′
为预测协方差矩
阵，h为测量矩阵，r为观测噪声，k为卡尔曼增益，f为系统状态方程，q为过程噪音，p为状态协方差矩阵。
61.步骤s36：根据l
″
＝l
′
kw计算第一融合定位数据，其中，l
″
为第一融合定位数据。
62.步骤s4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
63.根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。数据队列中存入的融合定位数据构成的曲线如图2所示。本发明公开设定的正常可用值与不可用值是在实验之前进行设定的。
64.步骤s5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
65.步骤s51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值。在两组数据切换的过程中，由于两者坐标系存在偏差，所以同一个切换的位置点会产生一个偏差值delt_a，偏差值delt_a＝(delt_x,del_y,delt_z,delt_yaw)，其中，delt_x、del_y、delt_z分别表示三个方向上的位置偏差值，单位为米，delt_yaw表示航向角偏差，单位度。
66.步骤s52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值；如果不满足第三设定条件，则定位失败，结束，具体如图3所示。第一设定条件为delt_x《0.1m，delt_y《0.1m，delt_z《0.1m且delt_yaw《5
°
；第二设定条件为0.1m≤delt_x《0.3m，0.1m≤delt_y《0.3m，0.1m≤delt_z《0.3m且5
°
≤delt_yaw《10
°
；第三设定条件为0.3m≤delt_x《0.5m，0.3m≤delt_y《0.5m，0.3m≤delt_z《0.5m且10
°
≤delt_yaw《15
°
。
67.将所述偏差值分成n份获得均值，具体公式为：avg_data(avg_x,avg_y,avg_z,avg_yaw)＝delt_a(delt_x,delt_y,delt_z，delt_yaw)/n；其中，avg_data为将所述偏差值delt_a分成n份后获得的均值，avg_x、avg_y、avg_z分别为三个方向上的位置均值，delt_x、delt_y、delt_z分别为三个不同方向上的位置偏差值，avg_yaw、delt_yaw分别为航向角均值和航向角偏差值，avg_x、avg_y、avg_z、delt_x、delt_y、delt_z单位均为米，avg_yaw、delt_yaw的单位均为度。
68.步骤s6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；当切换点前融合定位数据为第一融合定位数据时，则切换点后的融合定位数据为第二融合定位数据；或者当切换点前融合定位数据为第二融合定位数据时，则切换点后的融合定位数
据为第一融合定位数据；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。本发明利用滑动窗将切换点前n/2个融合定位数据减去均值，将切换点后n/2个融合定位数据加上均值，这样就保证了在组合导航不同定位方式切换时产生的误差平均分到了n组融合结果中，也就保证了数据队列output_data中定位数据组成的曲线是平滑的，没有跳变。
69.步骤s7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据，保证数据队列中包括200个融合定位数据，这样往复循环；由于队列中定位数据的曲线是经过本专利算法进行平滑的，所以就保证了发布定位结果的曲线也是平滑的，从而保证了车辆行驶的平稳性。
70.本发明将公开的平滑处理方法应用到无人机驾驶扫地车上，就可以实现无人驾驶扫地车在高楼、隧道等无gps信号的区域到空旷广场等有gps信号的区域实现平稳的运行。
71.实施例2
72.如图4所示，本发明还公开一种基于无人扫地车融合导航输出结果平滑处理系统，所述系统包括：
73.获取模块401，用于获取激光雷达数据、里程计数据、imu惯导数据和gps原始数据。
74.第二融合定位数据确定模块402，用于以gps原始数据为观测数据，采用卡尔曼滤波算法将所述gps原始数据、所述里程计数据和所述imu惯导数据进行融合，获得第二融合定位数据。
75.第一融合定位数据确定模块403，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述imu惯导数据进行融合，获得第一融合定位数据。
76.数据存入模块404，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中。
77.均值计算模块405，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值。
78.平滑处理模块406，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
79.数据更新模块407，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
80.作为一种可选的实施方式，本发明所述均值计算模块405，具体包括：
81.偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值。
82.判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第
三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
83.作为一种可选的实施方式，本发明所述数据存入模块404，具体包括：
84.根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
85.与实施例1相同的部分在此不再一一赘述，具体参考实施例1。
86.实施例3
87.采用本发明公开的方法进行验证，将不加平滑算法直接将定位结果输出，则车辆在gnss、lidar、里程计辅助路用惯导融合推算的部分就会出现左右摆动的情况，如果融合偏差继续扩大，可能导致车辆定位丢失，更严重的情况可能导致撞车。如图5为原始没有进行平滑的定位曲线，图6为进行平滑处理后的曲线，转弯处有gnss和lidar导航方式的切换，这证明本发明对曲线进行平滑处理效果明显，且是成功的。
88.本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
89.本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。