CN114131612B - 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 - Google Patents
基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 Download PDFInfo
- Publication number
- CN114131612B CN114131612B CN202111560509.8A CN202111560509A CN114131612B CN 114131612 B CN114131612 B CN 114131612B CN 202111560509 A CN202111560509 A CN 202111560509A CN 114131612 B CN114131612 B CN 114131612B
- Authority
- CN
- China
- Prior art keywords
- interpolation
- speed
- mechanical arm
- point
- joint
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 230000001133 acceleration Effects 0.000 claims abstract description 62
- 239000012636 effector Substances 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 claims description 14
- 239000013598 vector Substances 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 238000005457 optimization Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 125000004122 cyclic group Chemical group 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013016 damping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 229920006395 saturated elastomer Polymers 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
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
-
- 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)
- Manipulator (AREA)
Abstract
基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,涉及冗余机械臂应用领域。本发明通过给定机械臂末端目标路径点,构造NURBS参数曲线作为末端期望路径,加减速控制算法得到末端规划插补速度,在笛卡尔空间考虑了由插补带来的机械臂末端路径误差,根据弓高误差对插补速度进行限制,并引入前瞻算法,考虑机械臂关节速度、加速度约束对插补速度进行缩放,同时实现减速点的预测,采用预估校正法实现NURBS曲线插补获得末端期望轨迹,逆运动学求解关节变量,并利用冗余特性采用梯度投影法实现关节限位。本发明实现了满足机械臂运动学约束条件的实时轨迹规划,在不超出机械臂物理极限的前提下,保证了末端轨迹跟踪精度。
Description
技术领域
本发明涉及冗余机械臂应用技术领域,尤其涉及一种基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法。
背景技术
在机械臂末端执行器的工作空间内,已知一系列离散的目标路径点,工业机器人多采用直线和圆弧插补方法进行路径拟合。但是,对于复杂路径,需要大量的直线段及圆弧段,计算过程复杂且路径平滑性难以保证。
由于样条曲线具有连续性好、曲率变化均匀的特点,适用于自由构型曲线的拟合,因此,已有人采用样条曲线进行机械臂轨迹规划。但是,现有采用样条曲线实现的机械臂轨迹规划方法,存在以下技术问题:机器人控制***是高度约束的,关节具有边界位置,关节速度及加速度会受到驱动器限制。在机器人运动时,各个关节变量是直接控制的对象,应满足机械臂运动学约束条件。由于关节空间与末端执行器的工作空间并不是线性相关的,如果只在工作空间中对机械臂末端进行轨迹规划,在执行时可能会造成关节变量饱和,使机械臂末端执行器偏离期望轨迹,产生较大的跟踪误差,甚至可能导致机械臂与工作空间中的其他物体产生碰撞,最终紧急停止。
发明内容
基于此,本发明提供一种基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,以解决现有技术存在的问题。
本发明为解决技术问题所采用的技术方案如下:
本发明饿基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,包括以下步骤:
S1:根据给定的机械臂末端执行器目标路径点,反算NURBS曲线的节点矢量及控制顶点,构造NURBS曲线作为机械臂末端执行器期望路径;
S2:首先利用加减速控制算法规划匀加速段及匀速段的末端执行器期望插补速度;然后在笛卡尔空间中计算由弓高误差限制的末端执行器插补速度;最后引入前瞻算法,考虑机械臂关节速度及加速度约束对插补速度进行缩放;
S3:采用预估校正法确定插补参数,利用一阶泰勒展开式预估插补参数值,根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正,最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置;
S4:根据冗余机械臂闭环逆运动学控制算法解算关节变量,并通过梯度投影法进行优化以实现关节限位;
S5:每个采样周期均重复步骤S2至步骤S4,前瞻窗口随轨迹向前移动,直至前瞻窗口末端插补参数值为1时,认为此次迭代为减速段开始点;
S6:从预测减速点开始进入减速阶段,停止前瞻算法不再更新速度缩放因子,将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度,重复步骤S3和S4,使得机械臂平稳减速直至末端到达给定的路径终点。
进一步的,步骤S1具体由以下步骤实现:
S1.1:由插补参数u所决定的NURBS曲线参数方程如下:
其中,C(u)表示NURBS曲线参数方程,{di}表示n+1个控制顶点,{wi}表示n+1个与控制顶点相对应的权因子,Ni,p(u)表示定义在非均匀节点矢量U=[u0,u1,...,un+p+1]上的第i个p次B样条基函数,其表达式如下:
其中,Ni,p(u)的第一下标i表示序号,等于其支承区间[ui,ui+k+1]左端节点的下标,第二下标p表示样条曲线的次数;
S1.2:采用积累弦长参数化方法对给定的机械臂末端路径点进行参数化处理,并将其进行规范化得到三次NURBS曲线的节点矢量;
S1.3:令NURBS曲线的首末端点一致,令NURBS曲线的首末路径点一致,令NURBS曲线分段连接点与路径中间点一致,并在NURBS曲线两端点处补充自由端点条件,代入NURBS曲线参数方程(1),构造线性方程组,求解后得到控制顶点;
S1.4:给定权因子,根据步骤S1.2所确定的节点矢量及步骤S1.3解算的控制顶点,可唯一地确定一条经过给定的机械臂末端执行器目标路径点的三次NURBS曲线。
进一步的,步骤S2具体由以下步骤实现:
S2.1:根据加减速规划算法,由机械臂末端最大运动速度及前瞻窗口长度规划匀加速段及匀速段的机械臂末端执行器期望插补速度;
S2.2:在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度;
S2.3:根据前瞻算法,考虑机械臂关节速度及加速度极限更新速度缩放因子;
S2.4:对规划插补速度与弓高误差限制的插补速度进行比较,取小值后通过速度缩放因子进行修正,最终得到机械臂末端的最大容许期望插补速度值。
进一步的,步骤S2.1具体由以下步骤实现:
2.1.1:已知机械臂末端最大运动速度值,令加速段时长与前瞻窗口长度相同,当插补时间小于前瞻窗口长度时,得到匀加速段的运动加速度,从而得到匀加速段的插补速度;
2.1.2:到达匀速段后,规划的机械臂末端执行器插补速度等于给定的机械臂末端最大运动速度值。
进一步的,步骤S2.2具体由以下步骤实现:
S2.2.1:将NURBS曲线当前插补点与下一插补点之间的曲线段近似为圆弧,该圆弧半径等于NURBS曲线在当前插补点处的曲率半径,圆弧段起点与当前插补点重合,圆弧段终点为近似下一插补点,近似计算插补速度与弓高误差的关系;
S2.2.2:给定最大弓高误差,得到由弓高误差限制的机械臂末端执行器插补速度。
进一步的,步骤S2.3具体由以下步骤实现:
S2.3.1:由一阶泰勒展开式确定前瞻插补参数值,代入NURBS曲线参数方程(1)得到未来时刻的期望轨迹;
S2.3.2:经过逆运动学算法解算前瞻窗口起点及终点处的雅可比矩阵,计算得到期望关节速度;
S2.3.3:分别将各个关节速度极值与前瞻窗口起点及终点处期望关节速度做比并取最小值,得到前瞻窗口起点及终点处的插补速度缩放因子,取两者中较小值作为由关节速度约束得到的缩放因子;
S2.3.4:通过关节加速度极值及前一时刻的关节速度,预测当前时刻关节速度允许区间,将由关节加速度约束的关节速度极值与期望关节速度做比,取最小值作为关节加速度约束的插补速度缩放因子;
S2.3.5:分别将步骤S2.3.3及S2.3.4得到的插补速度缩放因子存入环形缓冲器并从中取出最小值,比较两个速度缩放因子,若两者中较小值小于1,则认为该缩放因子作为此次前瞻算法得到的插补速度缩放因子,否则,缩放因子取为1,不对末端执行器插补速度进行修改。
进一步的,步骤S3具体由以下步骤实现:
S3.1:在预估阶段,插补参数按照一阶泰勒展开式进行更新;
S3.2:将预估插补参数值带入NURBS曲线参数方程(1),计算得到预估插补点,利用差分法计算相应的预估插补速度;
S3.3:计算预估插补速度和期望插补速度之间的误差,判断该误差是否在期望的插补速度误差允许阈值之内,若当前的插补速度在误差允许范围之内,则认为当前的预估插补点是满足要求的;否则对预估插补点进行校正,重复步骤S3.2和S3.3直至插补速度误差满足阈值;
S3.4:最终得到满足要求的预估插补点,即为机械臂末端当前时刻的期望位置。
进一步的,步骤S6具体由以下步骤实现:
根据最后一次前瞻计算得到机械臂末端距离路径终点的路程,减速段开始点处的插补速度为前瞻插补速度,最终速度减为0,根据位移公式计算得到匀减速段的加速度及减速时间,进而得出匀减速段的规划插补速度。
进一步的,在步骤S1之后、步骤S2之前还包括以下步骤:
在步骤S2开始前,需要进行初始化处理,初始化实时轨迹规划算法中的各个初始量,初始量包括常值及随迭代而不断更新的变量:常值包括采样周期、弓高误差最大容许值、插补速度相对误差的允许阈值、机械臂末端最大运动速度值、前瞻窗口长度、关节位置、速度及加速度的最大允许值;变量包括插补参数值、速度缩放因子和迭代次数。
本发明的有益效果是:
本发明提供了一种满足冗余机械臂运动学约束的NURBS实时插补算法,主要应用于冗余机械臂实时轨迹规划。通过加减速控制算法得到末端规划插补速度,在笛卡尔空间考虑了由插补带来的机械臂末端路径误差,根据弓高误差对插补速度进行限制,并引入前瞻算法,考虑机械臂关节速度、加速度约束对插补速度进行缩放,实现满足机械臂运动学约束条件的实时轨迹规划,在不超出机械臂物理极限的前提下,保证末端轨迹跟踪精度。
与现有技术相比,本发明具有以下优点:
(1)本发明考虑了机械臂关节变量饱和对末端轨迹跟踪精度的影响,不仅在笛卡尔空间通过弓高误差约束插补速度来提高插补精度,还通过前瞻算法根据机械臂关节速度、加速度约束条件对末端插补速度进行缩放,在每个采样周期都计算末端最大容许插补速度,以避免关节变量饱和带来较大的轨迹跟踪误差。
(2)本发明同时对机械臂关节加速度与关节速度进行约束,关节速度约束不会改变末端插补速度方向,加速度约束是通过关节加速度极限获得关节速度的允许区间,进而产生末端插补速度缩放因子。
(3)本发明提出了一种合理的减速点预测方式及加减速规划算法,在前瞻算法更新机械臂末端插补速度缩放因子的过程中实现减速点的预测,自减速点开始后停止前瞻算法且不再对插补速度缩放因子进行更新,根据预测得到的机械臂末端距离路径终点的距离以及减速点开始处的插补速度,可计算得到减速段的减速时间及加速度,进而规划出减速段的插补速度,算法简单且计算效率高,适用于实时轨迹规划中的速度规划。
(4)本发明所采用的NURBS(非均匀有理B样条)曲线不仅具有贝塞尔曲线、B样条曲线的优点,还具有更一般的数学表达形式,能够拟合解析曲线,例如圆、圆锥曲线等,并且可通过修改各个控制点的权重因子实现局部修改曲线形状的功能,具有计算效率高、数值稳定性好的特点。
附图说明
图1为本发明的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法的流程图。
图2为本发明的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法步骤S2.2.1中的插补误差近似模型图。
图3为本发明的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法步骤S4中的闭环逆运动学算法框图。
具体实施方式
本发明的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,首先根据给定的机械臂末端目标路径点,构造NURBS参数曲线作为末端期望路径;然后采用预估校正法实现NURBS曲线插补获得期望轨迹;再通过闭环逆运动学控制算法解算关节变量,并利用冗余特性实现关节限位。末端执行器的期望插补速度首先是通过加减速控制算法来进行规划的;然后在笛卡尔空间根据弓高误差限制插补速度以提高插补精度;再引入前瞻算法,考虑机械臂关节速度、加速度约束对插补速度进行缩放。前瞻算法还用于预测减速点,在减速阶段,将停止前瞻算法不再更新速度缩放因子,将加减速规划算法得到的匀减速段的规划插补速度作为机械臂末端期望插补速度。本发明的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法能够满足冗余机械臂物理极限,保证了机械臂末端轨迹跟踪精度。
以下结合附图对本发明的技术方案进行详细描述。
如图1所示,本发明的一种基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,具体包括步骤S1~步骤S6。
在步骤S1中,根据给定的机械臂末端执行器目标路径点,反算NURBS曲线的节点矢量及控制顶点,构造NURBS曲线作为机械臂末端执行器期望路径。在该步骤中,NURBS曲线是由控制点、节点矢量决定的,而机械臂实际工作时是根据笛卡尔空间中给定的目标路径点(包括起始点、终点及中间点)生成机械臂末端执行器经过路径点的光滑路径,因此需要通过目标路径点反算得到节点矢量和控制顶点,进而构造通过这些目标路径点的NURBS曲线。
进一步地,步骤S1具体由以下步骤实现:
S1.1:由插补参数u所决定的NURBS曲线参数方程如下:
其中,C(u)表示NURBS曲线参数方程,{di}表示n+1个控制顶点,{wi}表示n+1个与控制顶点相对应的权因子,Ni,p(u)表示定义在非均匀节点矢量U=[u0,u1,...,un+p+1]上的第i个p次B样条基函数,其表达式如下:
其中,Ni,p(u)的第一下标i表示序号,等于其支承区间[ui,ui+k+1]左端节点的下标,第二下标p表示样条曲线的次数。
S1.2:采用积累弦长参数化方法对给定的n-1个机械臂末端执行器目标路径点Pi(i=0,1,2,...,n-2)进行参数化处理,并将其进行规范化可得到三次NURBS曲线的节点矢量U:
其中,首末节点的重复度为4,u3+i(i=0,1,...,n-2)为定义域内节点,而u0,u1,u2与un+2,un+3,un+4均为定义域外节点;
S1.3:令NURBS曲线的首末端点一致,并且令NURBS曲线的首末路径点一致可得式(4),令NURBS曲线分段连接点与路径中间点一致可得式(5),并在NURBS曲线两端点即首末端点处补充自由端点条件可得式(6),代入NURBS曲线方程(1),构造线性方程组,通过求解线性方程组,可确定n+1个控制顶点di(i=0,1,2,...,n)。
P0=d0,Pn-1=dn+k-2 (4)
C(u3+i)=Pi i=0,1,...,n-2 (5)
w0d0=w1d1,wn-1dn-1=wndn (6)
其中,P0和d0分别表示NURBS曲线的首末端点,Pn-1和d分别表示NURBS曲线的首末路径点,C(u3+i)表示NURBS曲线分段连接点;
S1.4:给定权因子,根据步骤S1.2所确定的节点矢量及步骤S1.3所解算的控制顶点,可唯一地确定一条经过给定的机械臂末端执行器目标路径点的三次NURBS曲线。
在步骤S2中,首先通过加减速控制算法来规划匀加速段及匀速段的机械臂末端执行器期望插补速度,然后在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度,最后引入前瞻算法,考虑机械臂关节速度、加速度约束对插补速度进行缩放。
在步骤S2开始前,需要进行初始化处理。初始化实时轨迹规划算法中的各个初始量,初始量包括常值及随迭代而不断更新的变量;其中,常值包括采样周期、弓高误差最大容许值、插补速度相对误差的允许阈值、机械臂末端最大运动速度值、前瞻窗口长度、关节位置、速度及加速度的最大允许值;变量包括插补参数值、速度缩放因子和迭代次数。
具体为:初始化随迭代而不断更新的变量,包括插补参数值u0=0、速度缩放因子s0=1及迭代次数k=0,并给定采样周期Ts、弓高误差最大容许值ERmax、插补速度相对误差的允许阈值ε、机械臂末端最大运动速度值vlim、前瞻窗口长度hTs,h∈N+及关节位置、速度及加速度的最大允许值qlim
进一步地,步骤S2具体由以下步骤实现:
S2.1:根据加减速规划算法,由机械臂末端最大运动速度及前瞻窗口长度规划匀加速段及匀速段的机械臂末端执行器期望插补速度,具体采用以下步骤实现:
S2.1.1:已知机械臂末端最大运动速度值vlim,令加速段时长与前瞻窗口长度相同,当插补时间t=kTs小于前瞻窗口长度时,机械臂末端执行器插补速度处于匀加速阶段,则运动加速度α1为:
经计算可得匀加速段的机械臂末端执行器插补速度Vp,k为:
Vp,k=a1kTs (8)
S2.1.2:到达匀速段后,规划的机械臂末端执行器插补速度等于给定的机械臂末端最大运动速度值vlim;即在t=hTs时刻,机械臂末端执行器插补速度恰好为指定的机械臂末端最大运动速度,可得匀速段的机械臂末端执行器插补速度Vp,k为:
Vp,k=vlim (9)
S2.2:在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度,具体采用以下步骤实现:
S2.2.1:插补误差可用弓高误差来衡量。将NURBS曲线当前插补点C(uk)与下一插补点C(uk+1)之间的曲线段近似为圆弧,近似模型如图2所示,该圆弧半径等于NURBS曲线在当前插补点处的曲率半径ρk,圆弧段起点O(uk)与当前插补点重合,圆弧段终点O(uk+1)为圆弧段上的近似下一插补点,则可近似计算插补速度与弓高误差的关系。机械臂末端执行器插补速度Vk与采样周期Ts的乘积即为近似插补步长ΔLk:
ΔLk=VkTs=|O(uk+1)-O(uk)| (10)
由勾股定理可得到近似弓高误差hk:
由此可得到机械臂末端执行器插补速度Vk与弓高误差hk之间的关系如下:
S2.2.2:给定最大弓高误差hmax,可得到由弓高误差限制的机械臂末端执行器插补速度Vc,k:
S2.3:根据前瞻算法,考虑机械臂关节速度及加速度极限更新速度缩放因子,具体采用以下步骤实现:
S2.3.1:采用一阶泰勒展开式确定前瞻插补参数值从上一时刻插补参数uk开始计算,令/>则:
其中,sk-1Vd,k+i-1为期望插补速度,sk-1为上一时刻经过前瞻算法得到的插补速度缩放因子,Vp,k+i-1为规划速度。
将迭代计算得到的前瞻插补参数值代入NURBS曲线参数方程(1),可得到未来时刻的期望轨迹。
S2.3.2:经过逆运动学算法解算雅可比矩阵eJ,并将其转换为表示在基坐标系下的雅可比矩阵bJ:
其中,bRi表示基于基坐标系的旋转变换矩阵。
令期望插补速度为末端期望线速度,根据公式(16)计算得到期望关节速度:
其中,表示期望关节速度,/>表示基于末端坐标系的雅可比伪逆矩阵,/>表示末端期望线速度。
S2.3.3:分别将各个关节速度极值与期望关节速度做比并取最小值,可得到插补速度缩放因子svel:
假设机械臂共n个关节,上标j表示第j个关节,表示第j个关节的速度极值,/>表示第j个关节的期望关节速度。如果/>则认为/>
为简化计算,只考虑前瞻窗口起点及终点处的信息,可得到前瞻窗口起点处的速度缩放因子svel,k及前瞻窗口终点处的速度缩放因子svel,k+h,取两者中较小值作为由关节速度约束得到的缩放因子svel:
svel=min{svel,k,svel,k+h} (18)
S2.3.4:通过关节加速度极值及前一时刻的关节速度,可得到由关节加速度约束的关节速度允许区间:
其中,表示前瞻窗口起点处由关节加速度约束的关节速度极值,/>表示上一时刻的关节速度,/>分别表示关节加速度的最大值和最小值,δt表示离散时间间隔。
将根据关节加速度极限确定的关节速度极值与期望关节速度做比,取最小值可得到关节加速度约束的插补速度缩放因子sacc:
如果则认为/>假设机械臂共n个关节,上标j表示第j个关节,表示第j个关节在前瞻窗口起点处得到的根据关节加速度极限确定的速度极值,/>表示第j个关节在前瞻窗口起点处的期望关节速度。
S2.3.5:分别将步骤S2.3.3及S2.3.4得到的插补速度缩放因子存入环形缓冲器,环形缓冲器中保留了之前h次前瞻算法得到的速度缩放因子,实际上,环形缓冲器是一个长度为h的循环更新数组,初始化为1,数据存满后,新存入的速度缩放因子代替最老的数据,数组序号重新开始计数,依次进行数据更新。
分别从环形缓冲器中取出最小值,比较两个速度缩放因子,若两者中较小值小于1,则认为该速度缩放因子作为此次前瞻算法得到的插补速度缩放因子sk,否则,缩放因子取为1,不对末端执行器插补速度进行修改:
sk=min{1,svel,sacc} (21)
S2.4:对规划插补速度与弓高误差限制的插补速度进行比较,取小值后通过速度缩放因子进行修正,最终得到机械臂末端的最大容许期望插补速度值:
在步骤S3中,采用预估校正法确定插补参数,利用一阶泰勒展开式预估插补参数值,根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正,最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置。在该步骤中,一阶泰勒展开法忽略高阶项并存在截断误差,曲线切向速度与期望速度之间存在一定的偏差,如果预估插补点不满足指定的插补速度误差允许阈值,校正环节将根据当前的速度反馈进行调整,使当前预估插补速度和期望插补速度之间的偏差总是在期望的插补速度误差允许阈值之内。
进一步地,步骤S3具体由以下步骤实现:
S3.1:在预估阶段,插补参数按照一阶泰勒展开式进行更新:
S3.2:将预估插补参数值带入NURBS曲线参数方程(1),计算可得预估插补点:
利用差分法计算相应的预估插补速度,则相应的预估插补速度为:
S3.3:计算当前插补速度和期望的插补速度之间的误差,判断该误差是否在期望的插补速度误差允许阈值εPCI之内:
当δk≤εPCI时,即当前的插补速度在误差允许范围之内,可认为当前的预估插补点是满足要求的,即为所求插补点Ck+1;否则按式(22)对预估插补点进行校正,重复步骤S3.2和S3.3直至插补速度误差满足阈值:
其中,ξ为校正系数。
S3.4:最终得到满足要求的预估插补点,该点即为机械臂末端当前时刻的期望位置。
在步骤S4中,根据冗余机械臂闭环逆运动学控制算法解算关节变量,并通过梯度投影法进行优化以实现关节限位。
进一步地,步骤S4具体由以下步骤实现:
对于冗余机械臂,已知末端执行器的期望轨迹xdes,可通过基于雅可比转置法的数值迭代求解关节变量,梯度投影法实现优化。逆运动学闭环控制算法流程如图3所示,控制律可表示为:
式中,J为雅可比矩阵;Kp为末端顶点的刚度矩阵;B为关节阻尼系数;Δx=x-xdes,xdes为设计的末端期望位姿,将关节角度代入正运动学方程f(q)中计算得到实际的末端位姿x;末端期望速度与雅可比伪逆/>的乘积作为前馈环节以提高***性能;/>为梯度投影算子,梯度投影系数β取负值;H为优化目标函数:
其中,上标j表示第j个关节,表示第j个关节的位置极限,qj表示第j个关节的位置。
在逆运动学求解过程中将优化目标函数H投影至雅可比矩阵零空间,驱使关节向关节许可位移的中值移动,以实现关节限位。
在步骤S5中,每个采样周期都重复步骤S2至步骤S4,前瞻窗口随轨迹向前移动,直至前瞻窗口末端插补参数值为1时,认为此次迭代得到的插补点Ck+1为减速段开始点。在该步骤中,每个采样周期都需要用前瞻算法对速度缩放因子进行更新,前瞻窗口及环形缓冲器随着机械臂末端轨迹向前移动,同时,前瞻算法还用于预测减速点,当窗口末端的前瞻插补参数值uk+h=1时,意味着在等于前瞻窗口长度的时间内,机械臂末端将会以等于前瞻插补速度的速度运动至路径终点,因此从该预测减速点处开始进行减速是合理的。
在步骤S6中,根据最后一次前瞻可计算得到机械臂末端距离路径终点的路程,减速段开始点处的插补速度为前瞻插补速度,最终速度减为0,根据位移公式可计算得到匀减速段的加速度及减速时间,进而得出匀减速段的规划插补速度。从预测减速点开始进入减速阶段,停止前瞻算法不再更新速度缩放因子,将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度,重复步骤S3和S4,使得机械臂平稳减速直至末端到达给定的路径终点。
进一步地,步骤S6具体由以下步骤实现:
当窗口末端的前瞻插补参数值uk+h=1时,前瞻插补速度为sk-1Vp,k+h,由于减速段之前为匀速段,故可认为前瞻插补速度为sk-1vlim,则在等于前瞻窗口长度的时间内,机械臂末端距离路径终点的路程为sk-1vlim·hTs;
在减速段开始点处,不再对速度缩放因子更新,则整个减速段的速度缩放因子为send=sk-1,减速段开始点处末端速度等于前瞻插补速度,最终速度减为0,匀减速段的总路程等于机械臂末端距离路径终点的路程,根据位移公式,
(sendvlim)2=2a(sk-1vlim·h) (30)
可计算匀减速段的加速度为:
匀减速段的减速时间为2h,故匀减速段的规划插补速度为:
Vp,h-i=a2(2h-i)Ts,i=1,2,...,2h (32)
由于从预测减速点开始进入减速阶段,停止前瞻算法不再对规划速度进行修正,则规划的插补速度即为期望插补速度:
重复步骤S3和S4,使得机械臂平稳减速直至末端到达给定的路径终点。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。
Claims (7)
1.基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,包括以下步骤:
S1:根据给定的机械臂末端执行器目标路径点,反算NURBS曲线的节点矢量及控制顶点,构造NURBS曲线作为机械臂末端执行器期望路径;
S2:首先利用加减速控制算法规划匀加速段及匀速段的末端执行器期望插补速度;然后在笛卡尔空间中计算由弓高误差限制的末端执行器插补速度;最后引入前瞻算法,考虑机械臂关节速度及加速度约束对插补速度进行缩放;
S2.1:根据加减速规划算法,由机械臂末端最大运动速度及前瞻窗口长度规划匀加速段及匀速段的机械臂末端执行器期望插补速度;
S2.2:在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度;
S2.3:根据前瞻算法,考虑机械臂关节速度及加速度极限更新速度缩放因子;
S2.3.1:由一阶泰勒展开式确定前瞻插补参数值,代入NURBS曲线参数方程得到未来时刻的期望轨迹;
S2.3.2:经过逆运动学算法解算前瞻窗口起点及终点处的雅可比矩阵,计算得到期望关节速度;
S2.3.3:分别将各个关节速度极值与前瞻窗口起点及终点处期望关节速度做比并取最小值,得到前瞻窗口起点及终点处的插补速度缩放因子,取两者中较小值作为由关节速度约束得到的缩放因子;
S2.3.4:通过关节加速度极值及前一时刻的关节速度,预测当前时刻关节速度允许区间,将由关节加速度约束的关节速度极值与期望关节速度做比,取最小值作为关节加速度约束的插补速度缩放因子;
S2.3.5:分别将步骤S2.3.3及S2.3.4得到的插补速度缩放因子存入环形缓冲器并从中取出最小值,比较两个速度缩放因子,若两者中较小值小于1,则认为该缩放因子作为此次前瞻算法得到的插补速度缩放因子,否则,缩放因子取为1,不对末端执行器插补速度进行修改;
S2.4:对规划插补速度与弓高误差限制的插补速度进行比较,取小值后通过速度缩放因子进行修正,最终得到机械臂末端的最大容许期望插补速度值;
S3:采用预估校正法确定插补参数,利用一阶泰勒展开式预估插补参数值,根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正,最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置;
S4:根据冗余机械臂闭环逆运动学控制算法解算关节变量,并通过梯度投影法进行优化以实现关节限位;
S5:每个采样周期均重复步骤S2至步骤S4,前瞻窗口随轨迹向前移动,直至前瞻窗口末端插补参数值为1时,认为此次迭代为减速段开始点;
S6:从预测减速点开始进入减速阶段,停止前瞻算法不再更新速度缩放因子,将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度,重复步骤S3和S4,使得机械臂平稳减速直至末端到达给定的路径终点。
2.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,步骤S1具体由以下步骤实现:
S1.1:由插补参数u所决定的NURBS曲线参数方程如下:
其中,C(u)表示NURBS曲线参数方程,{di}表示n+1个控制顶点,{wi}表示n+1个与控制顶点相对应的权因子,Ni,p(u)表示定义在非均匀节点矢量U=[u0,u1,...,un+p+1]上的第i个p次B样条基函数,其表达式如下:
其中,Ni,p(u)的第一下标i表示序号,等于其支承区间[ui,ui+k+1]左端节点的下标,第二下标p表示样条曲线的次数;
S1.2:采用积累弦长参数化方法对给定的机械臂末端路径点进行参数化处理,并将其进行规范化得到三次NURBS曲线的节点矢量;
S1.3:令NURBS曲线的首末端点一致,令NURBS曲线的首末路径点一致,令NURBS曲线分段连接点与路径中间点一致,并在NURBS曲线两端点处补充自由端点条件,代入NURBS曲线参数方程,构造线性方程组,求解后得到控制顶点;
S1.4:给定权因子,根据步骤S1.2所确定的节点矢量及步骤S1.3解算的控制顶点,可唯一地确定一条经过给定的机械臂末端执行器目标路径点的三次NURBS曲线。
3.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,步骤S2.1具体由以下步骤实现:
2.1.1:已知机械臂末端最大运动速度值,令加速段时长与前瞻窗口长度相同,当插补时间小于前瞻窗口长度时,得到匀加速段的运动加速度,从而得到匀加速段的插补速度;
2.1.2:到达匀速段后,规划的机械臂末端执行器插补速度等于给定的机械臂末端最大运动速度值。
4.根据权利要求3所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,步骤S2.2具体由以下步骤实现:
S2.2.1:将NURBS曲线当前插补点与下一插补点之间的曲线段近似为圆弧,该圆弧半径等于NURBS曲线在当前插补点处的曲率半径,圆弧段起点与当前插补点重合,圆弧段终点为近似下一插补点,近似计算插补速度与弓高误差的关系;
S2.2.2:给定最大弓高误差,得到由弓高误差限制的机械臂末端执行器插补速度。
5.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,步骤S3具体由以下步骤实现:
S3.1:在预估阶段,插补参数按照一阶泰勒展开式进行更新;
S3.2:将预估插补参数值带入NURBS曲线参数方程,计算得到预估插补点,利用差分法计算相应的预估插补速度;
S3.3:计算预估插补速度和期望插补速度之间的误差,判断该误差是否在期望的插补速度误差允许阈值之内,若当前的插补速度在误差允许范围之内,则认为当前的预估插补点是满足要求的;否则对预估插补点进行校正,重复步骤S3.2和S3.3直至插补速度误差满足阈值;
S3.4:最终得到满足要求的预估插补点,即为机械臂末端当前时刻的期望位置。
6.根据权利要求5所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,步骤S6具体由以下步骤实现:
根据最后一次前瞻计算得到机械臂末端距离路径终点的路程,减速段开始点处的插补速度为前瞻插补速度,最终速度减为0,根据位移公式计算得到匀减速段的加速度及减速时间,进而得出匀减速段的规划插补速度。
7.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法,其特征在于,在步骤S1之后、步骤S2之前还包括以下步骤:
在步骤S2开始前,需要进行初始化处理,初始化实时轨迹规划算法中的各个初始量,初始量包括常值及随迭代而不断更新的变量:常值包括采样周期、弓高误差最大容许值、插补速度相对误差的允许阈值、机械臂末端最大运动速度值、前瞻窗口长度、关节位置、速度及加速度的最大允许值;变量包括插补参数值、速度缩放因子和迭代次数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111560509.8A CN114131612B (zh) | 2021-12-20 | 2021-12-20 | 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111560509.8A CN114131612B (zh) | 2021-12-20 | 2021-12-20 | 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114131612A CN114131612A (zh) | 2022-03-04 |
CN114131612B true CN114131612B (zh) | 2024-01-30 |
Family
ID=80382853
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111560509.8A Active CN114131612B (zh) | 2021-12-20 | 2021-12-20 | 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114131612B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114211380A (zh) * | 2022-02-21 | 2022-03-22 | 江苏天健智能装备制造有限公司 | 一种基于等步长法的封头抛光路径规划的方法 |
CN114474072B (zh) * | 2022-03-18 | 2023-07-04 | 中科新松有限公司 | 一种轨迹融合方法、装置、设备及存储介质 |
CN114851190B (zh) * | 2022-04-21 | 2024-05-14 | 深圳市华成工业控制股份有限公司 | 面向低频驱控一体的机械臂轨迹规划方法及*** |
CN114952852A (zh) * | 2022-06-13 | 2022-08-30 | 天津大学 | 机器人nurbs曲线速度规划方法、设备及存储介质 |
CN114952868B (zh) * | 2022-07-26 | 2022-11-11 | 之江实验室 | 7自由度srs型机械臂控制方法及装置、弹琴机器人 |
CN115328032B (zh) * | 2022-10-11 | 2023-03-31 | 济南邦德激光股份有限公司 | 基于s形加减速的b样条速度前瞻控制方法、设备和介质 |
CN116079714A (zh) * | 2022-12-02 | 2023-05-09 | 盐城工学院 | 一种基于b样条的六轴机械臂轨迹规划方法 |
CN115890682A (zh) * | 2022-12-09 | 2023-04-04 | 沈阳新松机器人自动化股份有限公司 | 一种双臂直驱机械手的轨迹控制方法 |
CN116795044B (zh) * | 2023-08-16 | 2023-11-14 | 通用技术集团机床工程研究院有限公司 | 速度规划方法、装置、机床控制***和存储介质 |
CN117047776A (zh) * | 2023-09-25 | 2023-11-14 | 苏州诺克汽车工程装备有限公司 | 一种六自由度机械臂对复杂零件抓取的路径规划方法 |
CN117406669B (zh) * | 2023-12-14 | 2024-04-12 | 法奥意威(苏州)机器人***有限公司 | 插补方法、装置、电子设备及存储介质 |
CN117687417A (zh) * | 2024-01-29 | 2024-03-12 | 安徽大学 | 一种工业机器人外部运动轨迹规划方法及*** |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH06182682A (ja) * | 1992-12-18 | 1994-07-05 | Mitsubishi Heavy Ind Ltd | ロボットの速度制御方法 |
WO1995023054A1 (en) * | 1994-02-25 | 1995-08-31 | Uk Robotics Limited | Manipulator controller |
JPH09265313A (ja) * | 1996-01-24 | 1997-10-07 | Mitsubishi Electric Corp | ロボットの速度演算装置、およびロボットの速度演算方法 |
US6317651B1 (en) * | 1999-03-26 | 2001-11-13 | Kuka Development Laboratories, Inc. | Trajectory generation system |
CN101493687A (zh) * | 2009-03-02 | 2009-07-29 | 广西大学 | 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 |
CN104597847A (zh) * | 2013-10-31 | 2015-05-06 | 中国科学院沈阳计算技术研究所有限公司 | 基于Akima样条曲线拟合的前瞻插补方法 |
CN104597845A (zh) * | 2013-10-31 | 2015-05-06 | 中国科学院沈阳计算技术研究所有限公司 | 一种用于高质量加工的样条曲线插补算法 |
BR102012006723A2 (pt) * | 2012-03-26 | 2015-08-11 | Lucas Möller | Sistema de recapagem de pneus constituído por braço robotizado com movimentos de interpolação angular |
CN105710881A (zh) * | 2016-03-16 | 2016-06-29 | 杭州娃哈哈精密机械有限公司 | 一种机器人末端连续轨迹规划过渡方法 |
CN105785921A (zh) * | 2016-03-25 | 2016-07-20 | 华南理工大学 | 一种工业机器人nurbs曲线插补时的速度规划方法 |
US10065311B1 (en) * | 2016-06-08 | 2018-09-04 | X Development Llc | Singularity handling for robot jogging |
EP3745982A1 (en) * | 2018-04-27 | 2020-12-09 | Alcon Inc. | Stereoscopic visualization camera and integrated robotics platform |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9731419B2 (en) * | 2010-08-03 | 2017-08-15 | Praxair S.T. Technology, Inc. | System and method for programming robots |
KR102206198B1 (ko) * | 2013-07-10 | 2021-01-22 | 삼성전자주식회사 | 수술 로봇 시스템 및 그 제어 방법 |
EP3545444A4 (en) * | 2016-11-30 | 2020-09-30 | Fited, Inc. | 3D MODELING SYSTEMS AND METHODS |
JP7222803B2 (ja) * | 2019-04-25 | 2023-02-15 | 株式会社日立製作所 | 軌道計画装置、軌道計画方法及びプログラム |
-
2021
- 2021-12-20 CN CN202111560509.8A patent/CN114131612B/zh active Active
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH06182682A (ja) * | 1992-12-18 | 1994-07-05 | Mitsubishi Heavy Ind Ltd | ロボットの速度制御方法 |
WO1995023054A1 (en) * | 1994-02-25 | 1995-08-31 | Uk Robotics Limited | Manipulator controller |
JPH09265313A (ja) * | 1996-01-24 | 1997-10-07 | Mitsubishi Electric Corp | ロボットの速度演算装置、およびロボットの速度演算方法 |
US6317651B1 (en) * | 1999-03-26 | 2001-11-13 | Kuka Development Laboratories, Inc. | Trajectory generation system |
CN101493687A (zh) * | 2009-03-02 | 2009-07-29 | 广西大学 | 实时前瞻全程加减速控制的nurbs曲线自适应分段插补方法 |
BR102012006723A2 (pt) * | 2012-03-26 | 2015-08-11 | Lucas Möller | Sistema de recapagem de pneus constituído por braço robotizado com movimentos de interpolação angular |
CN104597847A (zh) * | 2013-10-31 | 2015-05-06 | 中国科学院沈阳计算技术研究所有限公司 | 基于Akima样条曲线拟合的前瞻插补方法 |
CN104597845A (zh) * | 2013-10-31 | 2015-05-06 | 中国科学院沈阳计算技术研究所有限公司 | 一种用于高质量加工的样条曲线插补算法 |
CN105710881A (zh) * | 2016-03-16 | 2016-06-29 | 杭州娃哈哈精密机械有限公司 | 一种机器人末端连续轨迹规划过渡方法 |
CN105785921A (zh) * | 2016-03-25 | 2016-07-20 | 华南理工大学 | 一种工业机器人nurbs曲线插补时的速度规划方法 |
US10065311B1 (en) * | 2016-06-08 | 2018-09-04 | X Development Llc | Singularity handling for robot jogging |
EP3745982A1 (en) * | 2018-04-27 | 2020-12-09 | Alcon Inc. | Stereoscopic visualization camera and integrated robotics platform |
Non-Patent Citations (4)
Title |
---|
基于改进Bezier拟合算法的工业机器人轨迹规划;于佳琳;言勇华;王嘉宁;;机电一体化(第02期);第13-18+26页 * |
基于视觉和动态运动基元的机械臂抓取轨迹规划;叶子涵;陈俊洪;杨振国;刘文印;;机器人技术与应用(第03期);第35-40页 * |
多臂空间机器人的视觉伺服与协调控制;冯骁;卢山;侯月阳;王奉文;贾英宏;;宇航学报(第02期);第100-109页 * |
空间机械臂多目标综合轨迹规划研究;金明河;李鹏浩;夏进军;;机械与电子(第07期);第36-40+44页 * |
Also Published As
Publication number | Publication date |
---|---|
CN114131612A (zh) | 2022-03-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114131612B (zh) | 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 | |
CN106393106B (zh) | 参数自适应密化的机器人nurbs曲线运动插补方法 | |
CN106647282B (zh) | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 | |
CN108568817B (zh) | 一种基于贝塞尔曲线的Delta机器人轨迹连接控制方法 | |
CN111399514B (zh) | 一种机器人时间最优轨迹规划方法 | |
US11188056B2 (en) | Feedrate scheduling method for five-axis dual-spline curve interpolation | |
CN105785921B (zh) | 一种工业机器人nurbs曲线插补时的速度规划方法 | |
CN113552877A (zh) | 用于机器人优化动作规划的初始参考生成 | |
CN112975992B (zh) | 一种误差可控的机器人轨迹同步优化方法 | |
CN110647147A (zh) | 基于灰狼算法优化pid的路径跟踪控制方法 | |
CN112269356A (zh) | 一种机器人nurbs轨迹插补方法 | |
CN113204215A (zh) | 一种数控加工全局nurbs轨迹实时插补方法及其应用 | |
CN111230881B (zh) | 一种六自由度机械臂空间轨迹优化方法 | |
CN116079714A (zh) | 一种基于b样条的六轴机械臂轨迹规划方法 | |
CN111240275A (zh) | 基于对数几率函数在运动和误差限制下的进给率规划方法 | |
CN113721626A (zh) | 一种制动盘补偿累计误差的机器人轨迹规划方法 | |
JP3204042B2 (ja) | ロボットの軌道の生成装置 | |
Zou et al. | A novel robot trajectory planning algorithm based on NURBS velocity adaptive interpolation | |
CN113276116B (zh) | 一种误差可控的机器人轨迹同步过渡方法 | |
CN114952852A (zh) | 机器人nurbs曲线速度规划方法、设备及存储介质 | |
CN111819037A (zh) | 校正函数生成装置、机器人控制***及机器人*** | |
CN116909154B (zh) | 一种通过特征查表进行前馈补偿的机器人轨迹优化方法 | |
CN115958597B (zh) | 一种工业机器人连续姿态路径光顺方法及*** | |
CN110788859A (zh) | 一种控制器参数全域自适应调节*** | |
CN115464653A (zh) | 时间最优非对称s型加减速控制方法 |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |