CN106989748A - 一种基于云模型的农业移动机器人人机合作路径规划方法 - Google Patents

Abstract

本发明公开了一种基于云模型的农业移动机器人人机合作路径规划方法，动态引导A*(DGA*)算法进行人机合作路径规划，同时结合人的专业知识和喜好等形成专家系统引入基于云模型的在线不确定性推理，实现更快速的路径规划策略。本发明所达到的有益效果：以农业移动机器人为研究对象，基于人机合作的云在线推理方法，将大量的实验数据处理放在云端，大大降低了时延，提高了农业移动机器人自主路径规划的实时可靠性。提出的基于云模型的人机协同路径规划方法与DGA*算法的结合，既充分发挥了人的主观能动性，又保证了农业移动机器人在复杂动态的环境中的自主行走要求，对于静态、以及突发动态障碍物的避障都有着较好地工作性能。

Description

一种基于云模型的农业移动机器人人机合作路径规划方法

技术领域

本发明涉及一种基于云模型的农业移动机器人人机合作路径规划方法，属于机器人路径规划算法技术领域。

背景技术

农业移动机器人的应用是农业向现代化智能化发展的标志。随着现代高新技术发展，很多学者都致力于对农业移动机器人更高智能化控制的研究工作中。其中，路径规划问题是移动机器人在行驶中能够绕过障碍物从起始点到达目标点的问题，是移动机器人研究领域一个非常重要的方面。

近年来，很多路径规划方法和智能算法被研究并提出，以实现移动机器人的路径规划问题。Noboru Noguchi等提出的基于神经网络和遗传算法的方法，在不同的目标函数的条件下，农业移动机器人都能寻找恰当的避障的路径。马建光等利用概率路径规划方法，生成启发式节点，实现了移动机器人在障碍物密集环境下的路径规划。孙凤池等提出一种利用局部路径规则路图辅助建立全局路图的方法，改善了障碍物附近的局部路径质量，实现了就移动机器人动态环境下的路径规划。赵百轶等提出一种基于四叉树和改进蚁群算法的路径规划方法，实现了机器人高效率大区域的路径规划。李擎等利用模糊逻辑算法设计的模糊控制器，实现了动态环境中机器人快速、准确地无碰撞路径的寻找。史恩秀等利用蚁群算法，仿真获得最短路径，实现了机器人在不同工作环境下的全局高效路径规划。孟蕊等采用人工势力场与模糊控制方法相结合的路径规划方法，克服了传统人工势力场存在的目标不可达、局部最小问题，实现了机器人的路径规划。王殿君对A*算法进行改进设计，减少了生成的轨迹点数目，简化了路径，实现了移动机器人的拐点处的自身姿态调整，以满足其在室内的全自主行走。

然而，由于农业机器人作业环境的复杂多变性，需要处理的作业信息的大量多样性，以上描述的路径规划算法对采集到的图像进行处理规划路径时，会产生很大的时延问题，使得农业机器人不能准确进行自主规划作业。

发明内容

为解决现有技术的不足，本发明的目的在于提供一种基于云模型的农业移动机器人人机合作路径规划方法，解决目前在进行路径规划时所存在的延时问题，大大缩小了搜索空间，提高了路径规划效率。

为了实现上述目标，本发明采用如下的技术方案：

一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，包括如下步骤:

步骤1)采集机器人位置，用四维向量(x^*，y^*，θ^*，φ^*)表示其在二维平面内的位置信息，其中x^*，y^*分别表示机器人在x轴方向和y轴方向的值，θ^*表示二维平面下机器人头部朝向与x轴正方向的夹角、φ^*表示机器人的车轮转向角；

步骤2)基于二维空间规划空间模型，对障碍物采用圆形覆盖，形成若干威胁区域，每个威胁区域作为一个节点；

步骤3)将规划空间中第i个节点的信息表示为：n_i＝{x_i,y_i,θ_i,L_si,father_i,g_i,h_i,f_i}，其中，(x_i，y_i)为第i个威胁区域的中心坐标，θ_i为节点i的水平角度；L_si是起始点到节点i的实际距离；father_i为节点i的父辈节点；g_i、h_i、f_i分别为节点i的实际代价值、预估代价值和总代价值；

