改进A*算法的采摘机器人路径规划与跟踪控制**

代玉梅，张瑞玲，马黎

(商丘职业技术学院软件学院，河南商丘，476000)

0 引言

采摘是农业生产的重要环节，在收获季节需要大量的人力在较短时间内完成采摘工作[1-2]。尤其是在果园面积较大时，需要投入更多的人工。随着智能化机械设备的不断发展，农业采摘机器人应运而生，针对复杂的地形和作业环境，需要采摘机器人具备移动、定位和采摘的功能来完成自动化的采摘作业，但是当前采摘机器人的智能化和自主化水平还比较低[3-7]。为了提高采摘机器人的工作效率和控制效果，从路径规划与跟踪控制两方面展开研究。

国内外相关学者在此领域也有研究，郑嫦娥等[8]为了解决深度强化学习对采摘路径规划效率低的问题，提出了基于分步迁移策略的深度确定性策略梯度算法，并进行了苹果采摘轨迹规划，该方法能够在一定程度上提高路径规划效率，但是不涉及路径跟踪控制，实际意义不大；马冀桐等[9]为了解决采摘机器人的避障问题，利用深度学习Mask R-CNN神经网络对障碍物信息进行训练和识别，提出了一种快速扩展随机树的改进算法，该方法能够实现采摘机器人的避障运动规划，但是算法复杂程度较高；马冀桐等[10]针对采摘机器人的避障问题，提出了一种基于构型空间的改进型双向快速扩展随机树算法，该算法能够实现机械臂的避障路径规划，但是算法条件较严格，并且不涉及路径跟踪控制；张玲等[11]利用遗传算法设计了轮式移动机器人和机械臂的轨迹控制方法，虽然实现了最优的轨迹控制，但是没有考虑避障问题。

为此，通过引入人工势场法改进了A*算法的效率，并设计了采摘机器人的路径规划方法，然后利用状态观测器估计出系统状态，并通过设计的终端滑模控制律准确跟踪路径指令，从而提高了机械臂的控制精度。

1 建立采摘机器人数学模型

采摘机器人主要包括移动车和机械臂两个部分。为了能更好分析采摘机器人的运动特性，对采摘机器人进行建模，如图1扬示。假设移动车的质心坐标为A（xA,yA），θl表示左轮转动角度，θr表示右轮转动角度，移动车可以在果园里自由移动来确定采摘的位置。机械臂安装在移动车质心处，机械臂末端相对移动车质心的坐标为B（xB,yB,zB）；θ1表示关节1在水平面内转动的角度；θ2表示关节2在地垂面内转动的角度，θ3表示关节3相对果实的转动角度。其中，θ1和θ2用来调整机械臂末端方位，θ3用来调整采摘角度。

图1 采摘机器人建模Fig.1 Modeling of picking robot

采摘机器人的动力学模型[12-13]

式中：θ——采摘移动车左轮、右轮和3个机械关节的转动角度，θ=[θl,θr,θ1,θ2,θ3]T,(°)；

——采摘移动车左轮、右轮和3个机械关节的转动角速度，,(°)/s;

——采摘移动车左轮、右轮和3个机械关节的转 动 角 加 速 度，,(°)/s2；

m——采摘机器人的惯性矩阵；

n——科氏矩阵；

g——重力加速度，m/s2；

u——采摘机器人左轮、右轮和3个机械关节电机的输出力矩，u=［ul,ur,u1,u2,u3］T，N·m；

Q——位置坐标，Q=［xA,yA,xB,yB,zB］T，m。

则采摘机器人的运动模型

式中：A——变换矩阵。

令s1=Q，̇，则采摘机器人的数学模型可以化简为

式中：d——采摘机器人的摩擦干扰。

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

A*算法是一种全局最优的轨迹搜索算法，可以用来对采摘机器人的路径进行规划，由于A*算法具有局部最优的缺点，为此本文利用人工势场法来提高A*算法的路径规划效率，实现对采摘机器人快速和准确的路径规划。

2.1 A*算法描述

A*算法步骤[14-15]如下。

Step1：确定采摘起点和终点的空间坐标，将采摘空间细分为一系列小方格，作为轨迹搜索的最小单元。

Step2：针对起点方格建立Open集和Close集，将起点方格紧挨的8个方格列入Open集，然后分别计算起点方格到Open集各点的代价值，代价值的表达式为

式中：g(c)——自起点至Open集各点的代价值；

h(c)——Open集各点至终点的代价值。

Step3：比较Open集中各点的代价值，选取代价值最小的点作为新的子起点，并将该点的上一级起点列入Close集。

Step4：清空Open集，将子起点相邻的7个节点（上级起点除外）列入Open集，根据式（4）、式（5）和式（6）计算该子起点到Open集各点的代价值。

