CN113199474A - 一种机器人行走与作业智能协同的运动规划方法 - Google Patents

Abstract

本发明涉及一种机器人行走与作业智能协同的运动规划方法，用于修剪绿篱的机器人，首先获取工作环境信息，规划出经过所有待修剪绿篱且运动总行程最短的全局路径，再确定修剪作业区，规划移动底盘的局部路径，对局部路径进行平滑处理，随后规划机器人机械臂的轨迹。本发明的运动规划方法可以实现机器人的全自动作业，提升了机器人的自动化、智能化水平。

Description

一种机器人行走与作业智能协同的运动规划方法

技术领域

本发明涉及机器人技术领域，特别涉及一种机器人行走与作业智能协同的运动规划方法。

背景技术

近年来随着中国经济的持续发展和城镇化进程推进，城市园林绿化建设的市场容量和边界不断扩大，迎来了新的发展机遇，但随之而来的是城市绿化的养护问题，人工绿篱修剪成本越来越高，人们开始对绿篱修剪提出了高度机械化自动化的要求。目前国内很多研究机构对园艺机器人进行了一定程度的研究，比如：长安大学研制了一种能完成水平、垂直修剪的纯电动履带式园艺机器人，控制方式为人工遥控操作，自动化程度较低；广西大学研制了可以进行圆柱形绿篱修剪的车载绿篱修剪机器人，以人工操作摇杆的方式进行作业，同时也提出了一种基于无人行走系统和修剪机械手的全自动绿篱修剪机。

目前用于绿篱修剪的园艺机器人大部分由机械手和行走系统组成，机械手用于执行修剪作业，行走系统用于移动至作业区域，绿篱维护人员移动机械手到达圆柱形绿篱上方中心处后，启动修剪程序，手部旋转几圈完成绿篱的修剪。目前该技术的缺点是：修剪机的移动和机械手的作业多需要作业人员进行人工辅助操作，且移动底盘和修剪机械手独立作业。这种工作模式智能化、自动化程度有所欠缺。

公开于该背景技术部分的信息仅仅旨在增加对本发明的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

发明内容

有鉴于此，本发明要解决的技术问题是，如何提供一种机器人行走与作业智能协同的运动规划方法，以解决现有的园艺机器人工作模式智能化、自动化程度低的问题。

为解决以上技术问题，本发明提供一种机器人行走与作业智能协同的运动规划方法，用于修剪绿篱的机器人，所述运动规划方法包括：

步骤S1，规划全局路径：获取工作环境信息，识别出工作环境中的绿篱，构建地图模型，将起始点、待修剪的绿篱、终点分解为一系列的目标点，规划出经过所有目标点且运动总行程最短的行走顺序，即得到全局路径；

步骤S2，确定修剪作业区域：以待修剪绿篱的对中点为圆心，以机器人机械臂末端执行器的最大可达距离为半径，拟合一个圆形区域，在该圆形区域内机械臂无碰撞逆运动学存在可执行解；同时采样获取机器人移动底盘的无碰撞点，获得可行区域；所述圆形区域和所述可行区域的并集即为修剪作业区域；

步骤S3，规划移动底盘的局部路径：机器人移动底盘按照步骤S1规划的全局路径行走，到达步骤S2确定的每一个修剪作业区，规划出时间最短的局部路径，对局部路径进行平滑处理；

步骤S4，规划机器人机械臂的轨迹，分为三个阶段：

S41，规划出机械臂从起始位置移动至修剪位置的运动时间最短的运动轨迹，移动底盘运动自适应机械臂的移动；

S42，移动底盘在修剪作业区域按照规划路径匀速运动，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点执行修剪作业，机械臂完成修剪；

S43，机械臂从修剪位置回到起始位置。

在一种可能的实现方式中，步骤S1基于优化的分级粒子群-遗传算法来规划全局路径，包括以下步骤：

S11，获取工作环境信息，识别出工作环境中的绿篱，使用格栅法构建地图模型，将起始点、待修剪的绿篱、终点分解为一系列的目标点，将目标点信息导入地图中，计算任意2个目标点之间的曼哈顿距离，并将所得距离保存在矩阵target_d中，target_d为n行n列方阵，n为目标数；