以节点i的水平方向角为中心线向左右两边各扩展至车轮的最大转弯角，将所形成的扇形面积作为下一个节点(i+1)的扩展范围；

步骤4)确立代价函数f(i)＝g(i)+h(i)，其中g(i)表示实际代价函数，h(i)表示预估代价函数；

步骤5)将起始点到目标点生成的引导点按升序排列，以引导点为中心形成引导点，目标点作为引导半径为零的最后一个引导点，使机器人按顺序通过各个引导点，探寻高效最优路径。

进一步地，所述步骤2)具体内容为：设(x，y)为规划空间某一点的坐标，规划空间模型为：{(x,y)|0≤x≤X_max,0≤y≤Y_max}，将规划空间已存在或后来新增的障碍物以最小的圆形进行覆盖，形成威胁区域，设(x_i，y_i)为第i个威胁区域的中心坐标，r_i为第i个威胁的半径，则第i个威胁区域模型可表示为：(x-x_i)²+(y-y_i)²≤r_i ²。

进一步地，所述步骤3)中以节点i的水平方向角为中心线向左右两边各扩展至车轮的最大转弯角，将所形成的扇形面积作为下一个节点(i+1)的扩展范围的具体方式为：以角度α作为节点i的扩展子节点的单位角度，M为节点i产生的子节点的总数，l为扩展步长，θ_max为机器人的最大转弯角，(x_i+1，y_i+1)为子节点(i+1)的坐标值，则：

进一步地，所述步骤4)中实际代价函数g(i)的求解步骤为：设路径规划的起点坐标为S，目标点坐标为D，节点i的坐标为C_i＝(x_i，y_i)，机器人从起点到达节点i的路径Z用节点形式可以描述为Z＝(S，C₁，C₂，...，C_i)，节点i的实际代价为g(i)＝ω₁J_length(i)+ω₂J_risk(i)+ω₃J_smooth(i)；

式中，ω₁，ω₂，ω₃分别对应距离、风险和光滑度成本的权重系数；

J_length(i)表示路径的距离成本，用于测量从起点S到当前节点i的长度，d(C_j，C_j+1)指节点i和i+1之间的距离，

J_risk(i)表示路径的风险成本，用于计算路径规避危险源的风险总代价，用F_j代表第j段路径的风险成本，则其中，K指某路径段摄像头检测到的障碍物的数量；

J_smooth(i)表示路径的光滑度成本，用于计算路径所有节点转向角之和，指两相邻有向路径段的夹角之和，则

进一步地，所述F_j代表第j段路径的风险成本时考虑路径的a₁、a₂、a₃三个位置，a₁、a₂、a₃均为(0,1)内的不同值，依据经验值选取；

其中，K指某路径段摄像头检测到的障碍物的数量；

和分别指第j段路径的a₁、a₂、a₃点处到第k个障碍物的距离；

和分别指在a₁、a₂、a₃点处摄像头检测到第k个障碍物的概率；

其中，c₁和c₂分别是与摄像头拍摄效果和模型有关的常数；σ为摄像头的每次截取的实际有效面积；R指障碍物与机器人之间的实际距离。

进一步地，所述步骤4)中预估代价函数h(i)的求解步骤为：考虑从当前节点i到目标点D的已存在的静态障碍物和路径长度成本，利用算法规划带有引导点的可行性路径Z’，其预估代价函数h(i)为 其中，G_m表示第m个引导点，ω₁为距离成本系数；N为当前节点i到目标点D之间引导点的总数；r_k+i是第k+i个引导点的半径，依据经验值选取；d(G_m+i，G_m+i+1)表示两引导点的距离。

