CN114952852A - 机器人nurbs曲线速度规划方法、设备及存储介质 - Google Patents
机器人nurbs曲线速度规划方法、设备及存储介质 Download PDFInfo
- Publication number
- CN114952852A CN114952852A CN202210661093.7A CN202210661093A CN114952852A CN 114952852 A CN114952852 A CN 114952852A CN 202210661093 A CN202210661093 A CN 202210661093A CN 114952852 A CN114952852 A CN 114952852A
- Authority
- CN
- China
- Prior art keywords
- speed
- planning
- curve
- robot
- constraint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种机器人NURBS曲线速度规划方法,对加工路径进行NURBS曲线分段并得到各段参数;对应每段曲线,比较由关节速度约束得到末端限制速度对指令速度进行修正;建立时间最优速度规划问题的目标函数及约束条件;对首末段进行规划,以加工时间最短作为目标函数,约束条件包括机器人力矩约束及修正指令速度约束,求解目标函数最优解,得到首末段规划速度并与修正指令速度进行组合,得到首末段规划后速度曲线;然后对该曲线依次进行力矩及插补误差约束检查,对其中不满足约束部分重新规划至满足约束条件,处理后得到考虑插补误差约束的速度规划曲线。本发明考虑了插补误差限制,在保持加工效率的同时使理论输给控制器的信息更精确。
Description
技术领域
本发明涉及一种机器人曲线速度规划方法,特别涉及一种机器人NURBS曲线速度规划方法、设备及存储介质。
背景技术
目前,研磨、去毛刺、铣削和钻孔等材料去除过程越来越多地由具有串行运动学的工业机器人执行,因为它们具有灵活性、多功能性和成本效益。材料去除过程的轨迹一般由CAM软件生成,随着汽车外形、发动机叶片等特殊形状工件的要求增多,其轨迹逐渐由简单直线、圆弧向自由曲线发展。这些自由曲线中很多是以非均匀有理B样条(NURBS)来表示的,因为其强大的曲线造型能力,成为工业产品模型数据交换标准。
针对NURBS曲线路径跟踪的任务,需要控制***具备相应的速度规划及插补算法。数控机床方面对NURBS曲线的研究较早,西门子、发那科、三菱等企业,都研发了具有NURBS曲线速度规划及插补功能的高端数控设备。由于这类多轴机床价格昂贵、且不便于使用与维护,中小型企业使用不多。而在加工精度不高,材质较软的材料,以及大型曲面结构件的现场高效加工方面,工业机器人可以代替机床的作用,因此对NUBRS在工业机器人中研究的重要性也日益凸显,并且相关研究也越来越多。
由于在速度规划后机器人还需要通过插补点位置计算来实现连续轨迹的跟踪,并且曲线插补会产生插补误差,请参见图2至图3,根据曲线曲率以及插补误差约束对极限速度进行求解。对于材料去除任务,弓高误差是个重要的约束。极限速度多来源于弓高误差,请参见图2。弓高误差是理想轨迹与相邻插补点间连线的距离,一般用圆弧逼近办法进行计算。图2中,理想轨迹为实线,插补点间连线为长划线,弓高误差为箭头线,符号为δ。
对于直角坐标机器人或机床来说,弓高误差即插补误差,以此求解极限速度。由于关节型机器人运动的非线性,各关节精插补不论是采取直线还是PVT插补等插补方式,得到的实际轨迹为近似圆形的曲线,请参见图3,它与理想轨迹的偏差即插补误差,与弓高误差存在区别。图3中,理想轨迹为实线,实际轨迹为点划线,插补误差为箭头线,符号为Ei。所以对于关节型机器人,弓高误差不是实际插补误差。而对该类机器人进行速度规划,若仍以弓高误差对极限速度进行约束,会导致极限速度求解不准确,并降低了运动平稳性。
为了最大限度地减少加工时间,在机器人学的研究领域中,寻找各种约束条件下的时间最优路径至关重要。目前,有3种方法可以解决这个问题:间接法包括数值积分法、直接转录法包括各种凸优化方法、动态规划法。然而不论采用哪种方法进行规划,都没有考虑到运动的非线性以及插补策略,实际插补输出都会偏离理想路径。所以有必要在时间最优的速度规划中考虑插补误差。
发明内容
本发明为解决公知技术中存在的技术问题而提供在时间最优的速度规划中考虑插补误差的一种机器人NURBS曲线速度规划方法、设备及存储介质。
本发明为解决公知技术中存在的技术问题所采取的技术方案是:一种机器人NURBS曲线速度规划方法,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数;对应每段NURBS曲线,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正,得到修正的指令速度;建立时间最优速度规划问题的目标函数及约束条件;首先对首末段进行规划,以加工时间最短作为目标函数,约束条件包括机器人力矩约束及修正指令速度约束,求解目标函数最优解,得到首末段规划速度,将首末段规划速度与修正指令速度进行组合,得到首末段规划后速度曲线;然后对首末段规划后速度曲线依次进行力矩及插补误差约束检查,对首末段规划后速度曲线中不满足约束条件的部分进行重新规划,直至满足约束条件,处理后得到考虑插补误差约束的速度规划曲线。
进一步地,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数的方法包括:首先用基于辛普森弧长近似的区间二分法,对曲线弧长进行求解,并将曲线按照近似弧长拟合精度划分为了若干个子参数区间,然后在每个区间上采用三次多项式对该曲线段进行参数弧长关系的拟合,得到曲线段数、每段的端点参数以及每段弧长参数多项式的系数。
进一步地,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正的方法包括:根据机器人各关节的速度约束求解机器人的末端极限速度,具体方法为:在各段NURBS曲线的末端端点处,通过雅可比矩阵将各关节的极限速度映射到机器人末端速度,得到末端极限速度Vqjn;再对指令速度Vref按下式进行修正,得到修正后的指令速度Vref′:
Vref′=min(Vqjn,Vref);
式中:Vref为指令速度;Vqjn为末端极限速度;Vref′为修正后的指令速度;min()表示取两者中较小值。
进一步地,建立时间最优速度规划问题的目标函数及约束条件的方法包括如下步骤:
忽略摩擦力条件下,建立六自由度刚性机器人的动力学方程如下:
将NURBS曲线由参数u(0-1)表示,设关节运动时间区间为[0,T],设t∈[0,T],则关节运动路径q(u)中u为随时间t变化的变量,q(u)用q(u(t))表示;
对于已知给定的运动路径,机器人的关节速度和加速度表示如下
将式(2)和式(3)带入到式(1)中,得到一个自变量简化的方程:
其中:
Mu(u)=M(q(u))q′(u) (5)
Cu(u)=M(q(u))q″(u)+C(q(u),q′(u))q′(u) (6)
Gu(u)=G(q(u)) (7)
由式(4)可以得到力矩约束条件:
根据修正指令速度,机器人末端速度由下式表示:
则有:
dup=Norm(Vref′)/Norm(P′(u)) (10)
将运动时间区间[0,T]中的T值通过下式求解:
建立如下机器人路径跟踪的时间最优问题的目标函数及约束条件:
式中:
M(q)为质量矩阵;
G(q)为重力矩阵;
q为机器人关节角度;
τ为机器人关节力矩矩阵;
q(u)为关节运动路径;
q'(u)为关节运动路径对u的偏导;
q″(u)为关节运动路径对u的二阶偏导;
τ(u)为自变量为u的机器人关节力矩矩阵;
Mu(u)为自变量为u的质量矩阵;
Cu(u)为自变量为u的科氏和离心效应矩阵;
Gu(u)为自变量为u的重力矩阵;
M(q(u))为自变量为q(u)的质量矩阵;
C(q(u),q′(u))为自变量为q(u)和q′(u)的科氏和离心效应矩阵;G(q(u))为自变量为q(u)的重力矩阵;
τmin为关节力矩约束下限;
τmax为关节力矩约束上限;
v(u)为机器人末端速度;
P(u)为NURBS曲线描述的机器人末端坐标;
Norm(·)为对向量取模运算;
P′(u)为NURBS曲线描述的机器人末端坐标对u的偏导;
u(0)为初始点参数;
u(T)为终点参数;
进一步地,求解目标函数最优解的方法包括:采用直接转录法,引入优化变量,将目标函数及约束条件离散化,通过MATLAB求解目标函数最优解,具体方法如下:
设引入的优化变量为:
a(u)=ü (13)
优化变量之间存在如下关系:
b′(u)=2a(u) (15)
将式(13)至式(15)代入式(12),得到下式:
然后根据NURBS曲线分段点对式(12)离散化,得到下式:
式中,
k为第k个点;
a(u)为代替ü的优化变量;
b(0)为初始点的伪速度的平方;
b(1)为终点的伪速度的平方;
uk为第k个点的参数值;
Δuk为第k个点与前一点的参数差值;
Mu(uk)为自变量u取第k个点参数值时的质量矩阵;
Cu(uk)为自变量u取第k个点参数值时的科氏和离心效应矩阵;
Gu(uk)为自变量u取第k个点参数值时的重力矩阵;
bk+1为第k+1个点的优化变量b;
bk为第k个点的优化变量b;
ak为第k个点的优化变量a;
b0为初始点的伪速度的平方;
用MATLAB非线性优化函数Fmincon对式(17)进行求解即得到目标函数最优解。
进一步地,对首段进行规划的方法包括:
对正序第i个分段点进行规划,设i初始值为2;调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行首段速度规划,直到有解;
对末段进行规划的方法包括:
对倒序第i个分段点进行规划,设i初始值为2;调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行末段速度规划,直到有解。
进一步地,对首末段规划后速度曲线进行力矩约束检查的方法包括:
建立力矩约束检查的判断公式如下:
式中:
|·|为绝对值运算;
j为第j个关节;
panduank j为第k个分段点处第j个关节判断的结果,若该值小于等于0,则检查合格,否则不合格;
若第k个点处结果有小于0的,则记录此点,然后在此处进行速度规划;若所有约束都满足,则输出规划结果。
进一步地,对首末段规划后速度曲线进行力矩约束检查后对速度曲线进行重新规划,对力矩检查重规划后速度曲线进行插补误差约束检查的方法包括:
首先选用基于参数区间细分的NURBS曲线插补方法,以初始的规划速度进行插补,其中二次插补选用PVT方法,并计算插补误差;然后根据插补误差约束条件,判断是否满足约束条件,如满足,则输出结果;如不满足,则对此处按照缩小速度上限的方法进行速度规划,然后开始循环,更新待规划速度,继续进行插补,直到满足约束条件,并输出重规划结果。
本发明还提供了一种机器人NURBS曲线速度规划设备,包括存储器和处理器,所述存储器用于存储计算机程序;所述处理器,用于执行所述计算机程序并在执行所述计算机程序时实现如上述的机器人NURBS曲线速度规划方法步骤。
本发明还提供了一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,该计算机程序被处理器执行时,实现如上述的机器人NURBS曲线速度规划方法步骤。
本发明具有的优点和积极效果是:本发明提出一种考虑插补误差约束的机器人NURBS曲线速度规划方法。该算法与考虑弓高误差的算法相比,考虑了插补误差限制,在保持加工效率的同时,使理论输给控制器的信息更精确。
附图说明
图1是本发明的一种机器人NURBS曲线速度规划方法流程简图。
图2是直角坐标机器人、机床进行NURBS插补产生的弓高误差示意图。
图2中:理想轨迹为实线,插补点间连线为长划线,弓高误差为箭头线,符号δ。R表示与理想轨迹近似的圆弧的半径,S表示相邻插补点间连线的距离。
图3是关节型机器人进行NURBS插补(精插补或二次插补采取直线或PVT等方式)产生的插补误差示意图。
图3中:理想轨迹为实线,实际轨迹为点划线,插补误差为箭头线,符号Ei;S表示相邻插补点间连线的距离。
图4是NURBS曲线速度规划中首段规划的流程简图。
图5是NURBS曲线速度规划中末段规划的流程简图。。
图6是初始首末段速度规划曲线示意图。
图6中:Vref表示修改后指令速度的值,v表示速度,u表示参数。
中间水平线表示修改后的指令速度,水平线左下方长划线表示首段速度规划曲线,水平线右下方点划线表示末段速度规划曲线。
图7是对初始首末段规划后速度曲线进行力矩约束检查及重新规划流程简图。
图8是对首末段规划后速度曲线进行力矩约束检查及重新规划的示意图。
图8中:实线表示初始首末段规划后的速度曲线,实线下方曲线表示此处原速度曲线经力矩约束检查不符合,然后进行重新规划后的速度曲线。
图9是对力矩检查重规划后速度曲线进行插补误差约束检查及重新规划流程简图。
图10是插补误差计算方法示意图。
图10中:虚线上点为二次插补对应的实际机器人末端位置;实线中的点xn表示理想轨迹上的点,Ei1~Ei1为对应插补点的插补误差。
图11是对初始力矩检查重规划后速度曲线进行插补误差约束检查并重新规划的示意图。
图11中:虚线表示此处原速度曲线经插补误差检查不符合,然后进行重新规划后的速度曲线,实线表示力矩检查重规划后速度曲线。
图12是考虑插补误差约束的速度规划曲线示意图。
具体实施方式
为能进一步了解本发明的发明内容、特点及功效,兹列举以下实施例,并配合附图详细说明如下:
请参见图1至图12,一种机器人NURBS曲线速度规划方法,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数;对应每段NURBS曲线,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正,得到修正的指令速度;建立时间最优速度规划问题的目标函数及约束条件;首先对首末段进行规划,以加工时间最短作为目标函数,约束条件包括机器人力矩约束及修正指令速度约束,求解目标函数最优解,得到首末段规划速度,将首末段规划速度与修正指令速度进行组合,得到首末段规划后速度曲线;然后对首末段规划后速度曲线依次进行力矩及插补误差约束检查,对首末段规划后速度曲线中不满足约束条件的部分进行重新规划,直至满足约束条件,处理后得到考虑插补误差约束的速度规划曲线。
NURBS(Non Uniform Rational B-Spline)为非均匀有理B样条曲线,一个p次NURBS曲线的数学定义如下:
Ni,p(u)由递归数学形式表示如下:
指令速度指进给速度指令,在材料去除过程,根据加工形式、主轴转速、刀具参数等会有一个推荐的加工进给速度指令Vref。
首末段分别指整个曲线的初始部分和结束部分。
优选地,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数的方法可包括:首先可用基于辛普森弧长近似的区间二分法,对曲线弧长进行求解,并可将曲线按照近似弧长拟合精度划分为了若干个子参数区间,然后可在每个区间上采用三次多项式对该曲线段进行参数弧长关系的拟合,得到曲线段数、每段的端点参数以及每段弧长参数多项式的系数。
优选地,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正的方法可包括:可根据机器人各关节的速度约束求解机器人的末端极限速度,具体方法可为:在各段NURBS曲线的末端端点处,可通过雅可比矩阵将各关节的极限速度映射到机器人末端速度,得到末端极限速度Vqjn;可再对指令速度Vref按下式进行修正,得到修正后的指令速度Vref′:
Vref′=min(Vqjn,Vref);
式中:Vref为指令速度;Vqjn为末端极限速度;Vref′为修正后的指令速度;min()表示取两者中较小值。
优选地,建立时间最优速度规划问题的目标函数及约束条件的方法可包括如下步骤:
可在忽略摩擦力条件下,建立六自由度刚性机器人的动力学方程如下:
可将NURBS曲线由参数u(0-1)表示,可设关节运动时间区间为[0,T],设t∈[0,T],则关节运动路径q(u)中u为随时间t变化的变量,q(u)用q(u(t))表示;
对于已知给定的运动路径,机器人的关节速度和加速度可表示如下
可将式(2)和式(3)带入到式(1)中,得到一个自变量简化的方程:
其中:
Mu(u)=M(q(u))q′(u) (5)
Cu(u)=M(q(u))q″(u)+C(q(u),q′(u))q′(u) (6)
Gu(u)=G(q(u)) (7)
可由式(4)可以得到力矩约束条件:
根据修正指令速度,机器人末端速度可由下式表示:
则可有:
dup=Norm(Vref′)/Norm(P′(u)) (10)
可将运动时间区间[0,T]中的T值通过下式求解:
可建立如下机器人路径跟踪的时间最优问题的目标函数及约束条件:
式中:
M(q)为质量矩阵;
G(q)为重力矩阵;
q为机器人关节角度;
τ为机器人关节力矩矩阵;
ü为伪加速度;
q(u)为关节运动路径;
q'(u)为关节运动路径对u的偏导;
q″(u)为关节运动路径对u的二阶偏导;
τ(u)为自变量为u的机器人关节力矩矩阵;
Mu(u)为自变量为u的质量矩阵;
Cu(u)为自变量为u的科氏和离心效应矩阵;
Gu(u)为自变量为u的重力矩阵;
M(q(u))为自变量为q(u)的质量矩阵;
C(q(u),q′(u))为自变量为q(u)和q′(u)的科氏和离心效应矩阵;
G(q(u))为自变量为q(u)的重力矩阵;
τmin为关节力矩约束下限;
τmax为关节力矩约束上限;
v(u)为机器人末端速度;
P(u)为NURBS曲线描述的机器人末端坐标;
Norm(·)为对向量取模运算;
P′(u)为NURBS曲线描述的机器人末端坐标对u的偏导;
u(0)为初始点参数;
u(T)为终点参数;
优选地,求解目标函数最优解的方法可包括:可采用直接转录法,引入优化变量,可将目标函数及约束条件离散化,通过MATLAB求解目标函数最优解,具体方法如下:
可设引入的优化变量为:
a(u)=ü (13)
优化变量之间存在如下关系:
b′(u)=2a(u) (15)
可将式(13)至式(15)代入式(12),得到下式:
然后可根据NURBS曲线分段点对式(12)离散化,得到下式:
式中,
k为第k个点;
a(u)为代替ü的优化变量;
b(0)为初始点的伪速度的平方;
b(1)为终点的伪速度的平方;
uk为第k个点的参数值;
Δuk为第k个点与前一点的参数差值;
Mu(uk)为自变量u取第k个点参数值时的质量矩阵;
Cu(uk)为自变量u取第k个点参数值时的科氏和离心效应矩阵;
Gu(uk)为自变量u取第k个点参数值时的重力矩阵;
bk+1为第k+1个点的优化变量b;
bk为第k个点的优化变量b;
ak为第k个点的优化变量a;
b0为初始点的伪速度的平方;
可用MATLAB非线性优化函数Fmincon对式(17)进行求解即得到目标函数最优解。
直接转录法:求解优化控制问题时,将连续问题转化成离散问题的方法。本文公式13-17描述了该方法。
MATLAB是美国MathWorks公司出品的商业数学软件,用于数据分析、深度学习、图像处理与计算机视觉、信号处理、机器人,控制***等领域。
Fmincon函数为求解非线性多元函数最小值的MATLAB函数,对有约束优化问题进行求解。简单理解就是,我们需要求解的函数可以有多个未知量。fmincon是一种基于梯度的方法,旨在解决客观和约束函数都是连续的并且具有连续的一阶导数的问题。
优选地,对首段进行规划的方法可包括:
可对正序第i个分段点进行规划,设i初始值为2;可调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行首段速度规划,直到有解;
对末段进行规划的方法可包括:
可对倒序第i个分段点进行规划,设i初始值为2;可调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行末段速度规划,直到有解。
优选地,对首末段规划后速度曲线进行力矩约束检查的方法可包括:
可建立如下所示的力矩约束检查的判断公式:
式中:
|·|为绝对值运算;
j为第j个关节;
panduank j为第k个分段点处第j个关节判断的结果,若该值小于等于0,则检查合格,否则不合格;
若第k个点处结果有小于0的,则记录此点,然后在此处进行速度规划;若所有约束都满足,则输出规划结果。
优选地,可对首末段规划后速度曲线进行力矩约束检查后对速度曲线进行重新规划,对力矩检查重规划后速度曲线进行插补误差约束检查的方法可包括:
首先可选用基于参数区间细分的NURBS曲线插补方法,可以初始的规划速度进行插补,其中二次插补可选用PVT方法,并计算插补误差;然后根据插补误差约束条件,判断是否满足约束条件,如满足,则输出结果;如不满足,则对此处按照缩小速度上限的方法进行速度规划,然后开始循环,更新待规划速度,继续进行插补,直到满足约束条件,并输出重规划结果。
请参见图10,虚线上点为二次插补对应的实际机器人末端位置,插补时即可得到。然后取实线中的点xn(图中xn为10个),计算每个虚线上点与实线上点的长度,取最短长度为该虚线点处插补误差Eii,然后取虚线点中最大插补误差Eii为当前插补周期的插补误差Ei。实线中的点为理想轨迹上的点。
PVT插补即位置-速度-时间(Position-Velocity-Time)插补,该模式的本质是根据工控机所提供的驱动轴轨迹段上起止点的位置P和速度V,再结合该轨迹段的运动时间T,创建一光滑连续的Hermite样条曲线的路径。该样条曲线的表达式是关于时间的二阶连续可导的三次函数,既能保证驱动轴的位移和速度连续,又避免了次数过高引起的拟合精度减小的问题。在PVT模式下,轨迹段的插补周期为一常量,加速度随时间线性变化,据此,该模式下,关节空间内的驱动轴在两相邻插补点间的加加速度、加速度、速度及位置可以分别表示为:
j(t)=j0 0≤t≤T;
a(t)=j0t+a0 0≤t≤T;
给定起点和终点的位置q0、q1和速度v0、v1,可得
将j0、a0代入q(t)的表达式中,可得其矩阵形式为
本发明还提供了一种机器人NURBS曲线速度规划设备,包括存储器和处理器,所述存储器用于存储计算机程序;所述处理器,用于执行所述计算机程序并在执行所述计算机程序时实现如上述的机器人NURBS曲线速度规划方法步骤。
本发明还提供了一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,该计算机程序被处理器执行时,实现如上述的机器人NURBS曲线速度规划方法步骤。
下面以本发明的一个优选实施例来进一步说明本发明的工作流程及工作原理:
本发明是一种考虑插补误差约束的机器人NURBS曲线速度规划方法,其主要步骤如图1所示:
一、速度规划准备阶段
1.NURBS曲线参数弧长关系式求解。
首先用基于辛普森弧长近似的区间二分法,不仅可以对曲线弧长进行求解,还顺便将曲线按照近似弧长拟合精度划分为了若干个子参数区间。然后在每个区间上采用三次多项式对该曲线段进行参数弧长关系的拟合。通过上述步骤,得到了曲线段数n、每段的端点参数、每段弧长参数多项式的系数等。
2.指令速度求解。
首先求解关节速度限制下的末端极限速度。机器人每个关节有一个关节限制速度,然后在NURBS曲线n+1个端点处,通过雅可比矩阵将其映射到机器人末端速度,即得到关节速度限制下的末端极限速度Vqjn。然后进行指令速度修正。在材料去除过程,根据加工形式、主轴转速、刀具参数等会有一个推荐的指令速度Vref。比较Vqjn的最小值与Vref的大小,对推荐速度Vref进行修正,Vref大于Vqjn时,指令速度取Vqjn值,得到修正后的指令速度Vref'。
二、考虑插补误差的速度规划。
材料去除过程,修正后的指令速度需要长时间保持以保证加工效果。为了提高加工效率,需要在满足机器人关节速度、力矩以及插补误差约束下,提高加减速过程中的加速度。为了满足上述要求,速度规划阶段分成加减速段速度规划、检查力矩约束与重规划、检查插补误差与重规划三个阶段。
由于速度规划的三个阶段都用到直接转录法,下面先对直接转录法速度规划问题进行介绍。
对于六自由度刚性机器人,忽略摩擦力的动力学方程如下:
考虑到NURBS曲线可以由参数u(0-1)表示,则(1)式可以进行化简,将原始的一个关于时间的多维优化问题简化为一个二维优化问题。如果运动时间设为区间[0,T],则关节运动路径q(u)中u随着t的变化进行相应地变化,变成q(u(t))。
对于已知给定的路径,机器人的关节速度和加速度可表示为
将(2)式和(3)式带入到方程(1)式中,即可得到一个自变量简化的方程:
式中,
Mu(u)=M(q(u))q′(u) (5)
Cu(u)=M(q(u))q″(u)+C(q(u),q′(u))q′(u) (6)
Gu(u)=G(q(u)) (7)
由式(4)可以得到力矩约束条件:
式中,τmin为关节力矩约束下限;τmax为关节力矩约束上限。
则
dup=Norm(Vref′)/Norm(P′(u)) (10)
式中,v(u)为机器人末端速度;P(u)为NURBS曲线描述的机器人末端坐标;Norm(·)为对向量取模运算。
最终,机器人路径跟踪的时间最优问题可以用下式进行表述:
然后,根据直接转录法,引入优化变量,并将其离散化,即可得到MATLAB可以处理的问题。
其中引入的优化变量为:
a(u)=ü (13)
优化变量之间存在关系如下:
b′(u)=2a(u) (15)
于是,根据式(13)-(15),式(12)变为:
然后根据NURBS曲线分段点对其离散化,可得:
式中,k为第k个离散点。
公式(17)可以用MATLAB非线性优化函数Fmincon进行求解。
1.首末段速度规划
此阶段速度规划的优化目标为加工时间,约束是机器人力矩约束以及修正后的指令速度。首段速度规划步骤如图4所示,末段速度规划步骤如图5所示。
取前i个点进行规划,初始i为2。调用Fmincon对问题进行求解,若有解则输出规划结果,没有则将i加1,然后循环进行速度规划,直到有解。末段速度规划的步骤与首段类似,不同之处在于其取点是从后往前进行的。
首末段速度规划结果如图6所示。
2.检查力矩约束与重规划
将步骤1得到的首末段规划速度与修改后的指令速度进行组合,得到首末段规划后速度。然后对其进行检查力矩约束与重规划,具体步骤如图7所示。
力矩约束检查的判断公式如下:
式中,|·|为绝对值运算;j为第j个关节;
根据式(18),若第k个点处结果有小于0的,则记录此点,然后在此处进行速度规划。若所有约束都满足,则输出规划结果。
检查力矩约束与重规划的结果如图8所示。
3.检查插补误差与重规划
将步骤2得到的重规划速度与首末段规划后速度进行组合,得到力矩检查重规划后速度。然后对其进行检查插补误差与重规划,具体步骤如图9所示。
首先选用基于参数区间细分的NURBS曲线插补方法以待规划速度进行插补,其中二次插补选用PVT方法。然后记录插补误差,插补误差计算方法如图10所示:
然后根据插补误差约束,判断约束是否满足,如满足则输出结果。如不满足则确定不满足插补误差的点,对此处进行缩小速度上限的速度规划,然后开始循环,更新待规划速度,继续进行插补,直到约束满足,则输出重规划结果
检查插补误差与重规划的结果如图11所示。
最后,将上述规划结果进行组合,得到如图12所示的最终规划速度。
上述步骤组合在一起成为考虑插补误差约束的机器人NURBS曲线速度规划方法。
以上所述的实施例仅用于说明本发明的技术思想及特点,其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施,不能仅以本实施例来限定本发明的专利范围,即凡本发明所揭示的精神所作的同等变化或修饰,仍落在本发明的专利范围内。
Claims (10)
1.一种机器人NURBS曲线速度规划方法,其特征在于,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数;对应每段NURBS曲线,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正,得到修正的指令速度;建立时间最优速度规划问题的目标函数及约束条件;首先对首末段进行规划,以加工时间最短作为目标函数,约束条件包括机器人力矩约束及修正指令速度约束,求解目标函数最优解,得到首末段规划速度,将首末段规划速度与修正指令速度进行组合,得到首末段规划后速度曲线;然后对首末段规划后速度曲线依次进行力矩及插补误差约束检查,对首末段规划后速度曲线中不满足约束条件的部分进行重新规划,直至满足约束条件,处理后得到考虑插补误差约束的速度规划曲线。
2.根据权利要求1所述的机器人NURBS曲线速度规划方法,其特征在于,对加工路径进行NURBS曲线分段并得到各段NURBS曲线参数的方法包括:首先用基于辛普森弧长近似的区间二分法,对曲线弧长进行求解,并将曲线按照近似弧长拟合精度划分为了若干个子参数区间,然后在每个区间上采用三次多项式对该曲线段进行参数弧长关系的拟合,得到曲线段数、每段的端点参数以及每段弧长参数多项式的系数。
3.根据权利要求1所述的机器人NURBS曲线速度规划方法,其特征在于,由根据机器人关节速度约束得到末端限制速度对指令速度进行修正的方法包括:根据机器人各关节的速度约束求解机器人的末端极限速度,具体方法为:在各段NURBS曲线的末端端点处,通过雅可比矩阵将各关节的极限速度映射到机器人末端速度,得到末端极限速度Vqjn;再对指令速度Vref按下式进行修正,得到修正后的指令速度Vref′:
Vref′=min(Vqjn,Vref);
式中:Vref为指令速度;Vqjn为末端极限速度;Vref′为修正后的指令速度;min()表示取两者中较小值。
4.根据权利要求1所述的机器人NURBS曲线速度规划方法,其特征在于,建立时间最优速度规划问题的目标函数及约束条件的方法包括如下步骤:
忽略摩擦力条件下,建立六自由度刚性机器人的动力学方程如下:
将NURBS曲线由参数u(0-1)表示,设关节运动时间区间为[0,T],设t∈[0,T],则关节运动路径q(u)中u为随时间t变化的变量,q(u)用q(u(t))表示;
对于已知给定的运动路径,机器人的关节速度和加速度表示如下
将式(2)和式(3)带入到式(1)中,得到一个自变量简化的方程:
其中:
Mu(u)=M(q(u))q′(u) (5)
Cu(u)=M(q(u))q″(u)+C(q(u),q′(u))q′(u) (6)
Gu(u)=G(q(u)) (7)
由式(4)可以得到力矩约束条件:
根据修正指令速度,机器人末端速度由下式表示:
则有:
dup=Norm(Vref′)/Norm(P′(u)) (10)
将运动时间区间[0,T]中的T值通过下式求解:
建立如下机器人路径跟踪的时间最优问题的目标函数及约束条件:
式中:
M(q)为质量矩阵;
G(q)为重力矩阵;
q为机器人关节角度;
τ为机器人关节力矩矩阵;
q(u)为关节运动路径;
q'(u)为关节运动路径对u的偏导;
q″(u)为关节运动路径对u的二阶偏导;
τ(u)为自变量为u的机器人关节力矩矩阵;
Mu(u)为自变量为u的质量矩阵;
Cu(u)为自变量为u的科氏和离心效应矩阵;
Gu(u)为自变量为u的重力矩阵;
M(q(u))为自变量为q(u)的质量矩阵;
C(q(u),q′(u))为自变量为q(u)和q′(u)的科氏和离心效应矩阵;
G(q(u))为自变量为q(u)的重力矩阵;
τmin为关节力矩约束下限;
τmax为关节力矩约束上限;
v(u)为机器人末端速度;
P(u)为NURBS曲线描述的机器人末端坐标;
Norm(·)为对向量取模运算;
P′(u)为NURBS曲线描述的机器人末端坐标对u的偏导;
u(0)为初始点参数;
u(T)为终点参数;
5.根据权利要求4所述的机器人NURBS曲线速度规划方法,其特征在于,求解目标函数最优解的方法包括:采用直接转录法,引入优化变量,将目标函数及约束条件离散化,通过MATLAB求解目标函数最优解,具体方法如下:
设引入的优化变量为:
优化变量之间存在如下关系:
b′(u)=2a(u) (15)
将式(13)至式(15)代入式(12),得到下式:
然后根据NURBS曲线分段点对式(12)离散化,得到下式:
式中,
k为第k个点;
b(0)为初始点的伪速度的平方;
b(1)为终点的伪速度的平方;
uk为第k个点的参数值;
Δuk为第k个点与前一点的参数差值;
Mu(uk)为自变量u取第k个点参数值时的质量矩阵;
Cu(uk)为自变量u取第k个点参数值时的科氏和离心效应矩阵;
Gu(uk)为自变量u取第k个点参数值时的重力矩阵;
bk+1为第k+1个点的优化变量b;
bk为第k个点的优化变量b;
ak为第k个点的优化变量a;
b0为初始点的伪速度的平方;
用MATLAB非线性优化函数Fmincon对式(17)进行求解即得到目标函数最优解。
6.根据权利要求5所述的机器人NURBS曲线速度规划方法,其特征在于,对首段进行规划的方法包括:
对正序第i个分段点进行规划,设i初始值为2;调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行首段速度规划,直到有解;
对末段进行规划的方法包括:
对倒序第i个分段点进行规划,设i初始值为2;调用Fmincon对目标函数进行最优解求解,若有解则输出规划结果,若没有则将i加1,然后循环进行末段速度规划,直到有解。
8.根据权利要求5所述的机器人NURBS曲线速度规划方法,其特征在于,对首末段规划后速度曲线进行力矩约束检查后对速度曲线进行重新规划,对力矩检查重规划后速度曲线进行插补误差约束检查的方法包括:
首先选用基于参数区间细分的NURBS曲线插补方法,以初始的规划速度进行插补,其中二次插补选用PVT方法,并计算插补误差;然后根据插补误差约束条件,判断是否满足约束条件,如满足,则输出结果;如不满足,则对此处按照缩小速度上限的方法进行速度规划,然后开始循环,更新待规划速度,继续进行插补,直到满足约束条件,并输出重规划结果。
9.一种机器人NURBS曲线速度规划设备,包括存储器和处理器,其特征在于,所述存储器用于存储计算机程序;所述处理器,用于执行所述计算机程序并在执行所述计算机程序时实现如权利要求1至8中任一项所述的机器人NURBS曲线速度规划方法步骤。
10.一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,其特征在于:该计算机程序被处理器执行时,实现如权利要求1至8中任一项所述的机器人NURBS曲线速度规划方法步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210661093.7A CN114952852A (zh) | 2022-06-13 | 2022-06-13 | 机器人nurbs曲线速度规划方法、设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210661093.7A CN114952852A (zh) | 2022-06-13 | 2022-06-13 | 机器人nurbs曲线速度规划方法、设备及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114952852A true CN114952852A (zh) | 2022-08-30 |
Family
ID=82961271
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210661093.7A Pending CN114952852A (zh) | 2022-06-13 | 2022-06-13 | 机器人nurbs曲线速度规划方法、设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114952852A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116079714A (zh) * | 2022-12-02 | 2023-05-09 | 盐城工学院 | 一种基于b样条的六轴机械臂轨迹规划方法 |
-
2022
- 2022-06-13 CN CN202210661093.7A patent/CN114952852A/zh active Pending
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116079714A (zh) * | 2022-12-02 | 2023-05-09 | 盐城工学院 | 一种基于b样条的六轴机械臂轨迹规划方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11188056B2 (en) | Feedrate scheduling method for five-axis dual-spline curve interpolation | |
CN112757306B (zh) | 一种机械臂逆解多解选择和时间最优轨迹规划算法 | |
Sencer et al. | Feed optimization for five-axis CNC machine tools with drive constraints | |
CN114131611B (zh) | 机器人重力位姿分解的关节误差离线补偿方法、***及终端 | |
CN115122329B (zh) | 一种便捷的工业机器人关节空间轨迹规划方法 | |
WO2008053601A1 (fr) | Dispositif de contrôle de travail et son programme | |
CN111633668B (zh) | 一种用于机器人加工三维自由曲面的运动控制方法 | |
CN114952852A (zh) | 机器人nurbs曲线速度规划方法、设备及存储介质 | |
CN115502984A (zh) | 六自由度机器人侧铣运动规划方法及*** | |
CN113189939B (zh) | 一种用于摇篮式机床的全局最优进给速度规划方法 | |
He et al. | High-speed high-accuracy spatial curve tracking using motion primitives in industrial robots | |
CN113341876A (zh) | 一种基于微分向量优化的五轴曲面加工轨迹规划方法 | |
CN116872199A (zh) | 并联机器人动态轮廓误差预补偿方法及装置 | |
CN114670177B (zh) | 一种两转一移并联机器人姿态规划方法 | |
CN113276116B (zh) | 一种误差可控的机器人轨迹同步过渡方法 | |
Guo et al. | Near-time optimal feedrate planning for the NURBS curve considering interpolation error constraints | |
CN116529035A (zh) | 数值控制装置以及数值控制*** | |
CN113145907A (zh) | 一种基于能量最优机器人铣削加工进给方向优化方法 | |
Liu et al. | Planning and implementation of motion trajectory based on C 2 PH spline | |
Xu et al. | Global optimal trajectory planning of mobile robot grinding for high-speed railway body | |
CN114740800B (zh) | 一种参数曲线加工路径的双滑模直接轮廓控制方法 | |
Karasek et al. | Feedrate Planning for a Delta Parallel Kinematics Numerically Controlled Machine using NURBS Toolpaths. | |
Jiao et al. | Research on trajectory planning of 6-DOF cutting-robot in machining complex surface | |
CN114029956B (zh) | 一种考虑奇异点与碰撞避免的高阶光滑机器人曲面加工过程优化方法 | |
Lin et al. | Asymmetric S-curve Motion Planning for Industrial Manipulators Based on Evolutionary Computation Algorithm |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |