CN114700937A - 机械臂及其运动路径规划方法、控制系统、介质及机器人 - Google Patents

Abstract

本发明公开一种机械臂及其运动路径规划方法、机械臂驱动控制系统、计算机可读存储介质及机器人，该机械臂运动路径规划方法包括以下步骤：设置起始点及目标点；分别以起始点及目标点作为根节点建立随机树进行双向搜索，以获得连接起始点与目标点的初始路径；对初始路径进行路径优化处理，以得到连接起始点与目标点的最短路径。本发明解决了现有的机械臂的路径规划方法存在大量无效搜索及不能优化路径长度的问题。

Description

机械臂及其运动路径规划方法、控制系统、介质及机器人

技术领域

本发明涉及机械臂技术领域，特别涉及一种机械臂及其运动路径规划方 法、机械臂驱动控制系统、计算机可读存储介质及机器人。

背景技术

机械臂可以广泛应用于老人陪护、医疗手术协助、娱乐家居以及工业生 产等诸多领域，而机械臂的路径规划是机械臂研究内容的核心之一，路径规 划能够对确定的多自由度的机械臂，当给定起始点和目标点时，计算出满足 约束的路径，使得机械臂可以在从起始点无碰撞地移动到目标点。

目前，机械臂的路径规划方法通常是在整个空间搜索，大部分时间都耗 费在无效的搜索扩展中，难以搜索出有效路径，且不具有优化路径长度的功 能。

发明内容

本发明的主要目的是提出一种机械臂及其运动路径规划方法、机械臂驱 动控制系统、计算机可读存储介质及机器人，旨在解决现有的机械臂的路径 规划方法存在大量无效搜索及不能优化路径长度的问题。

为实现上述目的，本发明提出一种机械臂运动路径规划方法，该方法包 括：

设置起始点及目标点；

分别以起始点及目标点作为根节点建立随机树进行双向搜索，以获得连 接起始点与目标点的初始路径；

对初始路径进行路径优化处理，以得到连接起始点与目标点的最短路径。

其中，对初始路径进行路径优化处理包括：

基于初始路径进行随机采样，获得第一随机采样点；

选取初始路径中离第一随机采样点最近的第一点位，并由第一点位向第 一随机采样点移动第一预设长度，以生成第二点位；

判断初始路径上与第一点位相邻的两个点位，分别与第二点位之间是否 存在障碍物；

若与第一点位相邻的两个点位分别与第二点位之间均不存在障碍物，且 与第一点位相邻的两个点位到第二点位的路径总长度小于与第一点位相邻的 两个点位到第一点位的路径总长度，则用第二点位替换第一点位，并更新初 始路径为当前路径；

当当前路径达到预设目标时，定义当前路径为最短路径。

其中，对初始路径进行路径优化处理包括：

设定几何空间，初始路径位于几何空间内；

基于初始路径在几何空间内随机采样，获得第二随机采样点；

选取初始路径中离第二随机采样点最近的第三点位，并由第三点位向第 二随机采样点移动第二预设长度，以生成第四点位；

判断初始路径上与第三点位相邻的两个点位，分别与第四点位之间是否 存在障碍物；

若与第三点位相邻的两个点位，分别与第四点位之间均不存在障碍物， 且与第三点位相邻的两个点位到第四点位的路径总长度小于与第三点位相邻 的两个点位到第三点位的路径总长度，则用第四点位替换第三点位，并更新 初始路径为当前路径；

基于当前路径缩小几何空间，当前路径位于几何空间内；

当当前路径达到预设目标时，定义当前路径为最短路径。

其中，几何空间为椭球体、球体、立方体中的任意一种。

其中，几何空间为椭球体，椭球体与当前路径之间满足以下关系：

l_a＝||q_s-q_e||

c＝cost(V_f)

其中，q_s为起始点；q_e为目标点；q_s与q_e分别作为椭球体的两个焦点，l_a为椭球体的两个焦点之间的距离；l_b为椭球体的短轴长度；V_f为当前路径，c 为当前路径的长度。

其中，基于当前路径缩小几何空间包括：

计算当前路径的长度；

根据当前路径的长度计算并更新椭球体的短轴长度。

其中，当当前路径达到预设目标时，定义当前路径为最短路径包括：

当当前路径的长度小于或等于预设路径长度时，定义当前路径为最短路 径；或，

当随机采样次数达到预设次数时，定义当前路径为最短路径。

其中，分别以起始点及目标点作为根节点建立随机树进行双向搜索，以 获得连接起始点与目标点的初始路径具体包括：

根据起始点及目标点分别建立第一随机树和第二随机树；

基于第一随机树在机械臂的关节空间内随机采样，获得第三随机采样点；