进一步地，所述步骤5)选取规划的最佳路径方法为：设置起始点S的引导数为1，表示起始点应该扩展至第一个引导点G₁，不断重复步骤2)-4)，在起始点S和引导点G₁进行路径规划，取每步扩展的后续节点中最小代价函数对应的节点作为下一步要扩展的节点，形成路径，到下一个节点的距离依据经验值设定，引导至引导点G₁；重复上述步骤依次引导至下一个引导点；当节点n的引导数为k，在探寻每一个继任节点n’时，若路径能够通过引导点G_k，则该节点n’的引导数为k+1，表明该路径通过引导点G_k；否则，该继任节点n’的引导数仍为k，表明探寻树中的计算路径仍然没有通过引导点G_k。

进一步地，所述引导点G_k的中心通过如下步骤设定：

步骤81)当路径搜索至障碍物的距离小于阈值时，显示前方有障碍物，将当前障碍物进行评估，包括障碍危险程度x_a、农业机械速度高低x_b、任务重要性x_c，生成一个合适的安全距离d＝Φ(x_a,x_b,x_c)；

步骤82)将障碍物区域用圆形覆盖，形成障碍区圆，所述障碍区圆是能够覆盖障碍物的最小圆，圆心为G；

步骤83)将当前位置作为扩展节点A，所要到达的目标位B，在障碍区圆上找到与连线AB平行的所有切线MN，切点为C；

步骤84)设定当前障碍区圆外部的引导点E，使得连线EG⊥MN，且CE＝d。

本发明所达到的有益效果：本方案利用IP摄像头采集视频图像获取农业机器人位置信息，引入云模型的在线推理方法把大量的数据处理大大减少时延，并将人工干预加入动态引导A*(DGA*)算法中，减少了产生的节点数，大大提高了搜索效率，人机协同作业，充分发挥人与机器的智能，实现机器人的动态大范围可靠路径规划；以农业移动机器人为研究对象，基于人机合作的云在线推理方法，将大量的实验数据处理放在云端，大大降低了时延，提高了农业移动机器人自主路径规划的实时可靠性。提出的基于云模型的人机协同路径规划方法与DGA*算法的结合，既充分发挥了人的主观能动性，又保证了农业移动机器人在复杂动态的环境中的自主行走要求，对于静态、以及突发动态障碍物的避障都有着较好地工作性能。

附图说明

图1是利用引导点进行节点扩展的示意图；

图2是引导点的确定方式示意图。。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明提出一种动态引导A*(DGA*)算法进行人机合作路径规划，同时结合人的专业知识和喜好等形成专家系统引入基于云模型的在线不确定性推理，实现更快速的路径规划策略。

下面结合附图对具体的步骤进行简要的说明：

步骤1)采集机器人位置，用四维向量(x^*，y^*，θ^*，φ^*)表示其在二维平面内的位置信息，其中x^*，y^*分别表示机器人在x轴方向和y轴方向的值，θ^*表示二维平面下机器人头部朝向与x轴正方向的夹角、φ^*表示机器人的车轮转向角，其中车体长为2a，车宽为2b，轴距为L。

步骤2)农业移动机器人在自主行走时，若周围环境无新的障碍物，会按照原先设定的航线行驶。在行驶过程中出现突然障碍物，机器人需要暂停行驶并重新进行路径规划，因此本步骤基于二维空间规划空间模型，对障碍物采用圆形覆盖，形成若干威胁区域，每个威胁区域作为一个节点。

具体内容为：设(x，y)为规划空间某一点的坐标，规划空间模型为：{(x,y)|0≤x≤X_max,0≤y≤Y_max}，将规划空间已存在或后来新增的障碍物以最小的圆形进行覆盖，形成威胁区域，设(x_i，y_i)为第i个威胁区域的中心坐标，r_i为第i个威胁的半径，则第i个威胁区域模型可表示为：(x-x_i)²+(y-y_i)²≤r_i ²。

步骤3)本步骤用于扩展节点，现有技术中A*算法作为一种启发式搜索算法，在路径规划中应用广泛。传统的A*算法受栅格限制，搜索范围过广，导致很多无用节点被扩展，搜索效率较低。由于其不能预先感知所需规划空间的前方威胁区域，往往容易陷入地图死端。为了避免传统A*算法的弊端，本文提出了一种基于稀疏A*(SAS)算法动态A*(DGA*)路径规划算法，减少了产生的节点数，大大提高了搜索效率。

