一种果园履带机器人路径规划方法与流程

本发明涉及机器人算法领域，具体是一种果园履带机器人路径规划方法。

背景技术：

传统的农业机械在农作过程中存在误操作、能源利用率低及作业人员重负荷等问题。针对以上问题进行路径规划已经成为发展智能化农业机械的重要研究内容。路径规划是指根据某种最优原则(如：工作代价最小，行走路线最短，行走时间最短)，在工作空间中寻找一条从起始位置到目标位置的、避开障碍物的最优路径。而针对农用机器人的路径规划是指合理而高效的搜寻一条在农田环境内除障碍物及农作物以外的最优无碰撞路径。

路径规划的核心是算法的设计，目前路径规划算法已经得到了广泛的发展。基于遗传算法的路径规划是一种局部路径规划，适用于变化的环境，实时性较好，但规划速度较慢及占用存储空间较多；蚁群算法有很好的稳定性及适应性，能够找到全局最优解，与遗传算法相比，搜索时间显著减少，但在处理较大区域的路径规划时，会出现停滞现象或陷入局部最优解；而最基础的Dijkstra算法适用于小范围的路径搜索计算，较易实现，但遍历计算节点多，效率低。

技术实现要素：

本发明的目的是提供一种果园履带机器人路径规划方法，以解决现有技术机器人路径算法存在的问题。

为了达到上述目的，本发明所采用的技术方案为：

一种果园履带机器人路径规划方法，其特征在于：利用C#结合ArcGIS Engine二次开发编程实现农用机器人基于A*和B样条算法的路径规划软件平台，首先需要绘制用于农用机器人路径规划的地图；再基于ArcGIS Engine组件和C#语言的Windows桌面应用软件的开发；在此基础上编写A*算法以实现机器人最短路径规划及B样条曲线算法平滑路径，进而实现农用机器人最优路径规划，最后利用Socket网络编程及3G模块传输，将规划好的路径传输到农用机器人上，使机器人按该最优路径运行，具体过程如下：

(1)、地图数据的处理：

地图数据的获取及处理为基础工作，是为了在基于ArcGIS Engine开发中提供图层数据；首先使用地图下载软件获取地图数据，并使用ArcMap软件对下载的地图数据进行图像配准；再绘制道路shp图，利用ArcToolbox工具箱中的数据管理工具--要素--要素转点工具将道路图转化为道路关键点shp图，两图通过Id属性相关联；由此即完成对地图数据的处理，建立了用于路径规划的地图；

(2)、基于ArcGIS Engine的开发：

2.1)加载地图文档：在Visual Studio2010软件中新建基于C#语言的Windows桌面应用程序，并加入ArcGIS控件和ArcGIS Engine库引用，由此便可开发基于ArcGIS Engine的程序，在窗体中添加LicenseControl和AxMapControl控件，再放置一个名为打开文件的button控件，编写程序实现将地图数据加载到AxMapControl控件中；

2.2)添加选取路径起点及终点功能：在窗体中添加选取起点和选取终点的button控件，并添加axMapControl1_OnMouseDown事件，编程使当按下选取起点或终点按钮时，利用ITextElement类的cre ateTextElement方法绘制起点或终点，并将该起点或终点通过axMapControl1.ActiveView.GraphicsCont ainer.AddElement和axMapControl1.Refresh方法绘制到地图控件中；

2.3)创建地图矩阵：在对路径进行A*算法规划之前需要创建所需路径，即地图矩阵；首先获取加载的地图的图层信息ILayer接口，由于IFeatureLayer是ILayer的子类，IFeature是IFeatureLayer中的一个要素，获得IFeatureLayer后，可以从IFeatureLayer中得到IFeatureClass，再利用游标IFeatureCursor就可以把IFeature一个个遍历出来，即可把道路图层中的每一条道路和道路关键点图层中的每一点取出以备创建地图矩阵；

在获得点集和道路线集之后，利用ArcGIS Engine提供的IRelationalOperator接口的Contain方法，判定点是否在道路线上，若在，则该点与此道路线上的所有点都是连通的，矩阵中的值为1，若不在，则在矩阵中设定为0。由此，即可创建出地图矩阵；

(3)、A*及B样条算法的实现：

在完成基于ArcGIS Engine的开发后，即获得了实现A*及B样条算法的数据来源，首先依据地图矩阵进行A*算法寻路，得到最短路径后再进行B样条曲线算法优化路径，使路径更加光滑平稳以便农用机器人顺畅运行，过程如下：

3.1)A*算法寻路：根据实现A*算法的流程需要，首先创建开启和关闭列表分别用于存放未被查询和已被查询过的节点，对于开启列表的每一节点在地图矩阵中查找与其相通且距离最短的相邻节点，加入到关闭列表中；将相邻节点的父节点设为当前节点，并记录该相邻节点的G和F值；判断当终点节点加入到开启列表时，结束算法，路径已被找到；

3.2)B样条算法光滑路径：得到最短路径后，采用三次B样条曲线生成算法，产生出连续、平滑的路径曲线，以优化最短路径；首先得到最短路径中的路径点作为型值点，进而求出控制点，每相邻的两型值点之间用一条B样条曲线连接，而每段B样条曲线由4个控制点组成，即可通过B样条算法方程求出优化曲线的路径点。

