机械手路径规划方法及装置
技术领域
本发明涉及机械手技术领域,特别涉及机械手路径规划方法及装置。
背景技术
机械手是一种能模仿人手和臂的某些动作功能,用以按固定程序抓取、搬运物件或操作工具的自动操作装置。通过操纵机械手沿设定路径移动来代替人的重复性劳动,以实现生产的机械化和自动化。目前机械手的移动路径为分段的,路径主要由折线段组成,因此机械手在沿移动路径移动时,其移动轨迹常有折弯、拐弯,变相降低了移动速度,同时也增加了关节、机械结构的磨损。
发明内容
(一)发明目的
为克服上述现有技术存在的至少一种缺陷,增加机械手的移动效率,减少关节、机械结构的磨损,本发明提供了以下技术方案。
(二)技术方案
作为本发明的第一方面,本发明公开了机械手路径规划方法,包括:
获取机械手的第一路径,所述第一路径包括相连的至少两条折线段;
在所述第一路径上选取N个数据点,所述N个数据点包含在所述至少两条折线段中选取的M个数据点,N≥M≥2;
基于所述N个数据点,通过B样条算法对所述第一路径的部分或全部线段进行拟合,得到B样条曲线以及包含所述B样条曲线的第二路径。
在一种可能的实施方式中,所述第一路径由多条相连的折线段组成。
在一种可能的实施方式中,N=M,且通过所述B样条算法对所述第一路径的全部线段进行拟合,得到B样条曲线。
在一种可能的实施方式中,在所述第一路径的各条线段上选取数据点的方式包括以下至少一种:
选取线段端点作为数据点;
选取线段上靠近端点的点作为数据点;
选取线段中点作为数据点;
选取线段上靠近中点的点作为数据点。
在一种可能的实施方式中,在驱动所述机械手以所述第二路径移动的过程中,在所述第一路径变动的情况下,获取所述第二路径中的剩余路径,基于所述剩余路径以及所述第一路径的变动部分生成第三路径,在所述第三路径上选取数据点并进行拟合,得到包含B样条曲线的第四路径,并驱动所述机械手以所述第四路径移动。
在一种可能的实施方式中,所述获取所述第二路径中的剩余路径包括:
实时获取所述机械手的绝对坐标和/或所述机械手相对于设定原点的相对坐标;
基于所述绝对坐标和/或相对坐标获取所述第二路径中的剩余路径。
在一种可能的实施方式中,在所述基于所述剩余路径以及所述第一路径的变动部分生成第三路径包括:
获取所述机械手当前的位置坐标及运动参数;
基于所述位置坐标预测所述机械手在沿所述第二路径移动设定时间之后所到达的预测坐标;
将所述预测坐标与所述第一路径变动部分连接,形成第三路径。
在一种可能的实施方式中,所述将所述预测坐标与所述第一路径变动部分连接为:
将所述预测坐标与所述第一路径变动部分中最近线段的内部点连接;或,
将所述预测坐标与所述第一路径变动部分中非最近线段的内部点连接。
作为本发明的第二方面,本发明公开了机械手路径规划装置,包括:
第一路径获取模块,用于获取机械手的第一路径,所述第一路径包括相连的至少两条折线段;
数据点选取模块,用于在所述第一路径中选取N个数据点,所述N个数据点包含在所述至少两条折线段中选取的M个数据点,N≥M≥2;
第二路径生成模块,用于基于所述N个数据点,通过B样条算法对所述第一路径的部分或全部线段进行拟合,得到B样条曲线以及包含所述B样条曲线的第二路径;
驱动模块,用于驱动所述机械手以所述第二路径移动。
在一种可能的实施方式中,所述数据点选取模块在所述第一路径的各条线段上选取数据点的方式包括以下一种或多种:
选取线段端点作为数据点;
选取线段上靠近端点的点作为数据点;
选取线段中点作为数据点;
选取线段上靠近中点的点作为数据点。
在一种可能的实施方式中,所述装置还包括:
剩余路径获取模块,用于在所述驱动所述机械手以所述第二路径移动的过程中,在所述第一路径变动的情况下,获取所述第二路径中的剩余路径;
第三路径生成模块,用于基于所述剩余路径以及所述第一路径的变动部分生成第三路径;
第四路径生成模块,用于使所述数据点选取模块对所述第三路径生成模块生成的第三路径进行数据点选取,并使所述第二路径生成模块对选取的数据点进行拟合,得到包含B样条曲线的第四路径,并使所述驱动模块驱动所述机械手以所述第四路径移动。
在一种可能的实施方式中,所述第三路径生成模块包括:
当前坐标获取单元,用于获取所述机械手当前的位置坐标及运动参数;
预测坐标获取单元,用于基于所述位置坐标及运动参数预测所述机械手在沿所述第二路径移动设定时间之后所到达的预测坐标;
第三路径生成单元,用于将所述预测坐标与所述第一路径变动部分连接,形成第三路径。
(三)有益效果
本发明提供的机械手路径规划方法及装置,将原先的折线式的路径拟合为曲线,增加机械手的移动效率,并使机械手的移动轨迹平滑化、速度处处连续,以使机械手的移动动作具有柔顺型,减少关节、机械结构的磨损。
附图说明
以下参考附图描述的实施例是示例性的,旨在用于解释和说明本发明,而不能理解为对本发明的保护范围的限制。
图1是本发明提供的机械手路径规划方法第一实施例的流程示意图。
图2、图4、图6是只包含折线段的相同第一路径下不同的数据点选取得到不同第二路径的路径图。
图3、图5、图7是包含折线段和曲线段的相同第一路径下不同的数据点选取得到不同第二路径的路径图。
图8是机械手在移动过程中改变路径轨迹得到第四路径的路径图。
图9是本发明提供的机械手路径规划装置第二实施例的结构框图。
具体实施方式
为使本发明实施的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行更加详细的描述。
需要说明的是:在附图中,自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。所描述的实施例是本发明一部分实施例,而不是全部的实施例,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
在本文中,“第一”、“第二”等仅用于彼此的区分,而非表示它们的重要程度及顺序等。
本文中的模块、单元或组件的划分仅仅是一种逻辑功能的划分,在实际实现时可以有其他的划分方式,例如多个模块和/或单元可以结合或集成于另一个系统中。作为分离部件说明的模块、单元、组件在物理上可以是分开的,也可以是不分开的。作为单元显示的部件可以是物理单元,也可以不是物理单元,即可以位于一个具体地方,也可以分布到网格单元中。因此可以根据实际需要选择其中的部分或全部的单元来实现实施例的方案。
下面参考图1-图8详细描述本发明的提供的机械手路径规划方法的第一实施例。本实施例主要应用于机械手移动路径优化,将原先的折线式的路径拟合为曲线,增加机械手的移动效率,并使机械手的移动轨迹平滑化、速度处处连续,以使机械手的移动动作具有柔顺型,减少关节、机械结构的磨损。
如图1所示,本实施例提供的机械手路径规划方法包括如下步骤:
步骤100,获取机械手的第一路径,第一路径包括相连的至少两条折线段。
第一路径为未进行路径规划之前的机械手移动路径,主要由相连的折线段组成,例如图2中实线所示的移动路径,由3条折线段组成。第一路径也可能包含曲线段,例如图3中实线所示的移动路径,由3条折线段和1条曲线段组成。
需要说明的是,第一路径可以只是机械手在一次移动过程中的部分路径,而不一定是全部路径,例如机械手需要从点A处移动到点E处,第一路径可以是AB、BC、CD、DE共4条线段组成,也可以只是BC、CD共2条线段组成。
步骤200,在第一路径上选取N个数据点,N个数据点包含在至少两条折线段中选取的M个数据点,N≥M≥2。
在第一路径上选取出多个数据点,以为之后的路径规划做提供数据源。在选取数据点时,通常需要每条线段上都要选取出至少1个数据点,但也可以根据折线段与曲线段之间的角度不在曲线段中选取,只在折线段中选取数据点,并在最终得到拟合的曲线后,将拟合的曲线与原先第一路径中的曲线段连接,形成最终路径。
需要说明的是,若第一路径的全部即为上述相连的至少两条折线段,则第一路径只由折线段组成,在折线段上选取M个数据点即是在第一路径上选取N个数据点,此时M=N。若第一路径还包括曲线段,则在上述相连的至少两条折线段中选取M个数据点,此时若也在曲线段中选取数据点,则总共在第一路径上选取N个数据点,其中曲线段上选取数据点的数量为N-M个;此时若不在曲线段中选取数据点,只在折线段中选取,则M=N。
步骤300,基于N个数据点,通过B样条算法对第一路径的部分或全部线段进行拟合,得到B样条曲线以及包含B样条曲线的第二路径。
第二路径,即样条曲线,为经过拟合后的平滑路径。
样条曲线(Spline Curves)是指给定一组控制点而得到一条曲线,曲线的大致形状由这些点予以控制。B样条曲线(B-spline Curves)是样条曲线中的一种,可以通过B样条算法(B-spline algorithm)生成,B样条算法是一种现有的算法,是构造复杂非线性曲线或曲面的重要工具,本发明中,通过B样条算法能够将直线段拟合得到B样条曲线。
B样条算法能够将直线段拟合得到样条曲线。样条曲线是指给定一组控制点而得到一条曲线,曲线的大致形状由这些点予以控制,
得到N个数据点之后,若第一路径中只包含折线段,则通过B样条算法对第一路径的全部线段进行拟合,最终得到一条作为第二路径的样条曲线。若第一路径中还包含曲线段,则在选取的数据点中包含从曲线段中选取的数据点时,通过B样条算法对第一路径的全部线段进行拟合,最终得到一条作为第二路径的样条曲线;而在选取的数据点中不包含从曲线段中选取的数据点时,只对选取有数据点的线段进行拟合,并将得到的样条曲线与第一路径中的原曲线段进行拼接,最终得到一条作为第二路径的样条曲线。
在一个实施方式中,第一路径由多条相连的折线段组成。如图2所示,图中实线为第一路径ABCD,其由折线段AB、BC、CD组成,在上述三条线段中每线段中各选取一个数据点,即线段AB的端点A、线段BC上的点E以及线段CD的端点D,此时,N=M=3,每条线段中均选取有数据点,而不是只从部分线段中选取数据点。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径AED,即图中虚线部分。
第一路径还可以由多条折线段以及至少一条曲线段组成。如图3所示,图中实线为第一路径aABCD,其由曲线段aA以及折线段AB、BC、CD组成,在上述四条线段中每条线段中各选取一个数据点,即线段aA的端点a、线段AB的端点A、线段BC上的点E以及线段CD的端点D,此时N=4>M=3,每条线段中均选取有数据点,而不是只从部分线段中选取数据点。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径aAED,即图中虚线部分。
由上述两个拟合的实施方式可看出,经拟合后第二路径的相较于原先的第一路径更为平滑顺畅,机械手的移动速度也可预料地速度处处连续,能够减少机械手各机械结构之间的磨损。
在一个实施方式中,在第一路径的各条线段上选取数据点的方式主要包括以下四种方式的一种或多种:
第一种:选取线段端点作为数据点。
第二种:选取线段上靠近端点的点作为数据点。
第三种:选取线段中点作为数据点。
第四种:选取线段上靠近中点的点作为数据点。
在对第一路径选取数据点的过程中,可以根据第一路径的形状来判断采取其中一种或两种混合的方式选取数据点。例如,第一路径为矩形,则既可以选取矩形的四个端点作为数据点,也可以选取矩形四条线段的四个中点作为数据点,还可以在矩形的四条线段中的每条线段的靠近自身两端端点处各选一个数据点,共八个数据点。再例如,第一路径呈L型,则可以选取两条线段的所有端点作为数据点,共三个数据点,也可以选取两条线段的外端端点,以及两条线段的靠近相交处的位置各选一个数据点,加上两个端点共四个数据点。
图4及图5所示为同时采用了上述第一种和第三种方式来选取数据点。图4中实线为第一路径ABCD,其由折线段AB、BC、CD组成,选取上述三条线段中的端点,即点A、点B、点C、点D作为数据点,同时线段BC的长度为其他两条线段的两倍,因此还选取线段BC的中点点E作为数据点,得到五个数据点,此时N=M=5。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径ABECD,即图中虚线部分。
图5中实线为第一路径aABCD,其由曲线段aA以及折线段AB、BC、CD组成,选取上述四条线段中的端点,即点a、点A、点B、点C、点D作为数据点,同时线段BC的长度为其他两条线段的两倍,并且考虑到线段aA的形状,因此还选取线段AB的中点点E以及线段BC的中点点F作为数据点,得到七个数据点,此时N=M=5。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径ABECD,即图中虚线部分。
图6及图7所示为同时采用了上述四种方式来选取数据点。图6中实线为第一路径ABCD,其由折线段AB、BC、CD组成,选取线段AB的端点A、线段CD的端点D作为数据点,在线段AB、线段BC上分别选取靠近端点B的点E、点F作为数据点,线段BC的长度为其他两条线段的两倍,因此还选取线段BC的中点点G作为数据点,在选取线段BC上靠近端点C的点H作为数据点,还选取线段CD中靠近中点的点I作为数据点,共得到七个数据点,此时N=M=7。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径AEFGHID,即图中虚线部分。
图7中实线为第一路径aABCD,其由曲线段aA以及折线段AB、BC、CD组成,选取线段aA的端点a、线段AB的端点A以及线段CD的端点D作为数据点,选取线段AB、线段BC上靠近端点B的点F、点G作为数据点,选取线段BC上靠近端点C的点I作为数据点,选取线段BC的中点点H作为数据点,选取线段AB上靠近中点的点E、线段CD上靠近中点的点J作为数据点,得到九个数据点,此时N=9>M=8。选出数据点后,通过B样条算法对第一路径的全部线段进行拟合,得到第二路径aAEFGHIJD,即图中虚线部分。
由图4-图7可知,数据点选取的数量和位置以及位置会根据第一路径中各线段的长度、角度、是否存在曲线有一定关系,而数据点的数量和位置也影响到最终得到的第二路径的走向。
得到第二路径之后,机械手的驱动机构就会驱动机械手末端沿第二路径移动。但在机械手沿第二路径移动的过程中,由于存在第一路径初始设定错误等情况,第一路径可能需要改正,而此时根据第一路径规划出第二路径并已驱动机械手沿第二路径。
因此为了避免机械手损坏以及使机械手正确沿路径到达目的坐标,如图8所示,在一个实施方式中,第一路径为ABCD,根据第一路径得到的第二路径为ABECD,在机械手正在沿第二路径移动,并且即将移动到点F处时,发现需要对第一路径进行修改,点D的位置应位于点D’的位置。在第一路径变动的情况下,立即获取第二路径中的剩余路径,即曲线段FCD,基于曲线段FCD以及第一路径的变动部分CD’生成第三路径,即,连接点F和点C,以及点C和点D’,得到第三路径FCD’。第三路径为机械手移动过程中,路径发生突变时,将剩余路径进行补救修正后的路径。得到第三路径后,立即在第三路径上选取数据点并进行拟合,选取数据点的方式可采用前述四种选取方式中的一种或多种,本实施例中,选取端点F和端点D’作为数据点,进行拟合后得到第四路径F D’,由图可知,当机械手在点F处改向时,从第二路径到第四路径的过渡较为平滑,无生硬转折。在机械手运动到点F处后,驱动机械手改向沿第四路径移动,即机械手若不进行改向,则其实际移动路径为ABEFCD,若进行改向,则其实际移动路径为ABEFD’。
在一个实施方式中,在第一路径有变动且需要对机械手进行改向时,获取第二路径中剩余路径的方式为:实时获取机械手的绝对坐标和/或相对坐标,基于绝对坐标和/或相对坐标获取第二路径中的剩余路径。其中,绝对坐标为机械手末端在世界坐标系下的坐标,通过机械手当前的绝对坐标以及第二路径的各组成点的坐标,可得知机械手当前沿第二路径运动到何位置,由此得到第二路径中的剩余路径。相对坐标为机械手末端相对于预先设定的原点的坐标,例如以机械手的底部中心处为原点,可通过机械手的相对坐标得到机械手的具体位置,再结合第二路径中各组成点相对于原点的坐标,即可得知机械手在第二路径上的具体位置。
在一个实施方式中,基于剩余路径以及第一路径的变动部分生成第三路径包括以下步骤:
步骤410,获取机械手当前的位置坐标及运动参数,掌握机械手当前的动向。运动参数包括移动速度、移动加速度等。
步骤420,基于位置坐标预测机械手在沿第二路径移动设定时间之后所到达的预测坐标。由于要实现机械手改向需要获取机械手的位置及参数,生成第三路径并算出第四路径,在得到第四路径之后机械手才能够改向,而上述为了改向而做的准备工作需要一定时间来执行运作,因此根据机械手的位置及运动参数,预测在上述准备工作所需花费的设定时间之后得到第四路径时,机械手大致会移动到何坐标处,该坐标处即为预测坐标,机械手从预测坐标处开始改向,第四路径也从预测坐标处作为起始,避免因为准备工作延迟或机械手运动超前导致机械手第四路径无法实施。
如图8所示,点F即为预测坐标,机械手当前处于线段EF之间的某点处,将预测坐标设置到点F处,并在机械手运动到点F处之前,运算得出第四路径,使得机械手在运动到点F处时,可以按照根据点F修正得到的第四路径移动。
步骤430,将预测坐标与第一路径变动部分连接,形成第三路径。如图8所示,变动部分为从线段CD改为线段CD’,预测坐标为点F,则将点F与线段CD的点C连接,得到第一路径变动后的第三路径FCD’。
在一个实施方式中,步骤430中将预测坐标与第一路径变动部分连接的方式可采取以下其中一种:
第一种,将预测坐标与第一路径变动部分中最近线段的内部点连接。内部点包括线段端点以及线段中其他的点。例如图8中,预测坐标F与第一路径变动部分中的距离点F最近的线段CD’的端点C连接。由于变动部分只有一条线段CD’,因此线段CD’就是最近线段。
第二种,将预测坐标与第一路径变动部分中非最近线段的内部点连接。由于变动部分可能包含有多条线段,因此在最近线段相较于其他线段较短或其他情况下,可以直接跳过最近线段甚至最近的几条线段,直接从后续的线段中选取端点或其他内部点进行连接。
下面参考图9详细描述本发明的提供的机械手路径规划装置的第二实施例。本实施例主要应用于需要优化移动路径的机械手,将原先的折线式的路径拟合为曲线,增加机械手的移动效率,并使机械手的移动轨迹平滑化、速度处处连续,以使机械手的移动动作具有柔顺型,减少关节、机械结构的磨损。
本实施例提供的机械手路径规划装置主要包括有:第一路径获取模块、数据点选取模块、第二路径生成模块以及驱动模块。
第一路径获取模块用于获取机械手的第一路径,第一路径包括相连的至少两条折线段。
数据点选取模块与第一路径获取模块电连接,用于在第一路径获取模块获取的第一路径中选取N个数据点,N个数据点包含在至少两条折线段中选取的M个数据点,N≥M≥2。
第二路径生成模块与数据点选取模块电连接,用于基于数据点选取模块选出的N个数据点,通过B样条算法对第一路径的部分或全部线段进行拟合,得到B样条曲线以及包含B样条曲线的第二路径。
驱动模块与第二路径生成模块电连接,用于驱动机械手以第二路径生成模块生成的第二路径移动。
在一个实施方式中,数据点选取模块在第一路径的各条线段上选取数据点的方式包括以下一种或多种:
1)选取线段端点作为数据点。
2)选取线段上靠近端点的点作为数据点。
3)选取线段中点作为数据点。
4)选取线段上靠近中点的点作为数据点。
在一个实施方式中,该装置还包括:剩余路径获取模块、第三路径生成模块和第四路径生成模块。
剩余路径获取模块用于在机械手沿第二路径移动的过程中,在第一路径变动的情况下,获取第二路径中的剩余路径。
第三路径生成模块与剩余路径获取模块以及数据点选取模块电连接,用于基于剩余路径获取模块获取的剩余路径以及第一路径的变动部分生成第三路径。具体的,第一路径的变动部分可以由剩余路径获取模块获取并作为第三路径生成模块生成第三路径的数据基础。
第四路径生成模块与数据点选取模块以及驱动模块电连接,用于使数据点选取模块对第三路径生成模块生成的第三路径进行数据点选取,并使第二路径生成模块对选取的数据点进行拟合,得到包含B样条曲线的第四路径,并使驱动模块驱动机械手以第四路径移动。可以理解的是,第四路径生成模块和第二路径生成模块的功能可以是同一模块负责。
图9中示出的实线部分为机械手正常规划路径的模块配合过程,虚线部分为第一路径变动时机械手改向的模块配合过程。
在一个实施方式中,剩余路径获取模块实时获取机械手的绝对坐标和/或机械手相对于设定原点的相对坐标,并基于该绝对坐标和/或相对坐标获取第二路径中的剩余路径。
在一个实施方式中,第三路径生成模块主要包括:当前坐标获取单元、预测坐标获取单元和第三路径生成单元。
当前坐标获取单元与机械手电连接,用于获取机械手当前的位置坐标及运动参数。
预测坐标获取单元与当前坐标获取单元电连接,用于基于当前坐标获取单元获取的位置坐标及运动参数预测机械手在沿第二路径移动设定时间之后所到达的预测坐标。
第三路径生成单元与预测坐标获取单元电连接,用于将预测坐标获取单元预测的预测坐标与第一路径变动部分连接,形成第三路径。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以所述权利要求的保护范围为准。