target_d(i,j)＝(X_target_i-X_target_j)+(Y_target_i-Y_target_j),i≠j (1)

(1)式中：X_target_i、X_target_j分别为目标点i、j的横坐标；Y_target_i、Y_target_j分别为目标点i、j的纵坐标；

S12，计算种群适应度值并记录当前全体最优和个体最优，适应度值函数fitness_d表示在不存在障碍物情况下机器人遍历所有目标点后的经过的曼哈顿距离总和，单个粒子的适应度值计算公式如下：

fitness_d_m＝target_d(m₁,n)+target_d(m₂,n)+...+target_d(m_n,n) (2)

式中：m₁,m₂,…，m_n为1～n之间的正整数，且m₁≠m₂≠…≠m_n；

S13，对个体最优进行交叉和变异操作，记录求得的新适应度值，并将求得的值与历史最优值进行比较，若当前最优小于历史最优，则替换历史最优，并删除交叉和变异操作中产生的相同元素；若当前最优大于历史最优，则保持历史最优不变；

S14，返回步骤S13重新执行，若求出最优解或者迭代次数达到最大值，终止求解最优行走路线程序，并保存最优行走顺序；进一步，将起始点提取到行走路线第一位，且保持行走顺序不变；

S15，保存路径并退出。

在一种可能的实现方式中，步骤S3包括以下步骤：

S31，按照步骤S1规划的行走顺序，将起始点命名为目标点1，待修剪的绿篱目标点分别命名为目标点2、目标点3、目标点4，以此类推进行命名，直至终点；位于目标点1的机器人移动底盘以目标点3作为局部目标点，规划出一条经过目标点2的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S32，园艺机器人完成目标点2的修剪作业后，开始规划下一步的局部路径，以目标点4作为局部目标点，规划出一条经过目标点3的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S33，以此类推，重复步骤S31和S32，机器人可执行所有待修剪绿篱目标点的修剪作业。

在一种可能的实现方式中，步骤S31和S32规划时间最优的局部路径和进行平滑处理的方法如下：

(1)将机器人的状态使用一个元组

进行表示，x、y为位置坐标，

表示移动底盘的方向角，每个状态都以一个节点表示，所有节点组成节点树，节点颜色有红、黄、蓝、绿四种，颜色确定规则如下：

1)当起始节点与连续节点都不在修剪作业区，即当移动底盘不处在修剪作业区时，所有节点都标记为红色；

2)当节点在修剪作业区展开时，即移动底盘进入修剪作业区时，该节点被标记为黄色，黄色节点下一步扩展的子节点都是蓝色子节点；

3)所有蓝色节点扩展的子节点均为绿色，绿色节点的子节点也为绿色。

代价函数T(S)用来表示移动底盘的节点S到目标点S_goal的时间代价，T(S)的计算用到快速行进算法(fastmarching method)来计算节点间的最短距离，计算方法为：

如果S的节点为红色，则使用代价函数T₁＝min(FMM_Pi(X,Y)+FMMT(X_Pi,Y_Pi),如果节点的颜色不是红色，则使用代价函数T₂＝min(FMMT(X,Y))，其中Pi表示修剪作业区域的所有边缘节点，FMM_Pi(X,Y)为基于FMM算法的S节点到Pi节点的距离，FMMT(X_Pi,Y_Pi)为基于FMM算法的Pi节点到目标节点的距离；

(2)时间最优的局部路径的算法原理如下：

1)将起始节点加入到open表中；

2)将open表中代价函数T(S)最低的节点取出，作为路径点S；

3)对路径点S的各个子节点S`基于步骤(1)所述颜色确定规则进行颜色配置；

4)然后按步骤(1)所述代价函数的计算方法，对各个子节点计算代价函数T(S`)，并将节点S加入close表中；

5)并重复步骤2)-4)，直到open表中取出绿色节点并且与目标节点的距离达到预设距离；

6)按顺序提取close表中所有节点作为路径点输出；

(3)路径曲线平滑处理：

在规划空间(x,y,φ)内，局部路径轨迹的各分量用一个含有n+1个控制点的k阶B样条曲线来表示，即：

