一种移动设备的定位误差修正方法与流程

本发明涉及移动机器人的一种定位测量方法，尤其涉及一种机器人的定位误差修正方法。

背景技术：

1、近年来，随着科技水平不断提升，移动机器人技术已经广泛应用到日常生活的各个领域。基于激光雷达的移动机器人，越发受到人们的重视。

2、目前现有的移动机器人，经常需要进出各类复杂的环境；考虑到特定的工作场景，如空旷，长廊等不利因素，现有技术很难保证定位的鲁棒性。现有的部分移动设备，会在空旷场景或长廊，增加一些辅助定位的标签，但是这种方式需要对环境进行改造，不够鲁邦；另外一些会在进入特定区域，只使用里程计进行推算，但是这种方式需要太多的人工参与，不够智能；另外一些会增加相机等传感器，但是该方法会增加硬件成本，并且相机容易收到光线影响，使用感较差。

3、因此在长廊，空旷场景等环境下，依赖激光进行定位的移动机器人，会产生由于约束不足导致的退化问题，进而影响定位精度，现有技术缺少了一种机器人误差修正的方法来克服上述缺陷。

技术实现思路

1、为了解决背景技术中存在的问题，本发明的目的在于提供一种移动机器人误差修正的方法，提高机器人在各种复杂场景的定位精度及鲁棒性，提升机器人的智能化水平。

2、为解决上述问题，本发明采用了以下技术方案：

3、移动设备在移动过程中，实时采集所述移动设备周围的环境数据；

4、对环境数据中的里程计数据进行第一处理获得移动设备的运动状态信息并保存；

5、对环境数据中的激光数据进行第二处理，获取新的点云数据、修正维度和误差修正系数并保存；

6、对环境数据中的激光数据进行第三处理移动设备的物理运动量和筛选后的激光数据；

7、根据筛选的激光数据、误差修正系数、修正维度和移动设备的实际物理运动量进行实时误差修正，从而进行定位误差修正。

8、所述的移动设备可以为机器人。

9、所述移动设备上装备有里程计、惯性测量单元(imu)、激光雷达等传感器。

10、所述移动设备周围的环境数据具体为移动设备所装配的传感器数据，包括里程计的里程计数据、惯性测量单元的imu数据和激光雷达的激光数据。

11、所述对采集的里程计数据进行第一处理获得运动状态信息并保存，具体包括：

12、以移动设备当前获取的环境数据中的里程计数据和imu数据计算移动设备的运动状态；所述移动设备的运动状态包括移动设备的位置、方向、速度、时间等。

13、保存移动设备的运动状态信息。

14、所述对采集的激光数据进行第二处理，获取新的点云数据、修正维度和误差修正系数并保存，具体包括：

15、s1、计算当前视野内激光数据的法向量；

16、s2、计算当前视野内激光数据的有效数据占有率；

17、s3、将当前视野内激光数据的法向量进行投影变换为另一坐标系的点云数据；

18、s4、对新坐标系下的点云数据，利用主成份分析法pca计算特征值及特征向量；

19、s5、由特征值及特征向量确定误差修正对应的修正维度，回到步骤s3中处理获得新的点云数据；

20、s6、根据新的点云数据处理得到初步修正系数；

21、s7、根据初步修正系数结合有效数据占有率处理得到最终的误差修正系数；

22、s8、保存数据。

23、所述的激光数据为激光雷达采集的点云数据。

24、计算当前视野内激光数据的法向量，具体是针对点云数据中的每个点提取法向量，获得所有点的法向量。

25、所述s2计算当前视野内激光数据的有效数据占有率，具体包括：

26、由激光数据的所有法向量的数量、激光的视野角和角分辨率按照以下公式计算有效数据占有率k；

27、

28、其中，kα为有效数据占有率，nnormal为法向量数量，r,r分别为角分辨率和视野角，nline为激光雷达线数。

29、所述s3将当前视野内激光数据的法向量进行投影变换为另一坐标系的点云数据，具体包括：

30、法向量是由多维向量构成，根据所有法向量构建多维空间坐标系；

31、每个法向量以自身向量方向确定在多维空间坐标系内的方位角；

32、以每个法向量在激光数据中对应的点根据在自身坐标系下的模长及上一步在多维空间坐标系内的方位角，确定投影后的坐标点；

33、最终由所有投影后的坐标点构成点云数据。

34、所述的多维空间坐标系为三维空间坐标系或者二维空间坐标系。

35、当多维空间坐标系为三维空间坐标系时，方位角包括法向量与x,y平面之间的夹角以及法向量投影到x,y平面后与y轴之间的夹角；并且按照以下公式确定投影后的坐标点：

36、xcoor＝r·cos(ω)·sin(α)

37、ycoor＝r·cos(ω)·cos(α)

38、zcoor＝r·sin(ω)

39、其中，r为模长，ω为法向量与x,y平面之间的夹角，α为法向量投影到x,y平面后与y轴之间的夹角，xcoor,ycoor,zcoor为投影后的坐标点。

