基于A*算法的丘陵山区田间路网路径规划

王铭枫 李云伍 徐俊杰

摘要：丘陵山区田间道路起伏不平，路网结构复杂，针对农业机械在丘陵山区田间道路上自动行驶的路径规划问题，提出一种基于转向约束和能耗最优的A*路径规划算法。在采用载波相位差分（real-time kinematic，简称RTK）全球导航卫星系统（global navigation satellite system，简称GNSS）精确采集及存储田间道路路径信息的基础上，针对道路坡度起伏大的问题，建立起伏道路的能耗计算函数，对A*算法估价函数进行重新设计;同时，针对部分路口转向困难的问题，引入路口转向曲率进行约束判断。实地仿真结果表明，该算法能够规划出一条满足农业机械几何转向约束的能耗最优全局路径，验证了改进A*算法的有效性和实用性。

关键词：丘陵山区;田间路网;能耗最优;A*算法;载波相位差分全球导航卫星系统（RTK-GNSS）

中图分类号： S126  文献标志码： A  文章编号：1002-1302（2019）07-0232-05

路径规划即根据全局约束目标如距离最短、时间最少、能耗最低等找到一条最优路径，是实现农业机械自动化作业的关键技术之一[1-2]。农业机械或其他车辆在田间道路上自动行驶时，首先应在起点与目的地之间规划出一条合理的行驶路径。丘陵山区田间道路起伏不平、狭窄弯曲，路网结构多变，在其上自动行驶的路径规划应充分考虑丘陵山区田间道路的这些典型特征。

在农业应用领域，路径规划研究多应用于田地内作物行局部路径生成、地头转向最优以及全区域路径规划等问题[3]。苑严伟等为了提高苹果采摘机器人的采摘效率，将信息素自适应更新的改进蚁群算法应用于采摘机器人的路径规划中[4];张文等从生成路径的平滑设计、碰撞检测和算法实时性出发，提出一种基于曲线路径最短的方向A*算法，以解决复杂环境下的温室机器人路径规划问题[5];刘刚等针对农田平整问题，提出以无效作业状态最少、转向操作与重复行走最少为条件的全球导航卫星系统（global navigation satellite system，简称GNSS）农田平整全局路径规划方法，用于生成农田的土地平整路径[6]。

目前，针对田间道路拓扑网络的路径规划问题的研究较少。丘陵山区田间道路路径规划问题不同于上述田地内路径规划和传统的车辆最短路径规划问题，兼顾能效与安全成为最主要的约束目标。在按最短路径规划得到的路径中，往往忽略路口转向限制以及未考虑地形陡升陡降所造成的能效问题。因此，根据田间道路起伏不平以及路口弯曲率大的特征，提出一种基于转向约束和行驶能耗最低的改进A*算法，设计了启发式能耗估价函数，将农业机械在实际起伏道路条件下的能耗作为实际代价，同时对路口转向曲率进行判断筛选，最终规划化出一条切实可行的全局路径。

1 田间路网采集与存储

1.1 问题描述

随着国家对丘陵山区坡耕地的治理以及田间道路体系的完善，各地普遍建立了1.2～2.0 m宽的田间便道和3.5～5.5 m 宽的机耕道路[7]。图1为重庆丘陵山区某果园的田间道路环境卫星图，地图网络中包括田间路口、居民点、仓库等节点，以及由田块与田块、田块与居民点仓库之间纵横交错的水泥便道所构成的路径段。这些田间道路网络为农业机械转移和运输的自动化和智能化作业提供了有利条件。但由于这些田间道路没有建立起路网地图，要实现农业机械在田间道路上的自主行驶，应首先通过GNSS采集获取道路坐标信息，建立道路模型，在此基础上再利用路径规划算法，规划出农业机械在这些道路上行驶的合理路径。

1.2 地图信息采集及预处理

丘陵山区田间道路狭窄曲折、陡升陡降，道路基础设施差，须要实地精确采集道路网络信息以建立地图。本研究通过安装在田间道路搬运车上的载波相位差分（real-time kinematic，RTK）-GNSS系统采集田间道路的坐标信息，即平面坐标系下的东向、北向及天向坐标（x，y，z），采集流程如图2所示。串口接收RTK-GNSS接收机数据报文，并对GNSS信号错误点采用3次B样条插值拟合，经坐标转换后对平面坐标进行存储，同时对路口节点附近采样点进行去重复处理，得到平滑可靠的GNSS轨迹地图;最后将获取的地图坐标数据以路段为对象，储存该路段的路径点坐标并计算路径长度、路段能耗及路口转向曲率等数据。