选取第一随机树中距离第三随机采样点最近的第五点位，并由第五点位 向第三随机采样点移动第三预设长度，以生成第六点位；

判断第五点位与第六点位之间是否存在障碍物；

若第五点位与第六点位之间不存在障碍物，则将第六点位添加到第一随 机树中；

选取第二随机树中距离第六点位最近的第七点位，并由第七点位向第六 点位移动第四预设长度，以生成第八点位；

判断第七点位与第八点位之间是否存在障碍物；

若第七点位与第八点位之间不存在障碍物，则将第八点位添加到第二随 机树中；

当第一随机树与第二随机树相连时，生成初始路径。

其中，在设置起始点及目标点之前，机械臂运动路径规划方法还包括：

获取机械臂的信息与环境物体的信息，机械臂的信息包括机械臂的运动 学模型和几何结构信息，环境物体的信息包括环境物体与机械臂的相对位置 关系和几何结构信息；

根据机械臂的信息与环境物体的信息生成起始点及目标点。

本发明还提出一种机械臂驱动控制系统，所述机械臂驱动控制系统包括 存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处 理器执行所述计算机程序时实现如上所述机械臂运动路径规划方法的步骤。

本发明还提出一种计算机可读存储介质，所述计算机可读存储介质存储 有计算机程序，计算机程序被处理器执行时实现如上所述的机械臂运动路径 规划方法的步骤。

本发明还提出一种机械臂，所述机械臂包括存储器、处理器以及存储在 存储器中并可在处理器上运行的计算机程序，处理器执行所述计算机程序时 实现如上所述机械臂运动路径规划方法的步骤。

本发明还提出一种机器人，所述机器人包括存储器、处理器以及存储在 存储器中并可在处理器上运行的计算机程序，处理器执行所述计算机程序时 实现如上所述机械臂运动路径规划方法的步骤。

本发明机械臂运动路径规划方法通过设置路径规划的起始点及目标点， 并基于计算连接起始点及目标点的路径，从起始点和目标点同时生长两棵随 机树来搜索状态空间，最终规划一条无碰撞路径，再对初始路径进行路径优 化处理，以得到最短路径。本发明通过在得到初始路径后，对初始路径进行 优化，通过搜索更新新点位来更新连接起始点到目标点的路径长度，可以极 大地减小规划的路径代价，为机械臂规划出一条优化的路径。本发明解决了 现有的机械臂的路径规划方法存在大量无效搜索及不能优化路径长度的问题。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实 施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面 描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲， 在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的 附图。

图1为本发明机械臂运动路径规划方法一实施例的流程示意图；

图2为本发明机械臂运动路径规划方法步骤S30一实施例的细化流程示 意图；

图3为本发明机械臂运动路径规划方法步骤S30另一实施例的细化流程 示意图；

图4为本发明机械臂运动路径规划方法步骤S20一实施例的细化流程示 意图；

图5为本发明机械臂运动路径规划方法步骤另一实施例的流程示意图；

图6为本发明实施例方案涉及的机械臂硬件运行环境的终端结构示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步 说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例， 而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有 作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第 一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对 重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第 二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之 间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为 基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案 的结合不存在，也不在本发明要求的保护范围之内。

本发明提出一种机械臂运动路径规划方法。

本实施例中，该机械臂可以为常规意义上的机械臂，也可以是机器人， 或者其它需要进行路径规划的机械结构。机械臂一般由多个可动关节和至少 一个执行器组成，各关节可以被转动、滑动或者其它驱动方式，因此机械臂 具有多自由度，例如2自由度、4自由度、6自由度、7自由度等；具体地， 机械臂可以包括壳体、电控板等、在电控板上设置的处理器、电机驱动器、 电源等，处理器可以体现为DSP、FPGA、单片机等微处理器，或者其它可以进 行数据处理的装置。处理器为机械臂的控制中心，可以控制电机驱动器工作， 以驱动电机工作，进而驱动各关节运动，以实现机械臂从起始点移动至目标 点。然而，起始点移动至目标点中可能存在障碍物导致机械臂无法直接通过， 为此需要对机械臂的运动路径进行规划，使得机械臂可以根据规划后的路径 驱动机械臂的各关节运动，实现从起始点向目标点的无碰撞移动。

参照图5，在本发明一实施例中，机械臂运动路径规划方法包括：

步骤S01、获取机械臂的信息与环境物体的信息，机械臂的信息包括机械 臂的运动学模型和几何结构信息，环境物体的信息包括环境物体与机械臂的 相对位置关系和几何结构信息；

步骤S02、根据机械臂的信息与环境物体的信息生成路径规划的起始点及 目标点。

