基于A*与B样条算法的农用机器人路径规划系统

孟珠李，焦 俊，李郑涛，张政云，朱竹芳

(安徽农业大学 信息与计算机学院,安徽 合肥 230036)

为高效智能地规划农用机器人在农田中的运行路径，保证精确的作业行距及作业方向，实现机器人按照指定路径自动行驶，设计了基于A*与B样条算法的农用机器人路径规划系统.使用A*算法对农用机器人运行过程进行路径规划，并利用B样条算法平滑该路径，进而得到最优作业路径.Matlab仿真实验结果表明：A*算法和B样条算法结合起来应用于农用机器人的路径规划是可行的.

A*算法；B样条算法；路径规划；ArcGIS Engine；Matlab

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

路径规划的核心是算法的设计[5].基于遗传算法的路径规划是一种局部路径规划，适用于变化的环境，实时性较好，但规划速度较慢及占用存储空间较多[6].蚁群算法有较好的稳定性及适应性，能够找到全局最优解，与遗传算法相比，搜索时间显著减少，但在处理较大区域的路径规划时，会出现停滞现象或陷入局部最优解[7-8].Dijkstra算法适用于小范围的路径搜索计算，较易实现，但遍历计算节点多，效率低[9-10].笔者采用的A*算法适合信息已知的环境，该算法精巧、高效，在给定的代价函数和环境表示下，只要路径存在，就能找到一条最短路径.A*算法规划出的路径为线段的集合，并非连续平滑的曲线，这样会导致位置误差和减速齿轮受损[11-12].B样条算法能平滑路径，因此笔者将A*算法与B样条算法结合起来，以设计出连续、平滑的避障曲线.

1 A*算法及B样条算法

1.1 A*算法原理

A*算法是一种经典的启发式搜索算法[13]，是基于Dijkstra算法基础上的搜索算法.A*算法的优点在于：在选择下一节点时，已引入已知路径节点的信息，再计算所有候选节点到目标节点的代价.笔者选取最短距离作为代价，以此为据进而选择具有最短距离的节点作为下一个路径节点，直到搜索到目标节点.

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

f(n)=g(n)+h(n)，

(1)

其中：f(n)为起始节点经候选节点到目标节点的代价估计；g(n)为从起始节点到候选节点n实际花费的代价，用起始节点到候选节点的欧氏距离来表示；h(n)为从候选节点n到目标节点的估计代价，在实验中将h(n)定义为候选节点到目标节点的欧氏距离[14].

地图中，选定起始节点为S(Sx,Sy)、目标节点为T(Tx,Ty)、候选节点为C(Cx,Cy)，则A*算法的估价函数为

(2)

1.2 A*算法流程

A*算法实现的步骤如下：

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

(2) 重复如下步骤：

(a) 在Open表中搜索f值最小的节点，并将该节点设为候选节点.

(b) 把候选节点从Open表中删除，加入Close表.

(c) 对候选节点相邻的每一个节点执行如下操作：

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

(ii) 若该相邻节点不在Open表中，则将其加入Open表，并将该相邻节点的父节点设为候选节点，并保存该相邻节点的g值和f值；

(iii) 若该相邻节点已在Open表中，则判断候选节点到该相邻节点的g值是否小于原存的g值，若是，则将该相邻节点的父节点设为候选节点,并重新设置该相邻节点的g值和f值[15].

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

算法中涉及的循环结束条件有：当目标节点加入Open表，表示路径已被找到,应终止循环；当Open表为空，表明已无可添加的新节点,则意味着路径无法找到,此时循环应终止.

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

图1 基于A*算法的路径规划

1.3 B样条算法

B样条算法的优点为曲线会落在曲线阶数的控制点所形成的凸多边形内. 3次B样条算法在连接处2阶连续，将其用于路径规划时，速度和加速度都是连续的，因此采用3次B样条算法来平滑A*算法已规划出的路径.

n次B样条算法的数学表达式为

(3)

其中：Pi(i=0,1,2,…,n)为给定控制点的坐标；Fi,n(t)为n次B样条算法基函数，其表达式为

(4)

当n=3时，3次B样条算法的基函数为

(5)

3次B样条算法曲线方程为

(6)

用3次B样条算法来对基于A*算法的尖峰路径进行光滑处理.图2为利用Matlab仿真生成的B样条算法平滑后的路径，仿真实验的设置为：n=3,t∈[0,1],地图为32m×32m.图2中蓝线为A*算法生成的最短路径，红线为B样条算法处理后的最优路径，可看出经过B样条算法的处理后，(4,10)，(10,8)，(19,14)，(19,20)，(28,23)处的路径已不再尖锐，相比A*算法生成的路径更加光滑，所以将A*算法和B样条算法结合起来应用于农用机器人的路径规划是可行的.

