基于倍四元数的工业机器人自由曲线的轨迹规划控制方法技术

本发明专利技术公开了一种基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，采用笛卡尔空间的控制点数据对空间自由曲线轮廓进行描述，同时采用阿当姆斯微分方程进行NURBS插补密化计算，并以最大轮廓误差、最大加速度为约束条件自适应调整插补速度，继而将插补所得短直线段采用倍四元数将笛卡尔空间中的机器人位置与姿态转换至四维空间，用超球面旋转对机器人的运动轨迹进行球面线性插补，最终实现工业机器人NURBS自由曲线轨迹规划。

【技术实现步骤摘要】
基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
本专利技术涉及一种基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，属于机器人轨迹规划

技术介绍
现代制造业对机器人性能的要求也越来越高，而机器人在任务空间的轨迹规划算法在机器人控制系统中占有重要地位，直接影响着机器人末端运动性能和效率。且机器人运动控制中，基本的直线、圆弧轨迹曲线已不能满足工业加工的应用需求，而常用的B样条曲线、Bézier曲线、Clothoid曲线，均不能用一种精确统一的表示方法描述标准解析曲线和自由曲线。通常情况下，机器人的期望轨迹是事先给定一系列笛卡尔或者关节空间的点，且给定通过该点的速度或两点间的时间，另外还会限制机器人运动允许的最大速度，继而分别实现机器人末端执行器的位置轨迹规划和姿态轨迹规划，而工业机器人姿态轨迹规划通常采用欧拉角法、等效轴法对姿态进行插补，但欧拉角存在万向死锁的缺陷，等效轴法在旋转量为0时存在无法确定旋转轴的问题。虽然使用四元数法可解决机器人姿态轨迹插补的上述问题，但轨迹的位置插补则需使用其它的插补算法，运算量大，影响控制系统对轨迹规划的实时性要求。
技术实现思路
本专利技术所要解决的技术问题是克服现有技术的缺陷，提供一种基于倍四元数的工业机器人NURBS曲线轨迹规划控制方法，能够为工业机器人实现自由曲线轨迹规划提供一种高效、高精度的控制方法。为解决上述技术问题，本专利技术采用的技术方案如下：基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，包括以下步骤：1)建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；2)给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及控制点对应姿态R；所述控制点序列D表示为：D＝{d0，d1，…，dn}，n为控制点个数；所述控制点对应姿态采用姿态旋转矩阵R3×3表示；3)依据哈特利-贾德法求解所述步骤2)的控制点序列中控制点对应的节点矢量U，具体过程如下：为给定的控制点di，i＝0,1,…,n，预定义一条k次非均匀有理B样条曲线，同时确定它的节点矢量U＝[u0,u1,…,un+k+1]中的具体的节点值，节点值的求解如下：将两端节点的重复度取为k+1，将曲线的定义域取成规范参数域，即u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余节点值需计算求解，如下：计算公式如下：式中，lj为控制多边形的各边长，lj＝|dj-dj-1|，由式(4)可得：继而可得所有的节点值；其中，u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值；4)依据Adams微分方程理论算法对节点矢量U进行密化处理，具体过程如下：采用三步四阶Adams微分方程的隐格式表示为：其中，T为插补周期，分别为ui-2、ui-1、ui、ui+1的一阶导数；将代入上式，可得：ΔLi表示控制点di的进给步长；采用前、后向差分结合代替微分的方法进行简化：后向差分，前向差分，前向差分，前向差分将上式代入式(7)，得到：进而得到简化后的Adams微分方程插补算法迭代公式：继而可得密化后节点矢量，其中，表示ui+1的预估值；5)根据步骤1)中的机器人末端执行器空间位姿的数学模型，并采用自适应速度控制算法对密化后的节点矢量进行修正处理，最终获得最优的密化节点矢量，修正处理过程如下：将参数作为参数插补的预估值代入NURBS方程，得到相对应的预估插补点：表示预估值的预估插补点，从而得到对应的预估进给步长为：预估进给步长和进给步长ΔLi之间存在的偏差，用相对误差δi来表示：当相对误差δi在允许范围内时，则为所求p(ui+1)，否则按下式进行修正，直至达到δi允许范围内：最终获得最优的密化节点矢量；6)利用步骤5)的最优的密化节点矢量，并根据步骤1)中机器人末端执行器在任务空间NURBS描述的自由曲线数学模型，最终获得曲线上的插补点位置；7)根据相邻空间曲线的插补点位置与姿态数据，进行倍四元数转换，具体步骤如下：7-1)对于每一个机器人末端位姿齐次变换矩阵BTE，如下所示：首先将姿态旋转矩阵R3×3，经旋转矩阵与四元数的转换关系，得到姿态旋转矩阵对应的旋转四元数Q，同时获得平移向量P＝[px(u),py(u),pz(u)]T；7-2)将三维空间的平移向量P转换成四维空间的四元数，转换公式如下：Dp＝cos(ψ/2)+sin(ψ/2)v(16)式中，Rl为四维空间的大球半径，ψ＝|P|/Rl，v是平移向量上的单位矢量，v＝P/|P|；当|P|＝0时，v为零矢量；7-3)通过以下公式，计算得到机器人末端位姿转换至四维空间的倍四元数空间位姿的G部和H部；式中，为四元数Dp的共轭；7-4)倍四元数的双旋转轨迹进行离散化得到一系列插值倍四元数点，需要将其转换成旋转四元数和平移向量，转换算法如下：Q＝(G+H)/(2cosψ)(17)式中，8)对插补所得机器人末端执行器位姿进行逆运动学处理，获得关节角度，并驱动关节运动。前述的步骤1)中，基于倍四元数的机器人末端执行器空间位姿的数学模型为：其中，表示机器人末端执行器倍四元数空间位姿，ξ和η满足ξ2＝ξ，η2＝η，ξ+η＝1，ξη＝0，G和H均为单位四元数；机器人末端执行器在任务空间NURBS描述的自由曲线数学模型为：任意一条k次NURBS曲线均表示为一分段有理多项式矢函数：其中，p(u)表示机器人末端执行器在任务空间NURBS描述的自由曲线的位置矢量，ωi称为权因子；di为自由曲线控制点；n为控制点个数；Ni,k(u)是由节点矢量U＝[u0,u1,…,un+k+1]决定的B样条基函数，由德布尔-考克斯递推定义公式表示：式中，规定u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值。本专利技术所达到的有益效果：本专利技术能够为实现工业机器人在笛卡尔空间的NURBS自由曲线的轨迹规划，提供一种有效提高工业机器人的工作效率和工作质量、能够减小速度波动、改善机器人的工作环境的控制方法。附图说明图1为本专利技术基于倍四元数的工业机器人自由曲线的轨迹规划控制方法流程示意图；图2为本专利技术基于倍四元数的工业机器人自由曲线的轨迹规划控制方法的倍四元数表述空间直线段位置的示意图；图3为本专利技术基于倍四元数的工业机器人自由曲线的轨迹规划控制方法的倍四元数表述空间直线段姿态的示意图，图中箭头方向为姿态用四元数表示的旋转矢量轴。具体实施方式下面结合附图对本专利技术作进一步描述。以下实施例仅用于更加清楚地说明本专利技术的技术方案，而不能以此来限制本专利技术的保护范围。自由曲线概念的提出就是为了描述比较复杂的几何形状，以提高工业机器人的加工效率和精度。而非均匀有理B样条(NURBS，Non-UniformRationalB-Spline)曲线，可以准确的透视出曲线造型的控制点分布特征，并可以有效的解决型值点不能均匀分布的缺点。倍四元数是基于Clifford代数的一种新的数学建模工具，是在四元数的基础上发展而来。采用倍四元数能够对笛卡尔空间中的位置与姿态转换至四维空间中，既能够分别得表示空间本文档来自技高网...