机械臂的几何结构信息包括连杆的相对坐标关系，以及各个臂杆的几何 尺寸，例如长、宽、高、直径等。几何结构信息和机械臂的运动学模型可以 存储在机械臂的存储器或者处理器中。机械臂的运动学模型具体可以为存储 有建立好的机械臂的正逆运动学方程。其中，正运动学可以在已知各关节类 型、各相邻关节之间的尺寸、相邻关节相对变化量的大小时，求解各个关节 在指定坐标系，如基坐标系、工具坐标系、世界坐标系等，中的位置和姿态； 逆运动学则可以在已知各关节类型、各相邻关节之间的尺寸等信息，以及各 关节在指定坐标系，如基坐标系、工具坐标系、世界坐标系等，中的位置和 姿态时，求解相邻关节相对变化量的大小。

本实施例可以根据预设的正逆运动学模型，以及输入的起始点和目标点， 确定起始点在关节空间中的参数，即起始点位置及姿态，以及确定目标点在 关节空间中的参数，即目标点位置及姿态；比如，当机械臂为N自由度时， 起始点位置及姿态和目标点位置及姿态分别可以是N维参数，每一维参数分 别对应某一个关节的变量参数。

周围环境信息可通过3D相机实时传入，以对周边环境进行图像采集并将 图像传输至3D相机的图像处理模块进行处理，找到目标点所在的图片区域即 目标点区域，以及通过坐标提取模块对障碍物坐标值进行提取，以检测环境 中所有障碍物，进而获得环境中所有障碍物的大小、位置信息。通过周围环 境信息还可以确定关节空间的搜索空间，该搜索空间用于限制各关节的变量 参数的取值范围，例如某些变量参数为关节的相对变化角度等。

参照图1及图2，在本发明一实施例中，该机械臂运动路径规划方法包括 以下步骤：

步骤S10、设定起始点及目标点；

本实施例中，路径规划的起始点指的是机械臂需要执行任务的起始位置 (或包括姿态)，同理，目标点指的是机械臂需要执行任务的目标点位置(或 包括姿态)。其中，路径规划的起始点可以设置为机械臂起始关节三维坐标 值，或预设的初始坐标。目标点可通过3D相机实时获取，或指定坐标。

步骤S20、分别以起始点及目标点作为根节点建立随机树进行双向搜索， 以获得连接起始点与目标点的初始路径；

本实施例中，首先分别以起始点和目标点作为根节点建立两棵随机树， 两棵随机树不断朝向对方交替扩展，具体为，以获取的起始点或目标点中的 其一作为根节点构建的随机树，可通过随机采样的方式在搜索空间中选择随 机采样点，循环遍历随机树找到离随机采样点最近的点位，并按照设定步长 扩展出新点位，并判断在扩展过程中是否检测到碰撞，若在扩展过程中没有 检测到碰撞，则将该新点位An添加到随机树中。同时，以获取的起始点或目 标点中的另一作为根节点建立的另一随机树则以每次生成的新点位An作为扩展方向进行扩展，同理地，循环遍历该随机树找到离新点位An最近的点位， 并按照设定步长扩展出新点位Bn，并判断在扩展过程中是否检测到碰撞，若 在扩展过程中没有检测到碰撞，则将该新点位Bn添加到随机树中，如此循环 交替扩展。若在一随机树扩展过程中检测到碰撞，则放弃当次扩展，并重新 产生随机采样点，直至搜索到无碰撞的新点位。如此，通过上述方式，使两 棵随机树不断朝向对方交替扩展，直至两随机树建立无碰撞连接，即两个新 点位的距离小于设定阈值，代表两棵随机树连接，形成初始路径。、

在一些实施例中，进行随机采样的过程中，可以利用随机采样函数在搜 索空间中随机选择一个采样点，在随机树每次的生长过程中，根据随机概率 来决定树的生长方向是目标点位还是随机点位。在进行碰撞检测时，可以利 用获得的点位将机械臂等效成球体、圆柱体等，建立圆柱体或者球体中心， 球体、圆柱体的边界是否存在遮挡，即球体、圆柱体是否与障碍物区域有交 集。或者，将新点位和随机采样点之间的连线等分成n个节点，对这n个节 点和新点位分别进行碰撞检测，若均未发生碰撞，即满足无碰撞条件，则将 根据上述方法得到的新点位加入随机树中，若发生碰撞则重新选择随机采样 点。重复上述离随机采样点最近的点位的获取和碰撞检测过程直到向随机树 中添加一个无碰撞的新点位。碰撞检测方式不唯一。

步骤S30、对初始路径进行路径优化处理，以得到连接所述起始点与所述 目标点的最短路径。

