基于改进A*与DWA算法融合的温室机器人路径规划

2021-02-01劳彩莲

农业机械学报 2021年1期

劳彩莲李鹏冯宇

(1.中国农业大学现代精细农业系统集成研究教育部重点实验室，北京 100083；2.中国农业大学农业农村部农业信息获取技术重点实验室，北京 100083)

0 引言

随着我国农业的发展，温室技术变得高度智能化和高效化。在植株高度密集、高温和高湿度甚至有毒的温室环境下，不利于长时间进行人工作业，再加上劳动力不足和成本增加的影响，温室机器人应用越来越多[1]。利用移动机器人完成温室自主化作业符合精细化农业的需求，路径规划是移动机器人在温室作业自主导航的重要前提[2-4]。根据机器人的作业范围，可以将路径规划分为基于静态全局环境的全局路径规划和基于局部障碍的局部路径规划[5]。常见的全局路径规划算法包括以Dijkstra[6-7]、A*[8-9]为代表的搜索规划算法，以RRT[10]为代表的采样规划算法和以遗传算法[11]为代表的启发式规划算法。依据实时传感器数据的局部路径规划算法有人工势场法[12]和动态窗口法[13]。

Dijkstra算法采用遍历搜索方式，规划节点数较多，节点网络非常庞大，算法效率低下[14]。在Dijkstra算法的基础上，A*算法引入目标点到当前点的估计代价，根据估计代价决定路径搜索方向，提高了算法效率[15]。A*算法在已知环境下能快速实现移动机器人无碰、最短全局路径规划，主要通过节点状态检测和简单的估值功能规划出一条最佳的安全道路。但是，该算法忽略了机器人的运动约束，不能保证曲率连续，导致运动参量在拐点处发生跳变。文献[16]动态计算出机器人的旋转方向及角度，简化了路径,但增加了运行时间，实时性不高。文献[17]使用跳点搜索减小了算法搜索面积，加快了速度，但是规划出来的路径拐点过多。文献[18]采用直线-圆弧策略对路径进行平滑处理,消除了锯齿效应。文献[19]使用微分方法减少了拐点数，但数学计算过多，比较耗时。文献[20]优化了A*算法的启发函数，改进了关键点的选取策略，减少了冗余的路径点。文献[21]提出对邻域进行扩展，将8邻域扩展到24邻域，使机器人可以小角度行进，从而轨迹更加平滑。但这些改进方法没有考虑未知的障碍物，不具备动态避障能力。

动态窗口法[13，22-23](Dynamic window approach， DWA)通过结合传感器的数据进行在线实时局部路径规划，具有良好的避障能力，适用于动态环境中的机器人路径规划，但并不能满足全局路径的最优。将局部路径规划的算法结合到全局路径规划之中，在实际作业过程中，既考虑到环境中的障碍物，又能保证路径规划的完备性、实时性，满足机器人的运动约束。

本文提出基于改进A*算法与动态窗口法结合的温室机器人路径规划算法，利用选取策略对路径进行优化。通过结合动态窗口算法与超声波信息进行局部避障。在一维弦机器人操作系统上对改进A*算法与Dijkstra算法、A*算法、RRT算法进行仿真对比，验证改进A*算法的有效性。考虑机器人的实际尺寸，在真实环境下对栅格地图进行膨胀化处理，并结合超声波进行局部避障，完成真实环境下的机器人自主导航任务，并对算法的可行性进行验证。

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

1.1 传统A*算法

A*算法采用启发式搜索与广度优先算法结合，是静态环境中用于求解最优路径最有效的直接搜索算法。A*算法通过一个代价函数F(n)，选择搜索方向，从起点开始向周围扩展，通过启发函数H(n)计算得到周围每个节点的代价值，选择最小代价值作为下一个扩展点，重复这个过程，直到到达终点，生成从起点到终点的路径。在搜索过程中，由于路径上的每一个节点都是具有最小代价节点，得到的路径代价是最小的。A*算法的代价函数为