式中，B_i,k(u)为基函数，P_i为第i个控制点向量，基函数表达式由de Boor-Cox递推公式给出：

定义节点向量为U＝[u₀u₁……u_m]，其中各元素保持单调不减(u₁≤u_i+1)，且m＝k+n+1。取u∈[0,1]作为参数区间，由此得到平滑的局部路径轨迹K。

在一种可能的实现方式中，步骤S41中，确保机械臂从初始姿态移动至修剪位置的时间最短，以移动底盘当前的移动速度乘以机械臂运动最短时间来计算运动距离，从而确定初始位置，规划出从初始位置移动至修剪位置的运动时间最短的运动轨迹；当机械臂处在修剪位置时，末端执行器的目标点坐标在绿篱正上方，方向为机械臂腰关节到绿篱中心的连线；获得机器人联合空间解，确定移动底盘位移线速度，确保机器人的移动底盘沿着步骤S3规划的局部路径行走的同时，能够精确跟随机械手末端执行器，使之处于修剪位置；计算方法如下：

获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得机器人联合空间解，同时确定移动底盘位移线速度：

(2)在全局坐标框架中，机械臂沿笛卡尔任务空间中的规划轨迹移动执行修剪作业，定义移动机械臂的广义坐标为：

q＝[q_A q_P]^T＝[θ₁ … θ_n Sφ]^T

其中，q_A＝[θ₁ … θ_n]^T为机械臂的关节角矩阵，q_P＝[S φ]^T为移动底盘的配置矩阵，S为移动底盘沿X_P轴的笛卡尔坐标位置，

表示移动底盘的方向角；

(2)移动底盘的角速度通过移动底盘的平移速度

和局部路径轨迹曲率ρ确定

因此，该机械臂系统的关节速度矢量表示为：

(3)由末端执行器的平移和旋转速度分量定义其任务空间，如下所示：

由于末端执行器的笛卡尔速度是预定义的，故末端执行器在每个时间步长的位置和方向是已知的，末端执行器的笛卡尔速度矢量与关节速度(包括底盘沿路径的速度)表示为：

其中，J_A∈R^m×n是将末端执行器的笛卡尔速度与机械臂关节速度相关联的雅可比矩阵，J_P∈R^m×1是将末端执行器的笛卡尔速度与移动底盘沿其路径的线速度S关联的雅可比矩阵；

(5)基于一种RNN启发式算法，选取机械臂在任务空间的最优解，规划机械臂的最佳移动轨迹，具体步骤如下：

1)在任务空间中生成一个正态分布的随机方向向量

机械手坐标表示为

k为时间步长，λ_k为天线长度。代入目标函数

得到g_kL和g_kR；

2)计算得到新的机械手坐标位置r，即

则

g_k+1＝g(X_r(t),r_k+1)；

3)如果g_k+1＜g_k，则更新机械手状态至r_k+1，在下一个时间步长重复迭代过程；否则，保持原状态；

4)迭代终止；

(5)获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得园艺机器人联合空间解，同时确定移动底盘位移线速度。

在一种可能的实现方式中，步骤S42中，根据步骤S3得到的局部路径可知移动底盘在修剪作业区的移动距离L，单株绿篱修剪时间为t，移动底盘以v＝L/t的速度匀速前进，由此可得到移动底盘的位姿，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点，机械臂完成修剪。

本发明通过智能协同规划机器人的无人行走系统和机械臂，进行全局路径规划，机器人可以到达每个局部目标点，且移动路程最短、路径最优。根据机械臂最大工作范围，通过移动底盘的无碰撞点采样，确定机器人的修剪作业区域，在此区域内机械臂无碰撞逆运动学有解。进一步，基于一种时间最优路径搜索算法，规划出移动底盘运动的局部路径。基于一种控制算法，使底盘自适应机械手运动轨迹在规划路径上移动，同时基于一种RNN启发式算法，规划出机械臂移动轨迹。在移动底盘按照规划路径运动时，机械手执行修剪作业，可以实现机器人的全自动作业，提升了机器人的自动化、智能化水平。

根据下面参考附图对示例性实施例的详细说明，本发明的其它特征及方面将变得清楚。

附图说明

