CN116079714A - 一种基于b样条的六轴机械臂轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于B样条的六轴机械臂轨迹规划方法，首先自定义控制点参数及拟合方式，并根据控制点参数拟合出对应B样条方程；通过对B样条参数方程进行Simpson数值积分，得出各分段函数曲线的长度以及B样条曲线的总长，对B样条参数方程进行曲率的采样，根据采样速度计算最小运行时间，通过最小插值周期分割求最小整数的插值时间反求各分段最大限制速度，最后根据加加速度与加速度限制两次卷积，获得平滑速度规划，并求出插补参数与插补点，完成规划轨迹插补；最后对规划出来的插值点进行后处理，并从插值点集中取点下发执行器，通过模块化流程，方便替换适应性好，并通过全局线速度前瞻规划，使机械臂末端运动平滑、冲击小。

Description

一种基于B样条的六轴机械臂轨迹规划方法

技术领域

本发明涉及机械人轨迹规划技术领域，具体为一种基于B样条的六轴机械臂轨迹规划方法。

背景技术

在机械臂运动控制中，常用的规划方式分为关节运动规划和末端位姿规划，其中末端位姿运动规划常用的包括直线运动、圆弧运动、圆圈运动和样条轨迹运动。通常在机械臂喷涂、打磨、按摩和装配等针对复杂曲线、曲面的跟踪，目前较优的方法即是使用构造B样条的方式实现。B样条具有参数化、几何不变性、局部支撑性等优点，广泛应用在工业领域，高阶B样条曲率变化平缓，对***冲击小。但目前样条曲线规划存在全局速度控制难以满足工艺性、长距离曲率变化范围大时插补速度误差大以及规划工程不便于加入力位混合控制等问题。

发明内容

本发明提供一种基于B样条的六轴机械臂轨迹规划方法，解决了目前样条曲线规划存在全局速度控制难以满足工艺性、长距离曲率变化范围大时插补速度误差大以及规划工程不便于加入力位混合控制等问题。

为实现上述目的，本发明提供如下技术方案：一种基于B样条的六轴机械臂轨迹规划方法，包括以下步骤：

S1、首先拟合末端运动轨迹，自定义控制点参数及拟合方式，并根据控制点参数拟合出对应B样条方程；

S2、通过对上一步求得的B样条参数方程进行Simpson数值积分，得出各分段函数曲线的长度以及B样条曲线的总长，同时通过给定的采样间隔，对B样条参数方程进行曲率的采样，从而获取轨迹运动参数，为给后续速度前瞻规划及插补参数计算提供内容；

S3、对运动速度前瞻进行规划，先根据采样速度计算最小运行时间，然后通过最小插值周期分割求最小整数，之后通过最小整数插值时间反求各分段最大限制速度，最后根据加加速度与加速度限制两次卷积，获得平滑速度规划；

S4、根据已知B样条曲线方程以及速度规划，并求出插补参数与插补点，完成规划轨迹插补；

S5、最后对规划出来的插值点进行后处理，获得笛卡尔空间插补位姿，并从插值点集中取点下发执行器。

优选的，所述控制点参数包括位置、姿态以及权因子。

优选的，所述B样条参数方程为：

其中w_i为n+1个与控制顶点对应的权因子，对于准均匀规划因子为1；d_i为n+1个控制顶点；N_i，p(u)为节点矢量U＝[u₀，u₁，u₂，…，u_n+p+1]上的第i个p次基函数，其表达式为：

最终输出曲线参数方程C(u)。

优选的，所述B样条参数方程结果包括位置部分的曲率半径与姿态部分的曲率半径，其中位置曲率半径单位为米、姿态曲率半径单位为弧度。

优选的，所述Simpson自适应数值积分公式为：

其中a、b为Simpson采样间隔参数上下限，C(u)为曲线参数方程。

优选的，所述卷积公式为：

其中，第一次卷积中f(τ)为第二部分流程得到的速度限制，经过插值间隔规范后得出的最终速度限制；g(x-τ)为加速度信号：