F(n)=G(n)+H(n)

(1)

式中G(n)——从起点到当前节点的实际路径代价

F(n)——当前节点所在路径从起点到终点预估的路径代价总和

A*算法在执行路径规划时主要用2个表实现节点的扩展和最优点的选择，即通过Openlist表和Closelist表来记录节点。其中Openlist表作用是保存搜索过程中遇到的扩展节点，同时将这些节点按代价进行排序，取出F(n)值最小的节点，作为当前节点；然后将当前节点的所有邻节点按邻节点规则加入到Openlist表中。

1.2 A*算法优化策略

传统A*算法根据载入的地图，按照8邻域搜索得到一系列的点，规划出的路径转折次数太多，包含了一部分冗余点。如果转折点过多，移动机器人每次只能移动很短的距离，会造成机器人卡顿现象。在实际的机器人移动过程中，单次指令下，机器人的运动距离越长，机器人移动流畅度越好，产生的误差也会越少。这些问题造成在机器人的运动中平滑性欠缺，因此本文在传统A*算法基础上进行关键点的路径优化策略。该策略能够在栅格地图上求解出更优的路径，移动机器人路径执行具有了更高效率。

传统A*算法规划的路径如图1a所示，其中起始点为绿色方格，终点为红色方格，灰色方格为规划路径，从图中可以看出，路径转折点过多。

本文先从A*算法规划出的轨迹获得一系列的点，然后选取当前节点运动方向相同的点，最终优化成一条线段。通过相邻节点的斜率对机器人的运动方向进行判断。从路径规划的第2个节点开始，如果当前节点与前一个节点的运动方向一致，则认为前一个节点为冗余的，删除前一个节点，依次遍历所有路径的节点，删除所有的冗余点。机器人只需执行包含起点、拐点的一系列路径点，减少向机器人下达运动命令的次数，保证了机器人执行路径平滑性。

经过关键点选取策略处理后的路径如图1b所示，其中点A到点B有18格子，机器人的运动方向都是朝下，将A与B之间的点优化成2个点，即点A和点B，删除AB之间的冗余点，更新路径，使得机器人运动较为流畅。使用相同的优化策略，形成BC、CD、DE段路径，最终将路径优化成没有冗余点的最优路径。经过改进A*算法的路径平滑后，图1a中的多余转折点被消除，得到如图1b所示的路径。

2 动态窗口算法

在结构化温室环境中，作物按照一定的规则进行放置，其相对位置已知，但存在一些易变因素，很难获得完整的环境信息。为避免损害植物、保证机器人安全性，本文在全局路径规划的基础上，采用动态窗口法进行局部路径规划。动态窗口法根据机器人超声传感器检测到局部环境问题，进行实时的路径规划，具有良好避障能力。载入全局地图后，通过改进A*算法进行全局路径规划，下达给机器人运动命令开始运动。由于机器人本身的运动学约束和环境因素影响，造成机器人在运动过程中产生误差。动态窗口法主要是在速度空间内采样线速度和角速度，并根据机器人的运动学计算预测其下一时间间隔的轨迹。对其获得的待评价轨迹进行评分，从而获得更加安全、平滑的最优局部路径。

2.1 机器人运动模型

动态窗口法将移动机器人的位置控制转换为速度控制[24]。在利用速度模式对机器人运动轨迹进行预测时，首先需要对机器人的运动模型进行分析。移动机器人采用的是两轮差速模型，v(t)和w(t)分别代表机器人在世界坐标系下的平移速度与角速度，反映了机器人的运动轨迹。在机器人的编码器采样周期Δt内，位移较小，机器人作匀速直线运动，则机器人运动模型为

(2)

式中x(t)、y(t)、θ(t)——t时刻机器人在世界坐标下的位姿

2.2 速度采样