可以理解的是，搜索得到的路径是通过多个点位来连接起始点与目标点 而形成的满足无碰撞及其他约束条件的路径，也即初始路径，初始路径中包 括了多个能够实现起始点和目标点连接的点位，在此基础上，本实施例对得 到初始路径进一步优化，以得到最短路径。具体可以为，选取随机采样点， 从包括起始点和目标点为两个端点的初始路径中选取与该随机采样点最近的 点位并向该随机采样点移动一定步长，得到新点位，若新点位与原路径之间 无碰撞，且经过该新点位形成的路径相比原路径更短，则将该新点位替换距离随机采样点最近的所述点位，此时原路径即被替换为更短的路径，可以理 解为初始路径经过若干新点位的替换后，最终得到起始点与目标点之间的最 短路径。

如此，将规划出的路径进行优化后，得到最优的路径后，机械臂可以按 照规划好的路径，也即最短的路径进行运动，根据周围环境信息，可以获知 在运动过程中的当前位置信息，根据当前位置信息及目标点，即可判断机械 臂是否到达目标点，若没有到达目标点机械臂会继续运动，直至到达目标点 为止。

本发明机械臂运动路径规划方法通过设置路径规划的起始点及目标点， 并基于计算连接起始点及目标点的路径，从起始点和目标点同时生长两棵随 机树来搜索状态空间，最终规划一条无碰撞路径，再对初始路径进行路径优 化处理，以得到最短路径。本发明通过在得到初始路径后，对初始路径进行 优化，通过搜索更新新点位来更新连接起始点到目标点的路径长度，可以极 大地减小规划的路径代价，为机械臂规划出一条优化的路径。本发明解决了 现有的机械臂的路径规划方法存在大量无效搜索及不能优化路径长度的问题。

参照图2，在一实施例中，步骤S30、对初始路径进行路径优化处理，以 得到最短路径的步骤具体包括：

步骤S311、基于初始路径进行随机采样，获得第一随机采样点；

可以理解的是，初始路径包含有至少一条可以连接起始点和目标点路径。

步骤S312、选取初始路径中离第一随机采样点最近的第一点位，并由第 一点位向第一随机采样点移动第一预设长度，以生成第二点位；

本实施例中，遍历第一随机树寻找距离第一随机采样点最近的点位，即 第一点位，再从第一点位向随机采样点移动一定步长得到第二点位。

步骤S313、判断初始路径上与第一点位相邻的两个点位，分别与第二点 位之间是否存在障碍物，若与第一点位相邻的两个点位分别与第二点位之间 均不存在障碍物，且与第一点位相邻的两个点位到第二点位的路径总长度小 于与第一点位相邻的两个点位到第一点位的路径总长度，即第二点位与原路 径之间无碰撞且经过第二点位形成的路径相比原路径更短，则将第二点位作 为新点位替换初始路径中的第一点位；其中，判断上述点位是否存在障碍物 具体可以根据碰撞算法，例如OBB算法，AABB算法等等来确定。

通过搜索第一随机采样点找到第二点位，并将第二点位作为新点位替换 初始路径中的第一点位的过程中，判断第一点位相邻的两个点位到第二点位 的路径总长度是否小于与第一点位相邻的两个点位到第一点位的路径总长 度，也即将上一点位-第一点位-下一点位形成的路径长度，与上一点位-第二 点位-下一点位形成的路径长度进行比较，以确定以第二点位形成的新路径是 否更短，即更新后的当前路径相比初始路径(也可以理解为更新前的路径， 或上一当前路径)的长度是否更短；若是，则用第二点位替换初始路径中的 第一点位，并更新初始路径(或者更新前的路径)为当前路径；另外，在第 一点位向第二点位移动时，还需要满足第二点位与第一点位相邻的前后两个 点位之间无碰撞，也即不存在障碍物。由于会不断的产生新的第二点位，并 将新的第二点位替换原随机树上的第一点位，因此初始路径会被不断更新为 更短的路径，也即当前路径。

步骤S314、当当前路径达到预设目标时，定义当前路径为最短路径。

本实施例中，重复执行上述步骤S311～步骤S313，直至优化后的路径满 足预设目标，该预设目标可以为当当前路径的长度小于或等于预设路径长度 时，定义当前路径为最短路径；或，当随机采样次数达到预设次数时，定义 当前路径为最短路径。

本实施例中，以初始路径作为基础，不断的进行随机采样，得到随机采 样点，并且以初始路径上的点位(第一点位)作为基础，移动一定预设长度， 得到满足条件的第二点位，并将第二点位替换初始路径上的第一点位。如此 循环，直至达到预设目标，通过上述方式对初始路径进行优化，可以缩短经 以所述起始点及所述目标点作为根节点建立随机树进行双向搜索得到的初始 路径，通过更新初始路径的路径长度，极大地减小规划的路径代价，为机械 臂规划出一条最短路径。

