一种草坪修护机器人定位定向方法及系统

本发明涉及草坪修护机器人领域，具体涉及一种草坪修护机器人定位定向方法及系统。

背景技术：

1、随着现代城市建设过程中草坪绿化面积的不断增加，草坪修护的需求也越来越多，人工进行草坪修护需要耗费大量的人力物力且效率低下。因此使用草坪修护机器人代替人工修护已成为草坪修护行业的趋势。

2、目前已有的草坪修护机器人主要分为无定位系统机器人和有定位系统机器人。无定位系统机器人通过提前铺设电子篱笆防止机器人逃离工作区，机器人在工作区域的路径是随机的，无法对机器人路径进行规划，机器人工作过程中存在大量重复路径，导致机器人工作效率低下；有定位系统机器人无需铺设电子篱笆，可对机器人工作路径进行规划工作效率高。

3、目前有定位系统机器人主要通过惯导或卫星定位，单一惯导定位无法消除累计误差；单一卫星定位，卫星信号被遮挡时，定位精度大幅下降。有定位系统机器人主要通过磁力计或者双天线卫星定位模块实现定向，草坪修护机器人一般安装有大功率割草电机会产生强电磁干扰环境，对磁力计的定向产生很大的干扰；双天线卫星定位模块增加了一个卫星信号接收天线一方面会增加成本，另外双天线距离不能太短，否则会降低定向精度，这就限制了草坪养护机器人的尺寸。

技术实现思路

1、针对现有技术的不足，本发明公开了一种草坪修护机器人定位定向方法及系统，解决草坪修护机器人运行过程中定位定向问题，提高机器人定位定向精度。

2、本发明的目的可以通过以下技术方案实现：

3、一种草坪修护机器人定位定向方法，包括：

4、获取机器人的第一位置数据，以及机器人以第一位置作为参照，所述机器人沿着直线行驶的第二位置数据，所述机器人的第一位置数据和所述机器人的第二位置数据均通过gnss监测站监测所述机器人第一位置和第二位置获取；

5、基于机器人的第一位置数据和第二位置数据，获取机器人的初始航向数据；

6、将机器人的初始航向数据输入陀螺仪，获取机器人的初始状态数据；其中初始状态数据包括对应初始航向数据的机器人位置数据、速度数据和航向数据；

7、分别获取基于gnss监测站、陀螺仪和里程计监测得到机器人状态信息数据；其中状态信息数据包括机器人的位置数据、速度数据和航向数据；

8、基于初始状态信息数据初始化卡尔曼融合定位模块；将状态信息数据输入卡尔曼融合定位模块进行融合估计，得到当前时刻的机器人状态信息数据的最优估计值；

9、基于状态信息数据的最优估计值确定当前时刻机器人的位置、速度和航向。

10、在一些实施例中，基于机器人的第一位置数据和第二位置数据，获取机器人的初始航向数据，包括以下步骤：

11、第一位置a0的经纬度坐标为a0(a0j，a0w)，第二位置a1的经纬度坐标为a1(a1j，a1w)；

12、计算短距离下a0a1的航向h0：

13、

14、根据a1点所处的象限判断航向h0的具体值：a1在第一象限及y轴正半轴，h0＝h1；a1在第二象限，h0＝h1+360；a1在第三四象限及y轴负半轴，h0＝h1+180。

15、在一些实施例中，将机器人的初始航向数据输入陀螺仪，获取机器人的初始状态数据，包括以下步骤：

16、通过通用横轴墨卡托投影获取a1(a1j，a1w)在utm坐标系中的坐标a1(x，y)；

17、把陀螺仪的初始值设置为机器人的初始航向h0；通过坐标变换把a1(x，y)变换到机器人几何中心a'1(x0,y0)，以a'1(x0,y0)和h0作为机器人在utm坐标系中的初始位置，获得机器人初始状态x0＝[x0,y00,0,h0]t，x0,y00,0,h0分别为机器人在utm坐标系中的位置、速度和航向。

18、在一些实施例中，将状态信息数据输入初始化后的卡尔曼融合定位模块进行融合估计，还包括以下步骤：

19、将基于gnss监测站得到的状态信息数据通过通用横轴墨卡托投影转化为utm平面坐标系内的状态信息数据；

20、将基于陀螺仪和里程计得到的utm坐标系内的状态信息数据与基于gnss监测站的utm平面坐标系内的状态信息数据通过坐标变换到机器人的几何坐标中心的状态信息数据。

21、在一些实施例中，卡尔曼融合定位模块包括协方差矩阵p、预测噪声协方差矩阵q和传感器噪声协方差矩阵r。

22、在一些实施例中，当前时刻的状态信息数据的最优估计值的获取，包括以下步骤：

23、根据上一时刻机器人状态估计值预测机器人当前时刻状态计算公式为：

24、

25、

26、式中为状态转移矩阵；

27、为控制矩阵；

28、为t-1时刻的控制输入；

29、pt-1为t-1时刻的预测协方差矩阵，是t时刻协方差矩阵融合估计值，q为预测噪声协方差矩阵；

30、根据机器人当前时刻状态估计值和基于gnss监测站、陀螺仪和里程计得到机器人状态信息数据的观测值，确定机器人状态当前时刻的最优估计值计算公式为：

31、

32、

33、

34、其中kt为与传感器相关的卡尔曼系数，为与gnss监测站、陀螺仪和里程计相关的观测矩阵，r为与gnss监测站、陀螺仪和里程计相关的观测噪声协方差矩阵，zt为gnss监测站、陀螺仪和里程计的观测量，i为单位矩阵；机器人初始时刻状态为

35、在一些实施例中，还包括以下步骤：

36、得到当前时刻的状态信息数据的最优估计值后更新尔曼融合定位模块的预测协方差矩阵；

37、间隔t时间判断机器人是否直线行走且gnss信号良好：若是，则采集机器人直线上的两点重新计算机器人的航向信息并更新赋值给陀螺仪；若否，则跳过此次信息数据采集。

38、在一些实施例中，所述gnss监测站包括单天线gnss流动站和gnss基准站。

39、在一些实施例中，所述第一位置与第二位置的距离至少为2米。

40、本技术还提出一种草坪修护机器人定位定向系统，包括单天线gnss流动站、gnss基准站、无线数传电台、陀螺仪模块、里程计模块和融合定位模块；

41、其中单天线gnss流动站、陀螺仪模块、里程计模块和融合定位模块固定在草坪修护机器人；gnss基准站和草坪修护机器人之间通过无线传输模块进行数据传输。

42、本发明的有益效果：

43、本发明通过单天线gnss流动站实现机器人定向，相比双天线卫星定向在保证机器人定向精度的前提下，降低机器人定向成本和定向系统尺寸；相比磁力计定向，更不易受外界环境干扰，定向可靠性更高。各传感器定位数据在融合前先进行滤波处理，之后再采用多传感器融合算法估计机器人状态最优估计值，有益于提高最终融合定位的精度。融合了gnss、陀螺仪和里程计定位数据的位置估计方式，进一步提高了机器人的定位精度，且保证了gnss信号被遮挡时机器人仍然能在段时间内获得较高的定位精度。