包含在说明书中并且构成说明书的一部分的附图与说明书一起示出了本发明的示例性实施例、特征和方面，并且用于解释本发明的原理。

图1示出本发明提供的机器人行走与作业智能协同的运动规划方法的工艺流程图；

图2示出本发明提供的运动规划方法中的规划全局路径示意图；

图3示出本发明提供的运动规划方法中的节点树示意图；

图4示出本发明提供的运动规划方法中规划局部路径和平滑处理示意图；

图5示出本发明提供的运动规划方法中规划机器人机械臂轨迹示意图。

具体实施方式

下面结合附图，对本发明的具体实施方式进行详细描述，但应当理解本发明的保护范围并不受具体实施方式的限制。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。除非另有其它明确表示，否则在整个说明书和权利要求书中，术语“包括”或其变换如“包含”或“包括有”等等将被理解为包括所陈述的元件或组成部分，而并未排除其它元件或其它组成部分。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。

另外，为了更好的说明本发明，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本发明同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件未作详细描述，以便于凸显本发明的主旨。

实施例一

图1示出本发明实施例一提供的机器人行走与作业智能协同的运动规划方法的流程图。如图1所示，该运动规划方法用于修剪绿篱的机器人，所述运动规划方法包括以下步骤：

步骤S1，规划全局路径：获取工作环境信息，构建地图模型，识别出工作环境中的绿篱，将起始点、待修剪的绿篱、终点分解为一系列的目标点，规划出经过所有目标点且运动总行程最短的行走顺序，即得到全局路径；

步骤S2，确定修剪作业区域：以待修剪绿篱的对中点为圆心，以机器人机械臂末端执行器的最大可达距离为半径，拟合一个圆形区域，在该圆形区域内机械臂无碰撞逆运动学存在可执行解；同时采样获取机器人移动底盘的无碰撞点，获得可行区域；所述圆形区域和所述可行区域的并集即为修剪作业区域；

步骤S3，规划移动底盘的局部路径：机器人移动底盘按照步骤S1规划的全局路径行走，到达步骤S2确定的每一个修剪作业区，规划出时间最短的局部路径，对局部路径进行平滑处理；

步骤S4，规划机器人机械臂的轨迹，分为三个阶段：

S41，规划出机械臂从起始位置移动至修剪位置的运动时间最短的运动轨迹，移动底盘运动自适应机械臂的移动；

S42，移动底盘在修剪作业区域按照规划路径匀速运动，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点执行修剪作业，机械臂完成修剪；

S43，机械臂从修剪位置回到起始位置。

最后，将规划好的路径发至机器人的控制模块。

其中，步骤S1基于优化的分级粒子群-遗传算法来规划全局路径，包括以下步骤：

S11，获取工作环境信息，识别出工作环境中的绿篱，使用格栅法构建地图模型，如图2所示，将起始点、待修剪的绿篱、终点分解为一系列的目标点并编号，将目标点信息导入地图中，计算任意2个目标点之间的曼哈顿距离，并将所得距离保存在矩阵target_d中，target_d为n行n列方阵，n为目标数；

target_d(i,j)＝(X_target_i-X_target_j)+(Y_target_i-Y_target_j),i≠j (1)

(1)式中：X_target_i、X_target_j分别为目标点i、j的横坐标；Y_target_i、Y_target_j分别为目标点i、j的纵坐标；

S12，计算种群适应度值并记录当前全体最优和个体最优，适应度值函数fitness_d表示在不存在障碍物情况下机器人遍历所有目标点后的经过的曼哈顿距离总和，单个粒子的适应度值计算公式如下：

fitness_d_m＝target_d(m₁,n)+target_d(m₂,n)+...+target_d(m_n,n) (2)

式中：m₁,m₂,…，m_n为1～n之间的正整数，且m₁≠m₂≠…≠m_n；

S13，对个体最优进行交叉和变异操作，记录求得的新适应度值，并将求得的值与历史最优值进行比较，若当前最优小于历史最优，则替换历史最优，并删除交叉和变异操作中产生的相同元素；若当前最优大于历史最优，则保持历史最优不变；