【技术保护点】
基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，其特征在于，包括以下步骤：1)建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；2)给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及控制点对应姿态R；所述控制点序列D表示为：D＝{d0，d1，···，dn}，n为控制点个数；所述控制点对应姿态采用姿态旋转矩阵R3×3表示；3)依据哈特利‑贾德法求解所述步骤2)的控制点序列中控制点对应的节点矢量U，具体过程如下：为给定的控制点di，i＝0,1,…,n，预定义一条k次非均匀有理B样条曲线，同时确定它的节点矢量U＝[u0,u1,…,un+k+1]中的具体的节点值，节点值的求解如下：将两端节点的重复度取为k+1，将曲线的定义域取成规范参数域，即u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余节点值需计算求解，如下：计算公式如下：ui-ui-1=Σj=i-ki-1ljΣs=k+1n+1Σj=s-ks-1lj,i=k+1,...,n+1---(4)]]>式中，lj为控制多边形的各边长，lj＝|dj‑dj‑1|，由式(4)可得：ui=Σs=k+1i(us-us-1)=Σs=k+1iΣj=s-ks-1ljΣs=k+1n+1Σj=s-ks-1lj,i=k+1,...,n+1---(5)]]>继而可得所有的节点值；其中，u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值；4)依据Adams微分方程理论算法对节点矢量U进行密化处理，具体过程如下：采用三步四阶Adams微分方程的隐格式表示为：ui+1=ui+T24(9u·i+1+19u·i-5u·i-1+u·i-2)---(6)]]>其中，T为插补周期，分别为ui‑2、ui‑1、ui、ui+1的一阶导数；将代入上式，可得：ui+1=ui+124(9ΔLi+1x·i+12+y·i+12+z·i+12+19ΔLix·i2+y·i2+z·i2-5ΔLi-1x·i-12+y·i-12+z·i-12+ΔLi-2x·i-22+y·i-22+z·i-22)---(7)]]>ΔLi表示控制点di的进给步长；采用前、后向差分结合代替微分的方法进行简化：后向差分，前向差分，前向差分，前向差分将上式代入式(7)，得到：ui+1=ui+T24(9u·i+1+19u·i-5u·i-1+u·i-2)---(8)]]>进而得到简化后的Adams微分方程插补算法迭代公式：u~i+1=14(9ui-6ui-1+ui-2)---(9)]]>继而可得密化后节点矢量，其中，表示ui+1的预估值；5)根据步骤1)中的机器人末端执行器空间位姿的数学模型，并采用自适应速度控制算法对密化后的节点矢量进行修正处理，最终获得最优的密化节点矢量，修正处理过程如下：将参数作为参数插补的预估值代入NURBS方程，得到相对应的预估插补点：p~(ui+1)=p(u~i+1)---(10)]]>表示预估值的预估插补点，从而得到对应的预估进给步长为：ΔL~i=|p~(ui+1)-p(ui)|=(x~i+1-xi)2+(y~i+1-xi)2+(z~i+1-xi)2---(11)]]>预估进给步长和进给步长ΔLi之间存在的偏差，用相对误差δi来表示：δi=|ΔLi-ΔL~i|ΔLi×100%---(12)]]>当相对误差δi在允许范围内时，则为所求p(ui+1)，否则按下式进行修正，直至达到δi允许范围内：ui+1=ui+ΔLiΔL~i(u~i+1-ui)---(13)]]>最终获得最优的密化节点矢量；6)利用步骤5)的最优的密化节点矢量，并根据步骤1)中机器人末端执行器在任务空间NURBS描述的自由曲线数学模型，最终获得曲线上的插补点位置；7)根据相邻空间曲线的插补点位置与姿态数据，进行倍四元数转换，具体步骤如下：7‑1)对于每...

