基于D*算法的农用履带机器人路径规划研究

陈 靖，辜丽川，李倩倩，何屿彤，吴亚文，焦 俊

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

随着计算机技术和地理信息科学的发展，GIS(地理信息系统)因其强大的网络分析功能越来越多的应用到人们的日常生活中。路径规划无论是在交通运输，还是在城市规划等方面，都起到了至关重要的作用。路径规划部分作为机器人领域的一个重点，经过数十年的不断发展，已经取得了较为明显的进步[1]。各国学者提出了很多针对性的方法，常见的有：栅格法、人工势场法、神经网络、遗传算法、蚁群算法等。文献[2-3]将栅格法应用于路径规划过程中，便于计算机存储，信息更新快，但栅格法本身搜索具有盲目性，依赖于对精度的要求，当环境复杂时，算法搜索效率较低；文献[4-5]通过对人工势场法的拓展，使之可以进行动态环境下的路径规划，但人工势场法本身的一些问题没有解决，比如存在局部最优等。文献[6]和文献[7]将神经网络应用于动态路径规划，有较好的学习能力，但在大规模不确定环境下网络结构过于庞大。文献[8-9]分别用遗传算法和蚁群算法进行动态环境下的路径规划，取得了不错的效果，但都在不同程度上存在实时性较差的问题。

D*算法是一种基于信息部分已知动态环境下的算法，具有计算量小、实时性强、复杂程度低易于与其他算法结合等优点。本文采用D*算法对农用履带机器人进行路径规划方法研究，重点对路径规划过程中生成路径的平滑设计、碰撞检测和算法的实时性进行研究，并将其应用于农用履带机器人，旨在实现农用履带机器人在农田环境下的自主导航功能。

1 D*算法

A*算法是一种经典的启发式搜索算法，它结合了BFS(Best First Search)算法和Dijkstra算法的优点，通过定义估价函数来评估代价而确定最优路径[10]。A*算法将实际代价看成两部分之和，即已经付出的代价和将要付出的代价，该代价函数可表示为式(1)

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

(1)

式中：f(n)是节点的估计代价函数，g(n)是从起点开始到目前位置节点n的消耗的代价，通过以起始节点出发到达当前节点n的欧氏距离来计算；h(n)是从当前节点n出发到终点的估计需要消耗的代价，同以当前节点到目标节点的欧氏距离表示[11]。

D*算法是在A*算法的基础上发展而来的动态路径搜索算法，适用于动态环境下的路径规划[12]。D*算法开辟了三个状态列表OPEN、CLOSED和NEW，分别存储不同的路径代价：OPEN列表的集合为A，用于存储未经访问节点的路径代价值；CLOSE列表的集合为B，用于存储已访问的路径代价；NEW列表的集合为C，用于存储待更新节点的路径代价[13]。D*算法中每个节点X都有一个标识函数t(X)

(2)

为了寻求最短路径p(最优解)的单调序列Y，设k(X)为Y的最小路径估计价值，即

k(X)=min[h(X),X∈B]

(3)

首先，对OPEN、CLOSED和NEW赋初始值。其中，G，φ,W分别为OPEN、CLOSED和NEW的初始化集合。

从集合B中遍历路径估计函数h(Y，X)，按式(3)分析最小路径估计值k(X)的变化，结合式(2)，动态更新路径估计函数h(Y，X)，以下述三种情况更新OPEN和CLOSED的所有出边：

新的路径估计值过低，k(X)

新的路径估价值无变化，k(X)=h(Y)，且OPEN和CLOSED有节点迁移，在OPEN表中插入当前路径估价值h(Y)+c(X，Y)。

新的路径估价值过高，k(X)>h(Y)，且OPEN和CLOSED有节点迁移，在OPEN表中更新当前路径估价值h(Y)。

如此循环执行，直至满足终止条件：

或者是相应的路径序列不存在。从而获得最短路径p。

2 仿真试验

2.1 仿真试验环境

