CN112975961A - 基于ctb-rrt*算法的采摘机械臂运动规划方法 - Google Patents

Abstract

本发明公开了基于CTB‑RRT*算法的采摘机械臂运动规划方法，该方法融合RRT‑connect的双向贪婪搜索和RRT*的渐进最优性，并引入柯西采样法、目标引力与动态步长法和节点拒绝策略，通过视觉系统确定采摘目标位置和障碍物信息，通过逆运动学求解获得机械臂末端采摘位置和姿态，使用CTB‑RRT*算法进行运动规划，获得连接起始位姿和目标位姿的无碰撞路径，控制机械臂沿着无碰撞路径到达目标采摘点。本发明以柯西采样取代随机采样，减小采样的盲目性；引入目标引力，并动态调节随机生长方向和目标方向的步长，提高局部搜索速度；最后通过节点拒绝策略，减少不必要的采样节点，提高路径搜索效率。

Description

基于CTB-RRT*算法的采摘机械臂运动规划方法

技术领域

本发明属于机械臂运动规划领域，具体是一种基于CTB-RRT*算法的采摘机械臂运动规划方法。

背景技术

基于采样的随机算法是当前解决高维运动规划问题的最新技术。最著名的基于采样的算法包括概率路线图(PRM)和快速探索随机树(RRT)。但是，如果事先不知道障碍物的几何形状，则PRM往往是无效的。因此，为了在现实世界中得出有效的运动规划解，快速探索随机树(RRT)算法被广泛采用。目前提出了各种增强原始RRT的算法，如Lazy-RRT、MGB-RRT(Memory-Goal-BiasingRRT)、H-RRT(Heuristic RRT)、RRT-connect等，不管是否事先知道障碍物的特定几何形状，这些算法都提供了路径解。RRT最著名的变体之一是RRT*，与原始RRT算法不同，该算法引入了路径成本和最优性，RRT*通过不断迭代优化其路径可确保最终收敛到最优路径解，但计算度复杂，会增加大量搜索时间。RRT另一著名变体是RRT-connect，从起点和终点同时生成两棵随机数，并通过贪婪策略进行双向连接，加快了算法的搜索速度，但存在搜索路径较长的情况。如今较为流行的是RRT*的双向版本，称为B-RRT*，它是RRT*的简单双向随机树的实现，使用经细微改变的RRT-connect贪婪启发式连接，但这种方式减弱了其收敛到最优路径的能力，并且使其计算成本很高。

为了提高效率，降低成本，本文在B-RRT*的基础上，提出了CTB-RRT*算法，在随机树生长过程中以柯西采样策略进行采样，降低采样的盲目性；以目标引力和动态步长法加快局部搜索速度，减少了很多不必要的采样节点，提高了局部搜索速度；最后通过节点剔除策略进行修枝处理，进一步降低路径成本。

发明内容

本发明的目的在于克服现有技术的不足，提出一种基于CTB-RRT*算法的采摘机械臂运动规划方法。本方法采用改进的双向RRT*算法，可以降低采样的盲目性，提高局部搜索速度，大大减小规划时间和路径成本。

本发明至少通过如下技术方案之一实现。

基于CTB-RRT*算法的采摘机械臂运动规划方法，包括以下步骤：

S1、通过视觉系统确定采摘目标位置和障碍物信息；

S2、利用逆运动学求解获得目标位置所对应机械臂末端的位置和姿态；

S3、通过视觉系统获取周围环境障碍物信息，建立三维障碍物模型；

S4、使用改进的CTB-RRT*算法对三维障碍物模型进行运动规划，得到从初始位姿到目标位姿的无碰撞路径；

S5、上位机控制机械臂沿着无碰撞路径运动直至到达目标位置。

优选的，所述改进的CTB-RRT*算法融合RRT-connect的双向贪婪搜索和RRT*的渐进最优性，并引入柯西采样法、目标引力与动态步长法和节点拒绝策略。

优选的，所述柯西采样法包括：

根据柯西分布的规律改变采样的特性，设置采样概率密度函数为：

其中，l表示采样点与障碍物的距离，γ为尺度参数；设置碰撞函数为：