参照图3，在一实施例中，步骤S30、对初始路径进行路径优化处理，以 得到最短路径的步骤具体包括：

步骤S321、设定几何空间，初始路径位于几何空间内；

步骤S322、基于初始路径在几何空间内随机采样，获得第二随机采样点；

步骤S323、选取初始路径中离第二随机采样点最近的第三点位，并由第 三点位向第二随机采样点移动第二预设长度，以生成第四点位；

步骤S324、判断初始路径上与第三点位相邻的两个点位，分别与第四点 位之间是否存在障碍物：若与第三点位相邻的两个点位，分别与第四点位之 间均不存在障碍物，且与第三点位相邻的两个点位到第四点位的路径总长度 小于与第三点位相邻的两个点位到第三点位的路径总长度，则用第四点位替 换初始路径中的第三点位，并更新初始路径为当前路径；

步骤S325、基于当前路径缩小几何空间，当前路径位于几何空间内；

步骤S326、当当前路径达到预设目标时，比如当当前路径的长度小于或 等于预设路径长度时，定义当前路径为最短路径；或，当随机采样次数达到 预设次数时，定义当前路径为最短路径。

本实施例中，设定一个几何空间，进而在该几何空间内进行搜索，该几 何空间为椭球体、球体、立方体或其他形式的几何空间中的任意一种。本实 施例以椭球体为例进行说明，在所设定的椭球体内进行随机采样，获得第二 随机采样点，选择初始路径包含的所有点位中离第二随机采样点最近的点位， 即第三点位，从第三点位向第二随机采样点移动第二预设长度得到第四点位。 其中，最初的椭球体基于起始点和目标点，即初始路径的两个端点，以及初 始路径长度而建立，因此初始路径始终位于椭球体内。

判断初始路径上与第三点位相邻的两个点位分别与第四点位之间是否存 在障碍物，若与第三点位相邻的两个点位分别与第四点位之间均不存在障碍 物，且与第三点位相邻的两个点位到第四点位的路径总长度小于与第三点位 相邻的两个点位到第三点位的路径总长度，即，即第四点位与原路径之间无 碰撞且经过第四点位形成的路径相比原路径更短，则将第四点位作为新点位 替换初始路径中的第三点位。

通过搜索更新第二随机采样点找到第四点位，并将第四点位作为新点位 替换初始路径中的第三点位的过程中，判断第三点位相邻的两个点位到第四 点位的路径总长度是否小于与第三点位相邻的两个点位到第三点位的路径总 长度，也即将上一点位-第三点位-下一点位形成的路径长度，与上一点位-第 四点位-下一点位形成的路径长度，将这两个路径进行比较，以确定第四点位 形成的路径是否更短，若是，则用第四点位替换初始路径中的第三点位，并 更新初始路径为当前路径；另外，在第三点位向第四点位移动时，还需要满 足第三点位与第四点位之间无碰撞，也即不存在障碍物。其中，判断上述点 位是否存在障碍物具体可以根据碰撞算法，例如OBB算法，AABB算法等等来 确定。

当以新点位更新当前路径时，基于原路径和起始点、目标点而建立的几 何空间则随之变化；因起始点和目标点不变，而原路径更新为更短的新路径， 则几何空间也会随之减小，因而于该集合空间内进行的随机采样的搜索空间 也随之减小，也就是说，基于随机采样的初始路径优化效率也会随之提高， 大大减少了无效搜索，减小搜索资源浪费。

在一些实施例中，几何空间为椭球体时，初始路径与几何空间之间的函 数关系为：

l_a＝||q_s-q_e||

c＝cost(V_f)

其中，l_a为椭球体的两个焦点之间的长度，即起始点与目标点之间的直线 距离；l_b为椭球体的短轴长度；q_s为路径规划的起始点；q_e为路径规划的目标 点；V_f为无碰撞的初始路径；c为当前路径的总长度，可由初始路径的花费代 价函数计算获得。可以理解的是，根据建立几何空间采用的几何体不同，初 始路径与几何空间之间的函数关系则不同。

本实施例中，通过设定搜索范围的几何空间，并以初始路径作为基础， 不断的进行随机采样，基于随机采样点和原路径搜索最近点位、基于最近点 位和设定步长获得新点位、基于新点位和原路径进行碰撞检测，以及基于新 点位更新得到更短路径、基于新路径更新几何空间参数，如何循环，直至达 到预设目标。本实施例通过上述方式对初始路径进行优化，由于几何空间适 应于当前路径所经过的三维空间，当当前路径不断更新为更短的路径时，几 何空间也随之缩小，以几何空间为界的随机采样范围也随之缩小，也就是搜 索空间随当前路径的更新而缩小，从而能够缩短经以起始点及目标点作为根 节点建立随机树进行双向搜索得到的初始路径，进而得到最短路径。本发明 通过设定搜索范围的几何空间，从而在有限的几何空间内，以初始路径为基 础，对运动路径优化，可以进一步地提高效率。