对图1所示的田间路网实地采集并提取道路网络结果如图3所示，该环境中包括居民点8、22，田间仓库25、26以及其他若干田块路口。

1.3 路网地图存储及实现

在田间道路的路径规划中，须要对采集的三维点坐标进行存储，并提取道路的拓扑网络结构。因此，针对3类地图构成元素即路口节点、路段和网络关系分别进行描述：（1）针对路口节点的存储，采用一维数组的结构，储存节点编号、三维坐标以及关联路径段数。（2）路段信息与节点存储结构相同，由多个采样点一维数组组合为二維数组，储存路段编号、路段内所有采样点坐标以及路段起止节点编号。（3）对于地图中拓扑网络关系，本研究将考虑起伏道路的能耗，相同道路不同行驶方向能耗往往不一致，为了便于对地图进行存储和搜索，采用邻接矩阵存储地图数据[8]。在该结构中，建立邻接矩阵Q，矩阵Q中行列数为道路节点个数，其中，第u行第v列元素用二元数组[dist（u，v），E（u，v）]表示，若节点u、v相邻，dist（u，v）、E（u，v）分别设置为对应路径段的路径长度及能耗，若节点u、v不相邻，则将dist（u，v）、E（u，v）设置为无穷。以上信息可通过离线计算并存储，规划时直接调用。

2 基于改进A*算法的路径规划

2.1 基本A*算法

2.4.2 算法流程 同基本A*算法一致，在算法搜索过程中须要不断更新Open表和Close表2个列表，Open表为待扩展列表，Close表为已扩展节点列表;u为当前扩展节点，v为与u相连节点，f、g、h含义同式（1）、式（2）。算法输入为起点和终点坐标，输出为全局路径坐标点序列，其基本流程如下：（1）输入起点和终点坐标，选择距离最近的节点作为规划的起点s和终点e;（2）初始化，将所有节点f、g值置为无穷;建立Open表与Close表，并将起点纳入Open表并作为当前节点u，更新g（u）=0，然后将节点u移入Close表;（3）根据式（4）、式（5）对节点u的临近节点转向曲率进行判断，筛选曲率符合转向约束的节点v，计算f（v）、g（v）、h（v），并将其加入Open表中;（4）若v已在Open表中，计算节点v实际代价g（v），若经过节点u的代价小于原值，则更新g（v），并替换u为其父节点，否则不作改变;（5）从Open表中选取具有最小f值的节点作为扩展节点，更新该节点为u，并将其纳入Close表中;（6）检查所选节点u是否为终点e，若是，则表示最优路径已经找到，转至步骤（7）;否则返回步骤（3）继续搜索;若Open表为空，则路径不存在，结束搜索;（7）从终点开始，沿父节点层层回溯得到最优路径，按节点顺序组合预存GNSS采样点，输出完整的全局路径坐标点序列。

3 算法仿真试验

3.1 试验环境及条件

为了验证提出的A*算法在田间道路网络中的规划应用效果，以搬运车在田间路网中自主行驶为目标，在MATLAB环境中进行路径规划仿真试验。选择如图3所示的田间道路网络作为规划对象，共计36个节点、54个路徑段。在该地图中，存在较大坡度起伏的路段以及路口弯曲度较大的路口，能够较好地体现丘陵山地田间道路的特征。同时采用Dijkstra算法进行规划，仅以路径长度作为约束条件，忽略道路起伏及转向约束信息，以比较路径长度最短时的车辆能耗及道路特征与改进A*算法的差异。同时，为了对比道路起伏情况，定义道路累计高程变化[12]H（n′，n），如式（14）所示，式中各参数意义同式（3）、式（10）。搬运车仿真参数见表1。

3.2 仿真结果与分析

仿真设置多组不同起点和终点的路径规划场景，规划结果如表2所示。

由表2可知，按路径最短和能耗最少为目标均能生成全局路径，且在部分规划场景中规划结果相同。同时，由于不同道路起伏情况不一致，当路径长度相近时，其路径能耗往往差异较大。如规划路径1→21与17→23，基于Dijkstra算法的路径长度相差仅约11 m，但总能耗分别为5.19、20.16 W·h，这是由于1→21为下坡路段，能耗主要由滚动阻力产生，而17→23为起伏路段，重力势能转化为摩擦热较大。