动态窗口法将避障问题描述为速度空间中带约束的优化问题，其中约束主要包括差速机器人的非完整约束、环境障碍物的约束以及机器人结构的动力学约束。DWA算法的速度矢量空间示意图如图2所示，横坐标为机器人角速度w，纵坐标为机器人线速度v，其中vmax、vmin为机器人最大、最小线速度，wmax、wmin为机器人最大、最小角速度；整个区域为Vs，所有白色区域Va为机器人安全区域，Vd为考虑电机扭矩在控制周期内限制的机器人可达速度范围，Vr为上述3个集合的交集最终确定的动态窗口。

根据机器人的速度限制，定义Vs为机器人线速度与角速度的集合，即动态窗口算法搜索求解的最大范围，满足

Vs={(v,w)|vmin≤v≤vmax,wmin≤w≤wmax}

(3)

整个机器人的运动轨迹，可以细分为若干个直线或圆弧运动，为保证机器人安全区域，在最大减速度条件下，当前速度应能在撞击障碍物之前减速为0，则定义机器人碰撞可行区域的线速度与角速度集合Va满足

(4)

dist(v,w)——轨迹上与障碍物最近的距离

由于机器人电机转矩的限制，采样周期Δt内存在机器人最大、最小可到达的速度v和角速度w范围，需要进一步缩小动态窗口。在给定当前线速度vc和角速度wc条件下，下一时刻动态窗口Vd满足

(5)

最终的速度范围为上述3个速度的合集，定义动态窗口Vr满足

Vr=Vs∩Va∩Vd

(6)

在速度矢量空间Vr中，根据线速度、角速度采样点数，将连续的速度矢量空间Vr离散化，得到离散的采样点(v,w)。对于每一个采样点，根据机器人运动学模型预测下一时刻机器人的多个运动轨迹生成，如图3所示。

2.3 评价函数

通过设计评价函数，在速度空间内有选取最优轨迹。在局部规划中，保证最优轨迹靠近A*算法的全局路径，完成避障任务，朝向目标快速运动。定义评价函数为

G(v,w)=k(αHeading(v,w)+βGoal(v,w)+
γPath(v,w)+σOcc(v,w))

(7)

式中k——平滑函数

α、β、γ、σ——各子函数的加权系数

Path(v,w)——路径评价子函数，计算轨迹末端点到全局路径的距离，距离越短说明轨迹越靠近全局路径

Heading(v,w)——方位角不断地朝向终点位置函数

Goal(v,w)——评价机器人局部路径末端点到终点的距离函数，其目的是不断缩短与终点的距离

Occ(v,w)——评价机器人轨迹到障碍物距离函数

当G(v,w)值最小时，获得最佳路径。本文中通过改进A*算法路径规划获得全局路径，如图4中黑色实线所示，虚线为局部规划路径，之间的距离计算公式为

(8)

式中 (x1,y1)——机器人根据运动学模型估计的局部路径末端点坐标

(x2,y2)——全局路径规划的节点坐标

在移动过程中，Head(v,w)函数用于使机器人的朝向不断趋向终点方向，θ越小，说明与终点的方位角越小，其示意图如图5所示，计算公式为

Head(v,w)=180°-θ

(9)

Goal(v,w)函数用于评价机器人局部路径末端与终点的距离，通过函数不断缩短与终点的距离，其示意图如图6所示。

Occ(v,w)函数用于评价机器人轨迹到障碍物的距离，体现了机器人的避障能力，如果机器人的轨迹到障碍物的距离大于机器人半径，则没有发生碰撞的危险；反之，就说明碰撞风险大，舍弃这条轨迹。其示意图如图7所示，R为机器人底盘半径，H为机器人轨迹到障碍物的距离。

3 融合算法

移动机器人超声波检测温室局部环境信息，结合动态窗口法进行在线实时局部路径规划，检测窗口滚动前进，具有避障能力，获取局部的最优路径。通过全局路径规划与局部路径规划结合，将改进A*算法进行全局路径规划与动态窗口算法融合，以保证动态规划路径的全局最优性。