g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1，(Vmax/Acc/Ts))

上式Vmax为给定的整个速度规划中的最大速度，Acc为给定加速度，Ts为插值间隔时间；

第二次卷积中f(τ)为第一次卷积后得到的规划速度；

g(x-τ)为加加速度信号：

g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1，(Acc/Jerk/Ts))

上式Acc为给定加速度，Jerk为给定的加加速度，Ts为插值间隔时间。

优选的，所述S4中，所述规划轨迹插补包括以下步骤：

(1)当插值周期间隔较小，采用一阶泰勒展开式求参数，参数代入方程求出插值点、插值点一阶导及二阶导，再根据弓高误差式求出其速度误差补入插值点速度得出实际速度，之后将插值点规划速度与实际速度做差，当速度差<规定误差ε时进行下一步，否则参数u通过规划速度/实际速度的比值进行补偿迭代，直至最终全部规划完成；

(2)当插值周期间隔较长时，将一阶泰勒展开替换为n阶泰勒展开，其中n＞1，并在计算了n+1个值后使用Adams显隐式法或龙格库塔法计算后续插值点；

(3)当速度与曲率半径的比值较大时，采用二分法迭代，牺牲迭代次数来提高稳定性；

(4)根据所述S3中规划出来的速度V，建立其与参数u关于参数方程C(u)的关系，以求出插补参数u，进而带入参数方程C(u)得出插补位姿，并采用一阶泰勒展开式的方式迭代；

(5)基于上一时刻的插补参数u_i及其关于速度V(u_i)，求出下一个插补参数u_i+1：

其中u为插补参数，i为插补计数，t为时间一般而言t_i+1-t_i也就是固定插补周期Ts，引入速度V与曲线参数方程C(u)的关系：

其中，V(u_i)为第i个插补时的规划速度，除以参数方程C(u)在u_i处的二范数变化率，即求出i插补时刻关于此时规划速度V(u_i)的插补参数u的变化率；

(6)通过提高泰勒展开式阶数减小展开式截断误差，并通过当前插补点曲率半径r和实际速度v计算出补偿ER，补偿到实际速度上来减小弓高误差；

(7)计算C(u_i)处的曲率曲率半径弓高误差 加入误差补偿后的修正实际速度

(8)在插值间隔时间Ts较小时，使插值参数u_i直接修正为其中V(u_i)为当前插值点规划速度，最终将规划的参数u代入参数方程C(u)得到规划插补位姿。

8.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述S5中，后处理流程包括当已获取到一段B样条规划的笛卡尔空间位姿插补点集之后，开始从插值点集中取点下发执行器，并选择是否进入力控调整模块，对插补点进行二次修改，之后把插值点笛卡尔位姿根据机器人DH参数逆解为关节角θ，最终下发到各关节执行。

与现有技术相比，本发明的有益效果：

1、本发明中，通过全局线速度前瞻规划，使机械臂末端运动平滑、冲击小，满足如喷涂、打磨等工艺要求。

2、本发明中，通过模块化流程，方便替换适应性好，通过针对不同的计算与执行环境以及要求替换不同的流程模块，如针对在线或离线编程选取不同样条来拟合。

3、本发明可一次性规划完一段轨迹后经过力控算法后处理，对力控的实时性有较大提升，控制周期大幅缩短；同时在循环往复工况，如来回打磨、按摩时，只需一次计算，可反复取用减少计算负荷。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

在附图中：

图1是本发明的整体流程图；

图2是本发明的速度规划曲线图；

图3是本发明的NURBS曲线弓高误差补偿示意图。

具体实施方式

以下结合附图对本发明的优选实施例进行说明，应当理解，此处所描述的优选实施例仅用于说明和解释本发明，并不用于限定本发明。

实施例：如图1所示，一种基于B样条的六轴机械臂轨迹规划方法，整体流程包含五个部分：拟合末端运动轨迹，轨迹运动参数获取，运动速度前瞻规划，规划轨迹插补，轨迹插补后处理。