图2 3次B样条算法平滑后的路径

2 系统设计与实现

2.1 地图数据的处理

地图数据的获取及处理为基础工作，是为了在基于ArcGISEngine开发中提供图层数据.使用地图下载软件获取地图数据，使用ArcMap软件对下载的地图数据进行图像配准，再绘制道路shp图.利用ArcToolbox工具箱中的数据管理工具将道路shp图转化为道路关键点shp图.

2.2 基于ArcGIS Engine的开发

(1) 加载地图文档.在VisualStudio2010软件中新建基于C#语言的Windows桌面应用程序，并加入ArcGIS控件和ArcGISEngine库.在窗体中添加LicenseControl和AxMapControl控件，再放置一个名为“打开文件”的button控件，编写程序以实现将地图数据加载到AxMapControl控件.关键代码如下：

privatevoidloadMapDocument()

{

openFileDialog1.Title= "打开Map地图";

openFileDialog1.Filter= "mapdocuments(*.mxd, *.mxt, *.pmf)|*.mxd;*.mxt;*.pmf";

openFileDialog1.ShowDialog();

}

(2) 添加选取路径起点及终点的功能.在窗体中添加“选取起点”和“选取终点”的button控件，并添加axMapControl1_OnMouseDown事件，采用createTextElement方法绘制起点或终点的标记，并将该起点或终点的标记通过axMapControl1，ActiveView，GraphicsContainer，AddElement，axMapControl1和Refresh显示在地图控件中.关键代码如下：

privatevoidaxMapControl1_OnMouseDown(objectsender,IMapControlEvents2_OnMouse

DownEvente)

{

pointStart=DrawPoint(e.x,e.y, 255, 0, 0);

ITextElementte=createTextElement(e.mapX,e.mapY, "起点");

axMapControl1.ActiveView.GraphicsContainer.AddElement(teasIElement, 1);

axMapControl1.Refresh(esriViewDrawPhase.esriViewGraphics,null,null);

}

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

publicvoidrestartPath(ListresultList)

{

ILayerlayer=axMapControl1.get_Layer(0);

IFeatureLayerpFeatureLayer=layerasIFeatureLayer;

IFeatureCursorfeatureCursor=pFeatureLayer.Search(null,false);

IFeaturepFeature=featureCursor.NextFeature();//遍历查询结果

while(pFeature!=null)

{

dicMapPointValue.Add(pFeature.ShapeasIPoint,int.Parse(pFeature.

get_Value(2).ToString()));

pointMiddle.Add(pFeature.ShapeasIPoint);

pFeature=featureCursor.NextFeature();

}

}

获得点集和道路线集之后，利用ArcGISEngine提供的Contain方法，判定点是否在道路线上，若在，则该点与此道路线上的所有点都是连通的，此时将矩阵值设定为1，若不在，则将矩阵值设定为0.关键代码如下：

foreach(KeyValuePairkvLineindicMapLineValue1)

{

IPolylineline=kvLine.Key;

IRelationalOperatorrelationalOperator= (IRelationalOperator)line;

foreach(KeyValuePairkvPointindicMapPointValue)

{

IPointpoint2 =kvPoint.Key;

if(relationalOperator.Contains(point2))

{

if(i==j)Rmap1[i,j] = 0;

Rmap1[i,j] = 1;

}

}

}

2.3 A*及B样条算法的实现

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

(1)A*算法寻路.根据A*算法的要求，创建的开启和关闭列表分别用于存放未被查询和已被查询过的节点.对开启列表的每一节点，在地图矩阵中查找与其相通且距离最短的相邻节点，且将其加入关闭列表.将相邻节点的父节点设为候选节点，并记录该相邻节点的g和f值.当终点节点加入开启列表时，结束算法，路径被找到.关键代码如下：

ListopenList = new List();//Open开启列表

ListclosedList = new List();//Close关闭列表

private Point GetMinFFromOpenList()//从开启列表查找F值最小的节点

{

Point point =openList.ToArray();

if (point[i].G + point[i].H >= point[j].G + point[j].H)Pmin = point[i];

return Pmin;

}

private void CheckP8(Point p0, List pointMap1, Point pa, ref PointpEnd)//检查候选节点附近的节点