起始点和目标点之间的距离始终不变，但由于垂直于起始点和目标点的 连线的方向上的空间会随当前路径的更新而逐渐缩小，因而几何空间在这一 方向上的 在一实施例中，基于当前路径缩小几何空间包括：

计算当前路径的长度；

根据当前路径的长度计算并更新椭球体的短轴高度。

本实施例中，椭球体的短轴高度可以限定搜索空间的边界，在搜索的过 程中，起始点和目标点之间的距离始终不变，但由于垂直于起始点和目标点 的连线的方向上的空间会随当前路径的更新而逐渐缩小，即当前路径的总长 度会随搜索的次数的增加而缩短，根据上述函数关系对短轴高度进行更新时， 会使得短轴高度缩短，因而椭球体在这一方向上的空间也随之缩小，而短轴 高度的缩短，又会使得椭球体的体积缩小，也即搜索空间缩小，以椭球体的 空间为界的随机采样范围也随之缩小，也就是搜索空间随当前路径的更新而 缩小，可以减小无效的搜索次数，并且缩短搜索时间，同时还可以提高搜索 效率。

参照图4，在一实施例中，步骤S20、分别以起始点及目标点作为根节点 建立随机树进行双向搜索，以获得连接起始点与目标点的初始路径的步骤具 体包括：

S21、根据确定的路径规划的起始点及目标点分别建立第一随机树和第二 随机树；

S22、基于第一随机树在机械臂的关节空间内随机采样，获得第三随机采 样点；

S23、选取第一随机树中距离第三随机采样点最近的第五点位，并由第五 点位向第三随机采样点移动第三预设长度，以生成第六点位；

S24、判断第五点位与第六点位之间是否存在障碍物，若第五点位与第六 点位的连线之间不存在障碍物，则将第六点位添加到第一随机树中；

S25、选取第二随机树中距离第六点位最近的第七点位，并由第七点位向 第六点位移动第四预设长度，以生成第八点位；

S26、判断第七点位与第八点位之间是否存在障碍物，若第七点位与第八 点位的连线之间不存在障碍物，则将第八点位添加到第二随机树中；

S27、第一随机树与第二随机树相连时，生成初始路径。

本实施例中，采用分别从起始点和目标点构建第一随机树和第二随机树， 在搜索空间随机采样，以起始点作为第一随机树的根节点，在第一随机树上 选取随机采样点，找到距离随机采样点最近的第五点位，从第五点位到随机 采样点的方向上扩展一定的距离，也即第二预设长度，得第六点位，并对第 六点位与第五点位之间进行无碰撞测试，在第六点位与第五点位之间无碰撞 时，将第六点位添加到第一随机树上。扩展完第一棵随机树的第六点位后， 以这个新点位作为第二棵随机树扩展的方向，第二随机树朝着第六点位的方向继续扩展分支；第二随机树将目标点作为根点位，以第一随机树得到的第 六点位作为第二随机树扩展的方向，即把第六点位作为当前树的采样点，选 取第二随机树中距离第六点位最近的第七点位以上述步骤相同的方式持续扩 展新分支，由第七点位向第六点位移动第四预设长度，以生成第八点位，若 第七点位与第八点位的连线之间无碰撞，则将第八点位添加到第二随机树中， 如此重复搜索，两棵随机树不断朝向对方交替扩展，并且第一随机树尝试与 第二随机树连接，若连接则算法终止，若经过以上搜索后第一随机树与第二随机树没有连接，则在第一随机树朝着目标点的方向开始重复上述扩展过程， 直至与第二随机树连接以形成一条连接起始点和目标点的路径。其中，判断 上述点位是否存在障碍物具体可以根据碰撞算法，例如OBB算法，AABB算法 等等来确定。

在一实施例中，连接起始点与目标点的路径满足以下约束条件：

机械臂与自身无碰撞；

机械臂与周围环境无碰撞；

初始路径中的新点位在所设置的关节限位内；

初始路径中的新点位非奇异。