其中，ε为采样点与障碍物的最小距离，当l小于ε时，机器人与障碍物发生碰撞，collision(l)＝0；当l大于ε时，机器人与障碍物未发生碰撞，collison(l)＝1，采样概率密度函数

通过设置尺度参数γ和最小距离ε的参数值，改进尺度参数来改变搜索的随机特性；

优选的，所述柯西采样法的采样过程首先通过柯西采样函数RandSample()获得第一采样点q₁，然后通过采样概率密度函数P(l,γ)获得第二采样点q₂，如果第一采样点q₁在无障碍自由空间C_free中，第二采样点q₂不在自由空间C_free中，则采样点q_rand赋值为第一采样点q₁，否则采样点q_rand赋值为第二采样点q₂。

优选的，所述目标引力与动态步长法具体如下：

建立原始RRT节点扩展公式：

式中，q_new为新节点，q_rand为随机节点，q_nearest为随机树中与随机节点q_rand最近的节点，ρ为步长；

改进的CTB-RRT*算法的引力势能为：

式中，k为引力系数，q_goal为目标点，得到q_goal对q_nearest的目标引力为：

F＝k||q_goal-q_nearest||

改进的CTB-RRT*算法节点扩展公式为：

式中，ρ₁为随机扩展方向的步长，ρ₂为目标扩展方向的步长，在q_rand和q_goal的联合影响下，随机树扩展得到新节点q_new，通过引力系数k控制生长方向，k减小，随机树向随机点方向扩展，k增大，随机树向目标方向生长，根据||q_goal-q_nearest||的大小调整引力系数k；通过步长ρ₁和ρ₂控制扩展速度，当没有遇到障碍物时取ρ₁＜ρ₂，当遇到障碍物时取ρ₁＞ρ₂。

优选的，所述节点拒绝策略具体如下：

在采样过程选择一个节点之后，在节点扩展前使用节点剔除策略，如果公式||q-q_start||+||q_goal-q||＞σ_best成立，即如果当前的最低成本低于直接从初始节点到该节点以及从该节点到目标节点的总成本，则剔除此节点；其中q_start和q_goal为起始节点和目标点，q为候选节点，而σ_best为当前最低路径成本。

优选的，所述改进的CTB-RRT*算法的运动规划过程包括：

首先以起始点和目标点构建两棵随机数T_a、T_b，然后在无障碍自由空间C_free中，通过柯西采样函数进行采样得到随机节点q_rand，搜索随机树T_a中所有生成的节点，得到最接近q_rand的节点q_nearest，通过改进的CTB-RRT*算法节点扩展公式产生新节点q_new，当机械臂处于新节点时，使用节点拒绝策略，判断从初始节点到该节点以及从该节点到目标节点的总成本是否高于最低成本，如果是，则剔除该节点，重新采样；如果不是，则在随机树中找到以随机节点q_rand为中心的球形区域的临近节点集Q_near，通过排序列表过程对填充的集合Q_near进行排序，返回按成本函数升序排列的列表L_s，通过选择最优父节点过程遍历排序后的列表，返回无障碍自由空间中连接q_start和q_rand的最优父节点q_min∈Q_near，如果存在，将q_rand的最优父节点q_min插入随机树中，然后执行重布线过程，找到随机树T_b中距离q_new最近的节点q_connect，然后执行连接过程以连接两棵随机树；如果尝试建立连接成功，则Connect函数将返回连接q_rand和q_goal的无碰撞路径σ_new，将此无碰撞路径σ_new的成本与先前计算的路径成本σ_best进行比较，如果σ_new的成本小于σ_best的成本，则σ_best将被σ_new覆盖，如果σ_new的成本大于σ_best的成本，则σ_best仍为原值，最后交换随机树，在下一个迭代中对另一棵机树执行相同的过程。

优选的，所述排序列表过程包括：用GetSortedList函数构造一个元组，并将该元组作为列表L_s返回无障碍自由空间，该列表的每个元素都是形式为(q′，c(σ′)，σ′)∈L_s的三元组，其中节点q′为临近节点集Q_near中的元素，σ′为节点q′对应的最短无碰撞路径，c(σ′)为路径σ′所对应的路径成本函数，列表L_s按成本函数c(σ′)的升序排序。