Step5：判断路径终点是否在Open集中。若在，则输出起点与子起点的轨迹，即为路径规划结果，算法结束；否则，转Step3。

当采摘空间划分的足够小时，该算法可以求得采摘机器人路径的最优解，但是求解过程的计算量太大，导致路径规划的效率偏低。

2.2 人工势场法

为了提高A*算法的路径规划效率，引入人工势场法[16-17]。人工势场法的基本思想是将采摘机器人的运动变换成在引力场中的运动，终点会对采摘机器人产生引力势场，而障碍物则会对采摘机器人产生斥力势场，最后根据合力得到采摘机器人的运动路径。

采摘机器人的引力势场

式中：ky——引力势场的比例系数；

dy——采摘机器人末端与终点之间的距离。

采摘机器人的障碍斥力势场

式中：kc——斥力势场的比例系数；

dc——采摘机器人末端与障碍物间的空间长度；

d0——障碍对于采摘机器人的斥力半径。

则采摘机器人的合力势场

求解合力势场的负梯度，可以得到合力势场减小最快的路径，此路径即为采摘机器人的路径规划结果。

当目标点在障碍物的斥力空间内时，传统人工势场法容易陷入局部最优，如图2扬示。

图2 目标点和障碍物接近Fig.2 Target point is close to obstacle

当采摘机器人在接近目标点的过程中，合力势场会在障碍物和目标点之间的某一点（非目标点）减小到0，最终得到局部最优解，导致路径规划结果错误。

2.3 改进A*路径规划算法步骤

为了提高A*路径规划算法的效率，并克服人工势场法的局部最优缺点，提出了改进A*路径规划算法来优化采摘路径，算法步骤如下。

Step1：建立采摘空间模型，确定障碍物、起点和终点位置。

Step2：根据式（7）、式（8）和式（9）计算采摘空间的合力势场。

Step3：根据采摘空间的合力势场，得到合力势场减小最快的采摘路径。

Step4：跟随合力势场的减小路径，找到合力势场为0的点，并判断此点是否为终点，若是，则输出路径规划结果。若不是，则转下一步。

Step5：将该点作为子起点。

Step6：将子起点相邻的8个节点列入Open集，判断路径终点是否在Open集中。若在，则输出子起点与终点的轨迹，即为路径规划结果，算法结束。否则，执行下一步。

Step7：根据式（4）、式（5）和式（6）计算该子起点到Open集各点的代价值。

Step8：比较Open集中各点的代价值，选取代价值最小的点作为新的子起点，转Step6。

3 终端滑模控制律

本文设计了终端滑模控制律来确保采摘机器人快速、准确跟踪改进A*路径规划算法输出的采摘路径，系统结构如图3扬示。

图3 系统结构Fig.3 System structure

3.1 状态观测器设计

针对采摘机器人的数学模型式（3），设计状态观测器[18]。

——采 摘 机 器 人 位 置 坐 标 的 估 计 误 差，͂=-s1；

——s2的估计 值；

——采摘机器人转动角速度的估计误差=-s2；

ρ1、ρ2——正常数；

sign（）——符号函数；0＜α＜1；

——采摘机器人摩擦干扰d的估计值。

利用RBF神经网络[19]估计摩擦干扰d，则神经网络自适应律描述

式中：̂——RBF神经网络的权重系数；

ϕ(s1,s2)——RBF神经网络的高斯基函数[20]；

Γ——正定矩阵。

进一步可得到状态观测器式（10）的估计误差

为证明对采摘机器人设计的状态观测器式（10）是收敛的，令，则可得到

由于矩阵C1和矩阵C2是Hurwitz矩阵，则存在正定矩阵P=PT、B1=B1T、B2=B2T，且满足

构建Lyapunov函数

对式（15）求导可以得到

根据Young不等式[21]，可以得到

式中：k1、k2——正常数。

将式（17）和式（11）代入式（16），化简可得

式中：λmin(Π)——矩阵Π的最小元素。

选取较大k1和k2可确保λmin(Π)＞0，得到，即可确保状态观测器式（10）收敛。

3.2 终端滑模控制律设计

采摘机器人的位置坐标跟踪误差

式中：s1d——改进A*路径规划算法输出的采摘路径规划结果，即采摘机器人扬期望的位置坐标。

采摘机器人的转动角速度跟踪误差

式中：s2d——期望的转动角速度，。

设计如下积分终端滑模面[22]

式中：β1和β2——正常数；

ξ1和ξ2——滑模切换项。

其中，l1和l2为正常数；v1≥1；v2≥1；ξ1=ξ2/(2-ξ2)，0＜ξ2＜1。

对积分终端滑模面求导，并将式（10）代入得

在积分终端滑模面的基础上，设计了积分终端滑模控制律如式(25)。