本实施例中，机械臂的关节限位跟奇异位置根据机械臂自身的构型进行 设置，即机械臂自身的构型决定其关节限位及奇异角度，本实施例具体可以 根据机械臂几何结构信息得到机械臂的连杆参数等进行设置。在一具体实施 例中，点位在所设置的关节限位内可以设置各个关节的物理关节限位内，例 如某一关节的物理关节限位是[-180，+180]，点位在所设置的关节限位内则 可以设置其限位为[-175,+175]，当然还可以设置为其他限位，小于物理限位 即可。同理，点位非奇异的奇异角度也可以根据物理奇异角度进行设置，例 如某一奇异角度为0时，点位非奇异则可以设置为[-0.01，+0.01]，只要包 含奇异角度即可。机械臂与周围环境无碰撞具体可以将获得的设定点将机械 臂等效成球体、圆柱体等，建立圆柱体或者球体中心，球体、圆柱体的边界 是否存在遮挡，即球体、圆柱体是否与障碍物区域有交集。或者，将新点位 和随机采样点之间的连线等分成n个节点，对这n个节点和新点位分别进行 碰撞检测，若均未发生碰撞，即满足无碰撞条件，则将新点位作为新的树节 点加入随机树中，若发生碰撞则丢弃子节点。重复上述设定点位的获取和碰 撞检测过程直到向随机树中添加一个无碰撞的新的树节点。机械臂与自身无 碰撞则可以根据存储有建立好的机械臂的正逆运动学方程，以及与周围环境 信息、关节限位、奇异角度等进行设置。本实施例还可以设置机械臂的关节 限位及奇异角度，用于约束路径规划点位，保证生成的点位满足关节限位、 非奇异等要求。在另外一些实施例中，路径规划的约束条件还可以包括：点 位非奇异下机械臂的每个臂杆的长度、关节限位下的角度等设置，还可以包 括规划目标，例如，机械臂移动时间最短或者机械臂在根据规划好的路径移 动时各个关节行程最短等。

本发明还包括一种机器人，该机器人包括存储器、处理器以及存储在存 储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现如 上机械臂运动路径规划方法的步骤。

本发明还包括一种机械臂，机械臂包括存储器、处理器以及存储在存储 器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现如上 机械臂运动路径规划方法的步骤。

参照图6，图6为本发明实施例方案涉及的机械臂硬件运行环境的结构示 意图。

