一种用于智能割草机器人的定位方法及系统与流程

本发明属于导航定位，更具体涉及一种用于智能割草机器人的融合定位方法和系统。

背景技术：

1、随着我国生态文明建设获得显著进展，城市和城镇的绿化水平得到了显著提高。大量公共绿地，如公园草坪和足球场草坪等，需要进行日常维护。然而，草坪修剪是一项繁重的任务。因此，割草机器人应运而生，旨在实现“解放双手，降低劳动负担”的目标。但是目前市面大多数割草机存在着割草效率低，由于定位不准导致的重复割漏割问题突出。因此，设计一种用于智能割草机器人的具有高鲁棒性定位方法，使智能割草机器人能高效的割草的问题急需解决。

2、目前主流应用在割草机上的定位方法有基于gnss/rtk(差分全球导航定位系统)定位、基于基站的uwb(超宽带)定位。但是gnss/rtk需要预先在附近架设基站或者通过当地的基站获取rtk数据但是需要差分数据服务费两者成本都比较高，而且例如在房屋附近或者树木底下，由于信号减弱和多径问题导致定位精度降低，而基于基站uwb定位，需要事先安装定位标签较为繁琐，且定位精度很难控制在20厘米以内。以目前的技术而言，依靠单传感器实现鲁棒且高精度的定位显然是很难的，即使能实现其硬件成本也会很高，所以依靠多个消费级的传感器实现鲁棒且精度较高的定位是在成本和效果方面较好的权衡。

技术实现思路

1、为了克服上述智能割草机器人在复杂室外环境中面临的环境感知困难的问题，本发明提出一种用于智能割草机器人的定位方法及系统。

2、本发明提出了一种基于多状态约束卡尔曼滤波的定位方法和gnss-imu-vision集成系统，以解决在gnss不稳定或中断时的定位问题。在该系统中，多状态约束卡尔曼滤波可用于有效地融合来自gnss、imu和视觉传感器的测量数据，并通过约束方程对多个状态量进行约束，以提高定位的精确度和鲁棒性。该系统的优势在于不仅可以在gnss部分中断环境下提高性能，而且还可以通过视觉传感器抑制imu漂移，从而提高整个系统的性能。

3、为解决上述技术问题，本发明采用的技术方案为：

4、一种智能割草机器人定位方法，包括如下步骤：

5、s1:接受卫星数据，通过单点定位和双差分模型处理观测数据获得位置和速度；

6、s2:对imu和gnss进行联合初始化，imu获取原始数据，通过误差模型估计加速计和陀螺仪偏置，通过状态方程定义姿态、速度和位置；

7、s3:获取图像，通过角点检测和klt光流跟踪获得连续多张图片的约束关系获取相机的位姿；

8、s4：将前步骤s1、s2、s3的三个位姿估计进行时间同步，然后送入多状态约束卡尔曼滤波器进行更新，得到新的姿态、速度和位置；

9、s5：根据滤波所得的智能割草机器人位置，速度和姿态定位结果规划路径。

10、进一步，步骤s1中，对gnss数据的处理，结合无电离层线性观测模型为：

11、

12、其中，ppc和lpc为无电离层伪距和载波相位组合；f1和f2为双频消电离组合的频率参数。σ和c分别为真空中的波长和光速；ρ为卫星相位中心与gnss接受器天线相位中心之间的几何距离；tr和ts分别为接收机与卫星的时钟偏移量；为对流层延迟；nlc是整数模糊度；εl,lc表示观测噪声。

13、再进一步，在草地的环境下，智能割草机的gnss信号容易遭遇到树木的遮挡，导致ppp观测模型误差较大，本系统在观测模型上对伪距和载波相位进行双差分处理来进一步提高gnss定位精度，其双差分模型为：

14、

15、其中，为卫星间和接受器间的双差分算子；n表示双差分算子的歧义数量；h和δpr分别设计矩阵和位置误差；和为伪距噪声和载波相位噪声。

16、对应的状态向量可以描述为：

17、

18、其中，δxr、δyr和δzr为接收机的近似坐标修正；dwet表示天顶对流层延迟的湿分量的残差；nlc为无电离层组合模糊向量。

19、进一步，步骤s2中，imu的误差状态向量定义为：

20、

21、其中，δre、δve和φ分别为位置、速度和姿态在地球中心坐标系下的误差向量，δba和δbg为加速计和陀螺仪的偏置误差。

