CN114131612A - 基于nurbs曲线插补算法的冗余机械臂实时前瞻轨迹规划方法 - Google Patents

Abstract

基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，涉及冗余机械臂应用领域。本发明通过给定机械臂末端目标路径点，构造NURBS参数曲线作为末端期望路径，加减速控制算法得到末端规划插补速度，在笛卡尔空间考虑了由插补带来的机械臂末端路径误差，根据弓高误差对插补速度进行限制，并引入前瞻算法，考虑机械臂关节速度、加速度约束对插补速度进行缩放，同时实现减速点的预测，采用预估校正法实现NURBS曲线插补获得末端期望轨迹，逆运动学求解关节变量，并利用冗余特性采用梯度投影法实现关节限位。本发明实现了满足机械臂运动学约束条件的实时轨迹规划，在不超出机械臂物理极限的前提下，保证了末端轨迹跟踪精度。

Description

基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划 方法

技术领域

本发明涉及冗余机械臂应用技术领域，尤其涉及一种基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法。

背景技术

在机械臂末端执行器的工作空间内，已知一系列离散的目标路径点，工业机器人多采用直线和圆弧插补方法进行路径拟合。但是，对于复杂路径，需要大量的直线段及圆弧段，计算过程复杂且路径平滑性难以保证。

由于样条曲线具有连续性好、曲率变化均匀的特点，适用于自由构型曲线的拟合，因此，已有人采用样条曲线进行机械臂轨迹规划。但是，现有采用样条曲线实现的机械臂轨迹规划方法，存在以下技术问题：机器人控制系统是高度约束的，关节具有边界位置，关节速度及加速度会受到驱动器限制。在机器人运动时，各个关节变量是直接控制的对象，应满足机械臂运动学约束条件。由于关节空间与末端执行器的工作空间并不是线性相关的，如果只在工作空间中对机械臂末端进行轨迹规划，在执行时可能会造成关节变量饱和，使机械臂末端执行器偏离期望轨迹，产生较大的跟踪误差，甚至可能导致机械臂与工作空间中的其他物体产生碰撞，最终紧急停止。

发明内容

基于此，本发明提供一种基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，以解决现有技术存在的问题。

本发明为解决技术问题所采用的技术方案如下：

本发明饿基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，包括以下步骤：

S1：根据给定的机械臂末端执行器目标路径点，反算NURBS曲线的节点矢量及控制顶点，构造NURBS曲线作为机械臂末端执行器期望路径；

S2：首先利用加减速控制算法规划匀加速段及匀速段的末端执行器期望插补速度；然后在笛卡尔空间中计算由弓高误差限制的末端执行器插补速度；最后引入前瞻算法，考虑机械臂关节速度及加速度约束对插补速度进行缩放；

S3：采用预估校正法确定插补参数，利用一阶泰勒展开式预估插补参数值，根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正，最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置；

S4：根据冗余机械臂闭环逆运动学控制算法解算关节变量，并通过梯度投影法进行优化以实现关节限位；

S5：每个采样周期均重复步骤S2至步骤S4，前瞻窗口随轨迹向前移动，直至前瞻窗口末端插补参数值为1时，认为此次迭代为减速段开始点；

S6：从预测减速点开始进入减速阶段，停止前瞻算法不再更新速度缩放因子，将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度，重复步骤S3和S4，使得机械臂平稳减速直至末端到达给定的路径终点。

进一步的，步骤S1具体由以下步骤实现：

S1.1：由插补参数u所决定的NURBS曲线参数方程如下：

其中，C(u)表示NURBS曲线参数方程，{d_i}表示n+1个控制顶点，{w_i}表示n+1个与控制顶点相对应的权因子，N_i,p(u)表示定义在非均匀节点矢量U＝[u₀,u₁,...,u_n+p+1]上的第i个p次B样条基函数，其表达式如下：