根据以上分析，在MATLAB平台上进行仿真实验，如图1所示，本文采用栅格地图的方式对机器人的工作环境进行建模，在模拟地图中，分别使用传统A*算法、D*算法在相同栅格地图下对比[14]。假设机器人的工作环境为有限的二维空间区域，采用栅格法将空间区域划分为固定大小的栅格，将其映射到空间坐标系中。栅格地图中单元栅格为边长1m，地图尺寸10m×10m，模拟规划时，设置起点坐标(2，10)，终点坐标(5，1.4)，模拟区域范围[0 10 0 10]。栅格地图中白色区域是机器人的可规划轨迹空间为可通过路径(如农田中的道路)，黑色区域为不可操作区域(如农作物种植区)，黑色圆为随机障碍物。根据机器人运动学模型，设置农用机器人稳定行驶时速度为2.0m/s。

2.2 仿真及结果分析

传统A*算法采用8领域搜索节点，这样不仅让节点扩展过于繁琐，算法的计算量大，而且会导致锯齿效应，造成路径折线长、拐点多[15]。图1为传统A*算法和D*算法在相同栅格地图下的仿真实实验。表1列出了相同的实验条件采用两种不同算法产生的数据，由仿真结果分析可知两种算法均能计算出起点到终点的最短路径，但是D*算法缩短了优化路径的长度，减少了机器人搜索时间，提高了算法的搜索效率。D*算法在面对尖角拐弯处时，能够对尖角附近的路径重新规划，其他区域路径不变，具有较好的实时性，能够避免机器人在拐点处工作时发生碰撞或侧翻情况，而传统的A*算法是通过全局路径重规划对检测到的障碍物进行避障，需要对整个环境地图的栅格进行处理来生产一个全局最优路径，导致算法重复计算节点的比较多，算法效率偏低。由此可以看出，D*算法更能有效的解决移动机器人的路径规划问题。

图1 传统A*算法和D*算法在相同栅格环境下中仿真轨迹示意图

算法搜索长度搜索时间/s A∗算法3517.5D∗算法2814比较73.5

3 农用机器人的路径与导航试验

在对系统需求进行分析的基础上，主要从各节点结构和功能两个方面设计各节点模块方案：

(1)控制系统主节点A被设计为路径规划和数据存储服务端的节点，主要功能是：① 接收机器人控制器B传入的机器人位姿数据，并在地图上对机器人行驶轨迹进行标点；② 将指定目标点发送至机器人控制器B后，机器人实现对指定目标点的精准跟踪。

(2)利用处理器S3C2440为主控单元，添加传感器和模块电路，其中有：网络通信模块、数据采集及传输系统、运动控制模块等构成机器人控制器，作为系统节点B；主要功能是：① 采集、解析惯导设备的位姿数据后，发送至系统主节点A；② 将解析后的位姿数据与主节点发送的指定目标点进行坐标转换、比较后，输入至控制器，利用控制器完成对指定目标点的精准跟踪。

图2农业机器人路径规划研究实现流程图

3.1 CAN通信协议

CAN总线具有通信速率快、可靠性高、成本低、抗干扰能力强的优点，可以将经纬度信息数据以很高的通信速率可靠的发送给上层计算机，有效的解决农业机器人在工作环境恶劣下的通信实时性较差、电磁干扰、传输距离短等问题，所以本试验采用CAN总线作为通信方式[16]。针对控制系统通信网络需求，结合机器人控制系统中通信的特点以及用户自我需求，自行设计CAN总线应用层通信协议，对总线传输的数据进行了分类，重新定义了协议控制域和数据域[17]。

首先需要知晓惯导输出的数据格式，经查找相关资料后了解到SPAN—CPT内部差分解算后，可输出多种格式的机器人位姿数据[18]。农业机器人惯导实时输出的机器人位姿数据如图3所示，当机器人正常工作时，节点B将惯导采集到的机器人位置数据进行解析后，经过总线传输给节点A，由于CAN总线数据传输采用短帧结构，重新定义的数据帧结构包括帧起始、协议控制域、控制域、协议数据域、CRC域、应答域、帧结尾七个部分，每帧数据域最多为8个字节，在数据打包过程中，对于相关数据可以打包到1帧中，以保证数据的发送速率和总线宽带的利用率[19]。在协议中，字节1-7为数据域的实际数据。以经度：117.249 910 607、纬度：31.866 776 105数据为例，对数据域的分段编码和数据段进行设置后如表2～表3所示。