40、当多维空间坐标系为二维空间坐标系时，方位角仅包括法向量与y轴之间的夹角；并且按照以下公式确定投影后的坐标点：

41、xcoor＝rsin(α)

42、ycoor＝rcos(α)

43、其中，r为模长，α为法向量在坐标系下的角度，xcoor,ycoor为投影后的坐标点。

44、所述s5由特征值及特征向量确定误差修正对应的维度，回到步骤s3中处理获得新的点云数据，具体是以所有特征值中最大特征值对应方向作为x轴方向；并且以所有特征值中最小特征值对应方向作为误差修正对应的修正维度，当多维空间为三维时，作为z轴方向，当多维空间为二维时，作为y轴方向，以此回到步骤s3重新构建新的多维空间坐标系，并将所有法向量转换到新的多维空间坐标系下获得新的点云数据。

45、所述s6根据新的点云数据处理得到初步修正系数，具体是按照以下公式处理获得：

46、

47、其中，kβ为修正系数，n为估计的点云数量，x,y,z为新的多维空间坐标系下新的点云数据的坐标，dim为对应数据的维度，当为三维数据时x’取坐标点在x、y坐标轴的模长，当为二维数据是x’取坐标点在x轴上的模长；dist(x,y)为坐标为(x,y)的点到原点的距离；

48、所述s7根据初步修正系数结合有效数据占有率处理得到最终的误差修正系数；具体为按如下公式获得：

49、step＝sratio(kmax-kmin)

50、

51、k＝kα*kg

52、其中，kg为最终的修正系数，kmax,kmin为设定的修正参数阈值，sratio为步长系数，sbalance为平衡因子，k为最终的修正系数，step表示步长。

53、所述对采集的激光数据进行第三处理获得移动设备的实际物理运动量和筛选后的激光数据，具体包括：

54、从移动设备的运动状态信息当前激光数据的帧采集时间内的移动设备的物理运动量；所述的物理运动量包括位移、姿态和速度；

55、当前激光数据进行特征筛选获得筛选后的激光数据。

56、本发明的每帧激光数据都是从帧头到帧尾的极短时间内采集获得的点云数据。

57、所述从运动状态信息当前激光数据的帧采集时间内的移动设备的物理运动量，具体包括：

58、计算当前激光数据的帧采集时间内从帧头到帧尾下，由里程计和imu推算获得的运动状态量，作为物理运动量。

59、所述根据筛选的激光数据、误差修正系数、修正维度和移动设备的实际物理运动量进行误差修正，具体包括：

60、1)根据筛选的激光数据结合移动设备所运动下的地图，构建激光特征，具体是计算当前点与对应地图中对应点的距离构建特征残差作为激光特征，公式如下：

61、

62、t＝[so3 t]

63、其中，rl为构建的激光特征残差项，t为待优化的当前位姿，这里t为变换矩阵，由旋转矩阵so3和平移t组成，pj和nj分别为第j个特征点及对应的法向量；pc为地图中的点坐标；

64、在前二十帧以前，移动设备所运动下的地图是由当前帧以及之前所有帧的激光数据叠加构成。

65、在前二十帧之后，移动设备所运动下的地图是由当前帧以前的最近二十帧的激光数据叠加构成。

66、2)根据误差修正系数、修正维度和移动设备的实际物理运动量，构建运动量，具体是以物理运动量及这里以物理运动量和待优化量t求解运动量：

67、

68、其中，ro(t)为构建的运动量约束方程，todom为物理运动量，t为待优化的当前位姿，log为求解其对应的李代数，f(k,dir)为维度修正参数，dir为修正方向；这里f(k,dir)为构建一个dir对应维度为k的多维向量；

69、3)结合激光特征约束和运动量约束构建最大后验估计，同时建立以下目标函数：

70、

71、

72、4)针对目标函数求解物理运动量与优化量的差，由物理运动量与优化量的差加到移动设备的实际物理运动量上从而实现移动设备的运动量误差修正。

73、本发明方法是首先，通过激光数据与局部地图匹配，获得激光特征约束；然后，通过点云数据分布信息确定修正维度及修正系数；最后，结合激光特征约束及运动量约束，求解修正后的最优运动量。

74、本发明的有益效果是：

75、本发明通过激光数据与局部地图匹配，获得激光特征约束；然后，通过点云数据分布信息确定修正维度及修正系数；最后，结合激光特征约束及运动量约束，求解修正后的最优运动量。

76、本发明方法提升了移动机器人在退化场景下的定位精度及鲁棒性，提升移动设备的智能化水平。

77、本发明方法对带有2d和3d激光的移动设备和移动机器人同样适用，应用此方法，由激光数据的分布情况，求解修正方向及修正系数，进而自适应的融合激光和里程计数据，充分考虑了环境分布情况对传感器的影响，提高移动设备的定位精度及鲁棒性，提升移动设备的智能化水平。