在已知地图上进行全局路径规划，根据当前速度、角速度、线速度计算并发布轨迹，通过评价标准下发各个轨迹，抛弃不合理的轨迹和遇到障碍的轨迹，运用4个评价函数对某一路径进行评价，将各个评价函数累加，将成本最低的轨迹设定为最佳轨迹。如果当前最佳轨迹是可通过的，依照速度模式根据最佳轨迹的速度移动；如果遇到障碍，根据相关规则进行避障，融合算法流程如图8所示。

4 仿真实验与分析

4.1 改进A*算法仿真

Moro机器人是中国一维弦公司研发的移动机器人，其配套EwayOS机器人操作系统包含了SDK接口、运动算法等模块，能够对Moro机器人路径规划算法进行离线仿真，验证其算法的有效性。本文构建5个尺寸一致的栅格地图场景1～5，其水平方向上32个方格，竖直方向上29个方格，分辨率为10 cm×10 cm，其中黑色方块代表环境中的障碍物信息，白色区域为可行区域。本文将改进A*算法与传统A*、Dijkstra、RRT算法进行对比仿真实验，设定相同的起点坐标为(0,0)，终点坐标为(31,28)，其仿真实验结果如图9～13所示。

5个栅格地图场景障碍物的覆盖率逐渐增大，复杂程度也逐渐增大，其中包含了规则和不规则障碍物，具有一定代表性。针对每个栅格地图场景，本文使用4种路径规划算法运行10次，初始设置参数一致，得到10组路径规划算法的路径长度、计算时间以及拐点数，取其平均值，结果如表1所示。

从表1来看，在5个不同栅格地图场景中，4种路径规划算法都能最终规划出路径，但其路径长度、计算时间、拐点数存在一定差异。RRT算法的计算时间最短，但RRT算法是由随机树节点组成，导致生成的路线转折点过多，并非理想的平滑曲线，这对于机器人运动来说不利。当机器人运动是从一个节点走到另一个节点，首先需要在原地完成转向后，转向下一个节点再继续行走，拐点过多或路径过长，会造成机器人时间和能量的消耗。而改进A*算法、传统A*算法、Dijkstra算法获得的全局路径较优。Dijkstra算法是一种遍历算法，相对于改进A*算法更加耗时，尤其面对不规则障碍时，改进A*算法规划的路径是最优的。改进A*算法相对于传统A*算法在计算时间、路径长度和拐点数都有较大的改善。从实验数据可以看出，改进A*算法有较好的实时性，路径更短，路径平滑性更好，便于机器人执行路径轨迹达到目标点。

表1 不同路径规划算法的路径长度、计算时间与拐点数对比Tab.1 Comparison of length, calculation time and number of inflection points of different path planning algorithms

4.2 环境模型处理

在算法仿真实验中机器人理想化成一点，忽略了机器人本体尺寸，在真实环境中还要考虑机器人尺寸,避免机器人边缘和障碍物发生碰撞，执行路径失败。用栅格图构建机器人的工作环境，移动机器人在栅格间运动时，将固定障碍和机器人运动过程中超声波检测到的障碍设置为不可行区域。在此基础上对地图的障碍物进行膨胀化处理，保证机器人与障碍物间的安全区域，提高路径的可行性。如图14所示，膨胀后的路径，设有安全区域，更利于移动机器人安全执行规划后的路径。

在已知温室环境地图基础上，移动机器人通过超声波获取未知的动态障碍信息。本文使用的Moro机器人底盘前方的4个超声波雷达，检测范围为0～250 cm。移动机器人在地图中的位置已知，超声波雷达将检测到与动态障碍物的距离转换到世界坐标系下，增加局部障碍物，将其加入全局地图，机器人再根据融合算法进行局部路径规划。在实际环境中，如图15a所示，Moro机器人遇到动态障碍物，超声检测到障碍物如图15b所示，白色表示障碍，检测到障碍物与机器人坐标原点的距离为80 cm,实际测量值为82 cm,测量结果误差较小，可以满足实际的精度要求。如图16a所示,机器人对局部障碍进行膨胀处理，生成局部地图，重新规划路径，避开障碍物；图16b中障碍物厚度增加，绿色的线即为重新规划出的路径。