图3 惯导输出机器人坐标数据

表2 经度数据117.249 910 607传输示例

表3 纬度数据31.866 776 105传输示例

3.2 位置信息标注

通过SPAN-CPT的惯导定位系统来获取农用机器人的位置信息，将惯导系统架载在农用机器人上，在使用CAN 总线接收到惯导系统产生的机器人位置信息后[20]，可以通过ArcGIS Engine二次开发提供的类和接口实现机器人在地图上的标注[21]，再利用多线程管理实现机器人实时位置信息的标注，即实现路径跟踪的功能[22]。完成位置标注的功能程序流程如图4所示。

图4 经纬度信息位置标注

打开ArcMap软件，导入得到的经纬度信息数据信息[23]，选择对应的经纬度坐标信息，即可在ArcMap窗口中生成点数据，关键代码如下，

private void button4-Click(object sender， EventArgs e)

{

da.Fill(ds)；//读取数据库中数据

IPoint point；

for (int i=0； i< 7； i++)

{

point=new PointClass()；

point.SpatialReference=this.axMapControl1.SpatialReference；//设置点的坐标和参考系

point.X=(Double)ds.Tables[0].Rows[i][1]；//获取经度坐标

point.Y=(Double)ds.Tables[0].Rows[i][2]；//获取纬度坐标

point.PutCoords(point.X，point.Y)；//设置一个接口，将坐标信息赋值给X，Y

IMappMap=this.axMapControl1.Map；//设置一个控件

pGraphicsContainer=pMap as IGraphicsContainer；//在地图进行线的标注

IActiveViewpActiveView=pMap as IActiveView；//将pMap转为IActiveView接口类型

}

}

3.3 农田环境下的实时路径规划试验

为验证仿真结果，将其用于农用履带机器人并在模拟农田环境进行路径规划试验[24]。本试验的移动平台是THNYJQR-1型农业履带机器人，在对履带机器人进行轨迹跟踪试验前需要完成惯导移动站和基站、嵌入式控制系统硬件、电机驱动、CDMA数传模块、外围传感器等设备的架设、校准以及相应参数配置，如图5所示。

图5 试验平台搭建

图6 农田环境路径规划

基于GIS方法构建的实际地图如图6所示。其中，规划路径的起点为农田入口点A(0，0)，目标点为机器人工作停靠点E(2，0)。

试验过程中，机器人系统实时记录传统A*算法(图6中左侧曲线)D*算法规划的路径(图6中右侧曲线)。表4～表5为在农田环境下传统A*算法和D*算法实时行驶状态表，对比观察可知，基于A*算法时机器人按照规划路径由起点A经由拐点(B2、C2、D2)到达终点E，行驶速度为2m/s，总花费时间为161.452s；基于D*算法时机器人按照规划路径由起点A经由拐点(B1、C1、D1)到达终点E，行驶速度为2m/s，总花费时间为145.936s；由此可知D*算法花费时间更少，效率更高。

表4 传统A*算法

表5 D*算法

4 结论

本文针对农用机器在了农田环境下的路径规划问题，采用了一种更适合履带机器人在农田环境下工作的D*算法[25]，解决了传统A*算法只是求解最短路径，而未考虑到拐点处路径是否圆滑，是否适合机器人的正常工作的问题。本文采用了D*算法规划路径的方式，缩小了算法的搜索空间，降低了搜索路径长度和搜索时间，提高了算法在路径规划中的搜索效率，并且在生成平滑路径的同时兼顾了机器人本体的碰撞问题，更加符合实际需求，保证了算法的实时性。