优选的，所述重布线过程首先检查位于以q_rand为球心的球形区域内部的每个节点q′∈Q_near，如果通过q_rand连接q_start和q′的路径成本小于到达q′的现有成本，并且该路径位于无障碍空间C_free中，则删除q′原先的父节点，并将之改为q_rand。如果条件不成立，则不对随机树进行任何更改，算法将迭代地检查下一个节点。

优选的，所述步骤S5包括：上位机控制机械臂按照步骤S4的无碰撞路径运动，并实时反馈当前位置信息判断是否到达目标位姿，若没有到达则继续运动，直至运动到目标位置为止。

与现有技术相比，本发明的有益效果包括：

针对传统RRT算法采样随机、扩展缓慢以及消耗内存大的问题，本方法在以下三个方面进行了改进：(1)在采样过程中，以柯西分布采样取代随机均匀采样，降低采样的盲目性；(2)在随机树扩展过程中，引入目标引力，并动态调节随机生长方向和目标方向的步长，提高局部搜索速度；(3)在采样过程及扩展过程之间引入节点拒绝策略，减少不必要的采样节点，提高路径搜索效率。

附图说明

图1是本实施例基于CTB-RRT*算法的采摘机械臂运动规划方法的流程图；

图2是本实施例机械臂采摘系统；

图3是本实施例机械臂关节结构图；

图4a是本实施例CTB-RRT*算法计算临近节点扩展图；

图4b是本实施例CTB-RRT*算法产生新节点扩展图；

图4c是本实施例CTB-RRT*算法选择最优父节点扩展图；

图4d是本实施例CTB-RRT*算法重布线过程扩展图；

图4e是本实施例CTB-RRT*算法随机树连接扩展图；

图4f是本实施例CTB-RRT*算法生成最终路径扩展图；

其中，1-移动机构，2-上位机，3-末端执行器，4-机械臂，5-深度相机，6-果篮。

具体实施方式

下面结合具体实施例对本发明作进一步的详细描述，但本发明的实施方式不限于此。

如图1所示，本实施例的基于CTB-RRT*算法的采摘机械臂运动规划方法，首先搭建机器人采摘硬件装置和软件控制系统，通过视觉识别目标果实的采摘位置，通过机械臂逆运动学求解得到机械臂末端的位置和姿态；视觉系统获取周围环境的障碍物大小和位置信息，运动规划系统使用改进的CTB-RRT*算法规划出一条无碰撞的避障路径，将避障路径发送给上位机，上位机按照给定的路径控制机械臂运动，并判断当前是否到达目标位姿，若没有到达则继续运动，直至运动到目标位姿为止。所述方法具体包括以下步骤：

S1、搭建采摘硬件装置和软件控制系统，如图2所示，所述采摘硬件装置包括移动机构1、上位机2、末端执行器3、机械臂4、深度相机5和果篮6，所述软件控制系统包括视觉系统、运动规划系统和机械臂操作系统。通过视觉系统识别目标果实，获得采摘点空间三维坐标，通过三维建模建立周围障碍物模型，确定障碍物的位置、大小信息。根据起始点和周围环境规划出一条无碰撞路径，将路径节点发送至机械臂操作系统中，控制机械臂沿着规划的路径远动。

S2、通过逆运动学求解获得目标位置所对应机械臂末端的位置和姿态；机械臂DH参数如表1所示，各参数的定义如表2所示，机械臂关节结构如图3所示，根据机械关节结构及相应参数计算机械臂的末端位姿矩阵，通过运动学逆解得到机械臂末端的位置和姿态，判断所求逆解是否存在，若存在，则设定为目标位姿，若不存在，则继续求解直至求出可达的目标位姿为止。

表1机械臂DH参数

连杆i	a<sub>i-1</sub>(°)	α<sub>i-1</sub>(°)	d<sub>i</sub>	θ<sub>i</sub>关节变量处置(°)
					1	0	0	157	0
2	0	-90	119	-90
					3	266	180	0	90
4	256.5	180	0	0
					5	0	-90	102.5	90
6	0	90	104	0

表2机械臂DH参数的定义