【技术特征摘要】
1.基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，其特征在于，包括以下步骤：1)建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；2)给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及控制点对应姿态R；所述控制点序列D表示为：D＝{d0，d1，…，dn}，n为控制点个数；所述控制点对应姿态采用姿态旋转矩阵R3×3表示；3)依据哈特利-贾德法求解所述步骤2)的控制点序列中控制点对应的节点矢量U，具体过程如下：为给定的控制点di，i＝0,1,…,n，预定义一条k次非均匀有理B样条曲线，同时确定它的节点矢量U＝[u0,u1,…,un+k+1]中的具体的节点值，节点值的求解如下：将两端节点的重复度取为k+1，将曲线的定义域取成规范参数域，即u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余节点值需计算求解，如下：计算公式如下：式中，lj为控制多边形的各边长，lj＝|dj-dj-1|，由式(4)可得：继而可得所有的节点值；其中，u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值；4)依据Adams微分方程理论算法对节点矢量U进行密化处理，具体过程如下：采用三步四阶Adams微分方程的隐格式表示为：其中，T为插补周期，分别为ui-2、ui-1、ui、ui+1的一阶导数；将代入上式，可得：ΔLi表示控制点di的进给步长；采用前、后向差分结合代替微分的方法进行简化：后向差分，前向差分，前向差分，前向差分将上式代入式(7)，得到：进而得到简化后的Adams微分方程插补算法迭代公式：继而可得密化后节点矢量，其中，表示ui+1的预估值；5)根据步骤1)中的机器人末端执行器空间位姿的数学模型，并采用自适应速度控制算法对密化后的节点矢量进行修正处理，最终获得最优的密化节点矢量，修正处理过程如下：将参数作为参数插补的预估值代入NURBS方程，得到...

【专利技术属性】
技术研发人员：李宏胜，汪允鹤，
申请(专利权)人：南京工程学院，
类型：发明
国别省市：江苏;32