S14，返回步骤S13重新执行，若求出最优解或者迭代次数达到最大值，终止求解最优行走路线程序，并保存最优行走顺序；进一步，将起始点提取到行走路线第一位，且保持行走顺序不变；

S15，保存路径并退出。

其中，步骤S3包括以下步骤：

S31，按照步骤S1规划的行走顺序，将起始点命名为目标点1，待修剪的绿篱目标点分别命名为目标点2、目标点3、目标点4，以此类推进行命名，直至终点；位于目标点1的机器人移动底盘以目标点3作为局部目标点，规划出一条经过目标点2的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S32，园艺机器人完成目标点2的修剪作业后，开始规划下一步的局部路径，以目标点4作为局部目标点，规划出一条经过目标点3的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S33，以此类推，重复步骤S31和S32，机器人可执行所有待修剪绿篱目标点的修剪作业。

如图4所示，步骤S31和S32规划时间最优的局部路径和进行平滑处理的方法如下：

(1)将机器人的状态使用一个元组

进行表示，x、y为位置坐标，

表示移动底盘的方向角，每个状态都以一个节点表示，所有节点组成节点树，机器人的起始点不在修剪区域内时，其节点树如图3所示，节点颜色有红、黄、蓝、绿四种，颜色确定规则如下：

1)当起始节点与连续节点都不在修剪作业区，即当移动底盘不处在修剪作业区时，所有节点都标记为红色；

2)当节点在修剪作业区展开时，即移动底盘进入修剪作业区时，该节点被标记为黄色，黄色节点下一步扩展的子节点都是蓝色子节点；

3)所有蓝色节点扩展的子节点均为绿色，绿色节点的子节点也为绿色。

代价函数T(S)用来表示移动底盘的节点S到目标点S_goal的时间代价，T(S)的计算用到快速行进算法(fastmarching method)来计算节点间的最短距离，计算方法为：

如果S的节点为红色，则使用代价函数T₁＝min(FMM_Pi(X,Y)+FMMT(X_Pi,Y_Pi),如果节点的颜色不是红色，则使用代价函数T₂＝min(FMMT(X,Y))，其中Pi表示修剪作业区域的所有边缘节点，FMM_Pi(X,Y)为基于FMM算法的S节点到Pi节点的距离，FMMT(X_Pi,Y_Pi)为基于FMM算法的Pi节点到目标节点的距离；

(2)时间最优的局部路径的算法原理如下：

1)将起始节点加入到open表中；

2)将open表中代价函数T(S)最低的节点取出，作为路径点S；

3)对路径点S的各个子节点S`基于步骤(1)所述颜色确定规则进行颜色配置；

4)然后按步骤(1)所述代价函数的计算方法，对各个子节点计算代价函数T(S`)，并将节点S加入close表中；

5)并重复步骤2)-4)，直到open表中取出绿色节点并且与目标节点的距离达到预设距离；

6)按顺序提取close表中所有节点作为路径点输出；

(3)路径曲线平滑处理：

在规划空间(x,y,φ)内，局部路径轨迹的各分量用一个含有n+1个控制点的k阶B样条曲线来表示，即：

式中，B_i,k(u)为基函数，P_i为第i个控制点向量，基函数表达式由de Boor-Cox递推公式给出：

定义节点向量为U＝[u0u₁……u_m]，其中各元素保持单调不减(u₁≤u_i+1)，且m＝k+n+1。取u∈[0,1]作为参数区间，由此得到平滑的局部路径轨迹K。

其中，如图5所示，步骤S41中，确保机械臂从初始姿态移动至修剪位置的时间最短，以移动底盘当前的移动速度乘以机械臂运动最短时间来计算运动距离，从而确定初始位置，规划出从初始位置移动至修剪位置的运动时间最短的运动轨迹；当机械臂处在修剪位置时，末端执行器的目标点坐标在绿篱正上方，方向为机械臂腰关节到绿篱中心的连线；获得机器人联合空间解，确定移动底盘位移线速度，确保机器人的移动底盘沿着步骤S3规划的局部路径行走的同时，能够精确跟随机械手末端执行器，使之处于修剪位置；计算方法如下：

获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得机器人联合空间解，同时确定移动底盘位移线速度：