具体地，将规划空间中第i个节点的信息表示为：n_i＝{x_i,y_i,θ_i,L_si,father_i,g_i,h_i,f_i}，其中，(x_i，y_i)为第i个威胁区域的中心坐标，θ_i为节点i的水平角度；L_si是起始点到节点i的实际距离；father_i为节点i的父辈节点；g_i、h_i、f_i分别为节点i的实际代价值、预估代价值和总代价值；

为保证机器人前进的航线，以节点i的水平方向角为中心线向左右两边各扩展至车轮的最大转弯角，将所形成的扇形面积作为下一个节点(i+1)的扩展范围，具体地方式如下：

以角度α作为节点i的扩展子节点的单位角度，M为节点i产生的子节点的总数，l为扩展步长，θ_max为机器人的最大转弯角，(x_i+1，y_i+1)为子节点(i+1)的坐标值，则：

步骤4)确立代价函数f(i)＝g(i)+h(i)，其中g(i)表示实际代价函数，h(i)表示预估代价函数。

下面对g(i)、h(i)的具体求解进行说明：

实际代价函数g(i)的求解步骤为：

设路径规划的起点坐标为S，目标点坐标为D，节点i的坐标为C_i＝(x_i，y_i)，机器人从起点到达节点i的路径Z用节点形式可以描述为Z＝(S，C₁，C₂，...，C_i)，节点i的实际代价为g(i)＝ω₁J_length(i)+ω₂J_risk(i)+ω₃J_smooth(i)；

式中，ω₁，ω₂，ω₃分别对应距离、风险和光滑度成本的权重系数；

J_length(i)表示路径的距离成本，用于测量从起点S到当前节点i的长度，d(C_j，C_j+1)指节点i和i+1之间的距离，

J_risk(i)表示路径的风险成本，用于计算路径规避危险源的风险总代价，用F_j代表第j段路径的风险成本，则其中，K指某路径段摄像头检测到的障碍物的数量；

J_smooth(i)表示路径的光滑度成本，用于计算路径所有节点转向角之和，指两相邻有向路径段的夹角之和，则

本实施例中，F_j代表第j段路径的风险成本时考虑路径的1/6、1/2和5/6三个位置，

和分别指第j段路径的1/6点、1/2点和5/6点处到第k个障碍物的距离；

和分别指在1/6点、1/2点和5/6点处摄像头检测到第k个障碍物的概率；其中，c₁和c₂分别是与摄像头拍摄效果和模型有关的常数；σ为摄像头的每次截取的实际有效面积；R指障碍物与机器人之间的实际距离。

预估代价函数h(i)的求解步骤为：

考虑从当前节点i到目标点D的已存在的静态障碍物和路径长度成本，利用算法规划带有引导点的可行性路径Z’，其预估代价函数h(i)为其中，ω₁为距离成本系数；N为当前节点i到目标点D之间引导点的总数；r_k+i是第k+i个引导点的半径；d(G_m+i，G_m+i+1)表示两引导点的距离。

步骤5)将起始点到目标点生成的引导点按升序排列，以引导点为中心形成引导点，目标点作为引导半径为零的最后一个引导点，使机器人按顺序通过各个引导点，探寻高效最优路径。具体地，设置起始点S的引导数为1，表示起始点应该扩展至第一个引导点G₁，不断重复步骤2)-4)，在起始点S和引导点G₁进行路径规划，取每步扩展的后续节点中最小代价函数对应的节点作为下一步要扩展的节点，形成路径，到下一个节点的距离依据经验值设定，引导至引导点G₁；重复上述步骤依次引导至下一个引导点；当节点n的引导数为k，在探寻每一个继任节点n’时，若路径能够通过引导点G_k，则该节点n’的引导数为k+1，表明该路径通过引导点G_k；否则，该继任节点n’的引导数仍为k，表明探寻树中的计算路径仍然没有通过引导点G_k。