本发明中采用的A*算法适合完全已知的环境信息，算法精巧、高效，在给定的代价函数和环境表示下，只要路径存在，便定能找到一条最短路径。

本发明实现了基于A*与B样条算法的农用机器人路径规划。首先分析研究路径规划算法，使用Matlab仿真论证了A*及B样条算法在路径规划上的可行性；并使用Visual C#.NET与ArcGIS Engine结合编程设计实现了基于A*与B样条算法的农用机器人路径规划的软件平台，以供农用机器人路径规划使用，实现农用机器人根据最优路径运行，从而提高农业作业的高效化和智能化。

附图说明

图1为基于A*算法的模拟果园路径规划图。

图2为基于三次B样条曲线平滑A*算法已规划出的路径图。

图3为A*算法执行流程图。

图4为路径规划软件平台示意图。

图5为A*算法寻路示意图。

图6为三次B样条曲线光滑路径示意图。

具体实施方式

本发明原理包括：

一、A*算法原理：

A*算法是一种经典的启发式搜索算法，是基于Dijkstra算法基础上的最佳优先搜索算法。A*算法的最主要特征是在搜索选择下一节点时引入了已知的路径节点信息，计算所有候选节点到目标节点的代价，本发明选取最短距离作为代价，以此为据进而优先选择具有最短距离的节点作为下一个路径节点，直到搜索到目标节点。

建立A*算法的关键是确立如下形式的启发函数：

f(n)＝g(n)+h(n) (1)

其中，f(n)为经过候选节点n的最低代价解的估计代价值，g(n)为从起始节点到当前节点n的实际花费的代价，用起点到当前节点的欧氏距离来表示；而h(n)为从当前节点n到目标节点的估计代价，在实验中将h(n)定义为当前节点到目标节点的欧氏距离。

假设在地图中，选定起始节点S(S_x,S_y)，目标节点T(T_x,T_y)，当前节点C(C_x,C_y)，那么启发式A*算法的估价函数即可表示为：

A*算法流程及仿真：

A*算法原理简单的说就是从起始点，每走一步都选择代价最小的节点走，直至终止点。具体实现步骤如下：

(1)创建Open和Close表，并将起始节点放入Open表中，Close表初始化为空；

(2)重复如下步骤：

a.在Open表中搜索f值最小的节点，并将该节点设为当前节点；

b.把当前节点从Open表中删除，加入Close表中；

c.对当前节点相邻的每一个节点执行：

①若该相邻节点不可通行或已在Close表中，则什么也不执行，继续检验下一节点；

②若该相邻节点不在Open表中，则加入到Open表中，并将该相邻节点的父节点设为当前节点，并保存该节点的g值和f值；

③若该相邻节点已在Open表中，则判断由当前节点到该相邻节点的g值是否小于原存的g值，则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的g值和f值.

d.循环结束条件：

①当目标节点被加入到Open表中，表示路径已被找到,此时应终止循环；

②当Open表为空，表明已无可以添加的新节点,而已检验的节点中没有目标节点则意味着路径无法被找到,此时也结束循环。

(3)从目标节点开始沿父节点遍历,直到起点，遍历所得的节点就是最后得到的路径；

为了证明该算法的可用性，本发明利用Matlab软件进行了仿真实验，如图1所示，首先建立地图为32m×32m的栅格地图，建立直角坐标系，以两点间的欧氏距离为道路权重，假设每条道路上的速度是一致的。本发明地图环境中，黑色空心点表示为可通行点，黑色实心点表示为障碍物点，设定起点为(3,3)，终点为(29,22)，运用A*算法得出如图1所示的路径,最短路径长度为151.8857m。由图可看出在路径是由一些线段组成的，在某些路径拐角处如(4,10)、(10,8)、(19,14)、(19,20)、(28,23)等处比较尖锐，不够平滑，易导致农用机器人在拐弯时因为运动方向突变伤害减速齿轮，由于减速打滑而带来位置误差。因此本发明采用三次B样条曲线生成算法，产生出连续、平滑的路径曲线，以优化最短路径。

二、B样条算法平滑路径：

B样条曲线的特性之一为曲线会落在曲线阶数的控制点所形成的凸多边形内。由于三次B样条曲线在连接处二阶连续，将其用于路径规划时，速度和加速度都是连续的。因此，采用三次均匀B样条来平滑A*算法已规划出的路径。

n次B样条曲线的数学表达式为：

式中，P_i为给定n+1个控制点P_i(i＝0,1,2,...,n)的坐标，F_i,n(t)为n次B样条基函数，其表达式为：

式中：

当n＝3时，则有三次B样条曲线的基函数为：

因此，三次B样条曲线方程为：