{

foreach (KeyValuePair kvLine in dicMapLineValue1)

{

IPolyline line = kvLine.Key;

IRelationalOperator relationalOperator = (IRelationalOperator)line;

if (relationalOperator.Contains(point2))

{

if (!IsInCloseList(pointMap1[i].x, pointMap1[i].y))//排除障碍点和关闭列表中的点

{

Point pt = GetPointFromOpenList(pointMap1[i].x, pointMap1[i].y);

if (p0.x ==pt.x || p0.y == pt.y) G_new = p0.G + 10;

else G_new = p0.G + 14;

if (G_new < pt.G)

{

openList.Remove(pt);

pt.father = p0;

pt.G = G_new;

openList.Add(pt);

}

}

else//不在开启列表中

{

Point pt = new Point();

pt.x = pointMap1[i].x;

pt.y = pointMap1[i].y;

pt.father = p0;

pt.G = GetG(pt);

pt.H = GetH(pt, pEnd);

openList.Add(pt);

}

}

}

图3为A*算法流程，图4为路径规划应用程序，图5为A*算法寻路结果.图4中标注了起点及终点位置，并找到了起点到终点的最短路径，但可看出标注点10，13，3处的路径尖锐、不够平滑.

图3 A*算法流程

图4 路径规划应用程序

图5 A*算法寻路结果

(2) B样条算法光滑路径.得到最短路径后，使用3次B样条算法，来平滑路径.将最短路径中的路径点作为型值点，进而求出控制点，相邻的两型值点之间用一条B样条曲线连接，由(6)式可求出优化曲线的路径点.关键代码如下：

private Point GetPoint(double t, int[,] Matrix, Point pts)

{

doubletp = new double[4];

double temp = {Math.Pow(t, 3), Math.Pow(t, 2), t, 1 };

for (int j = 0; j < 4; j++)

{

tp[j] = tp[j] + temp[k] * Matrix[k, j];

}

for (int j = 0; j < 4; j++)

{

x = x +tp[j] * pts[j].x;

y = y +tp[j] * pts[j].y;

}

return new Point((float)x, (float)y, 0, 0, null);

}

图6为使用3次B样条算法后的路径效果图，红线为A*算法求出的最短路径，蓝线为使用3次B样条算法后的最优路径.由图6可看出，标注点10，13，3处的尖峰已得到优化，路径更加光滑，农用机器人会运行得更加顺畅.

图6 使用3次B样条算法后的路径效果图

3 结束语

笔者使用Matlab仿真论证了A*及B样条算法在路径规划上的可行性，基于C#语言与ArcGIS Engine设计了基于A*与B样条算法的农用机器人路径规划的软件平台，供农用机器人路径规划使用，实现了农用机器人按最优路径运行.

[1] 王殿君. 基于改进A*算法的室内移动机器人路径规划[J]. 清华大学学报 (自然科学版), 2012 (8): 1085-1089.

[2] 丁小辉. 复杂环境下移动机器人路径规划新算法的研究[D]. 北京： 北京邮电大学自动化学院, 2010.

[3] 刘浩, 鲍远律. A*算法在矢量地图最优路径搜索中的应用[J]. 计算机仿真, 2008 (4): 253-257.

[4] 王红卫, 马勇, 谢勇, 等. 基于平滑A*算法的移动机器人路径规划[J]. 同济大学学报 (自然科学版), 2010, 38 (11): 1647-1650.

[5] 郁晓慧. 基于智能终端的车载导航路径规划的研究[D]. 上海: 东华大学信息科学与技术学院, 2015.

[6] ELBANHAWI M, SIMIC M, JAZAR R N. Continuous path smoothing for car-like robots using B-spline curves[J]. Journal of Intelligent & Robotic Systems, 2015, 80: 1-34.

[7] 焦俊, 孔文, 辜丽川, 等. 基于UKF和SMO农用履带机器人滑动参数计算[J]. 系统仿真学报, 2015, 27 (7): 1577-1583.

[8] 张丽娟. 自主移动服务机器人路径规划方法研究[D]. 天津: 天津科技大学机械工程学院, 2012.

[9] 唐思静. 车辆定位导航系统中地图匹配和路径规划算法研究[D]. 西安: 西安电子科技大学通信工程学院, 2009.

[10] 王海梅. 基于GIS的最优路径算法研究与实现[D]. 南京: 南京理工大学自动化学院, 2008.

[11] 焦俊, 江朝晖, 金瑞春. 农用机器人转向系统自适应内模控制[J]. 农业机械学报, 2011, 42 (10): 186-191.

[12] 王宪, 盛巍, 宋书林, 等. 基于遗传算法和B样条曲线的平滑避障路径规划[J]. 计算机系统应用, 2012, 21 (2): 65-71.

[13] 董跃宇, 喻庆国, 刘朝蓬, 等. 基于类三维地图的湿地野外调查路线规划[J]. 林业调查规划, 2013, 38 (1): 1-4.

[14] 钱曙光. 基于移动平台的高速公路查询系统设计与实现[D]. 西安: 长安大学信息工程学院, 2014.

[15] 遇娜, 简广宁. 回溯法求解迷宫问题[J]. 天津职业院校联合学报, 2011, 13 (8): 46-49.