通过构建如图17a所示环境，验证机器人运动的定位精度，起点为绿色，终点为红色。EwayOS机器人操作系统实时记录融合算法的路径，即图17a中的灰色曲线与实际运动轨迹和图17a中的黄色曲线。Moro机器人按照规划路径成功抵达终点，路线中存在不重合的地方，分析实时的跟踪数据，如图17b所示，机器人实际跟踪误差始终小于0.22 m,基本满足实际需求。

5 真实环境下的实时路径规划

本文的融合算法适用于小范围结构化环境，其环境具有一定的特殊性，选择的3个与实验相似的结构化环境1～3，对算法可行性进行验证。利用Moro机器人分别在楼层走廊、模拟温室环境和真实的温室环境进行实验。3个实验环境地面光滑程度相似，其栅格地图中黑色为机器人可以到达的无障碍空间，白色为障碍物信息，融合算法的实验步骤描述为：①加载环境栅格地图，设定机器人作业起点与目标点。②改进A*算法全局路径规划。通过机器人定位方法，获取机器人实时位置信息，然后通过改进A*算法规划机器人当前位置至目标点的全局路径。③障碍物信息处理。机器人运动过程中通过超声波雷达实时检测环境障碍信息，并将传感器信息转换为机器人地图上的障碍信息，得到膨胀后的地图。④动态窗口法局部路径规划。根据机器人的当前位置信息，结合实时超声波信息，在全局路径的基础上进行动态窗口法局部路径规划，获得机器人最优运动轨迹，机器人按照最优轨迹对应的线速度与角速度执行运动。⑤根据机器人当前位置到目标点的距离判断是否到达。如果未到达终点，则返回步骤③继续运动；如果到达了目标点，机器人导航任务结束。

机器人的实验参数设置如下：最大线速度为0.6 m/s,最大角速度为0.3 rad/s,线速度采样个数为10，角速度采样个数为20，采样周期为0.2 s，子函数的加权系数α=0.1、β=10.0、γ=0.1、σ=0.2。

环境1为中国农业大学信息与电气工程学院楼层走廊,地面为光滑地板。图18a所示走廊实际长为10.5 m，宽为6.5 m,图18b所示栅格地图分辨率为10 cm×10 cm，起点为(1.0 m,1.0 m)，终点设为(9.1 m,4.2 m)，实际机器人路线如图18b中红线所示，终点为(9.3 m，4.0 m)，误差为0.22 m。

环境2为模拟温室盆栽环境，如图19a所示，设置2排10个盆栽花盆，地面为光滑地板。场景实际长为6 m，宽为4 m, 图19b所示栅格地图分辨率为10 cm×10 cm，起点为(0.5 m,0.5 m)，终点设为(5.5 m,3.5 m)，实际机器人路线如图19b中红线所示，终点为(5.5 m,3.4 m)，误差为0.1 m。

环境3为实际温室环境，如图20a所示，为复合薄膜型结构化温室，地面为光滑的地板，有4排植物生长金属框架。场景的实际长为8 m，宽为6 m，图20b所示栅格地图分辨率为10 cm×10 cm，起点为(0.5 m,0.5 m)，终点设为(7.0 m,2.5 m)，实际机器人路线如图20b中红线所示，终点为(7.1 m,2.3 m)，误差为0.28 m。

从实验结果来看，本文的融合算法能够实现避障，绕过障碍物前进到目标点，且规划的路径能够保持全局最优，机器人基本按照规划路径抵达目标。随着场景面积变大和复杂程度增高，机器人到达的实际目标点与设定的目标点之间的误差逐渐增大，主要是由于机器人转弯时与地面打滑以及超声波出现噪声点造成的，其误差不大于0.28 m，满足实际需求。