S3、融合RRT-connect的双向贪婪搜索和RRT*的渐进最优性，并引入柯西采样法、目标引力与动态步长法和节点拒绝策略构造改进的CTB-RRT*算法，通过深度相机获取周围环境障碍物信息，建立三维障碍物模型，使用改进后的算法进行运动规划，得到一条从初始位姿到目标位姿的无碰撞路径；

柯西采样法具体如下：根据柯西分布的规律改变采样的特性，设置采样概率密度函数为：

其中，l表示采样点与障碍物的距离，γ为尺度参数，l越小，函数值越大，l越大，函数值越小，即越靠近障碍物，采样密度函数越大，分布的采样点越多，越远离障碍物，采样密度越小，分布的采样点也就越小。为了避免与障碍物碰撞；

设置碰撞函数：

其中，ε为采样点与障碍物的最小距离，当l小于ε，机器人与障碍物发生碰撞，collision(l)＝0；l大于ε时，机器人与障碍物未发生碰撞，collison(l)＝1，采样概分布率函数为

通过设置尺度参数γ和最小距离ε的参数值，改进尺度参数来改变搜索的随机特性；

目标引力与动态步长法具体如下：

原始RRT节点扩展公式为：

式中，q_rand为随机节点，q_nearest为随机树中与q_rand最近的节点，ρ为步长，改进后的CTB-RRT*算法引入目标引力，其引力势能为：

式中k为引力系数，q_goal为目标点，得到q_goal对q_nearest的目标引力为：

F＝k||q_goal-q_nearest||

改进的CTB-RRT*算法节点扩展公式为：

式中，ρ₁为随机扩展方向的步长，ρ₂为目标扩展方向的步长，在q_rand和q_goal的联合影响下，随机树扩展得到新节点q_new，通过引力系数k控制生长方向，k减小，随机树向随机点方向扩展，k增大，随机树向目标方向生长，根据||q_goal-q_nearest||的大小来调整引力系数k；通过步长ρ₁和ρ₂控制扩展速度，步长越大，扩展速度越快，但局部路径的长度也会随之变长，当机械臂陷入局部路径时，其与障碍物碰撞的可能也随之增加，综合考虑上述因素，为了提高扩展速度，当没有遇到障碍物时取ρ₁＜ρ₂，当遇到障碍物时取ρ₁＞ρ₂。

节点拒绝策略具体如下：

在采样过程选择一个节点之后，在节点扩展前使用节点剔除策略，如果公式||q-q_start||+||q_goal-q||＞σ_best成立，即如果当前的最低成本低于直接从初始节点到该节点以及从该节点到目标节点的总成本，则剔除此节点；其中q_start和q_goal为起始节点和目标点，q为候选节点，而σ_best为当前最低路径成本。

CTB-RRT*算法扩展原理如图4a、图4b、图4c、图4e和图4f所示：首先以起始点和目标点构建两棵随机数T_a、T_b，然后在无障碍空间C_free中，通过柯西采样函数(CaucySample)进行采样得到随机节点q_rand，搜索随机树T_a中所有生成的节点，得到最接近q_rand的节点q_nearest，通过改进的CTB-RRT*算法节点扩展公式(目标引力与动态步长函数，AttractiveForce)产生新节点q_new，当机械臂处于新节点时，使用节点拒绝策略(NodeReject)，判断从初始节点到该节点以及从该节点到目标节点的总成本是否高于最低成本，如果是，则剔除该节点，重新采样。如果不是，则在随机树中找到以q_rand为中心的球形区域的邻近节点Q_near，然后通过排序列表过程(GetSortedList)对填充的集合Q_near进行排序，返回按成本函数升序排列的列表L_s。通过选择最优父节点(ChooseBestParent)过程遍历排序后的列表，返回无障碍空间中连接q_start和q_rand的最优父节点q_min∈Q_near，如果存在，将q_rand的最优父节点q_min插入随机树中，然后执行重布线过程(RewireVertices)。找到随机树T_b中距离q_new最近的节点q_connect，然后执行连接过程以连接两棵随机树。如果尝试建立连接成功，则Connect函数将返回连接q_start和q_goal的无碰撞路径σ_new，将此σ_new的成本与先前计算的路径成本σ_best进行比较，如果σ_new的成本小于σ_best的成本，则σ_best将被σ_new覆盖。最后交换随机树，在下一个迭代中对另一棵机树执行相同的过程。