第一部分：拟合末端运动轨迹，根据控制点参数拟合出对应B样条方程的过程，其控制点及拟合方式进行自定义。如图1所示的整体流程示意图，输入控制点位姿，通过求附加控制点密化并使其曲线经过控制点，再使用4阶改进准均匀B样条拟合控制点，得到分段的曲线参数方程；或者在已知各控制点与节点矢量的情况下，反求经过原函数曲线的新控制点，求得非均匀B样条曲线的参数方程；只输入控制点位姿的方式适用于机器人在线编程，有利于输入量少、操作便捷；输入控制点以及节点矢量的方式适用于机器人离线编程，有利于控制拟合的信息较丰富，能更精确地描述想要的曲线；

控制点参数包括其位置(x y z)、姿态(rx ry rz)以及权因子({w})。

B样条参数方程：

其中w_i为n+1个与控制顶点对应的权因子，对于准均匀规划因子为1；d_i为n+1个控制顶点；N_i，p(u)为节点矢量U＝[u₀，u₁，u₂，…，u_n+p+1]上的第i个p次基函数，其表达式为：

最终输出曲线参数方程C(u)。

第二部分：样条轨迹参数的获取是为了给后续速度前瞻规划及插补参数计算提供内容；一方面通过对上一步求得的B样条参数方程进行Simpson数值积分，得出各分段函数曲线的长度以及B样条曲线的总长；另一方面通过给定的采样间隔，对B样条参数方程进行曲率的采样，对于B样条参数方程的结果，包括位置部分的曲率半径与姿态部分的曲率半径，而位置曲率半径单位为米、姿态曲率半径单位为弧度，姿态的数值以机器人电机的最大加速度及连杆长度作为经验系数等效到位置曲率半径做对比，同一采样点以最小曲率半径作为前瞻速度限制值，并采用Simpson自适应积分的方式求线段长度，消耗较小的计算资源，从而获得较高的数值精度；而通过采用末端位姿的速度限制，满足如喷涂、打磨、按摩等情况末端线速度的要求，同时减少对机器人***的冲击。

Simpson自适应数值积分公式：

其中a、b为Simpson采样间隔参数上下限，C(u)为曲线参数方程。

曲率速度限制使用等向心加速度值的方法，已知公式：

当向心加速度a设为恒值时，采样曲率半径r决定了速度限制v，即得出轨迹曲线各处的速度限制，a的值根据机械臂性能与经验设定。

第三部分：轨迹速度前瞻，由于计算机***是离散化的***，存在插值间隔，为了能在周期内准确达位，需要规范速度和时间。先根据采样速度计算最小运行时间，然后通过最小插值周期分割求最小整数，之后通过最小整数插值时间反求各分段最大限制速度。最后通过卷积的性质，以很小的计算代价以及很好的适应性快速规划出轨迹速度，引入加速度和加加速度，将阶跃的曲线轨迹最大速度限制分两次卷积为平滑的速度规划曲线，如图2所示；

卷积公式：

其中，第一次卷积中f(τ)为第二部分流程得到的速度限制，经过插值间隔规范后得出的最终速度限制；g(x-τ)为加速度信号：

g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1，(Vmax/Acc/Ts))

上式Vmax为给定的整个速度规划中的最大速度(m/s)，Acc为给定加速度(m/s²)，Ts为插值间隔时间(s)。

第二次卷积中f(τ)为第一次卷积后得到的规划速度；g(x-τ)为加加速度信号：

g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1，(Acc/Jerk/Ts))

上式Acc为给定加速度(m/s²)，Jerk为给定的加加速度(m/s³)，Ts为插值间隔时间(s)。