(3)在全局坐标框架中，机械臂沿笛卡尔任务空间中的规划轨迹移动执行修剪作业，定义移动机械臂的广义坐标为：

q＝[q_A q_P]^T＝[θ₁ … θ_n Sφ]^T

其中，q_A＝[θ₁ … θ_n]^T为机械臂的关节角矩阵，q_P＝[S φ]^T为移动底盘的配置矩阵，S为移动底盘沿X_P轴的笛卡尔坐标位置，

表示移动底盘的方向角；

(2)移动底盘的角速度通过移动底盘的平移速度

和局部路径轨迹曲率ρ确定

因此，该机械臂系统的关节速度矢量表示为：

(3)由末端执行器的平移和旋转速度分量定义其任务空间，如下所示：

由于末端执行器的笛卡尔速度是预定义的，故末端执行器在每个时间步长的位置和方向是已知的，末端执行器的笛卡尔速度矢量与关节速度(包括底盘沿路径的速度)表示为：

其中，J_A∈R^m×n是将末端执行器的笛卡尔速度与机械臂关节速度相关联的雅可比矩阵，J_P∈R^m×1是将末端执行器的笛卡尔速度与移动底盘沿其路径的线速度S关联的雅可比矩阵；

(6)基于一种RNN启发式算法，选取机械臂在任务空间的最优解，规划机械臂的最佳移动轨迹，具体步骤如下：

1)在任务空间中生成一个正态分布的随机方向向量

机械手坐标表示为

k为时间步长，λ_k为天线长度。代入目标函数

得到g_kL和g_kR；

2)计算得到新的机械手坐标位置r，即

则

g_k+1＝g(X_r(t),r_k+1)；

3)如果g_k+1＜g_k，则更新机械手状态至r_k+1，在下一个时间步长重复迭代过程；否则，保持原状态；

4)迭代终止；

(5)获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得园艺机器人联合空间解，同时确定移动底盘位移线速度。

其中，步骤S42中，根据步骤S3得到的局部路径可知移动底盘在修剪作业区的移动距离L，单株绿篱修剪时间为t，移动底盘以v＝L/t的速度匀速前进，由此可得到移动底盘的位姿，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点，机械臂完成修剪。

前述对本发明的具体示例性实施方案的描述是为了说明和例证的目的。这些描述并非想将本发明限定为所公开的精确形式，并且很显然，根据上述教导，可以进行很多改变和变化。对示例性实施例进行选择和描述的目的在于解释本发明的特定原理及其实际应用，从而使得本领域的技术人员能够实现并利用本发明的各种不同的示例性实施方案以及各种不同的选择和改变。本发明的范围意在由权利要求书及其等同形式所限定。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

Claims (7)

1.一种机器人行走与作业智能协同的运动规划方法，其特征在于，所述运动规划方法包括：

步骤S1，规划全局路径：获取工作环境信息，构建地图模型，识别出工作环境中的绿篱，将起始点、待修剪的绿篱、终点分解为一系列的目标点，规划出经过所有目标点且运动总行程最短的行走顺序，即得到全局路径；

步骤S2，确定修剪作业区域：以待修剪绿篱的对中点为圆心，以机器人机械臂末端执行器的最大可达距离为半径，拟合一个圆形区域，在该圆形区域内机械臂无碰撞逆运动学存在可执行解；同时采样获取机器人移动底盘的无碰撞点，获得可行区域；所述圆形区域和所述可行区域的并集即为修剪作业区域；

步骤S3，规划移动底盘的局部路径：机器人移动底盘按照步骤S1规划的全局路径行走，到达步骤S2确定的每一个修剪作业区，规划出时间最短的局部路径，对局部路径进行平滑处理；

步骤S4，规划机器人机械臂的轨迹，分为三个阶段：

S41，规划出机械臂从起始位置移动至修剪位置的运动时间最短的运动轨迹，移动底盘运动自适应机械臂的移动；

S42，移动底盘在修剪作业区域按照规划路径匀速运动，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点执行修剪作业，机械臂完成修剪；

S43，机械臂从修剪位置回到起始位置。

2.根据权利要求1所述的运动规划方法，其特征在于，步骤S1基于优化的分级粒子群-遗传算法来规划全局路径，包括以下步骤：