本发明实施例的终端可以是PC，也可以是智能手机、平板电脑、电子书 阅读器、MP4(Moving Picture Experts Group Audio Layer IV)，动态影 像专家压缩标准音频层面3)播放器、便携计算机等具有显示功能的可移动式 终端设备。如图1，该机械臂可以包括处理器1001(例如CPU)，通信总线1002， 用户接口1003，网络接口1004，存储器1005。其中，通信总线1002用于实现 这些组件之间的连接通信；用户接口1003可以包括显示屏(Display)、输入 单元比如键盘(Keyboard)；网络接口1004可选的可以包括标准的有线接口、 无线接口(如WI-FI接口)；处理器1001可以是中央处理单元(Central ProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。 通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器1005可以是机械臂的内部存储单元，例如机械臂的硬盘或内存。 存储器1005也可以是机械臂的外部存储设备，例如机械臂上配备的插接式硬 盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡， 闪存卡(Flash Card)等。进一步地，存储器1005还可以既包括机械臂的内部 存储单元也包括外部存储设备。存储器1005用于存储计算机程序以及机械臂 所需的其他程序和数据。存储器还可以用于暂时地存储已经输出或者将要输 出的数据。

本领域技术人员可以理解，图1中示出的机械臂硬件运行环境的结构并 不构成对本发明机械臂的限定，可以包括比图示更多或更少的部件，或者组 合某些部件，或者不同的部件布置。

本发明还包括一种计算机可读存储介质，所述计算机可读存储介质存储 有计算机程序，所述计算机程序被处理器执行时实现如上所述机械臂运动路 径规划方法的步骤。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变 体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品 或者系统不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是 还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的 情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过 程、方法、物品或者系统中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述 实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通 过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的 技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体 现出来，该计算机软件产品存储在如上所述的一个存储介质(如ROM/RAM、磁 碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机， 服务器，机械臂，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述仅为本发明的可选实施例，并非因此限制本发明的专利范围， 凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构 变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范 围内。

Claims (13)

1.一种机械臂运动路径规划方法，其特征在于，包括：

设置起始点及目标点；

分别以所述起始点及所述目标点作为根节点建立随机树进行双向搜索，以获得连接所述起始点与所述目标点的初始路径；

对所述初始路径进行路径优化处理，以得到连接所述起始点与所述目标点的最短路径。

2.如权利要求1所述的机械臂运动路径规划方法，其特征在于，所述对所述初始路径进行路径优化处理包括：

基于所述初始路径进行随机采样，获得第一随机采样点；

选取所述初始路径中离所述第一随机采样点最近的第一点位，并由所述第一点位向所述第一随机采样点移动第一预设长度，以生成第二点位；

判断所述初始路径上与所述第一点位相邻的两个点位，分别与所述第二点位之间是否存在障碍物；

若与所述第一点位相邻的两个所述点位分别与所述第二点位之间均不存在障碍物，且与所述第一点位相邻的两个所述点位到所述第二点位的路径总长度小于与所述第一点位相邻的两个所述点位到所述第一点位的路径总长度，则用所述第二点位替换所述第一点位，并更新所述初始路径为当前路径；

当所述当前路径达到预设目标时，定义所述当前路径为最短路径。

3.如权利要求1所述的机械臂运动路径规划方法，其特征在于，所述对所述初始路径进行路径优化处理包括：

设定几何空间，所述初始路径位于所述几何空间内；

基于所述初始路径在所述几何空间内随机采样，获得第二随机采样点；

选取所述初始路径中离所述第二随机采样点最近的第三点位，并由所述第三点位向所述第二随机采样点移动第二预设长度，以生成第四点位；

判断所述初始路径上与所述第三点位相邻的两个点位，分别与所述第四点位之间是否存在障碍物；

若与所述第三点位相邻的两个所述点位，分别与所述第四点位之间均不存在障碍物，且与所述第三点位相邻的两个所述点位到所述第四点位的路径总长度小于与所述第三点位相邻的两个所述点位到所述第三点位的路径总长度，则用所述第四点位替换所述第三点位，并更新所述初始路径为当前路径；

基于所述当前路径缩小所述几何空间，所述当前路径位于所述几何空间内；

当所述当前路径达到预设目标时，定义所述当前路径为最短路径。

4.如权利要求3所述的机械臂运动路径规划方法，其特征在于，所述几何空间为椭球体、球体、立方体中的任意一种。

5.如权利要求4所述的机械臂运动路径规划方法，其特征在于，所述几何空间为椭球体，所述椭球体与所述当前路径之间满足以下关系：

l_a＝||q_s-q_e||

c＝cost(V_f)

其中，q_s为所述起始点；q_e为所述目标点；q_s与q_e分别作为所述椭球体的两个焦点，l_a为所述椭球体的两个所述焦点之间的距离；l_b为所述椭球体的短轴长度；V_f为所述当前路径，c为所述当前路径的长度。

6.如权利要求5所述的机械臂运动路径规划方法，其特征在于，所述基于所述当前路径缩小所述几何空间包括：

计算所述当前路径的长度；

根据所述当前路径的长度计算并更新所述椭球体的短轴长度。

7.如权利要求2或3所述的机械臂运动路径规划方法，其特征在于，当所述当前路径达到预设目标时，定义所述当前路径为最短路径包括：

当所述当前路径的长度小于或等于预设路径长度时，定义所述当前路径为最短路径；或，

当随机采样次数达到预设次数时，定义所述当前路径为最短路径。

8.如权利要求1所述的机械臂运动路径规划方法，其特征在于，所述分别以所述起始点及所述目标点作为根节点建立随机树进行双向搜索，以获得连接所述起始点与所述目标点的初始路径具体包括：

根据所述起始点及所述目标点分别建立第一随机树和第二随机树；

基于所述第一随机树在机械臂的关节空间内随机采样，获得第三随机采样点；

选取所述第一随机树中距离所述第三随机采样点最近的第五点位，并由所述第五点位向所述第三随机采样点移动第三预设长度，以生成第六点位；

判断所述第五点位与所述第六点位之间是否存在障碍物；

若所述第五点位与所述第六点位之间不存在障碍物，则将所述第六点位添加到所述第一随机树中；

选取所述第二随机树中距离所述第六点位最近的第七点位，并由所述第七点位向所述第六点位移动第四预设长度，以生成第八点位；

判断所述第七点位与所述第八点位之间是否存在障碍物；

若所述第七点位与所述第八点位之间不存在障碍物，则将所述第八点位添加到所述第二随机树中；

当所述第一随机树与第二随机树相连时，生成所述初始路径。

9.如权利要求1所述的机械臂运动路径规划方法，其特征在于，在所述设置起始点及目标点之前，所述机械臂运动路径规划方法还包括：

获取机械臂的信息与环境物体的信息，所述机械臂的信息包括所述机械臂的运动学模型和几何结构信息，所述环境物体的信息包括所述环境物体与所述机械臂的相对位置关系和几何结构信息；

根据所述机械臂的信息与所述环境物体的信息生成所述起始点及所述目标点。

10.一种机械臂驱动控制系统，其特征在于，所述机械臂驱动控制系统包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至9任意一项所述机械臂运动路径规划方法的步骤。

11.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如权利要求1至9任意一项所述的机械臂运动路径规划方法的步骤。

12.一种机械臂，其特征在于，所述机械臂包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至9任意一项所述机械臂运动路径规划方法的步骤。

13.一种机器人，其特征在于，所述机器人包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至9任意一项所述机械臂运动路径规划方法的步骤。

Images