以路口34到路口11规划为例分析不同算法规划的差异，图6-a、图6-b分别为改进A*与Dijkstra算法路径规划结果，图7为2个算法所规划道路的海拔高度变化对比。

由表2可知，按照Dijkstra算法规划的道路，其路径长度最短为548.09 m，但此时道路起伏较大（图7），累计高程变化达81.34 m，此时对应能耗为34.16 W·h。同时，道路中存在曲率较大而无法通行的路口，如13-12-11路口12处计算曲率为0.476，大于根据式（5）计算的搬运车最小转弯半径对应曲率。

改进A*算法能耗最优路径长度为556.12 m，略大于最短路径长度，最优能耗为28.08 W·h，通过图7可以出，此时道路起伏相对平缓，累计高程变化为57.04 m。从仿真结果中也可看出，并非路径长度越短的能耗就越低，根据能耗进行最优路径选择更为合理，搬运车在此路径下兼具能效和安全性能。

4 总结

利用RTK-GNSS采集路网信息， 以道路节点、路段和路网关系为对象存储道路信息，建立起地图存储模型，实现丘陵山区田间道路网络的存储。对路口节点引入曲率进行判定，提前过滤转向曲率较大路口，同时建立农机在起伏道路行驶的能耗函数，并设计A*算法能耗估价函数，提出一种基于能耗最优的丘陵山区田间道路路径规划方法。结果表明，相对最短路径寻优，本研究算法规划路径更为合理，对丘陵山区田间道路路径规划具有一定的参考和实用意义。

参考文献：

[1]董 胜，袁朝辉，谷 超，等. 基于多学科技术融合的智能农机控制平台研究综述[J]. 农业工程学报，2017，33（8）：1-11.

[2]孟志军，刘 卉，王 华，等. 农田作业机械路径优化方法[J]. 农业机械学报，2012，43（6）：147-152.

[3]苗玉彬，王明军. 农业车辆导航系统中路径规划策略的研究进展[J]. 农机化研究，2011，33（5）：12-15.

[4]苑严伟，张小超，胡小安. 苹果采摘路径规划最优化算法与仿真实现[J]. 农业工程学报，2009，25（4）：141-144.

[5]张 文，刘 勇，张超凡，等. 基于方向A*算法的温室机器人实时路径规划[J]. 农业机械学报，2017，48（7）：22-28.

[6]刘 刚，康 熙，夏友祥，等. 基于GNSS农田平整全局路径规划方法与试验[J]. 农业机械学报，2018，49（5）：27-33.

[7]张仕超，尚 慧，修维宁，等. 农村田间道路工程对局地土地利用景观格局的影响[J]. 西南大学学报（自然科学版），2010，32（11）：89-97.

[8]Ma B，Feng Y，Jia X，et al. Vehicle routing in urban areas based on the OCW-Dijkstra algorithm[J]. Iet Intelligent Transport Systems，2016，10（7）：495-502.

[9]刘 浩，鲍远律. A*算法在矢量地图最优路径搜索中的应用[J]. 计算机仿真，2008，25（4）：253-257.

[10]孙红伟，李云伍，王小娟，等. 田间道路无人驾驶搬运车自动循迹行驶控制策略[J]. 江苏农业科学，2018，46（7）：215-218.

[11]喻德生，程 程. 基于离散曲率的三次均匀B样条的局部光顺算法[J]. 浙江大学学报（理学版），2011，38（5）：511-517.

[12]宋 青，李晓磊，李 猛. 基于OpenStreetMap的城市自行车网建模与多判据路径规划[J]. 交通运输系统工程与信息，2017，17（3）：143-149.

[13]吴伟斌，张 成，洪添胜，等. 基于模糊PID的山地果园运输机动力稳定系统的设计与试验[J]. 湖南农业大学学报（自然科学版），2017，43（4）：443-450.

[14]顾 青，豆风铅，马 飞. 基于改进A*算法的电动车能耗最优路径规划[J]. 农业机械学报，2015，46（12）：316-322.赵俊伟，黄显雷，尹昌斌. PPP模式下养猪场户对粪污处理社会化服务的需求分析——以河南省为例[J]. 江苏农业科学，2019，47（7）：297-302.