S11，获取工作环境信息，识别出工作环境中的绿篱，使用格栅法构建地图模型，将起始点、待修剪的绿篱、终点分解为一系列的目标点，将目标点信息导入地图中，计算任意2个目标点之间的曼哈顿距离，并将所得距离保存在矩阵target_d中，target_d为n行n列方阵，n为目标数；

target_d(i,j)＝(X_target_i-X_target_j)+(Y_target_i-Y_target_j),i≠j (1)

(1)式中：X_target_i、X_target_j分别为目标点i、j的横坐标；Y_target_i、Y_target_j分别为目标点i、j的纵坐标；

S12，计算种群适应度值并记录当前全体最优和个体最优，适应度值函数fitness_d表示在不存在障碍物情况下机器人遍历所有目标点后的经过的曼哈顿距离总和，单个粒子的适应度值计算公式如下：

fitness_d_m＝target_d(m₁,n)+target_d(m₂,n)+...+target_d(m_n,n) (2)

式中：m₁,m₂,…，m_n为1～n之间的正整数，且m₁≠m₂≠…≠m_n；

S13，对个体最优进行交叉和变异操作，记录求得的新适应度值，并将求得的值与历史最优值进行比较，若当前最优小于历史最优，则替换历史最优，并删除交叉和变异操作中产生的相同元素；若当前最优大于历史最优，则保持历史最优不变；

S14，返回步骤S13重新执行，若求出最优解或者迭代次数达到最大值，终止求解最优行走路线程序，并保存最优行走顺序；进一步，将起始点提取到行走路线第一位，且保持行走顺序不变；

S15，保存路径并退出。

3.根据权利要求1所述的运动规划方法，其特征在于，步骤S3包括以下步骤：

S31，按照步骤S1规划的行走顺序，将起始点命名为目标点1，待修剪的绿篱目标点分别命名为目标点2、目标点3、目标点4，以此类推进行命名，直至终点；位于目标点1的机器人移动底盘以目标点3作为局部目标点，规划出一条经过目标点2的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S32，园艺机器人完成目标点2的修剪作业后，开始规划下一步的局部路径，以目标点4作为局部目标点，规划出一条经过目标点3的修剪作业区域的时间最优的局部路径，然后对该局部路径曲线进行平滑处理；

S33，以此类推，重复步骤S31和S32，机器人可执行所有待修剪绿篱目标点的修剪作业。

4.根据权利要求3所述的运动规划方法，其特征在于，规划时间最优的局部路径的方法为：

(1)将机器人的状态使用一个元组

进行表示，x、y为位置坐标，

表示移动底盘的方向角，每个状态都以一个节点表示，所有节点组成节点树，节点颜色有红、黄、蓝、绿四种，颜色确定规则如下：

1)当起始节点与连续节点都不在修剪作业区，即当移动底盘不处在修剪作业区时，所有节点都标记为红色；

2)当节点在修剪作业区展开时，即移动底盘进入修剪作业区时，该节点被标记为黄色，黄色节点下一步扩展的子节点都是蓝色子节点；

3)所有蓝色节点扩展的子节点均为绿色，绿色节点的子节点也为绿色。

代价函数T(S)用来表示移动底盘的节点S到目标点S_goal的时间代价，T(S)的计算用到快速行进算法(fastmarching method)来计算节点间的最短距离，计算方法为：

如果S的节点为红色，则使用代价函数T₁＝min(FMM_Pi(X,Y)+FMMT(X_Pi,Y_Pi),如果节点的颜色不是红色，则使用代价函数T₂＝min(FMMT(X,Y))，其中Pi表示修剪作业区域的所有边缘节点，FMM_Pi(X,Y)为基于FMM算法的S节点到Pi节点的距离，FMMT(X_Pi,Y_Pi)为基于FMM算法的Pi节点到目标节点的距离；

(2)时间最优的局部路径的算法原理如下：

1)将起始节点加入到open表中；

2)将open表中代价函数T(S)最低的节点取出，作为路径点S；

3)对路径点S的各个子节点S`基于步骤(1)所述颜色确定规则进行颜色配置；

4)然后按步骤(1)所述代价函数的计算方法，对各个子节点计算代价函数T(S`)，并将节点S加入close表中；