如图1所示，从S点到引导点G₁，每一个节点到下一个节点直接的距离都是事先设定好的，当S点前进一端距离到达节点1的时候，在进行节点的扩展，寻找继任节点，形成节点1到节点2的路径，将节点2进行扩展时，能够找到节点3，且节点3位于引导点G₁的内部，这样从S点到引导点G₁形成了节点1～节点3的引导路径。在这里，每一个引导点的半径都是根据经验值选取的，几乎为零，越趋近于0精度越高。

附图中还对节点n的扩展进行了说明，如图1，当前节点n(应该落在引导场G_k-1中)如果有三条路径，分别为n₁′、n₂′、n₃′，其中第三条n₃′刚好落在引导场G_k内，则定义其引导号码为k+1，即其下一个路径的目的是指向下一个引导场G_k+1，外两条在引导场G_k外，则它们的引导号码仍为当前引导号码k，即其下一个路径的目的仍然是指向引导场G_k。

本步骤中对于引导点G_k的中心通过如下步骤设定：

步骤81)当路径搜索至障碍物的距离小于阈值时，显示前方有障碍物，将当前障碍物进行评估，包括障碍危险程度x_a、农业机械速度高低x_b、任务重要性x_c，生成一个合适的安全距离d＝Φ(x_a,x_b,x_c)；这里的安全距离可以通过人为对碍危险程度x_a、农业机械速度高低x_b、任务重要性x_c指定一个评分标准，形成安全距离的取值方式。

步骤82)将障碍物区域用圆形覆盖，形成障碍区圆，所述障碍区圆是能够覆盖障碍物的最小圆，圆心为G；

步骤83)将当前位置作为扩展节点A，所要到达的目标位B，在障碍区圆上找到与连线AB平行的所有切线MN，切点为C。

步骤84)设定当前障碍区圆外部的引导点E，使得连线EG⊥MN，且CE＝d。

本实施例中，如图2所示，切线包括MN和PQ，对应的引导点分别为E、F。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (8)

1.一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，包括如下步骤:

步骤1)采集机器人位置，用四维向量(x^*，y^*，θ^*，φ^*)表示其在二维平面内的位置信息，其中x^*，y^*分别表示机器人在x轴方向和y轴方向的值，θ^*表示二维平面下机器人头部朝向与x轴正方向的夹角、φ^*表示机器人的车轮转向角；

步骤2)基于二维空间规划空间模型，对障碍物采用圆形覆盖，形成若干威胁区域，每个威胁区域作为一个节点；

步骤3)将规划空间中第i个节点的信息n_i表示为n_i＝{x_i,y_i,θ_i,L_si,father_i,g(i),h(i),f(i)}，其中，(x_i，y_i)为第i个威胁区域的中心坐标，θ_i为节点i的水平角度；L_si是起始点到节点i的实际距离；father_i为节点i的父辈节点；g(i)、h(i)、f(i)分别为节点i的实际代价值、预估代价值和总代价值；

以节点i的水平方向角为中心线向左右两边各扩展至车轮的最大转弯角，将所形成的扇形面积作为下一个节点(i+1)的扩展范围；

步骤4)确立代价函数f(i)＝g(i)+h(i)，其中g(i)表示实际代价函数，h(i)表示预估代价函数；

步骤5)将起始点到目标点生成的引导点按升序排列，以目标点作为引导半径为零的最后一个引导点，使机器人按顺序通过各个引导点，探寻高效最优路径。

2.根据权利要求1所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述步骤2)具体内容为：

设(x，y)为规划空间某一点的坐标，规划空间模型为：{(x,y)|0≤x≤X_max,0≤y≤Y_max}，其中，X_max,Y_max分别表示规划空间在x轴方向和y轴方向上的最大值，将规划空间已存在或后来新增的障碍物以最小的圆形进行覆盖，形成威胁区域，设(x_i，y_i)为第i个威胁区域的中心坐标，r_i为第i个威胁的半径，则第i个威胁区域模型可表示为：(x-x_i)²+(y-y_i)²≤r_i ²。

3.根据权利要求1所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述步骤3)中以节点i的水平方向角为中心线向左右两边各扩展至车轮的最大转弯角，将所形成的扇形面积作为下一个节点(i+1)的扩展范围的具体方式为：