综上，改进的的CTB-RRT*算法步骤如下：

S31.柯西采样函数(CaucySample)：采样概率分布函数为：

其中l表示采样点与障碍物的距离，γ为尺度参数，通过改进尺度参数来改变搜索的随机特性。

S32.目标引力与动态步长函数(AttractiveForce)：根据节点扩展公式

其中，ρ₁为随机扩展方向的步长，ρ₂为目标扩展方向的步长，k为引力系数，通过引力系数k控制生长方向，通过步长ρ₁和ρ₂控制扩展速度；

S33.节点拒绝策略(NodeReject)：通过节点拒绝公式||q-q_start||+||q_goal-q||＞σ_best判断是否对采样节点执行后续扩展过程，即如果当前的最低成本低于直接从初始节点到该节点以及从该节点到目标节点的总成本，则剔除此节点，重新采样。

S34.排序列表过程(GetSortedList):GetSortedList函数构造一个元组，并将其作为列表L_s返回，该列表的每个元素都是形式为(q′，c(σ′)，σ′)∈L_s的三元组，其中q′∈Q_near，列表L_s按成本函数的升序排序。

S35.选择最优父节点(ChooseBestParent)：此过程用于在列表L_s中搜索从初始状态q_start到随机节点q_rand的最短无碰撞路径σ′所对应的的最优父节点q_min∈L_s。

S36.重布线(RewireVertices)：首先检查位于以q_rand为球心的球形区域内部的每个节点q′∈Q_near，如果通过q_rand连接q_start和q′的路径成本小于到达q′的现有成本，并且该路径位于无障碍空间C_free中，则删除q′原先的父节点，并将之改为q_rand。如果条件不成立，则不对随机树进行任何更改，算法将迭代地检查下一个节点。

S4、上位机控制机械臂沿着规划的路径运动直至到达目标位置。将步骤S3规划出的一系列路径节点发送给上位机，然后上位机控制机械臂按照给定的路径运动，并实时反馈当前位置信息判断是否到达目标位姿，若没有到达则继续运动，直至运动到目标位姿为止。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，包括以下步骤：

S1、通过视觉系统确定采摘目标位置和障碍物信息；

S2、利用逆运动学求解获得目标位置所对应机械臂末端的位置和姿态；

S3、通过视觉系统获取周围环境障碍物信息，建立三维障碍物模型；

S4、使用改进的CTB-RRT*算法对三维障碍物模型进行运动规划，得到从初始位姿到目标位姿的无碰撞路径；

S5、上位机控制机械臂沿着无碰撞路径运动直至到达目标位置。

2.根据权利要求1所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述改进的CTB-RRT*算法融合RRT-connect的双向贪婪搜索和RRT*的渐进最优性，并引入柯西采样法、目标引力与动态步长法和节点拒绝策略。

3.根据权利要求2所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述柯西采样法包括：

根据柯西分布的规律改变采样的特性，设置采样概率密度函数为：

其中，l表示采样点与障碍物的距离，γ为尺度参数；设置碰撞函数为：

其中，ε为采样点与障碍物的最小距离，当l小于ε时，机器人与障碍物发生碰撞，collision(l)＝0；当l大于ε时，机器人与障碍物未发生碰撞，collison(l)＝1，采样概率密度函数

通过设置尺度参数γ和最小距离ε的参数值，改进尺度参数来改变搜索的随机特性。

4.根据权利要求3所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述柯西采样法的采样过程首先通过柯西采样函数RandSample()获得第一采样点q₁，然后通过采样概率密度函数P(l,γ)获得第二采样点q₂，如果第一采样点q₁在无障碍自由空间C_free中，第二采样点q₂不在自由空间C_free中，则采样点q_rand赋值为第一采样点q₁，否则采样点q_rand赋值为第二采样点q₂。

5.根据权利要求4所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述目标引力与动态步长法具体如下：

建立原始RRT节点扩展公式：