第四部分：轨迹速度插补，根据已知B样条曲线方程以及速度规划，求出插补参数与插补点的过程。根据速度与参数关于曲线方程的关系，使用泰勒展开式快速求参数，其中若插值周期间隔较小，可采用一阶泰勒展开式求参数，参数代入方程求出插值点、插值点一阶导及二阶导，再根据弓高误差式求出其速度误差补入插值点速度得出实际速度，之后将插值点规划速度与实际速度做差，当速度差<规定误差ε时进行下一步，否则参数u通过规划速度/实际速度的比值进行补偿迭代，直至最终全部规划完成。当插值周期间隔较长时，可以通过提高计算精度减少高阶误差的方式提高插值精度，如将一阶泰勒展开替换为n阶泰勒展开，其中n＞1，并在计算了n+1个值后使用Adams显隐式法或龙格库塔法计算后续插值点，既提高了精度又很大程度减少了计算负荷。同时，当速度与曲率半径的比值较大时，通过二分法迭代替换比例修正，从而提高收敛性能，以及通过牺牲迭代次数来提高稳定性。

根据第三部分规划出来的速度V，建立其与参数u关于参数方程C(u)的关系，以求出插补参数u，进而带入参数方程C(u)得出插补位姿。由于插补参数u难以求出其解析解，尤其是在高阶B样条参数方程下；故采用一阶泰勒展开式的方式迭代，基于上一时刻的插补参数u_i及其关于速度V(u_i)，求出下一个插补参数u_i+1：

其中u为插补参数，i为插补计数，t为时间一般而言t_i+1-t_i也就是固定插补周期Ts，引入速度V与曲线参数方程C(u)的关系：

其中，V(u_i)为第i个插补时的规划速度(m/s)，除以参数方程C(u)在u_i处的二范数变化率，即可求出i插补时刻关于此时规划速度V(u_i)的插补参数u的变化率。

如图3所示，由于插值间隔Ts不可能无限小，当速度V越大、曲率半径r越小时，u_i+1的值就越不精确，其误差一般包括迭代数值误差、展开式截断误差和弓高误差。展开式截断误差是由于舍去高阶级数导致的，通过提高泰勒展开式阶数减小；弓高误差也叫轮廓误差，是由于实际离散***插补点间为直线运动而非曲线，其长度小于拟合曲线的长度导致的，可通过当前插补点曲率半径r和实际速度v计算出补偿ER，补偿到实际速度上来减小弓高误差。

计算C(u_i)处的曲率曲率半径弓高误差

加入误差补偿后的修正实际速度

在插值间隔时间Ts较小时，可以使插值参数u_i直接修正为其中V(u_i)为当前插值点规划速度。

最终将规划的参数u代入参数方程C(u)得到规划插补位姿{x y z rx ry rz}。

第五部分：规划插补后处理是对规划出来的插值点进行后处理的流程，当已获取到一段B样条规划的笛卡尔空间位姿插补点集{x y z rx ry rz}之后，就开始从插值点集中取点下发执行器。可以选择是否进入力控调整模块，如恒力控制、力探寻或者导纳控制等，对插补点进行二次修改，之后把插值点笛卡尔位姿根据机器人DH参数逆解为关节角θ，最终下发到各关节执行。

最后应说明的是：以上所述仅为本发明的优选实例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于B样条的六轴机械臂轨迹规划方法，其特征在于，包括以下步骤：

S1、首先拟合末端运动轨迹，自定义控制点参数及拟合方式，并根据控制点参数拟合出对应B样条方程；

S2、通过对上一步求得的B样条参数方程进行Simpson数值积分，得出各分段函数曲线的长度以及B样条曲线的总长，同时通过给定的采样间隔，对B样条参数方程进行曲率的采样，从而获取轨迹运动参数，为给后续速度前瞻规划及插补参数计算提供内容；

S3、对运动速度前瞻进行规划，先根据采样速度计算最小运行时间，然后通过最小插值周期分割求最小整数，之后通过最小整数插值时间反求各分段最大限制速度，最后根据加加速度与加速度限制两次卷积，获得平滑速度规划；

S4、根据已知B样条曲线方程以及速度规划，并求出插补参数与插补点，完成规划轨迹插补；

S5、最后对规划出来的插值点进行后处理，获得笛卡尔空间插补位姿，并从插值点集中取点下发执行器。