以角度α作为节点i的扩展子节点的单位角度，M为节点i产生的子节点的总数，l为扩展步长，θ_max为机器人的最大转弯角，(x_i+1，y_i+1)为子节点(i+1)的坐标值，则：θ_i+1＝θ_i+n·α，

4.根据权利要求1所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述步骤4)中实际代价函数g(i)的求解步骤为：

设路径规划的起点坐标为S，目标点坐标为D，节点i的坐标为C_i＝(x_i，y_i)，机器人从起点到达节点i的路径Z用节点形式可以描述为Z＝(S，C₁，C₂，...，C_i)，节点i的实际代价为g(i)＝ω₁J_length(i)+ω₂J_risk(i)+ω₃J_smooth(i)；

式中，ω₁，ω₂，ω₃分别对应距离、风险和光滑度成本的权重系数；

J_length(i)表示路径的距离成本，用于测量从起点S到当前节点i的长度，d(C_j，C_j+1)指节点i和i+1之间的距离，

J_risk(i)表示路径的风险成本，用于计算路径规避危险源的风险总代价，用F_j代表第j段路径的风险成本，则

J_smooth(i)表示路径的光滑度成本，用于计算路径所有节点转向角之和，指两相邻有向路径段的夹角之和，则

5.根据权利要求4所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述F_j代表第j段路径的风险成本时考虑路径的a₁、a₂、a₃三个位置，a₁、a₂、a₃均为(0,1)内的不同值，依据经验值选取；

其中，K指某路径段摄像头检测到的障碍物的数量；

和分别指第j段路径的a₁、a₂、a₃点处到第k个障碍物的距离；

和分别指在a₁、a₂、a₃点处摄像头检测到第k个障碍物的概率；

其中，c₁和c₂分别是与摄像头拍摄效果和模型有关的常数；σ为摄像头的每次截取的实际有效面积；R指障碍物与机器人之间的实际距离。

6.根据权利要求1所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述步骤4)中预估代价函数h(i)的求解步骤为：

考虑从当前节点i到目标点D的已存在的静态障碍物和路径长度成本，利用算法规划带有引导点的可行性路径Z’，其预估代价函数h(i)为

其中，G_m表示第m个引导点，也表示当前节点i后的第一个引导点，ω₁为距离成本系数；N为当前节点i到目标点D之间引导点的总数；r_m+i是第m+i个引导点的半径，依据经验值选取；d(G_m+i，G_m+i+1)表示两引导点的距离。

7.根据权利要求1所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述步骤5)选取规划的最佳路径方法为：

设置起始点S的引导数为1，表示起始点应该扩展至第一个引导点G₁，不断重复步骤2)-4)，在起始点S和引导点G₁进行路径规划，取每步扩展的后续节点中最小代价函数对应的节点作为下一步要扩展的节点，形成路径，到下一个节点的距离依据经验值设定，引导至引导点G₁；重复上述步骤依次引导至下一个引导点；

当节点n的引导数为k，在探寻每一个继任节点n’时，若路径能够通过引导点G_k，则该节点n’的引导数为k+1，表明该路径通过引导点G_k；否则，该继任节点n’的引导数仍为k，表明探寻树中的计算路径仍然没有通过引导点G_k。

8.根据权利要求7所述的一种基于云模型的农业移动机器人人机合作路径规划方法，其特征是，所述引导点G_k的中心通过如下步骤设定:

步骤81)当路径搜索至障碍物的距离小于阈值时，显示前方有障碍物，将当前障碍物进行评估，包括障碍危险程度x_a、农业机械速度高低x_b、任务重要性x_c，生成一个合适的安全距离d＝Φ(x_a,x_b,x_c)；

步骤82)将障碍物区域用圆形覆盖，形成障碍区圆，所述障碍区圆是能够覆盖障碍物的最小圆，圆心为G；

步骤83)将当前位置作为扩展节点A，所要到达的目标位B，在障碍区圆上找到与连线AB平行的所有切线MN，切点为C；

步骤84)设定当前障碍区圆外部的引导点E，使得连线EG⊥MN，且CE＝d。