一种机器人果园高精度实时定位方法

1.本发明涉及机器人定位技术领域，特别是一种机器人果园高精度实时定位方法。


背景技术：

2.近年来国家对农业机器人的发展十分重视，在其研发事业上投入了较多的人员和资金，并在全社会积极推广使用自动化农机具，实现农业现代化生产。随着政策的推行，各种各样的智能农业机器人层出不穷，其中在机器人导航定位技术反面大多使用的是卫星导航定位系统，但这些农业型机器人由于作业环境的特殊性和复杂性，使其在野外导航定位时面临各种干扰和遮挡，这极易使得卫星定位信号丢失或者失准，也大大降低了机器人的可靠性和实用性。
3.以果园采摘机器人为例，机器人在果树行道间行走时，经常会被繁茂的枝叶遮挡或者覆盖，此时gnss的信号在传输时被果树遮挡，使得定位信号的精度下降，使用者将无法获取自主机器人的准确位置，同时机器人位置信号失准也会影响其导航路线的规划和后续采摘等工作。目前应对机器人定位信号失准问题的主流方法包括gnss结合视觉定位、gnss结合激光雷达定位、基于地图定位等，虽然在定位效果上有所提高，但是仍然无法保证gnss被遮挡情况下的定位精度，而且这些融合定位方法对应用环境有一定要求，无法在复杂环境下很好的工作。因此，研究一种机器人在果园等复杂野外环境下的高精度实时定位方法迫在眉睫。


技术实现要素：