用三次B样条曲线来对基于A*算法的尖峰路径进行光滑处理，图2为利用Matlab仿真生成的B样条曲线。仿真实验初始设置条件为：n＝3,t∈[0,1],地图为32m×32m。图中黑实线为A*算法生成的最短路径，虚线为B样条曲线算法处理后的最优路径，由图可看出经过B样条曲线的处理，(4,10)、(10,8)、(19,14)、(19,20)、(28,23)等处的路径已不再尖锐，路径相比基于A*算法生成的路径更加光滑平稳。由此可知，结合A*算法和B样条曲线算法应用于农用机器人路径规划上是可行的。

在经过对A*算法及B样条曲线算法的理论证明后，得知利用这两种算法生成最优路径是可行的。故本发明将利用C#结合ArcGIS Engine二次开发编程实现农用机器人基于A*和B样条算法的路径规划软件平台。首先需要绘制用于农用机器人路径规划的地图；再基于ArcGIS Engine组件和C#语言的Windows桌面应用软件的开发；在此基础上编写A*算法以实现机器人最短路径规划及B样条曲线算法平滑路径，进而实现农用机器人最优路径规划，最后利用Socket网络编程及3G模块传输，将规划好的路径传输到农用机器人上，使机器人按该最优路径运行，具体过程如下：

(1)、地图数据的处理：

地图数据的获取及处理为基础工作，是为了在基于ArcGIS Engine开发中提供图层数据。首先使用地图下载软件获取地图数据，并使用ArcMap软件对下载的地图数据进行图像配准；再绘制道路shp图，利用ArcToolbox工具箱中的数据管理工具--要素--要素转点工具将道路图转化为道路关键点shp图，两图通过Id属性相关联。由此即完成对地图数据的处理，建立了用于路径规划的地图。

(2)、基于ArcGIS Engine的开发：

2.1)加载地图文档。在Visual Studio2010软件中新建基于C#语言的Windows桌面应用程序，并加入ArcGIS控件和ArcGIS Engine库引用，由此便可开发基于ArcGIS Engine的程序。在窗体中添加LicenseControl和AxMapControl控件，再放置一个名为“打开文件”的button控件，编写程序实现将地图数据加载到AxMapControl控件中。关键代码如下：

2.2)添加选取路径起点及终点功能。在窗体中添加“选取起点”和“选取终点”的button控件，并添加axMapControl1_OnMouseDown事件，编程使当按下选取起点或终点按钮时，利用ITextElement类的cre ateTextElement方法绘制起点或终点，并将该起点或终点通过axMapControl1.ActiveView.GraphicsContainer.AddElement和axMapControl1.Refresh方法绘制到地图控件中。关键代码如下：

2.3)创建地图矩阵。在对路径进行A*算法规划之前需要创建所需路径，即地图矩阵。首先获取加载的地图的图层信息ILayer接口，由于IFeatureLayer是ILayer的子类，IFeature是IFeatureLayer中的一个要素，获得IFeatureLayer后，可以从IFeatureLayer中得到IFeatureClass，再利用游标IFeatureCursor就可以把IFeature一个个遍历出来，即可把道路图层中的每一条道路和道路关键点图层中的每一点取出以备创建地图矩阵。关键代码如下：

在获得点集和道路线集之后，利用ArcGIS Engine提供的IRelationalOperator接口的Contain方法，判定点是否在道路线上，若在，则该点与此道路线上的所有点都是连通的，矩阵中的值为1，若不在，则在矩阵中设定为0。由此，即可创建出地图矩阵。关键代码如下：

(3)、A*及B样条算法的实现：

在完成基于ArcGIS Engine的开发后，即获得了实现A*及B样条算法的数据来源。首先依据地图矩阵进行A*算法寻路，得到最短路径后再进行B样条曲线算法优化路径，使路径更加光滑平稳以便农用机器人顺畅运行。

3.1)A*算法寻路。根据上文中实现A*算法的流程需要，首先创建开启和关闭列表分别用于存放未被查询和已被查询过的节点，对于开启列表的每一节点在地图矩阵中查找与其相通且距离最短的相邻节点，加入到关闭列表中；将相邻节点的父节点设为当前节点，并记录该相邻节点的G和F值；判断当终点节点加入到开启列表时，结束算法，路径已被找到。关键代码如下：

图3为代码执行流程图，图4为路径规划软件平台，图5为A*算法寻路结果。图4中标注了起点及终点位置，并寻找到了从起点到终点的最短路径，但由图可看出在标注点10、13、3处路径尖锐，不够平滑，将会引起农业机器人运行不便，故本发明将使用B样条曲线算法平滑路径。

3.2)B样条算法光滑路径。得到最短路径后，采用三次B样条曲线生成算法，产生出连续、平滑的路径曲线，以优化最短路径。首先得到最短路径中的路径点作为型值点，进而求出控制点，每相邻的两型值点之间用一条B样条曲线连接，而每段B样条曲线由4个控制点组成；由上文(6)式方程即可求出优化曲线的路径点，关键代码如下：

图6为三次B样条曲线实现光滑路径的效果图，黑实线为A*算法求出的最短路径，黑虚线为光滑路径后的最优路径。由图可看出上文提到的一些尖峰道路段(如10到13段、13到3段)已得到优化，路径更加平稳光滑，农用机器人将运行的更加顺畅。