5)并重复步骤2)～4)，直到open表中取出绿色节点并且与目标节点的距离达到预设距离；

6)按顺序提取close表中所有节点作为路径点输出。

5.根据权利要求3所述的运动规划方法，其特征在于，平滑处理的方法如下：

(1)路径曲线平滑处理：

在规划空间(x,y,φ)内，局部路径轨迹的各分量用一个含有n+1个控制点的k阶B样条曲线来表示，即：

式中，B_i,k(u)为基函数，P_i为第i个控制点向量，基函数表达式由de Boor-Cox递推公式给出：

定义节点向量为U＝[u₀u₁……u_m]，其中各元素保持单调不减(u₁≤u_i+1)，且m＝k+n+1。取u∈[0,1]作为参数区间，由此得到平滑的局部路径轨迹K。

6.根据权利要求1所述的运动规划方法，其特征在于，在步骤S41中，确保机械臂从初始姿态移动至修剪位置的时间最短，以移动底盘当前的移动速度乘以机械臂运动最短时间来计算运动距离，从而确定初始位置，规划出从初始位置移动至修剪位置的运动时间最短的运动轨迹；当机械臂处在修剪位置时，末端执行器的目标点坐标在绿篱正上方，方向为机械臂腰关节到绿篱中心的连线；获得机器人联合空间解，确定移动底盘位移线速度，确保机器人的移动底盘沿着步骤S3规划的局部路径行走的同时，能够精确跟随机械手末端执行器，使之处于修剪位置；计算方法如下：

获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得机器人联合空间解，同时确定移动底盘位移线速度：

(1)在全局坐标框架中，机械臂沿笛卡尔任务空间中的规划轨迹移动执行修剪作业，定义移动机械臂的广义坐标为：

q＝[q_A q_P]^T＝[θ₁…θ_n S φ]^T

其中，q_A＝[θ₁…θ_n]^T为机械臂的关节角矩阵，q_P＝[S φ]^T为移动底盘的配置矩阵，S为移动底盘沿X_P轴的笛卡尔坐标位置，

表示移动底盘的方向角；

(2)移动底盘的角速度通过移动底盘的平移速度

和局部路径轨迹曲率ρ确定

因此，该机械臂系统的关节速度矢量表示为：

(3)由末端执行器的平移和旋转速度分量定义其任务空间，如下所示：

由于末端执行器的笛卡尔速度是预定义的，故末端执行器在每个时间步长的位置和方向是已知的，末端执行器的笛卡尔速度矢量与关节速度(包括底盘沿路径的速度)表示为：

其中，J_A∈R^m×n是将末端执行器的笛卡尔速度与机械臂关节速度相关联的雅可比矩阵，J_P∈R^m×1是将末端执行器的笛卡尔速度与移动底盘沿其路径的线速度S关联的雅可比矩阵；

(4)基于一种RNN启发式算法，选取机械臂在任务空间的最优解，规划机械臂的最佳移动轨迹，具体步骤如下：

1)在任务空间中生成一个正态分布的随机方向向量

机械手坐标表示为

k为时间步长，λ_k为天线长度。代入目标函数

得到g_kL和g_kR；

2)计算得到新的机械手坐标位置r，即

则

g_k+1＝g(X_r(t),r_k+1)；

3)如果g_k+1＜g_k，则更新机械手状态至r_k+1，在下一个时间步长重复迭代过程；否则，保持原状态；

4)迭代终止；

(5)获得机械手可执行任务空间解，机械臂轨迹规划结束，基于逆运动学原理，获得园艺机器人联合空间解，同时确定移动底盘位移线速度。

7.根据权利要求1所述的运动规划方法，其特征在于，在步骤S42中，根据步骤S3得到的局部路径可知移动底盘在修剪作业区的移动距离L，单株绿篱修剪时间为t，移动底盘以v＝L/t的速度匀速前进，由此可得到移动底盘的位姿，基于机械臂逆运动学解，获得每个移动底盘位姿对应的机械臂位姿，机械臂的末端执行器保持在被修剪的绿篱的对中点，机械臂完成修剪。

Images