4.本发明的目的在于提供一种机器人果园高精度实时定位方法，在机器人的卫星定位信号丢失或者受到干扰而失准时，仍然可以利用自身的传感器实现精度近似卫星定位信号的高精度实时定位，解决机器人在果园等复杂野外环境下卫星定位信号失锁难题。
5.实现本发明目的的技术方案为：
6.一种机器人果园高精度实时定位方法，通过gnss的高精度定位信息校准各个传感器的融合定位信息，使高精度的融合定位信息可以弥补gnss定位信号。所述的方法包括：
7.步骤1：双目视觉定位，利用双目视觉相机采集路面信息，推算出机器人的定位信息。
8.步骤2：激光雷达定位，利用激光雷达扫描周围环境，确定机器人的定位信息。
9.步骤3：传感器定位信息融合，将各个定位模块的定位信号进行信息融合，得到较为精准的定位结果。
10.步骤4：融合定位信息校准，在gnss rtk定位状态正常时，将gnss的高精度定位信号与融合算法得到的各个传感器的融合定位信息对比，根据比较结果调节融合算法参数。
11.步骤5：检测gnss的定位信号是否正常。
12.步骤6：定位信号切换，当检测到gnss定位信号有误时，系统不再使用gnss的定位结果，而是切换到已经校准的高精度融合定位信息。
13.进一步地，步骤1的具体过程包括：
14.步骤1.1：调整双目视觉相机，使其可以垂直拍摄水平路面，并根据机器人移动速度设定拍摄频率，使相邻帧的图像中有较多的重合区域。
15.步骤1.2：利用傅里叶变换将拍摄的相邻两帧图像转换到频域下，并分别以图像中心为原点建立对数极坐标。
16.步骤1.3：识别相邻两帧图像中的重合区域，将重合区域在两帧图像中旋转和缩放的变化转换为两对数极坐标轴的平移量，从而得到两个图像的角度的和缩放的变化系数。
17.步骤1.4：根据重合区域在二维平面的位置变化，得到机器人在横向和纵向的平移量，结合得到的相邻两帧图像的角度的和缩放的变化系数，可以计算出机器人在相邻两帧时间内的三维直角坐标变化量，从而得到相对于起始时刻机器人的三维直角坐标。
18.步骤1.5：重复步骤1.2到步骤1.4，不断利用相邻帧图像推算可以得到某时间段内机器人的三维坐标变化。
19.进一步地，步骤2的具体过程包括：
20.步骤2.1：利用3d激光雷达传感器360
°
扫描周围的环境，提取周围果树的激光点。
21.步骤2.2：传感器接收到周围果树激光点的数据后，系统会建立以激光雷达为中心的三维直角坐标系，随后将每个激光点的数据转换为三维坐标数据。
22.步骤2.3：选取足够的果树激光点作为定位参考点，随着机器人的移动，参考激光点的坐标不断变化，根据起止时刻参考点的坐标变化可以计算出机器人的位置变化量，从而得到相对于起始时刻机器人的三维直角坐标。
23.进一步地，步骤3的具体过程包括：
24.步骤3.1：将双目视觉相机和激光雷达传感器得到的两组定位坐标转换为世界大地坐标。
25.步骤3.2：建立融合模型，初始化融合参数，将已经转化为大地坐标的两组传感器定位数据进行线性融合。
26.进一步地，步骤4的具体过程包括：
27.步骤4.1：首先确定gnss rtk定位为正常定位状态，确保gnss定位结果的精度。
28.步骤4.2：将高精度的gnss定位结果与传感器的融合定位结果相比较，根据比较结果，调节融合定位算法的相关参数。
29.步骤4.3：重复步骤4.1和步骤4.2，不断调整融合定位算法的参数，使传感器融合定位结果不断趋近gnss的定位结果，从而提高融合定位结果的精度和可靠性。
30.进一步地，所述步骤5中检测gnss的定位信号是否正常的方法包括：直接法和间接法。所述的直接法是指直接利用gnss传输协议中的gga语句，以北斗卫星系统为例，检测bdgga语句中的模式指示位数据，若为0则表示定位无效，若非0，则表示定位有效。所述的间接法是指计算并保存相邻两帧定位数据的定位用时以及接收信号的时间差，并与预先设定的时间阈值对比，如果某两帧的定位用时和接收时间差超过了阈值，则判定为gnss定位信号受到干扰。
31.进一步地，所述步骤5的在整个机器人定位过程中一直开启，当检测到gnss信号正常时则进行融合定位数据的校准，当定位数据精度接近gnss精度后则不再进行校准，同时gnss信号继续处于实时检测状态，当检测到gnss定位信号受干扰时，则继续进行步骤6。
32.进一步地，步骤6的具体过程包括：
33.步骤6.1：当检测到gnss定位信号受干扰时，系统立即将定位信号切换为已校准的传感器融合定位结果，此时该结果的精度和gnss未失锁时的精度一致。
34.步骤6.2：在定位信号切换后，继续对gnss定位信号进行检测，当干扰消失且gnss定位精度恢复时，系统重新将定位信号切换为gnss定位结果。
35.与现有技术相比，本发明的有益效果为：本发明通过双目视觉相机和激光雷达传感器来弥补gnss定位信号受到干扰时的定位精度，设计了融合算法对双目视觉相机和激光雷达获取的机器人定位信息进行融合，再利用gnss rtk定位状态未锁定时的高精度定位信号对该融合定位信息进行校准，使传感器融合定位信息能够达到gnss的定位精度，当系统检测到gnss信号受干扰时，将会用校准后的高精度融合定位信息来代替gnss定位信号；机器人在果园等野外复杂环境下，卫星定位信号频繁受干扰时，该方法可以有效解决定位精度下降问题，而且该方法复杂度低，计算量小，可以快速频繁地实现信号校准和切换，从而为机器人提供高精度实时的定位信息。
附图说明
36.图1是本发明一种机器人果园高精度实时定位方法的场景示意图。
37.图2是本发明一种机器人果园高精度实时定位方法的流程示意图。
具体实施方式
38.本发明通过双目视觉相机和激光雷达传感器来提高gnss定位信号在受到干扰时机器人定位信号的精度，该方法可以实现机器人在果园等复杂环境下拥有高精度实时的定位信号。
39.结合图1所示，机器人在果园树行中行走时，gnss定位信号会被间隔的果树频繁的遮挡，本方法使用双目视觉相机高频拍摄地面图像，利用相邻帧的图像变化不断推算出机器人在横向、纵向以及高度上的变化，同时使用激光雷达扫描周围的环境，选取果树作为参考点，如图1中的7个示例参考激光点，利用与果树的距离变化，推算出机器人的坐标变化，并将两个传感器的定位信号进行融合，最后利用果树间隙中gnss信号良好的时间段，对融合信号进行校准，使精度达到gnss定位信号的精度，此时卫星信号再被遮挡时，系统会切换到融合定位信息，以保证机器人的高精度实时定位。
40.结合图2所示，一种机器人果园高精度实时定位方法，包括以下步骤：
41.步骤1：双目视觉定位，利用双目视觉相机采集路面信息，推算出机器人的定位信息。具体步骤如下：
42.步骤1.1：调整双目视觉相机，使其可以垂直拍摄水平路面，并根据机器人移动速度设定拍摄频率，使相邻帧的图像中有较多的重合区域；
43.步骤1.2：利用傅里叶变换将拍摄的相邻两帧图像转换到频域下，并分别以图像中心为原点建立对数极坐标；
44.步骤1.3：识别相邻两帧图像中的重合区域，将重合区域在两帧图像中旋转和缩放的变化转换为两对数极坐标轴的平移量，从而得到两个图像的角度的和缩放的变化系数；
45.步骤1.4：根据重合区域在二维平面的位置变化，得到机器人在横向和纵向的平移
量，结合得到的相邻两帧图像的角度的和缩放的变化系数，可以计算出机器人在相邻两帧时间内的三维直角坐标变化量，从而得到相对于起始时刻机器人的三维直角坐标；
46.步骤1.5：重复步骤1.2到步骤1.4，不断利用相邻帧图像推算可以得到某时间段内机器人相对于初始位置时的三维坐标(x1,y1,z1)。
47.步骤2：激光雷达定位，利用激光雷达扫描周围环境，确定机器人的定位信息。具体步骤如下：
48.步骤2.1：利用3d激光雷达传感器扫描周围的环境，提取周围果树的激光点。
49.步骤2.2：传感器接收到周围果树激光点的数据后，系统会建立以激光雷达为中心的三维直角坐标系，随后将每个激光点的数据转换为三维坐标数据。
50.步骤2.3：选取足够的果树激光点作为定位参考点，随着机器人的移动，参考激光点的坐标不断变化，根据起止时刻参考点的坐标变化可以计算出机器人的位置变化量，从而得到相对于起始时刻机器人的三维直角坐标(x2,y2,z2)。
51.步骤3：传感器定位信息融合，将各个定位模块的定位信号进行信息融合，得到较为精准的定位结果。具体步骤如下：
52.步骤3.1：将双目视觉相机和激光雷达传感器得到的两组定位坐标转换为世界大地坐标：
[0053][0054]
其中(b1,l1,h1)表示双目视觉定位坐标转换后的大地坐标，(b2,l2,h2)表示激光雷达定位坐标转换后的大地坐标。
[0055]
步骤3.2：建立融合模型如下：
[0056][0057]
其中(b,l,h)表示融合后的定位结果，α1和α2为纬度坐标的融合系数，β1和β2为经度坐标的融合系数，γ1和γ2为高度坐标的融合系数，a1、a2、b1、b2、c1和c2为融合系数调整速度参数，和为坐标微调参数。
[0058]
初始化融合参数，纬度、经度和高度的融合系数α1、α2、β1、β2、γ1和γ2默认为0.5，融合系数调整速度参数a1、a2、b1、b2、c1和c2默认为1，坐标微调参数和默认为0。随后将已经转化为大地坐标的两组传感器定位数据进行数据融合。
[0059]
步骤4：融合定位信息校准，在gnss rtk定位状态正常时，将gnss的高精度定位信号(b
gnss
,l
gnss
,h
gnss
)与各个传感器的定位结果进行第一步调参，以纬度坐标b为例：
[0060]
[0061][0062]
如果调节中参数α1、α2的变化幅度过低或者过高，会导致参数调节次数过多，则可以调节a1、a2，当a1和a2大于1时，纬度参数α1、α2调节幅度变大，当a1和a2小于1时，纬度参数α1、α2调节幅度变小，如果gnss的纬度坐标与某个传感器的纬度坐标近似，则对应的纬度参数α为0，表示该传感器的纬度无需调整。纬度参数α加快了纬度融合的速度。类似的，经度坐标l和高度坐标h融合过程中的参数调整也是如此。
[0063]
所述的第一步调参以后，将gnss的高精度定位信号(b
gnss
,l
gnss
,h
gnss
)与融合算法得到的各个传感器的融合定位信息(b,l,h)对比，进行第二步调参：
[0064][0065]
重复所述的第一步调参和第二步调参，直至融合定位信息(b,l,h)与gnss的高精度定位信号(b
gnss
,l
gnss
,h
gnss
)的差距小于设定为阈值，即融合定位信息的精度近似于gnss定位信号的精度。
[0066]
步骤5：检测gnss的定位信号是否正常。所述的检测gnss定位信号是否正常的方法包括：直接法和间接法。所述的直接法是指直接利用gnss传输协议中的gga语句，以北斗卫星系统为例，检测bdgga语句中的模式指示位数据，若为0则表示定位无效，若非0，则表示定位有效。所述的间接法是指计算并保存相邻两帧定位数据的定位用时以及接收信号的时间差，并与预先设定的时间阈值对比，如果某两帧的定位用时和接收时间差超过了阈值，则判定为gnss定位信号受到干扰。
[0067]
特别的，所述的gnss定位信号收干扰检测在整个机器人定位过程中一直开启，当检测到gnss信号正常时则进行融合定位数据的校准，当定位数据精度接近gnss精度后则不再进行校准，同时gnss信号继续处于实时检测状态，当检测到gnss定位信号受干扰时，则继续进行步骤6。
[0068]
步骤6：定位信号切换，不断更新gnss信号的进测结果，若gnss未受干扰，则融合定位信息/gnss定位信号选择开关选择输出gnss定位信号。当检测到gnss定位信号受干扰时，再进一步判断校正后的融合定位信息精度是否达到gnss正常工作时的定位精度，若精度符合要求，则融合定位信息/gnss定位信号选择开关选择输出融合定位信息，若精度不符合要求，则继续进行融合参数调整，直至融合定位信息精度达到要求，或者gnss恢复正常。
[0069]
步骤1和步骤2所述的定位不仅仅限制于双目视觉和激光雷达传感器，还可以选择其他传感器作为融合定位的定位信号源；且本发明方法不局限于所述方法步骤的顺序。
[0070]
出于说明的目的，上述的具体实施方式只是示例性的，是为了更好的使本领域技术人员能够理解本专利，不能理解为是对本专利包括范围的限制；凡是采用等同替换或等效变换的方式获得的技术方案均落在本发明的保护范围内。