其中，N_i,p(u)的第一下标i表示序号，等于其支承区间[u_i,u_i+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曲线参数方程，{d_i}表示n+1个控制顶点，{w_i}表示n+1个与控制顶点相对应的权因子，N_i,p(u)表示定义在非均匀节点矢量U＝[u₀,u₁,...,u_n+p+1]上的第i个p次B样条基函数，其表达式如下：

其中，N_i,p(u)的第一下标i表示序号，等于其支承区间[u_i,u_i+k+1]左端节点的下标，第二下标p表示样条曲线的次数。

S1.2：采用积累弦长参数化方法对给定的n-1个机械臂末端执行器目标路径点P_i(i＝0,1,2,...,n-2)进行参数化处理，并将其进行规范化可得到三次NURBS曲线的节点矢量U：

其中，首末节点的重复度为4，u_3+i(i＝0,1,...,n-2)为定义域内节点，而u₀,u₁,u₂与u_n+2,u_n+3,u_n+4均为定义域外节点；

S1.3：令NURBS曲线的首末端点一致，并且令NURBS曲线的首末路径点一致可得式(4)，令NURBS曲线分段连接点与路径中间点一致可得式(5)，并在NURBS曲线两端点即首末端点处补充自由端点条件可得式(6)，代入NURBS曲线方程(1)，构造线性方程组，通过求解线性方程组，可确定n+1个控制顶点d_i(i＝0,1,2,...,n)。

P₀＝d₀,P_n-1＝d_n+k-2 (4)

C(u_3+i)＝P_i i＝0,1,...,n-2 (5)

w₀d₀＝w₁d₁,w_n-1d_n-1＝w_nd_n (6)

其中，P₀和d₀分别表示NURBS曲线的首末端点，P_n-1和d分别表示NURBS曲线的首末路径点，C(u_3+i)表示NURBS曲线分段连接点；

S1.4：给定权因子，根据步骤S1.2所确定的节点矢量及步骤S1.3所解算的控制顶点，可唯一地确定一条经过给定的机械臂末端执行器目标路径点的三次NURBS曲线。

在步骤S2中，首先通过加减速控制算法来规划匀加速段及匀速段的机械臂末端执行器期望插补速度，然后在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度，最后引入前瞻算法，考虑机械臂关节速度、加速度约束对插补速度进行缩放。

在步骤S2开始前，需要进行初始化处理。初始化实时轨迹规划算法中的各个初始量，初始量包括常值及随迭代而不断更新的变量；其中，常值包括采样周期、弓高误差最大容许值、插补速度相对误差的允许阈值、机械臂末端最大运动速度值、前瞻窗口长度、关节位置、速度及加速度的最大允许值；变量包括插补参数值、速度缩放因子和迭代次数。

具体为：初始化随迭代而不断更新的变量，包括插补参数值u₀＝0、速度缩放因子s₀＝1及迭代次数k＝0，并给定采样周期T_s、弓高误差最大容许值ER_max、插补速度相对误差的允许阈值ε、机械臂末端最大运动速度值v_lim、前瞻窗口长度hT_s,h∈N⁺及关节位置、速度及加速度的最大允许值q_lim

进一步地，步骤S2具体由以下步骤实现：

S2.1：根据加减速规划算法，由机械臂末端最大运动速度及前瞻窗口长度规划匀加速段及匀速段的机械臂末端执行器期望插补速度，具体采用以下步骤实现：

S2.1.1：已知机械臂末端最大运动速度值v_lim，令加速段时长与前瞻窗口长度相同，当插补时间t＝kT_s小于前瞻窗口长度时，机械臂末端执行器插补速度处于匀加速阶段，则运动加速度α₁为：

经计算可得匀加速段的机械臂末端执行器插补速度V_p,k为：

V_p,k＝a₁kT_s (8)

S2.1.2：到达匀速段后，规划的机械臂末端执行器插补速度等于给定的机械臂末端最大运动速度值v_lim；即在t＝hT_s时刻，机械臂末端执行器插补速度恰好为指定的机械臂末端最大运动速度，可得匀速段的机械臂末端执行器插补速度V_p,k为：

V_p,k＝v_lim (9)

S2.2：在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度，具体采用以下步骤实现：

S2.2.1：插补误差可用弓高误差来衡量。将NURBS曲线当前插补点C(u_k)与下一插补点C(u_k+1)之间的曲线段近似为圆弧，近似模型如图2所示，该圆弧半径等于NURBS曲线在当前插补点处的曲率半径ρ_k，圆弧段起点O(u_k)与当前插补点重合，圆弧段终点O(u_k+1)为圆弧段上的近似下一插补点，则可近似计算插补速度与弓高误差的关系。机械臂末端执行器插补速度V_k与采样周期T_s的乘积即为近似插补步长ΔL_k：

ΔL_k＝V_kT_s＝|O(u_k+1)-O(u_k)| (10)

由勾股定理可得到近似弓高误差h_k：

由此可得到机械臂末端执行器插补速度V_k与弓高误差h_k之间的关系如下：

S2.2.2：给定最大弓高误差h_max，可得到由弓高误差限制的机械臂末端执行器插补速度V_c,k：

S2.3：根据前瞻算法，考虑机械臂关节速度及加速度极限更新速度缩放因子，具体采用以下步骤实现：

S2.3.1：采用一阶泰勒展开式确定前瞻插补参数值

从上一时刻插补参数u_k开始计算，令

则：

其中，s_k-1V_d,k+i-1为期望插补速度，s_k-1为上一时刻经过前瞻算法得到的插补速度缩放因子，V_p,k+i-1为规划速度。

将迭代计算得到的前瞻插补参数值代入NURBS曲线参数方程(1)，可得到未来时刻的期望轨迹。

S2.3.2：经过逆运动学算法解算雅可比矩阵^eJ，并将其转换为表示在基坐标系下的雅可比矩阵^bJ：

其中，^bR_i表示基于基坐标系的旋转变换矩阵。

令期望插补速度为末端期望线速度，根据公式(16)计算得到期望关节速度：

其中，

表示期望关节速度，

表示基于末端坐标系的雅可比伪逆矩阵，

表示末端期望线速度。

S2.3.3：分别将各个关节速度极值与期望关节速度做比并取最小值，可得到插补速度缩放因子s_vel：

假设机械臂共n个关节，上标j表示第j个关节，

表示第j个关节的速度极值，

表示第j个关节的期望关节速度。如果

则认为

为简化计算，只考虑前瞻窗口起点及终点处的信息，可得到前瞻窗口起点处的速度缩放因子s_vel,k及前瞻窗口终点处的速度缩放因子s_vel,k+h，取两者中较小值作为由关节速度约束得到的缩放因子s_vel：

s_vel＝min{s_vel,k,s_vel,k+h} (18)

S2.3.4：通过关节加速度极值及前一时刻的关节速度，可得到由关节加速度约束的关节速度允许区间：

其中，

表示前瞻窗口起点处由关节加速度约束的关节速度极值，

表示上一时刻的关节速度，

分别表示关节加速度的最大值和最小值，δt表示离散时间间隔。

将根据关节加速度极限确定的关节速度极值与期望关节速度做比，取最小值可得到关节加速度约束的插补速度缩放因子s_acc：

如果

则认为

假设机械臂共n个关节，上标j表示第j个关节，

表示第j个关节在前瞻窗口起点处得到的根据关节加速度极限确定的速度极值，

表示第j个关节在前瞻窗口起点处的期望关节速度。

S2.3.5：分别将步骤S2.3.3及S2.3.4得到的插补速度缩放因子存入环形缓冲器，环形缓冲器中保留了之前h次前瞻算法得到的速度缩放因子，实际上，环形缓冲器是一个长度为h的循环更新数组，初始化为1，数据存满后，新存入的速度缩放因子代替最老的数据，数组序号重新开始计数，依次进行数据更新。

分别从环形缓冲器中取出最小值，比较两个速度缩放因子，若两者中较小值小于1，则认为该速度缩放因子作为此次前瞻算法得到的插补速度缩放因子s_k，否则，缩放因子取为1，不对末端执行器插补速度进行修改：

s_k＝min{1,s_vel,s_acc} (21)

S2.4：对规划插补速度与弓高误差限制的插补速度进行比较，取小值后通过速度缩放因子进行修正，最终得到机械臂末端的最大容许期望插补速度值：

在步骤S3中，采用预估校正法确定插补参数，利用一阶泰勒展开式预估插补参数值，根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正，最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置。在该步骤中，一阶泰勒展开法忽略高阶项并存在截断误差，曲线切向速度与期望速度之间存在一定的偏差，如果预估插补点不满足指定的插补速度误差允许阈值，校正环节将根据当前的速度反馈进行调整，使当前预估插补速度和期望插补速度之间的偏差总是在期望的插补速度误差允许阈值之内。

进一步地，步骤S3具体由以下步骤实现：

S3.1：在预估阶段，插补参数按照一阶泰勒展开式进行更新：

S3.2：将预估插补参数值

带入NURBS曲线参数方程(1)，计算可得预估插补点：

利用差分法计算相应的预估插补速度，则相应的预估插补速度为：

S3.3：计算当前插补速度和期望的插补速度之间的误差，判断该误差是否在期望的插补速度误差允许阈值ε_PCI之内：

当δ_k≤ε_PCI时，即当前的插补速度在误差允许范围之内，可认为当前的预估插补点

是满足要求的，即为所求插补点C_k+1；否则按式(22)对预估插补点进行校正，重复步骤S3.2和S3.3直至插补速度误差满足阈值：

其中，ξ为校正系数。

S3.4：最终得到满足要求的预估插补点，该点即为机械臂末端当前时刻的期望位置。

在步骤S4中，根据冗余机械臂闭环逆运动学控制算法解算关节变量，并通过梯度投影法进行优化以实现关节限位。

进一步地，步骤S4具体由以下步骤实现：

对于冗余机械臂，已知末端执行器的期望轨迹x_des，可通过基于雅可比转置法的数值迭代求解关节变量，梯度投影法实现优化。逆运动学闭环控制算法流程如图3所示，控制律可表示为：

式中，J为雅可比矩阵；K_p为末端顶点的刚度矩阵；B为关节阻尼系数；Δx＝x-x_des，x_des为设计的末端期望位姿，将关节角度代入正运动学方程f(q)中计算得到实际的末端位姿x；末端期望速度

与雅可比伪逆

的乘积作为前馈环节以提高系统性能；

为梯度投影算子，梯度投影系数β取负值；H为优化目标函数：

其中，上标j表示第j个关节，

表示第j个关节的位置极限，q^j表示第j个关节的位置。

在逆运动学求解过程中将优化目标函数H投影至雅可比矩阵零空间，驱使关节向关节许可位移的中值移动，以实现关节限位。

在步骤S5中，每个采样周期都重复步骤S2至步骤S4，前瞻窗口随轨迹向前移动，直至前瞻窗口末端插补参数值为1时，认为此次迭代得到的插补点C_k+1为减速段开始点。在该步骤中，每个采样周期都需要用前瞻算法对速度缩放因子进行更新，前瞻窗口及环形缓冲器随着机械臂末端轨迹向前移动，同时，前瞻算法还用于预测减速点，当窗口末端的前瞻插补参数值u_k+h＝1时，意味着在等于前瞻窗口长度的时间内，机械臂末端将会以等于前瞻插补速度的速度运动至路径终点，因此从该预测减速点处开始进行减速是合理的。

在步骤S6中，根据最后一次前瞻可计算得到机械臂末端距离路径终点的路程，减速段开始点处的插补速度为前瞻插补速度，最终速度减为0，根据位移公式可计算得到匀减速段的加速度及减速时间，进而得出匀减速段的规划插补速度。从预测减速点开始进入减速阶段，停止前瞻算法不再更新速度缩放因子，将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度，重复步骤S3和S4，使得机械臂平稳减速直至末端到达给定的路径终点。

进一步地，步骤S6具体由以下步骤实现：

当窗口末端的前瞻插补参数值u_k+h＝1时，前瞻插补速度为s_k-1V_p,k+h，由于减速段之前为匀速段，故可认为前瞻插补速度为s_k-1v_lim，则在等于前瞻窗口长度的时间内，机械臂末端距离路径终点的路程为s_k-1v_lim·hT_s；

在减速段开始点处，不再对速度缩放因子更新，则整个减速段的速度缩放因子为s_end＝s_k-1，减速段开始点处末端速度等于前瞻插补速度，最终速度减为0，匀减速段的总路程等于机械臂末端距离路径终点的路程，根据位移公式，

(s_endv_lim)²＝2a(s_k-1v_lim·h) (30)

可计算匀减速段的加速度为：

匀减速段的减速时间为2h，故匀减速段的规划插补速度为：

V_p,h-i＝a₂(2h-i)T_s,i＝1,2,...,2h (32)

由于从预测减速点开始进入减速阶段，停止前瞻算法不再对规划速度进行修正，则规划的插补速度即为期望插补速度：

重复步骤S3和S4，使得机械臂平稳减速直至末端到达给定的路径终点。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (9)

1.基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，包括以下步骤：

S1：根据给定的机械臂末端执行器目标路径点，反算NURBS曲线的节点矢量及控制顶点，构造NURBS曲线作为机械臂末端执行器期望路径；

S2：首先利用加减速控制算法规划匀加速段及匀速段的末端执行器期望插补速度；然后在笛卡尔空间中计算由弓高误差限制的末端执行器插补速度；最后引入前瞻算法，考虑机械臂关节速度及加速度约束对插补速度进行缩放；

S3：采用预估校正法确定插补参数，利用一阶泰勒展开式预估插补参数值，根据预估插补速度与期望插补速度之间的速度误差判断是否对插补参数值进行校正，最终得到满足要求的预估插补点即为机械臂末端当前时刻的期望位置；

S4：根据冗余机械臂闭环逆运动学控制算法解算关节变量，并通过梯度投影法进行优化以实现关节限位；

S5：每个采样周期均重复步骤S2至步骤S4，前瞻窗口随轨迹向前移动，直至前瞻窗口末端插补参数值为1时，认为此次迭代为减速段开始点；

S6：从预测减速点开始进入减速阶段，停止前瞻算法不再更新速度缩放因子，将加减速规划算法得到的匀减速段的规划插补速度作为期望插补速度，重复步骤S3和S4，使得机械臂平稳减速直至末端到达给定的路径终点。

2.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S1具体由以下步骤实现：

S1.1：由插补参数u所决定的NURBS曲线参数方程如下：

其中，C(u)表示NURBS曲线参数方程，{d_i}表示n+1个控制顶点，{w_i}表示n+1个与控制顶点相对应的权因子，N_i,p(u)表示定义在非均匀节点矢量U＝[u₀,u₁,...,u_n+p+1]上的第i个p次B样条基函数，其表达式如下：

其中，N_i,p(u)的第一下标i表示序号，等于其支承区间[u_i,u_i+k+1]左端节点的下标，第二下标p表示样条曲线的次数；

S1.2：采用积累弦长参数化方法对给定的机械臂末端路径点进行参数化处理，并将其进行规范化得到三次NURBS曲线的节点矢量；

S1.3：令NURBS曲线的首末端点一致，令NURBS曲线的首末路径点一致，令NURBS曲线分段连接点与路径中间点一致，并在NURBS曲线两端点处补充自由端点条件，代入NURBS曲线参数方程(1)，构造线性方程组，求解后得到控制顶点；

S1.4：给定权因子，根据步骤S1.2所确定的节点矢量及步骤S1.3解算的控制顶点，可唯一地确定一条经过给定的机械臂末端执行器目标路径点的三次NURBS曲线。

3.根据权利要求2所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S2具体由以下步骤实现：

S2.1：根据加减速规划算法，由机械臂末端最大运动速度及前瞻窗口长度规划匀加速段及匀速段的机械臂末端执行器期望插补速度；

S2.2：在笛卡尔空间中计算由弓高误差限制的机械臂末端执行器插补速度；

S2.3：根据前瞻算法，考虑机械臂关节速度及加速度极限更新速度缩放因子；

S2.4：对规划插补速度与弓高误差限制的插补速度进行比较，取小值后通过速度缩放因子进行修正，最终得到机械臂末端的最大容许期望插补速度值。

4.根据权利要求3所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S2.1具体由以下步骤实现：

2.1.1：已知机械臂末端最大运动速度值，令加速段时长与前瞻窗口长度相同，当插补时间小于前瞻窗口长度时，得到匀加速段的运动加速度，从而得到匀加速段的插补速度；

2.1.2：到达匀速段后，规划的机械臂末端执行器插补速度等于给定的机械臂末端最大运动速度值。

5.根据权利要求4所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S2.2具体由以下步骤实现：

S2.2.1：将NURBS曲线当前插补点与下一插补点之间的曲线段近似为圆弧，该圆弧半径等于NURBS曲线在当前插补点处的曲率半径，圆弧段起点与当前插补点重合，圆弧段终点为近似下一插补点，近似计算插补速度与弓高误差的关系；

S2.2.2：给定最大弓高误差，得到由弓高误差限制的机械臂末端执行器插补速度。

6.根据权利要求5所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤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，不对末端执行器插补速度进行修改。

7.根据权利要求6所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S3具体由以下步骤实现：

S3.1：在预估阶段，插补参数按照一阶泰勒展开式进行更新；

S3.2：将预估插补参数值带入NURBS曲线参数方程(1)，计算得到预估插补点，利用差分法计算相应的预估插补速度；

S3.3：计算预估插补速度和期望插补速度之间的误差，判断该误差是否在期望的插补速度误差允许阈值之内，若当前的插补速度在误差允许范围之内，则认为当前的预估插补点是满足要求的；否则对预估插补点进行校正，重复步骤S3.2和S3.3直至插补速度误差满足阈值；

S3.4：最终得到满足要求的预估插补点，即为机械臂末端当前时刻的期望位置。

8.根据权利要求7所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，步骤S6具体由以下步骤实现：

根据最后一次前瞻计算得到机械臂末端距离路径终点的路程，减速段开始点处的插补速度为前瞻插补速度，最终速度减为0，根据位移公式计算得到匀减速段的加速度及减速时间，进而得出匀减速段的规划插补速度。

9.根据权利要求1所述的基于NURBS曲线插补算法的冗余机械臂实时前瞻轨迹规划方法，其特征在于，在步骤S1之后、步骤S2之前还包括以下步骤：

在步骤S2开始前，需要进行初始化处理，初始化实时轨迹规划算法中的各个初始量，初始量包括常值及随迭代而不断更新的变量：常值包括采样周期、弓高误差最大容许值、插补速度相对误差的允许阈值、机械臂末端最大运动速度值、前瞻窗口长度、关节位置、速度及加速度的最大允许值；变量包括插补参数值、速度缩放因子和迭代次数。

Images