式中：φ1、γ1和κ——正常数。

3.3 稳定性分析

将终端滑模控制律式（25）代入式（24）可以得到

构建Lyapunov函数

式中：γ2、和μ——正常数。

对式（27）求导可以得到

式中：矩阵F——正定矩阵。

进一步得

将式（30）代入式（28）可以得到

由Lyapunov稳定性定理得：变量E和终端滑模面h可以稳定收敛到0，即采摘机器人的位置坐标跟踪误差e1和转动角速度跟踪误差e2可以收敛到0，设计的积分终端滑模控制律式（25）能够确保采摘机器人稳定跟踪改进A*路径规划算法输出的采摘路径。

4 仿真试验结果

为了验证本文设计的方法能够实现对采摘机器人的路径规划和精确控制，分别采用传统A*算法进行路径规划、采用文献[23]的滑模控制方法进行跟踪控制，并与本文方法进行了Matlab仿真对比。设置控制系统参数如表1扬示。

表1 控制系统参数Tab.1 Control system parameters

4.1 仿真结果

设置移动车的起点坐标为(0，0)，终点坐标为(30，30)，2个障碍物的圆心分别为(10，10)和(15，20)，斥力半径分别为2 m和5 m，得到路径规划仿真结果如图4扬示。

图4 移动车路径规划结果Fig.4 Path planning results of mobile vehicle

在整个仿真过程中，传统A*算法的运行时间为13 s，路径规划结果如虚线扬示，路径长度为58.54 m；而本文改进A*算法的运行时间为6 s，路径规划结果如实线扬示，路径长度为47.82 m，通过二者对比突出本文算法具有更高的运行效率和具有更短的路径长度，并且路径更加圆滑，更加符合工程实际。

为了验证本文终端滑模控制方法的有效性，以实线为路径指令(xAd,yAd)进行跟踪控制仿真，并参考文献[23]的滑模控制方法比较，得到路径跟踪曲线如图5扬示。

图5（a）为路径跟踪曲线，图5（b）为跟踪误差。

由仿真图5可看出，文献[23]的滑模控制方法能够使移动车大致跟踪指令信号，但是跟踪误差较大，最大跟踪误差达到了1.6 m，跟踪效果不好；而扬设计的终端滑模控制方法能够使移动车精确跟踪指令信号，其最大跟踪误差仅为0.2 m，跟踪效果较好。

图5 移动车路径跟踪结果Fig.5 Path tracking results of mobile vehicle

4.2 机械臂仿真结果

设置采摘机械臂的起点为(0，0，0)，终点为(5，5，5)，2个障碍物的圆心分别为(2，2，2)和(4，4，4)，斥力半径分别为0.3 m和0.5 m，路径规划结果如图6扬示。

图6 机械臂路径规划结果Fig.6 Path planning results of manipulator

在整个仿真过程中，传统A*算法运行时间为5 s，路径规划结果如虚线扬示，路径长度为13.72 m；而本文改进A*算法的运行时间为2 s，路径规划结果实线扬示，路径长度为11.25 m，通过二者的对比突出了本文算法具有更高的运行效率，具有更短的路径长度，并且路径更加圆滑，更加符合工程实际。

为了验证本文终端滑模控制方法的有效性，以实线为路径指令(xBd，yBd，zBd)进行跟踪控制仿真，并与文献[23]的方法比较，得到的路径跟踪结果如图7扬示。其中，图7（a）为路径跟踪曲线，图7（b）为跟踪误差。

图7 机械臂路径跟踪结果Fig.7 Path tracking results of manipulator

由仿真图7可看出，文献[23]的滑模控制方法可使机械臂大致跟踪指令信号，误差较大，最大跟踪误差为0.54 m，跟踪效果不好；而本文的终端滑模控制方法能够使机械臂精确跟踪指令信号，最大跟踪误差仅为0.04 m，跟踪效果较好。

5 结论

为了促进农业采摘的智能化和自主化的发展，针对采摘机器人提出了一种基于改进A*算法的路径规划与跟踪控制方法，经过Matlab对比仿真，得到以下结论。

1）采用改进A*路径规划算法：移动车的运行时间为6 s，路径长度为47.82 m；机械臂运行时间为2 s，路径长度为11.25 m，比传统A*路径规划算法具有更高的运行效率和更短的路径长度，且路径更加圆滑，更加符合工程实际。

2）设计的终端滑模控制方法比滑模控制方法具有更优的控制效果，对移动车最大跟踪误差为0.2 m，对机械臂的最大跟踪误差仅为0.04 m。

3）通过改进A*路径规划算法和终端滑模控制方法有机结合，实现了对采摘机器人精确路径规划和跟踪控制，有效提高了农业采摘的智能化和自主化水平。