式中，q_new为新节点，q_rand为随机节点，q_nearest为随机树中与随机节点q_rand最近的节点，ρ为步长；

改进的CTB-RRT*算法的引力势能为：

式中，k为引力系数，q_goal为目标点，得到q_goal对q_nearest的目标引力为：

F＝k||q_goal-q_nearest||

改进的CTB-RRT*算法节点扩展公式为：

式中，ρ₁为随机扩展方向的步长，ρ₂为目标扩展方向的步长，在q_rand和q_goal的联合影响下，随机树扩展得到新节点q_new，通过引力系数k控制生长方向，k减小，随机树向随机点方向扩展，k增大，随机树向目标方向生长，根据||q_goal-q_nearest||的大小调整引力系数k；通过步长ρ₁和ρ₂控制扩展速度，当没有遇到障碍物时取ρ₁＜ρ₂，当遇到障碍物时取ρ₁＞ρ₂。

6.根据权利要求5所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述节点拒绝策略具体如下：

在采样过程选择一个节点之后，在节点扩展前使用节点剔除策略，如果公式||q-q_start||+||q_goal-q||＞σ_best成立，即如果当前的最低成本低于直接从初始节点到该节点以及从该节点到目标节点的总成本，则剔除此节点；其中q_start和q_goal为起始节点和目标点，q为候选节点，而σ_best为当前最低路径成本。

7.根据权利要求6所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述改进的CTB-RRT*算法的运动规划过程包括：

首先以起始点和目标点构建两棵随机数T_a、T_b，然后在无障碍自由空间C_free中，通过柯西采样函数进行采样得到随机节点q_rand，搜索随机树T_a中所有生成的节点，得到最接近q_rand的节点q_nearest，通过改进的CTB-RRT*算法节点扩展公式产生新节点q_new，当机械臂处于新节点时，使用节点拒绝策略，判断从初始节点到该节点以及从该节点到目标节点的总成本是否高于最低成本，如果是，则剔除该节点，重新采样；如果不是，则在随机树中找到以随机节点q_rand为中心的球形区域的临近节点集Q_near，通过排序列表过程对填充的集合Q_near进行排序，返回按成本函数升序排列的列表L_s，通过选择最优父节点过程遍历排序后的列表，返回无障碍自由空间中连接q_start和q_rand的最优父节点q_min∈Q_near，如果存在，将q_rand的最优父节点q_min插入随机树中，然后执行重布线过程，找到随机树T_b中距离q_new最近的节点q_connect，然后执行连接过程以连接两棵随机树；如果尝试建立连接成功，则Connect函数将返回连接q_rand和q_goal的无碰撞路径σ_new，将此无碰撞路径σ_new的成本与先前计算的路径成本σ_best进行比较，如果σ_new的成本小于σ_best的成本，则σ_best将被σ_new覆盖，如果σ_new的成本大于σ_best的成本，则σ_best仍为原值，最后交换随机树，在下一个迭代中对另一棵机树执行相同的过程。

8.根据权利要求7所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述排序列表过程包括：用GetSortedList函数构造一个元组，并将该元组作为列表L_s返回无障碍自由空间，该列表的每个元素都是形式为(q′，c(σ′)，σ′)∈L_s的三元组，其中节点q′为临近节点集Q_near中的元素，σ′为节点q′对应的最短无碰撞路径，c(σ′)为路径σ′所对应的路径成本函数，列表L_s按成本函数c(σ′)的升序排序。

9.根据权利要求8所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述重布线过程首先检查位于以q_rand为球心的球形区域内部的每个节点q′∈Q_near，如果通过q_rand连接q_start和q′的路径成本小于到达q′的现有成本，并且该路径位于无障碍空间C_free中，则删除q′原先的父节点，并将之改为q_rand；如果条件不成立，则不对随机树进行任何更改，算法将迭代地检查下一个节点。

10.根据权利要求9所述的基于CTB-RRT*算法的采摘机械臂运动规划方法，其特征在于，所述步骤S5包括：上位机控制机械臂按照步骤S4的无碰撞路径运动，并实时反馈当前位置信息判断是否到达目标位姿，若没有到达则继续运动，直至运动到目标位置为止。

Images