[发明专利]一种基于果园地图的异构多机器人定位和控制方法

权利要求书

1.一种基于果园地图的异构多机器人定位和控制方法，其在果园行间作业的特征在于，包括以下步骤：

(1)领航机器人由人遥控在果树行中行走，先构建一幅完整的果园地图，再通过GNSS坐标系转换，将果园地图中所有的果树中心点，换算成GNSS绝对坐标值[tree_i_gnss_x,tree_i_gnss_y]；领航机器人可以根据雷达扫描的角度和相对距离估计出该范围内最近的果树坐标[tree_n_gnss_x,tree_n_gnss_y]，通过式(1)与果园地图中的各果树坐标[tree_i_gnss_x,tree_i_gnss_y]对比，当d_min接近0时，得到果树的GNSS真实坐标；

距离机器人最近果树真实坐标判定计算公式如下：

(2)再根据需要的果树真实坐标[tree₁_gnss_x,tree₁_gnss_y]、[tree₂_gnss_x,tree₂_gnss_y]、[tree₄_gnss_x,tree₄_gnss_y]，行距r_s、株距p_s来估算出领航机器人和跟随机器人1、2自主导航的参照导航线方程和下一时刻机器人理想位置坐标[X₁,Y₁]，[X_f₁,Y_f₁]，[X_f₂,Y_f₂]；

机器人理想位置坐标计算公式如下：

其中[tree₁_gnss_x,tree₁_gnss_y]为领航机器人[270°,360°]最近的果树1真实坐标、[tree₂_gnss_x,tree₂_gnss_y]为领航机器人[0°,90°]最近的果树2真实坐标，[tree₄_gnss_x,tree₄_gnss_y]为跟随机器人[270°,360°]最近的果树4真实坐标；

(3)分别由各机器人计算其理想和实际位置坐标到参照导航线的横向偏差和航向偏差，并通过实时调整各机器人行进的角速度和线速度，使各机器人到达理想位置，结束行间作业；

机器人参照导航线计算公式如下：

A_j＝tree_i+1_gnss_y-tree_i_gnss_y

B_j＝tree_i_gnss_x-tree_i+1_gnss_x

C_j＝(tree_i+1_gnss_x-tree_i_gnss_y)-(tree_i_gnss_x-tree_i+1_gnss_y) (3)

A₁X_i+B₁Y_i+C₁＝0 (4)

A₂X_i+B₂Y_i+C₂＝0 (7)

其中A_i B_i C_i为机器人参照导航线方程的系数，式(4)为领航机器人和跟随机器人1的参照导航线1方程式，式(7)为跟随机器人2参照导航线2的参照导航线2方程。

2.根据权利要求1所述的一种基于果园地图的异构多机器人定位和控制方法，其在果园地头转弯作业的特征在于，包括以下步骤：

(1)根据果树1、2坐标和式(1)、(2)(5)得到参考导航线1的方程，随着领航机器人前进，通过式(2)不断生成领航点1的下一时刻理想位置坐标[X₁,Y₁]，并将领航机器人的下一时刻的坐标发送至跟随机器人1、跟随机器人2；

(2)跟随机器人1等待直至领航机器人行驶至第3次的理想位置点，跟随机器人1到达领航机器人发布的第1次的点，顺时针90度转向后停止运动；

(3)跟随机器人2等待跟随机器人1到达领航机器人发布的第3次的理想位置点，跟随机器人2达到领航机器人发布的第2次的点，顺时针90度转向后停止运动；

(4)当领航机器人到达第5次理想位置坐标时，开始顺时针90度转向后停止运动，此时跟随机器人1开始行进至第4次理想位置坐标点，并开始顺时针90度转向后停止运动，跟随机器人2可以行进至第3次理想位置坐标点，并开始顺时针90度转向后停止运动；当三个机器人皆到达目标点后，地头转弯作业结束。