本发明公开了一种基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，采用笛卡尔空间的控制点数据对空间自由曲线轮廓进行描述，同时采用阿当姆斯微分方程进行NURBS插补密化计算，并以最大轮廓误差、最大加速度为约束条件自适应调整插补速度，继而将插补所得短直线段采用倍四元数将笛卡尔空间中的机器人位置与姿态转换至四维空间，用超球面旋转对机器人的运动轨迹进行球面线性插补，最终实现工业机器人NURBS自由曲线轨迹规划。
1.基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，其特征在于，包括以下步骤：
1)建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；
2)给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及控制点对应姿态R；
所述控制点序列D表示为：D＝{d0，d1，…，dn}，n为控制点个数；
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的一阶导数；
采用前、后向差分结合代替微分的方法进行简化：后向差分， 前向差分，
进而得到简化后的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)式中，ψ＝|P|/Rl，Rl为四维空间的大球半径，v是平移向量上的单位矢量，v＝P/|P|；当|P|＝0时，v为零矢量；
7-3)通过以下公式，计算得到机器人末端位姿转换至四维空间的倍四元数空间位姿的G部和H部；
7-4)倍四元数的双旋转轨迹进行离散化得到一系列插值倍四元数点，需要将其转换成旋转四元数和平移向量，转换算法如下：Q＝(G+H)/(2cosψ) (17)式中，
8)对插补所得机器人末端执行器位姿进行逆运动学处理，获得关节角度，并驱动关节运动。
2.根据权利要求1所述的基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，其特征在于，所述步骤1)中，基于倍四元数的机器人末端执行器空间位姿的数学模型为：其中，表示机器人末端执行器倍四元数空间位姿，ξ和η满足ξ2＝ξ，η2＝η，ξ+η＝1，ξη＝0，G和H均为单位四元数；
机器人末端执行器在任务空间NURBS描述的自由曲线数学模型为：任意一条k次NURBS曲线均表示为一分段有理多项式矢函数：其中，p(u)表示机器人末端执行器在任务空间NURBS描述的自由曲线的位置矢量，ωi称为权因子；di为自由曲线控制点；n为控制点个数；Ni,k(u)是由节点矢量决定的B样条基函数，由德布尔-考克斯递推定义公式表示：式中，规定 u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+
基于倍四元数的工业机器人自由曲线的轨迹规划控制方法
技术领域
[0001] 本发明涉及一种基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，属于机器人轨迹规划技术领域。
背景技术
[0002] 现代制造业对机器人性能的要求也越来越高，而机器人在任务空间的轨迹规划算法在机器人控制系统中占有重要地位，直接影响着机器人末端运动性能和效率。且机器人运动控制中，基本的直线、圆弧轨迹曲线已不能满足工业加工的应用需求，而常用的B样条曲线、Bézier曲线、Clothoid曲线，均不能用一种精确统一的表示方法描述标准解析曲线和自由曲线。
[0003] 通常情况下，机器人的期望轨迹是事先给定一系列笛卡尔或者关节空间的点，且给定通过该点的速度或两点间的时间，另外还会限制机器人运动允许的最大速度，继而分别实现机器人末端执行器的位置轨迹规划和姿态轨迹规划，而工业机器人姿态轨迹规划通常采用欧拉角法、等效轴法对姿态进行插补，但欧拉角存在万向死锁的缺陷，等效轴法在旋转量为0时存在无法确定旋转轴的问题。虽然使用四元数法可解决机器人姿态轨迹插补的上述问题，但轨迹的位置插补则需使用其它的插补算法，运算量大，影响控制系统对轨迹规划的实时性要求。
发明内容
[0004] 本发明所要解决的技术问题是克服现有技术的缺陷，提供一种基于倍四元数的工业机器人NURBS曲线轨迹规划控制方法，能够为工业机器人实现自由曲线轨迹规划提供一种高效、高精度的控制方法。
[0005] 为解决上述技术问题，本发明采用的技术方案如下：
[0006] 基于倍四元数的工业机器人自由曲线的轨迹规划控制方法，包括以下步骤：
[0007] 1)建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；
[0008] 2)给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及控制点对应姿态R；
[0009] 所述控制点序列D表示为：D＝{d0，d1，…，dn}，n为控制点个数；
[0010] 所述控制点对应姿态采用姿态旋转矩阵R3×3表示；
[0011] 3)依据哈特利-贾德法求解所述步骤2)的控制点序列中控制点对应的节点矢量U，具体过程如下：
[0012] 为给定的控制点di，i＝0,1,…,n，预定义一条k次非均匀有理B样条曲线，同时确定它的节点矢量U＝[u0,u1,…,un+k+1]中的具体的节点值，节点值的求解如下：
[0013] 将两端节点的重复度取为k+1，将曲线的定义域取成规范参数域，即
[0014] u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余节点值需计算求解，如下：
[0015] 计算公式如下：
[0016]
[0017] 式中，lj为控制多边形的各边长，lj＝|dj-dj-1|，
[0018] 由式(4)可得：
[0019]
[0020] 继而可得所有的节点值；
[0021] 其中，u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值；
[0022] 4)依据Adams微分方程理论算法对节点矢量U进行密化处理，具体过程如下：
[0023] 采用三步四阶Adams微分方程的隐格式表示为：
[0024]
[0025] 其中，T为插补周期， 分别为ui-2、ui-1、ui、ui+1的一阶导数；
[0026] 将 代入上式，可得：
[0027]
[0028] ΔLi表示控制点di的进给步长；
[0029] 采用前、后向差分结合代替微分的方法进行简化：
[0030] 后向差分， 前向差分，
[0031] 前向差分， 前向差分
[0032] 将上式代入式(7)，得到：
[0033]
[0034] 进而得到简化后的Adams微分方程插补算法迭代公式：
[0035]
[0036] 继而可得密化后节点矢量，其中， 表示ui+1的预估值；
[0037] 5)根据步骤1)中的机器人末端执行器空间位姿的数学模型，并采用自适应速度控制算法对密化后的节点矢量进行修正处理，最终获得最优的密化节点矢量，修正处理过程如下：
[0038] 将参数 作为参数插补的预估值代入NURBS方程，得到相对应的预估插补点：
[0039]
[0040] 表示预估值 的预估插补点，
[0041] 从而得到对应的预估进给步长为：
[0042]
[0043] 预估进给步长 和进给步长ΔLi之间存在的偏差，用相对误差δi来表示：
[0044]
[0045] 当相对误差δi在允许范围内时，则 为所求p(ui+1)，否则按下式进行修正，直至达到δi允许范围内：
[0046]
[0047] 最终获得最优的密化节点矢量；
[0048] 6)利用步骤5)的最优的密化节点矢量，并根据步骤1)中机器人末端执行器在任务空间NURBS描述的自由曲线数学模型，最终获得曲线上的插补点位置；
[0049] 7)根据相邻空间曲线的插补点位置与姿态数据，进行倍四元数转换，具体步骤如下：
[0050] 7-1)对于每一个机器人末端位姿齐次变换矩阵BTE，如下所示：
[0051]
[0052] 首先将姿态旋转矩阵R3×3，经旋转矩阵与四元数的转换关系，得到姿态旋转矩阵对应的旋转四元数Q，同时获得平移向量P＝[px(u),py(u),pz(u)]T；
[0053] 7-2)将三维空间的平移向量P转换成四维空间的四元数，转换公式如下：
[0054] Dp＝cos(ψ/2)+sin(ψ/2)v (16)[0055] 式中，Rl为四维空间的大球半径，ψ＝|P|/Rl，v是平移向量上的单位矢量，v＝P/|P|；当|P|＝0时，v为零矢量；
[0056] 7-3)通过以下公式，计算得到机器人末端位姿转换至四维空间的倍四元数空间位姿 的G部和H部；
[0057]
[0058] 式中， 为四元数Dp的共轭；
[0059] 7-4)倍四元数的双旋转轨迹进行离散化得到一系列插值倍四元数点，需要将其转换成旋转四元数和平移向量，转换算法如下：
[0060] Q＝(G+H)/(2cosψ) (17)[0061]
[0062] 式中，
[0063] 8)对插补所得机器人末端执行器位姿进行逆运动学处理，获得关节角度，并驱动关节运动。
[0064] 前述的步骤1)中，
[0065] 基于倍四元数的机器人末端执行器空间位姿的数学模型为：
[0066]
[0067] 其中， 表示机器人末端执行器倍四元数空间位姿，ξ和η满足ξ2＝ξ，η2＝η，ξ+η＝1，ξη＝0，G和H均为单位四元数；
[0068] 机器人末端执行器在任务空间NURBS描述的自由曲线数学模型为：任意一条k次NURBS曲线均表示为一分段有理多项式矢函数：
[0069]
[0070] 其中，p(u)表示机器人末端执行器在任务空间NURBS描述的自由曲线的位置矢量，ωi称为权因子；di为自由曲线控制点；n为控制点个数；Ni,k(u)是由节点矢量U＝[u0,u1,…,un+k+1]决定的B样条基函数，由德布尔-考克斯递推定义公式表示：
[0071]
[0072] 式中，规定 u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值。
[0073] 本发明所达到的有益效果：
[0074] 本发明能够为实现工业机器人在笛卡尔空间的NURBS自由曲线的轨迹规划，提供一种有效提高工业机器人的工作效率和工作质量、能够减小速度波动、改善机器人的工作环境的控制方法。
附图说明
[0075] 图1为本发明基于倍四元数的工业机器人自由曲线的轨迹规划控制方法流程示意图；
[0076] 图2为本发明基于倍四元数的工业机器人自由曲线的轨迹规划控制方法的倍四元数表述空间直线段位置的示意图；
[0077] 图3为本发明基于倍四元数的工业机器人自由曲线的轨迹规划控制方法的倍四元数表述空间直线段姿态的示意图，图中箭头方向为姿态用四元数表示的旋转矢量轴。
具体实施方式
[0078] 下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0079] 自由曲线概念的提出就是为了描述比较复杂的几何形状，以提高工业机器人的加工效率和精度。而非均匀有理B样条(NURBS，Non-Uniform Rational B-Spline)曲线，可以准确的透视出曲线造型的控制点分布特征，并可以有效的解决型值点不能均匀分布的缺点。
[0080] 倍四元数是基于Clifford代数的一种新的数学建模工具，是在四元数的基础上发展而来。采用倍四元数能够对笛卡尔空间中的位置与姿态转换至四维空间中，既能够分别得表示空间中的位置和姿态，也能将空间中的位置和姿态用一种统一的方式进行表示，从而对四维空间中起止点的位姿用超球面旋转对机器人的运动轨迹进行球面线性插补。
[0081] 如图1所示，本发明的基于倍四元数的工业机器人自由曲线的轨迹规划控制方法的包括以下步骤：
[0082] 步骤一、建立基于倍四元数的机器人末端执行器空间位姿的数学模型与机器人末端执行器在任务空间NURBS描述的自由曲线数学模型；
[0083] 基于倍四元数的机器人末端执行器空间位姿的数学模型为：
[0084]
[0085] 式中， 表示机器人末端执行器倍四元数空间位姿，ξ和η满足ξ2＝ξ，η2＝η，ξ+η＝1，ξη＝0，G和H均为单位四元数。
[0086] 机器人末端执行器在任务空间NURBS描述的自由曲线数学模型为：任意一条k次NURBS曲线可表示为一分段有理多项式矢函数：
[0087]
[0088] 式中，p(u)表示机器人末端执行器在任务空间NURBS描述的自由曲线的位置矢量，即p(u)＝[px(u),py(u),pz(u)]T；di为自由曲线控制点，即di＝[xi,yi,zi]，则控制点序列D＝{d0，d1，…，dn}(i＝0,1,…,n)，n为控制点个数；ωi称为权因子，ωi分别与控制点di(i＝0,1,…,n)相联系，当ωi＝1(i＝0,1,…,n)时，一条k次NURBS曲线退化为一条k次B样条曲线；
[0089] Ni,k(u)是由节点矢量U＝[u0,u1,…,un+k+1]决定的B样条基函数，由广泛使用的德布尔-考克斯(De Boor-Cox)递推定义公式表示：
[0090]
[0091] 式中，规定 u为节点矢量U相邻两个节点间的密化节点值，ui，i＝0,1,……,n+k+1，表示节点矢量U中具体的某个节点值。
[0092] 则NURBS自由曲线的三维坐标形式为：
[0093]
[0094] 步骤二、给定机器人末端执行器在任务空间NURBS描述的自由曲线的控制点序列D，以及自由曲线空间控制点对应姿态R；
[0095] 给定机器人任务空间自由曲线的控制点序列D为：D＝{d0，d1，…，dn}(i＝0,1,…,n)，n为控制点个数；
[0096] 对应姿态采用姿态旋转矩阵R3×3的表示方式；
[0097] 步骤三、依据哈特利-贾德法求解任务空间NURBS描述的自由曲线控制点对应的节点矢量U；
[0098] 本步骤中为给定NURBS描述的自由曲线的控制点di，i＝0,1,…,n，预定义一条k次非均匀有理B样条(NURBS)曲线，同时必须确定它的节点矢量U＝[u0,u1,…,un+k+1]中的具体的节点值。
[0099] 为便于对曲线在曲线端点的行为有较好的控制，本发明在两端节点的重复度取为k+1，通常将曲线的定义域取成规范参数域，即u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余内节点需计算求解，方法如下：
[0100] 本步骤所采用的哈特利—贾德法与曲线次数的奇偶性无关，采用统一的计算公式，计算方法更具合理性，其计算公式如下：
[0101]
[0102] 式中，lj为控制多边形的各边长，即lj＝|dj-dj-1|。由式(4)可得：
[0103]
[0104] 继而可得控制点对应的节点矢量中的所有节点值。
[0105] 步骤四、依据Adams微分方程理论算法对节点矢量U进行密化处理；
[0106] 参数密化是指由三维轨迹空间到一维参变量空间的映射，在参数化插补方式下，数据的密化即表现为参数的密化过程，即由轨迹空间的进给步长ΔL映射到参变量空间以求取参变量增量Δu以及下一参数坐标：ui+1＝ui+Δui。
[0107] 本步骤所采用得阿当姆斯(Adams)微分方程对参变数进行密化，计算公式如下：
[0108] 采用三步四阶阿当姆斯微分方程的隐格式表示为：
[0109]
[0110] 式中，T为插补周期， 分别为ui-2、ui-1、ui、ui+1的一阶导数；
[0111] 将 代入上式，可得：
[0112]
[0113] ΔLi表示控制点di的进给步长。
[0114] 为保证高速NURBS曲线直接插补的要求，保证实时插补的计算速度，采用前、后向差分(如下式所示)结合代替微分的方法对该算法进行简化：
[0115] (后向差分)， (前向差分)，
[0116] (前向差分)， (前向差分)
[0117] 将上式代入式(7)，得到：
[0118]
[0119] 进而可得到简化后的阿当姆斯微分方程插补算法迭代公式：
[0120]
[0121] 继而可得密化后节点矢量，其中， 表示ui+1的预估值。
[0122] 步骤五、根据步骤一中机器人末端执行器在任务空间NURBS描述的自由曲线数学模型，采用自适应速度控制算法对密化后的节点矢量进行修正处理，最终获得最优的密化节点矢量；
[0123] 将参数 作为参数插补的预估值代入NURBS方程，得到相对应的预估插补点：
[0124]
[0125] 是预估插补点，
[0126] 从而得到对应的预估进给步长 为：
[0127]
[0128] 采用预估法得到的预估进给步长 和进给步长ΔL之间存在的偏差，可用相对误差进行评定：
[0129]
[0130] δi表示控制点di处的相对误差。
[0131] 当相对误差δi在允许范围内时，则可认为 为所求p(ui+1)，否则按下式进行修正，直至达到δi允许范围内：
[0132]
[0133] 最终获得优化后的密化节点矢量。
[0134] 步骤六、利用优化的密化节点矢量，并根据步骤一中机器人末端执行器在任务空间NURBS描述的自由曲线数学模型，最终获得曲线上的插补点位置。
[0135] 步骤七、根据相邻空间曲线的插补点位置与姿态数据，进行倍四元数转换；
[0136] 将位姿齐次变换矩阵转换为倍四元数，在给定三维空间中位姿矩阵的计算精度δm和机器人工作空间的最大边界L时，可通过公式：
[0137]
[0138] 得到四维空间的大球即超大球的半径Rl。
[0139] 将位姿齐次变换矩阵转换为倍四元数，其转换算法如下：
[0140] 7-1)对于每一个机器人末端位姿齐次变换矩阵BTE，即机器人末端法兰盘中心坐标系E相对于基坐标系B的齐次变换矩阵，如下所示：
[0141]
[0142] 首先将姿态旋转矩阵R3×3，经旋转矩阵与四元数的转换关系，得到姿态旋转矩阵对T应的旋转四元数Q，同时获得平移向量P＝[px(u),py(u),pz(u)]；
[0143] 7-2)近似的将三维空间的平移向量P转换成四维空间的四元数，转换公式如下：
[0144] Dp＝cos(ψ/2)+sin(ψ/2)v (16)[0145] 式中，ψ＝|P|/Rl，v是平移向量上的单位矢量，v＝P/|P|；当|P|＝0时，v为零矢量；
[0146] 7-3)通过以下公式，计算得到机器人末端位姿转换至四维空间的倍四元数空间位姿 的G部和H部；
[0147]
[0148] 式中， 为四元数Dp的共轭；
[0149] 7-4)倍四元数的双旋转轨迹进行离散化得到一系列插值倍四元数点，需要将其转换成旋转四元数和平移向量，转换算法如下：
[0150] Q＝(G+H)/(2cosψ) (17)[0151]
[0152] 式中，
[0153] 步骤八、对插补所得机器人末端执行器位姿进行逆运动学处理，获得关节角度，并驱动关节运动。
[0154] 具体方法为：将机器人任务空间中相邻插补点视为起止点，则可将起止点姿态旋转矩阵Rs与对应的平移向量Ps、姿态旋转矩阵Re与对应的平移向量Pe数据，分别转换成倍四元数空间位姿：
[0155]
[0156] 其中， 为姿态旋转矩阵Rs对应的倍四元数空间位姿，Gs和Hs为 对应的单位四元数， 为姿态旋转矩阵Re对应的倍四元数空间位姿，Ge和He为 对应的单位四元数。
[0157] 分别对起止点倍四元数空间位姿的G部和H部进行球面线性插值，可得：
[0158]
[0159]
[0160] 式中，G(t)表示起止点倍四元数空间位姿的G部的球面线性插值，H(t)表示起止点倍四元数空间位姿的H部的球面线性插值，α＝arccos(Gs·Ge)，β＝arccos(Hs·He)(Gs·Ge、Hs·He分别为Gs与Ge、Hs与He两四元数的点积)，l(t)∈[0,1]可通过归一化插补周期获得。
[0161] 根据上式，倍四元数空间位姿的球面线性插值可表示为：
[0162]
[0163] 该式是倍四元数球面线性插值的一种表示方法，在实际的插值计算过程中包含倍四元数的G部与H部两个部分的球面线性插值，分别通过G部与H部的单位四元数的球面线性插值完成倍四元数的球面线性插值，获得插补中间点的倍四元数，倍四元数球面线性插补空间直线段的位置与姿态分别如图2和3所示，继而通过步骤七的逆过程将倍四元数转化为插补中间点的位姿齐次变换矩阵T。最后通过机器人模型的逆运动学处理，获得实时关节角度，并驱动各关节运动。
[0164] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