2.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述控制点参数包括位置、姿态以及权因子。

3.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述B样条参数方程为：

其中w_i为n+1个与控制顶点对应的权因子，对于准均匀规划因子为1；d_i为n+1个控制顶点；N_i,p(u)为节点矢量U＝[u₀,u₁,u₂,…,u_n+p+1]上的第i个p次基函数，其表达式为：

最终输出曲线参数方程C(u)。

4.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述B样条参数方程结果包括位置部分的曲率半径与姿态部分的曲率半径，其中位置曲率半径单位为米、姿态曲率半径单位为弧度。

5.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述Simpson自适应数值积分公式为：

其中a、b为Simpson采样间隔参数上下限，C(u)为曲线参数方程。

6.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述卷积公式为：

其中，第一次卷积中f(τ)为第二部分流程得到的速度限制，经过插值间隔规范后得出的最终速度限制；g(x-τ)为加速度信号：

g(x-τ)＝1/(Vmax/Acc/Ts)*ones(1,(Vmax/Acc/Ts))

上式Vmax为给定的整个速度规划中的最大速度，Acc为给定加速度，Ts为插值间隔时间；

第二次卷积中f(τ)为第一次卷积后得到的规划速度；

g(x-τ)为加加速度信号：

g(x-τ)＝1/(Acc/Jerk/Ts)*ones(1,(Acc/Jerk/Ts))

上式Acc为给定加速度，Jerk为给定的加加速度，Ts为插值间隔时间。

7.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述S4中，所述规划轨迹插补包括以下步骤：

(1)当插值周期间隔较小，采用一阶泰勒展开式求参数，参数代入方程求出插值点、插值点一阶导及二阶导，再根据弓高误差式求出其速度误差补入插值点速度得出实际速度，之后将插值点规划速度与实际速度做差，当速度差＜规定误差ε时进行下一步，否则参数u通过规划速度/实际速度的比值进行补偿迭代，直至最终全部规划完成；

(2)当插值周期间隔较长时，将一阶泰勒展开替换为n阶泰勒展开，其中n>1，并在计算了n+1个值后使用Adams显隐式法或龙格库塔法计算后续插值点；

(3)当速度与曲率半径的比值较大时，采用二分法迭代，牺牲迭代次数来提高稳定性；

(4)根据所述S3中规划出来的速度V，建立其与参数u关于参数方程C(u)的关系，以求出插补参数u，进而带入参数方程C(u)得出插补位姿，并采用一阶泰勒展开式的方式迭代；

(5)基于上一时刻的插补参数u_i及其关于速度V(u_i)，求出下一个插补参数u_i+1：

其中u为插补参数，i为插补计数，t为时间一般而言t_i+1-t_i也就是固定插补周期Ts，

引入速度V与曲线参数方程C(u)的关系：

其中，V(u_i)为第i个插补时的规划速度，除以参数方程C(u)在u_i处的二范数变化率，即求出i插补时刻关于此时规划速度V(u_i)的插补参数u的变化率；

(6)通过提高泰勒展开式阶数减小展开式截断误差，并通过当前插补点曲率半径r和实际速度v计算出补偿ER，补偿到实际速度上来减小弓高误差；

(7)计算C(u_i)处的曲率

曲率半径

弓高误差

加入误差补偿后的修正实际速度

(8)在插值间隔时间Ts较小时，使插值参数u_i直接修正为

其中V(u_i)为当前插值点规划速度，最终将规划的参数u代入参数方程C(u)得到规划插补位姿。

8.根据权利要求1所述的一种基于B样条的六轴机械臂轨迹规划方法，其特征在于：所述S5中，后处理流程包括当已获取到一段B样条规划的笛卡尔空间位姿插补点集之后，开始从插值点集中取点下发执行器，并选择是否进入力控调整模块，对插补点进行二次修改，之后把插值点笛卡尔位姿根据机器人DH参数逆解为关节角θ，最终下发到各关节执行。

Images