22、在割草机器人上，imu传感器的数据比较稳定，但其误差会随着导航定位距离的增加而累积。考虑到其稳定性，本系统将其作为主要定位传感器，融合gnss消除累积误差，其imu和gnss的融合过程可以表示为：

23、δzk＝hgnss，k·δximu+vgnss,k   (6)

24、

25、

26、其中，δzk为在地心坐标系下的位置观测向量；hgnss，k为设计矩阵；δximu为状态相量；vgnss,k为测量噪声；和分别为gnss位置在地心坐标系下的位置向量和gnss位置通过imu计算的在地心坐标下的位置向量；lb分别表示imu坐标系到地心坐标系转换和gnss接收器到imu的平移距离；表示在地心坐标系下imu的姿态；er为位置误差向量。

27、另为本系统还融合了视觉传感器的定位结果，保证gnss在不稳定或中断时还能获得较好的定位结果。

28、进一步，步骤s3中，相机位姿可以表示为：，将新的相机位姿添加到滑动窗口的状态向量中，假设特征点fi可以在多幅图像mj中连续观测到，则特征点在图像mi中的位置可表示为:

29、

30、其中，ci是相机位姿；和分别为特征点fi的位置向量和imu坐标系在地心坐标系的旋转相量。和是特征点的位置。

31、imu和vision的状态向量定位为：

32、

33、其中，k是滑动窗口位姿的数量。

34、当系统接收的新图片时，通过fast角点检测和klt光流跟踪获得相机的误差姿态，然后将获得的误差姿态增广到滑动窗口的状态向量中。则开始多状态约束卡尔曼滤波的更新。在无gnss的情况下，imu和vision的残差可表示为：

35、

36、其中，a是的左零空间的残差矩阵。之后则可进行标准的ekf更新。

37、步骤s4中，在得到各传感器的状态之后，由于各传感器的采样频率并不统一，需要对传感器数据进行时间同步处理。本文提出一个时间插值的方法来处理，时间插值函数如下：

38、

39、

40、其中，m、n和k分别表示gnss、视觉和imu的当前迭代；tgnss,m、tvision,n和timu,k分别表示gnss、视觉和imu的当前迭代的时间；dataimu,k表示imu当前迭代的原始数据。

41、多状态约束器在滤波根据插值函数判断融合那几种传感器，gnss和imu的插值函数成立时将gnss数据和imu数据进行紧耦合，在视觉与imu的插值函数成立时将视觉数据跟imu数据进行紧耦合，在两个插值函数都成立时将gnss、imu和视觉进行紧耦合。其imu、gnss和视觉的融合可表示为：

42、δzk＝hgnss，k·δxk+vgnss,k   (14)

43、

44、

45、这里和式(6)-(8)的区别主要是，hgnss,k设计矩阵增加03×6k的向量和δxk增加了相机的状态向量。当gnss中断时×保持不变。

46、步骤s5中，将滤波所得的智能割草机器人位置，速度和姿态送入决策模块。决策模块采用actor-critic(演员-评论家)强化学习框架算法，通过轨迹规划网络预测出轨迹，然后行为决策网络根据当前草地环境信息选择行为决策，优化路径。其环境信息有视觉的障碍物检测和语义地图的草地边界。

47、一种智能割草机器人定位系统，包括：

48、卫星数据接受和处理模块，用于接收卫星数据，通过单点定位和双差分模型处理观测数据获得位置和速度；

49、初始化模块，对imu和gnss进行联合初始化，imu获取原始数据，通过误差模型估计加速计和陀螺仪偏置，通过状态方程定义姿态、速度和位置；

50、相机位姿获取模块，用于获取图像，通过角点检测和klt光流跟踪获得连续多张图片的约束关系获取相机的位姿；

51、位姿状态更新模块，用于将步骤卫星数据接受和处理模块、初始化模块、相机位姿获取模块的三个位姿估计进行时间同步，然后送入多状态约束卡尔曼滤波器进行更新，得到新的姿态、速度和位置；

52、路径规划模块，用于根据滤波所得的智能割草机器人位置，速度和姿态定位结果规划路径。

53、本发明的优点包括：

54、1.在有gnss信号时引入gnss与imu和视觉信号进行融合，得到高精度的全局定位信息。

55、2.对gnss信号进行鲁棒处理，在信号不稳定时仍能得到较好的定位信息。

56、3.采用滑动窗口和边缘化技术降低转态向量维度，提高计算效率。使系统能够在割草机机器人上运行。