具体实施方式
下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
图1示出了本发明实施例提供的一种机械臂路径规划方法的流程示意图。该方法应用于机械臂,如图1所示,该方法包括局部路径规划,该局部路径规划包括:
步骤110、在预设速度范围内采集若干采样速度;
其中,预设速度范围是指预先设置的速度最大值和速度最小值范围,可以根据多个条件确定预设速度范围,并且,预设速度范围为机械臂末端的速度范围。例如,根据机械臂的速度限制范围、电机的加减速度限制范围等条件确定预设速度范围。
在本实施例中,在预设速度范围内采集若干采样速度,具体为:对在预设速度范围内的值进行多次采样,以得到若干个采样速度。
其中,在步骤110之前,该方法还包括:获取机械臂末端当前位置与目标位置之间的直线全局路径;若直线全局路径存在障碍物,则采用局部路径规划重新进行障碍物之后的剩余路径的规划。其中,目标位置为预先设置的位置,即机械臂在本阶段中运动的终点位置。直线全局路径为机械臂末端当前位置与目标位置之间的连接的直线,若检测到直线全局路径存在障碍物,则当遇到障碍物时调用局部路径规划,并且,在绕过障碍物之后继续使用局部路径规划,直至机械臂末端到达目标位置。
步骤120、根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径。
其中,预设时间间隔可以根据实际情况自由设置,当预设时间间隔设置足够短时,机械臂末端的运动轨迹可视为一小段一小段直线构成。在本实施例中,如图2所示,步骤120进一步包括:
步骤121、根据各采样速度和预设时间间隔,确定机械臂末端的各局部位移变化量;
步骤122、根据机械臂末端当前位置以及机械臂末端的各局部位移变化量,确定各局部路径终点;
步骤123、将机械臂末端当前位置与各局部路径终点之间的连线确定为各采样速度对应的若干局部路径。
在本实施例中,机械臂末端的速度表示为v=(vx,vy,vz),其中,vx、vy、vz分别为x、y、z三个方向的速度分量,则预设时间间隔Δt内的机械臂末端的局部位移变化量为:
假设机械臂末端当前位置为(x,y,z),则局部路径终点为:
则,从机械臂末端当前位置(x,y,z)到局部路径终点(x',y',z')之间的连线为局部路径,当采样速度不同时,得到的局部路径终点也不同,则局部路径也不同。在本实施例中,若干采样速度在预设时间间隔Δt内对应若干局部路径。
步骤130、根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价。
其中,在步骤130之前,该方法还包括:采集机械臂的运动空间内的障碍物信息,建立基于八叉树结构的三维地图;根据三维地图,判断各局部路径是否存在障碍物。
其中,可以通过传感器采集点云数据信息,基于八叉树结构将点云数据信息转换为三维地图。其中,八叉树结构是指将空间目标划分为八个象限,每一象限表示八叉树的一个节点,从八叉树根节点开始,如果该节点的三维空间实体不存在障碍物,则为白节点,如果该节点的三维空间实体存在障碍物,则为黑节点。每个节点都有一个数据描述它是否被占据,例如可以用0、1两个数表示,其中1表示黑节点,代表该节点被占据,0表示白节点,代表该节点不被占据。判断局部路径是否存在障碍物,可以通过查询每个节点的数据实现,若局部路径中存在数据为1的黑节点,则该局部路径存在障碍物,若局部路径全部均为数据为0的白节点,则该局部路径不存在障碍物。
在步骤130中,预设评价模型为预先设置的为了确定最优局部路径而设置的模型,预设评价模型包括若干个评价参数,其中,预设评价模型中的评价参数可以包括:局部路径终点到目标位置的距离、局部路径终点到直线全局路径的距离、以及机械臂末端从机械臂末端当前位置运动到局部路径终点时机械臂的各个关节的运动量总和。其中,目标位置为机械臂末端设定的最终目标位置,直线全局路径为机械臂末端当前位置与目标位置之间的直线。例如,建立预设评价模型为:T=kg·dg+kp·dp+kq·dq,其中,T为评价分数,kg、kp、kq分别为各项评价参数,dg为局部路径终点到目标位置的距离,dp为局部路径终点到直线全局路径的距离,dq为机械臂末端从机械臂末端当前位置运动到局部路径终点时机械臂的各个关节的运动量总和。其中,各项评价参数的权重可以为kg>kq>kp。
步骤140、选择评价结果最好的局部路径作为最优局部路径。
其中,在本实施例中,评价结果最好的局部路径为评价分数T最低的局部路径。机械臂运动时,在可以到达目标位置的所有路径内,一般可以选择距离短、关节运动量小且路径内无障碍物的路径作为最优路径。因此,根据预设评价模型打分后,从dg、dp和dq等方面综合评价,评分T最小的局部路径可以被确定为最优路径。
在一些实施例中,该方法还包括:迭代进行局部路径规划,直至机械臂末端运动到目标位置。其中,迭代进行局部路径规划,即,当确定一最优局部路径且机械臂末端运动到该最优局部路径终点时,以该最优局部路径终点为机械臂末端当前位置,继续执行步骤110至步骤140,确定下一最优局部路径,直至机械臂末端运动到目标位置为止。
本实施例通过在预设速度范围内采集若干采样速度,根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径,根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价,选择评价结果最好的局部路径作为最优局部路径,对机械臂末端进行局部路径规划,从而能够对机械臂的路径进行实时动态规划。
在一些实施例中,如图3所示,在步骤110之前,该方法还包括:步骤150、确定机械臂末端的预设速度范围。确定机械臂末端的预设速度范围,具体包括:根据机械臂的关节结构,确定机械臂末端的最大速度和最小速度;根据机械臂的电机的最大加速度和最大减速度,确定机械臂末端在预设时间间隔内的速度变化范围;根据三维地图,确定机械臂末端的最大安全速度,以使机械臂末端在碰到障碍物时使速度降为零;根据机械臂末端的最大速度和最小速度、机械臂末端在预设时间间隔内的速度变化范围以及最大安全速度,确定预设速度范围。
其中,机械臂的结构影响着机械臂末端的最大运动速度和最小运动速度,根据机械臂的关节结构,确定机械臂末端的最大速度和最小速度,具体实施方式可以为:获取机械臂的关节结构数据,根据机械臂的关节结构数据,通过雅克比矩阵求得机械臂末端的最大速度和最小速度为:
其中,vx_min、vy_min、vz_min分别为机械臂末端在x、y、z方向上能达到的速度最小值,vx_max、vy_max、vz_max分别为机械臂末端在x、y、z方向上能达到的速度最大值。
其中,机械臂的各个关节安装有电机,受电机的最大加速度和最大减速度限制,机械臂末端在预设时间间隔Δt内的速度变化范围为:
其中,机械臂末端当前速度为(vcx,vcy,vcz),ax、ay、az分别为机械臂末端在x、y、z方向上的最大加(减)速度。
其中,为了确保安全,在碰到障碍物之前,机械臂须以最大减速度使速度降为零,则机械臂末端的最大安全速度为:
其中,sx、sy、sz分别为机械臂末端在x、y、z方向上距离障碍物的距离,ax、ay、az分别为机械臂末端在x、y、z方向上的最大减速度。
根据上述公式(1)、(2)、(3),取交集得到预设速度范围,从而确定速度范围。
在一些其他实施例中,如图3所示,该方法还包括:步骤160、基于扫掠球包围盒对机械臂进行模型简化。为了快速准确地判断机器人是否与障碍物发生碰撞,需要对机械臂进行模型简化,其中,模型简化是用集合特性简单的包围和近似地描述机械臂的关节和末端执行器,进而得到比较规则的、可以方便测量的机械臂几何外形。在多种模型简化方法中,混合层次简化方法就是根据机器人的几何特征选用不同的扫掠球量,分为多个层次混合包围体,基于扫掠球包围盒的机器人混合层次简化模型如图4所示。
本实施例通过在预设速度范围内采集若干采样速度,根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径,根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价,选择评价结果最好的局部路径作为最优局部路径,对机械臂末端进行局部路径规划,从而能够对机械臂的路径进行实时动态规划,并且避免发生碰撞,且不会出现规划失败的现象。
图5示出了本发明实施例提供的一种机械臂路径规划方法的应用实例的流程示意图。该方法应用于如图4所示的机械臂中,如图5所示,该方法包括:
步骤210、获取机械臂末端当前位置与目标位置之间的直线全局路径;
步骤220、检测到直线全局路径存在障碍物,则基于扫掠球包围盒对所述机械臂进行模型简化;
其中,图4中的机械臂有七个自由度,为了简化模型,用三个胶囊体扫掠球包围盒对机械臂进行包围。
步骤230、确定机械臂末端的预设速度范围,并在预设速度范围内采集若干采样速度;
步骤240、根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径;
步骤250、采集机械臂的运动空间内的障碍物信息,建立基于八叉树结构的三维地图;
步骤260、根据三维地图,判断各局部路径是否存在障碍物,并去除存在障碍物的局部路径;
步骤270、根据预设评价模型,对不存在障碍物的局部路径进行评价;
步骤280、选择评价结果最好的局部路径作为最优局部路径;
步骤290、迭代进行步骤230至步骤280,直至机械臂末端运动到目标位置。
本实施例通过在预设速度范围内采集若干采样速度,根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径,根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价,选择评价结果最好的局部路径作为最优局部路径,对机械臂末端进行局部路径规划,从而能够对机械臂的路径进行实时动态规划。
图6示出了本发明实施例提供的一种机械臂路径规划装置的结构示意图。如图6所示,该装置300包括:局部路径规划模块310。其中,局部路径规划模块310包括:速度采集单元311、局部路径确定单元312、评价单元313和选择单元314。
其中,速度采集单元311用于在预设速度范围内采集若干采样速度;局部路径确定单元312用于根据若干所述采样速度和预设时间间隔,确定各所述采样速度对应的若干局部路径;评价单元313用于根据预设评价模型,对所述局部路径中不存在障碍物的局部路径进行评价;选择单元313用于选择评价结果最好的局部路径作为最优局部路径。
在一种可选的方式中,该装置300还包括:地图建立单元和判断单元。地图建立单元用于采集所述机械臂的运动空间内的障碍物信息,建立基于八叉树结构的三维地图;判断单元用于根据所述三维地图,判断各所述局部路径是否存在障碍物。
在一种可选的方式中,该装置300还包括:预设速度范围确定单元。其中,预设速度范围确定单元具体用于:根据所述机械臂的关节结构,确定所述机械臂末端的最大速度和最小速度;根据所述机械臂的电机的最大加速度和最大减速度,确定所述机械臂末端在所述预设时间间隔内的速度变化范围;根据所述三维地图,确定所述机械臂末端的最大安全速度,以使所述机械臂末端在碰到障碍物时使速度降为零;根据所述机械臂末端的最大速度和最小速度、所述机械臂末端在所述预设时间间隔内的速度变化范围以及所述最大安全速度,确定所述预设速度范围。
在一种可选的方式中,局部路径确定单元312具体用于:根据各所述采样速度和预设时间间隔,确定所述机械臂末端的各局部位移变化量;根据所述机械臂末端当前位置以及所述机械臂末端的各局部位移变化量,确定各局部路径终点;将所述机械臂末端当前位置与各所述局部路径终点之间的连线确定为各所述采样速度对应的若干局部路径。
在一种可选的方式中,该装置300还包括:模型简化单元。模型简化单元用于基于扫掠球包围盒对所述机械臂进行模型简化。
在一种可选的方式中,该装置300还包括:迭代单元。迭代单元用于迭代进行所述局部路径规划,直至所述机械臂末端运动到目标位置。
在一种可选的方式中,所述预设评价模型中的评价参数包括:局部路径终点到目标位置的距离、局部路径终点到所述直线全局路径的距离、以及机械臂末端从所述机械臂末端当前位置运动到所述局部路径终点时所述机械臂的各个关节的运动量总和。
在一种可选的方式中,该装置300还包括:全局路径单元。全局路径单元用于:获取所述机械臂末端当前位置与所示目标位置之间的直线全局路径;若所述直线全局路径存在障碍物,则采用所述局部路径规划重新进行障碍物之后的剩余路径的规划。
需要说明的是,本发明实施例提供的机械臂路径规划装置是能够执行上述机械臂路径规划方法的装置,则上述基于机械臂路径规划方法的所有实施例均适用于该装置,且均能达到相同或相似的有益效果。
本实施例通过在预设速度范围内采集若干采样速度,根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径,根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价,选择评价结果最好的局部路径作为最优局部路径,对机械臂末端进行局部路径规划,从而能够对机械臂的路径进行实时动态规划。
本发明实施例提供了一种计算机存储介质,所述存储介质中存储有至少一可执行指令,所述可执行指令使处理器执行上述任意方法实施例中的机械臂路径规划方法。
本发明实施例提供了一种计算机程序产品,所述计算机程序产品包括存储在计算机存储介质上的计算机程序,所述计算机程序包括程序指令,当所述程序指令被计算机执行时,使所述计算机执行上述任意方法实施例中的机械臂路径规划方法。
图7示出了本发明实施例提供的一种机器臂的结构示意图,本发明具体实施例并不对机械臂的具体实现做限定。
如图7所示,该机械臂可以包括:机械臂本体401,处理器(processor)402、通信接口(Communications Interface)404、存储器(memory)406、以及通信总线408。处理器402、通信接口404、存储器406和通信总线408设置于机械臂本体401上。
其中:处理器402、通信接口404、以及存储器406通过通信总线408完成相互间的通信。通信接口404,用于与其它设备比如客户端或其它服务器等的网元通信。处理器402,用于执行程序410,具体可以执行上述任意方法实施例中的机械臂路径规划方法。
具体地,程序410可以包括程序代码,该程序代码包括计算机操作指令。
处理器402可能是中央处理器CPU,或者是特定集成电路ASIC(ApplicationSpecific Integrated Circuit),或者是被配置成实施本发明实施例的一个或多个集成电路。计算设备包括的一个或多个处理器,可以是同一类型的处理器,如一个或多个CPU;也可以是不同类型的处理器,如一个或多个CPU以及一个或多个ASIC。
存储器406,用于存放程序410。存储器406可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
本实施例通过在预设速度范围内采集若干采样速度,根据若干采样速度和预设时间间隔,确定各采样速度对应的若干局部路径,根据预设评价模型,对局部路径中不存在障碍物的局部路径进行评价,选择评价结果最好的局部路径作为最优局部路径,对机械臂末端进行局部路径规划,从而能够对机械臂的路径进行实时动态规划。
在此提供的算法或显示不与任何特定计算机、虚拟系统或者其它设备固有相关。各种通用系统也可以与基于在此的示教一起使用。根据上面的描述,构造这类系统所要求的结构是显而易见的。此外,本发明实施例也不针对任何特定编程语言。应当明白,可以利用各种编程语言实现在此描述的本发明的内容,并且上面对特定语言所做的描述是为了披露本发明的最佳实施方式。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
类似地,应当理解,为了精简本发明并帮助理解各个发明方面中的一个或多个,在上面对本发明的示例性实施例的描述中,本发明实施例的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,发明方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本发明的单独实施例。
本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。
此外,本领域的技术人员能够理解,尽管在此的一些实施例包括其它实施例中所包括的某些特征而不是其它特征,但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如,在下面的权利要求书中,所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
应该注意的是上述实施例对本发明进行说明而不是对本发明进行限制,并且本领域技术人员在不脱离所附权利要求的范围的情况下可设计出替换实施例。在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。位于元件之前的单词“一”或“一个”不排除存在多个这样的元件。本发明可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。上述实施例中的步骤,除有特殊说明外,不应理解为对执行顺序的限定。