参数自适应密化的机器人NURBS曲线运动插补方法技术领域
本发明涉及诸如焊接、切割、喷涂、搬运等工业机器人先进制造产业技术领域，特
别是涉及一种参数自适应密化的机器人NURBS曲线运动插补方法。
背景技术
工业机器人控制系统要求能够在一定的控制策略的作用下，使机器人能够快速准
确地根据预先规划的轨迹运行，其中，插补算法作为实现工业机器人运动控制的软件核心，
是实现工业机器人复杂轨迹规划算法的基础，其性能决定着工业机器人运动的实时性和精
度的好坏，是工业机器人机器人运动控制特性的重要评价指标之一。
传统的插补算法主要有直线和圆弧插补等，当需要复杂曲线时，需要通过直线或
圆弧的逼近来完成，这种实现方法存在以下问题：①采用直线或圆弧逼近时，会产生大量的
端点数据，造成数据量过大；②大量的小线段插补引起进给速度的波动，影响加工效率和精
度。
随着复杂曲线加工需求的增多以及加工精度和效率要求越来越高，曲线插补算法
成为研究热点。其中，NURBS(Non-Uniform Rational B-Spline，非均匀有理B样条)描述的
曲线曲面技术得到重视，并在CNC中得到广泛研究，也引起了机器人界的关注。NURBS曲线插
补算法可以应用于复杂轨迹插补，但计算过程复杂，且机器人关节在加减速过程中存在振
动和冲击。总的来说，上述常用的工业机器人运动轨迹插补方法存在以下问题：1)现有的插
补算法应用于复杂曲线插补过程的运动平稳性和运动精度较低，导致目前对于复杂曲线插
补问题仍然没有完善的解决方法；2)现有的NURBS插补算法多采用前瞻方法来确定减速点
的位置，只能对减速过程进行规划，而不能完全保证加速过程的柔顺性，无法应用于工业场
合；3)现有的插补算法不能解决在曲率变化大的地方，所需减速距离长的情况，导致很难满
足生产过程的安全要求。
发明内容
本发明的目的是为了克服已有的技术缺陷，解决基于三次NURBS曲线的工业机器
人运动轨迹插补方法中复杂曲线插补过程的运动平稳性和运动精度较低问题、只能对减速
过程进行规划而不能完全保证加速过程的柔顺性问题以及在曲率变化较大的地方所需减
速距离较长的问题，本发明提供一种参数自适应密化的机器人NURBS曲线运动插补方法，实
现工业机器人高效率、高精度且兼顾柔性的轨迹插补。
本发明采用一种参数自适应密化的机器人NURBS曲线运动插补方法，该方法包括
以下步骤：
步骤10)根据输入的运动参数，获得NURBS曲线表达式及参数密化过程中采用的导
函数表达式；
步骤20)利用改进的Admas微分方程，对步骤10)输入的运动参数中的节点矢量u进
行预估，并根据约束条件，自适应对运动参数中的节点矢量u进行校正；
步骤30)通过分段进行S型曲线位置插补，对自适应过程中的运动参数进行平滑处
理；
步骤40)采用球面线性插值slerp方法完成姿态插补，得到的位置插补坐标和姿态
插补坐标；
步骤50)对步骤40)得到的位置插补坐标和姿态插补坐标，进行机器人正逆解计
算，得到关节角插补序列。
作为优选例：所述的步骤10)具体包括：
步骤101)确定三次NURBS曲线表达式：对于给定的控制顶点{d0,d1,...,dn}及其权
值{ω0,ω1,...,ωn}和节点矢量{u0,u1,...,un+k+1}，其中，k＝3，得到三次NURBS曲线表达
式p(u)，如式(1)所示：
其中，ωi表示控制顶点di的权值，di表示第i个控制顶点，Ni,3表示3次B样条基函
数，B3、B2、B1和B0分别为分子系数；b3、b2、b1和b0分别为分母系数；
步骤102)求解三次NURBS曲线第i-2段表达式：
令其中3≤j≤
n+1，代入式(1)，得到式(2)：
式中，dj表示第j个控制顶点，ωj表示控制顶点dj的权值，Nj,3表示3次B样条基
函数，di-3表示第i-3个控制顶点，ωi-3表示控制顶点di-3的权值，di-2表示第i-2个控制
顶点，ωi-2表示控制顶点di-2的权值，di-1表示第i-1个控制顶点，ωi-1表示控制顶点di-1
的权值，di表示第i个控制顶点，ωi表示控制顶点di的权值，上标T表示转置，
步骤103)求解NUBRS曲线第i-2段表达式的分子系数和分母系数：将Mi代入式(2)，
得分子系数和分母系数分别为：
步骤104)计算NURBS曲线的导函数表达式：首先在式(2)所示的NURBS曲线表达式
上，利用求导公式对节点矢量u求导，获得NURBS曲线的一阶导函数；然后对一阶导函数再次
利用求导公式对节点矢量u求导，获得NURBS曲线的二阶导函数。
作为优选例：所述的步骤20)具体包括：
步骤201)密化自变量u：
当参数u索引i>2时，根据式(6)预估下一个参数；
当参数u索引1≤i≤2时，采用一阶泰勒级数展开，如式(3)所示：
式中，ui+1表示规划节点矢量，ui表示已知节点矢量，ΔLi表示规划周期内
的步长，T表示规划周期，表示机器人末端速度，
px(u)表示直角坐标系下x轴方向位置插补序列，py(u)表示直角坐标系下y轴方向位置插补
序列，pz(u)表示直角坐标系下z轴方向位置插补序列；
根据三步四阶Admas微分方程，表示参数密化的递推公式如式(4)所示：
其中，ui+1表示规划节点矢量，表示规划节点矢量的导数，ui表示已知的第i个节
点矢量，表示已知的第i个节点矢量的导数，表示已知的第i-1个节点矢量的导数，
表示已知的第i-2个节点矢量的导数；
步骤202)用差分代替微分：式(4)中的微分采用式(5)替换为差分，如式(6)所示：
其中，表示预估参数；
步骤203)计算相对误差：根据预估参数获得预估点位置坐标并获得
相应的预估步长如式(7)所示：
其中，表示预估步长，表示预估点x轴位置坐标，表示预估点y轴位置坐
标，表示预估点z轴位置坐标，xi表示已知点x轴位置坐标，yi表示已知点y轴位置坐标，zi
表示已知点z轴位置坐标；
计算预估步长与期望步长ΔLi的相对误差δi，如式(8)所示：
其中，期望步长ΔLi为自适应步长调节过程中获得的步长；
步骤204)校正预估参数：如果相对误差δi超出给定阈值，则采用式(9)对预估参数
进行校正：
其中，表示预估的插补点参数；
若预估的插补点数目大于等于1，则对预估的插补点进行位置插补，否则对。
作为优选例：所述的步骤203)中，计算期望步长具体包括：
步骤2031)计算弦高误差约束：采用式(10)计算弦高误差：
其中，为预估的插补点参数，ui为已知的插补点参数，pi为ui的插补点坐标，
为的插补点坐标，为和ui的中位点坐标，ei为预估点与已知点的距离，为
弦高误差；
设允许的最大弦高误差为hmax，则最大弦高误差约束下，允许的最大步长ΔLi1如式
(11)所示：
步骤2032)计算最大进给速度约束：在给定最大进给速度vmax约束下，允许的最大
步长ΔLi2如式(12)所示：
ΔLi2＝vmaxT 式(12)
步骤2033)计算给定最大进给加速度约束：在给定最大进给加速度约束下，允许的
步长ΔLi3如式(13)所示，其中，进给加速度不能超过给定最大进给加速度amax；
ΔLi-1-amaxT2≤ΔLi3≤ΔLi-1+amaxT2 式(13)
其中，ΔLi-1表示第i-2插补点和第i-1插补点间的步长；
即步长满足式(14)：
其中，ΔLi表示第i-1插补点和第i插补点间的步长；
如果ΔLi3>ΔLi-1+amaxT2，则令ΔLi3＝ΔLi-1+amaxT2；如果ΔLi-1-amaxT2>ΔLi3，则令
ΔLi3＝ΔLi-1-amaxT2；
步骤2034)计算给定最大法向加速度约束：
首先根据NURBS插补过程中进给速度vi与法向加速度ani的关系，采用式(15)计算
法向加速度ani：
式中，ρi表示ΔLi的曲率半径；
设允许的最大法向加速度为anmax，采用式(16)获得最大法向加速度约束下允许的
最大步长ΔLi4：
根据式(15)和式(16)，得出即最大法向加速度约束下允许的最
大步长为ΔLi4，如式(17)所示：
步骤2035)步长自适应优化调节：在自适应步长规划过程中，当前段的期望步长Δ
Li满足式(18)：
通过上述自适应步长调节获得一系列期望步长序列，然后选取步长序列中的极小
值点作为曲率最大的点，即拐点，最后根据获得的拐点将期望步长序列分成若干段，并记录
每段的总长度，以及拐点处的期望步长。
作为优选例：所述的步骤30)具体包括：首先根据拐点步长和各段的长度，对每段
采用七段式S曲线速度规划，获得速度序列，从而得到各个周期的进给步长；然后再次通过
参数密化过程获得参数u序列；最后每隔插补周期T＇取参数u，获得在直角坐标系下的位置
插补序列p(u)。
作为优选例：所述的步骤40)具体包括：
步骤401)采用四元数表示姿态：将起点、终点姿态以及中间插补姿态采用四元数
的表示形式，记法如下：
[r,q]＝[r,(R,P,Y)]
其中，r为姿态插补系数，取值范围为[0，1]；q为姿态向量；R为插补点的α旋转轴的
姿态值，P为插补点的β旋转轴的姿态值，Y为插补点的γ旋转轴的姿态值；
步骤402)选定姿态插补系数r：将已完成位置插补的总步长之和与NURBS曲线的总
长作为姿态插补系数r；
步骤403)采用slerp插补方法完成姿态插补，如式(19)所示：
qr＝k0qbeg+k1qend 式(19)
其中，qr表示系数r的姿态向量，qbeg表示起始姿态向量，qend表示终止姿态向量，k0
表示起始点系数，k1表示终止点系数；
ω表示qbeg到qend弧所截的角；
采用式(20)所示slerp插补方法完成姿态插补：
作为优选例：所述步骤50)具体为：首先根据位置插补坐标和姿态插补坐标，经机
器人正逆解计算得到对应的关节角插补序列，然后将该序列发送给伺服控制器，以控制机
器人完成相应的运动操作。
有益结果：本发明采用参数自适应密化的方法有效地解决了NURBS曲线插补加减
速过程的柔顺性问题以及曲率变化较大的地方所需减速距离较长的问题，通过改进的
Adams微分方程方法及自适应步长调节对参数进行预估-校正，从弦高误差、给定最大进给
速度、给定最大进给加速度和给定最大法向加速度四个约束条件下获得步长序列，提高了
预估参数的精度，通过规划周期T与插补周期T＇不同的方式，对自适应参数密化过程进行进
一步密化，降低规划误差。结合S型曲线速度规划方法对运动参数进行分段平滑处理，极大
的降低了计算量的同时保证了计算精度，实现了柔性加减速。此方法简单高效，能够满足工
业机器人在复杂曲线插补过程中的的运动平稳性和运动精度的需求，提高机器人的工作效
率和柔性加工能力，具有广阔的应用前景和良好的经济效益。
附图说明
图1为本发明实施例的流程图；
图2为本发明实施例中步骤10)的流程图；
图3为本发明实施例中步骤20)和步骤30)的流程图；
图4为本发明实施例中步骤20)参数密化校正过程示意图；
图5为本发明实施例中步骤203)的流程图；
图6为本发明实施例中步骤2031)中弦高误差示意图；
图7为本发明实施例中步骤2031)中弦高误差计算方法对比示意图；
图8为本发明实施例中步骤40)的流程图。
具体实施方式
下面结合附图和具体实施方式对本发明做进一步说明。
如图1所示，本发明的一种参数自适应密化的机器人NURBS曲线运动插补方法，包
括以下步骤：
步骤10)根据输入的运动参数，获得NURBS曲线表达式及参数密化过程中采用的导
函数表达式。
步骤10)首先根据给定的NURBS控制顶点及其权值、节点矢量等参数确定三次
NURBS曲线的表达式形式；其次选择合理的时间间隔，确定每段NURBS曲线的表达式；然后求
解每段NURBS曲线的分子分母系数；最后在NURBS曲线表达式的基础上，获得NURBS曲线的一
阶和二阶导函数表达式。步骤10)可以以降低插补阶段任务计算量。如图2所示，所述的步骤
10)具体包括步骤101)至步骤104)：
步骤101)确定三次NURBS曲线表达式：对于给定的控制顶点{d0,d1,...,dn}及其权
值{ω0,ω1,...,ωn}和节点矢量{u0,u1,...,un+k+1}，其中，k＝3，得到三次NURBS曲线表达
式p(u)，如式(1)所示：
其中，ωi表示控制顶点di的权值，di表示第i个控制顶点，Ni,3表示3次B样条基函
数，B3、B2、B1和B0分别为分子系数；b3、b2、b1和b0分别为分母系数；
步骤102)求解三次NURBS曲线第i-2段表达式：
令其中3≤j≤
n+1，代入式(1)，得到式(2)：
式中，dj表示第j个控制顶点，ωj表示控制顶点dj的权值，Nj,3表示3次B样条
基函数，di-3表示第i-3个控制顶点，ωi-3表示控制顶点di-3的权值，di-2表示第i-2个控
制顶点，ωi-2表示控制顶点di-2的权值，di-1表示第i-1个控制顶点，ωi-1表示控制顶点di-1
的权值，di表示第i个控制顶点，ωi表示控制顶点di的权值，上标T表示转置，
步骤103)求解NUBRS曲线第i-2段表达式的分子系数和分母系数：将Mi代入式(2)，
得分子系数和分母系数分别为：
步骤104)计算NURBS曲线的导函数表达式：首先在式(2)所示的NURBS曲线表达式
上，利用求导公式对节点矢量u求导，获得NURBS曲线的一阶导函数；然后对一阶导函数再次
利用求导公式对节点矢量u求导，获得NURBS曲线的二阶导函数。
步骤20)利用改进的Admas微分方程，对步骤10)输入的运动参数中的节点矢量u进
行预估，并根据约束条件，自适应对运动参数中的节点矢量u进行校正。
步骤20)首先对NURBS曲线表达式的自变量u采用Admas微分方程法进行密化；然后
采用差分代替微分法以降低计算量；最后根据预估步长与期望步长的相对误差对预估参数
进行校正以保证计算精度。如图3所示，所述的步骤20)具体包括：
步骤201)密化自变量u：
当参数u索引i>2时，根据式(6)预估下一个参数；
当参数u索引1≤i≤2时，采用一阶泰勒级数展开，如式(3)所示：
式中，ui+1表示规划节点矢量，ui表示已知节点矢量，ΔLi表示规划周期内
的步长，T表示规划周期，表示机器人末端速度，
px(u)表示直角坐标系下x轴方向位置插补序列，py(u)表示直角坐标系下x轴方向位置插补
序列，pz(u)表示直角坐标系下x轴方向位置插补序列；
当精度要求高时，泰勒级数展开需要高阶求导，计算复杂，因此根据三步四阶
Admas微分方程，表示参数密化的递推公式如式(4)所示：
其中，ui+1表示规划节点矢量，表示规划节点矢量的导数，ui表示已知的第i个节
点矢量，表示已知的第i个节点矢量的导数，表示已知的第i-1个节点矢量的导数，
表示已知的第i-2个节点矢量的导数；
步骤202)用差分代替微分：式(4)中的微分采用式(5)替换为差分，如式(6)所示：
其中，表示预估参数；
步骤203)计算相对误差：根据预估参数获得预估点位置坐标并获得
相应的预估步长如式(7)所示：
其中，表示预估步长，表示预估点x轴位置坐标，表示预估点y轴位置坐
标，表示预估点z轴位置坐标，xi表示已知点x轴位置坐标，yi表示已知点y轴位置坐标，zi
表示已知点z轴位置坐标；
计算预估步长与期望步长ΔLi的相对误差δi，如式(8)所示：
其中，期望步长ΔLi为自适应步长调节过程中获得的步长；
步骤204)校正预估参数：如图4所示，如果相对误差δi超出给定阈值，则采用式(9)
对预估参数进行校正：
其中，表示预估的插补点参数；
若预估的插补点数目大于等于1，则对预估的插补点进行位置插补。
在步骤203)中，根据约束条件，自适应对运动参数中的节点矢量u进行校正。
步骤203)首先采用自适应步长调节的方法，根据弦高误差、给定最大进给速度、给
定最大进给加速度和给定最大法向加速度等四个约束条件，对步长进行自适应优化调节；
然后通过选取拐点将步长序列分为若干段。如图5所示，所述的步骤203)具体包括步骤
2031)至步骤2035)：
步骤2031)计算弦高误差约束：弦高误差是指以直线代替曲线的所产生的误差，如
图6所示。本发明采用式(10)所示的近似方法计算弦高误差，即近似等于如图7中所示的弧
长ε。
其中，为预估的插补点参数，ui为已知的插补点参数，pi为ui的插补点坐标，
为的插补点坐标，为和ui的中位点坐标，εi为预估点与已知点的距离，为
弦高误差；
设允许的最大弦高误差为hmax，则最大弦高误差约束下，允许的最大步长ΔLi1如式
(11)所示：
步骤2032)计算最大进给速度约束：在给定最大进给速度vmax约束下，允许的最大
步长ΔLi2如式(12)所示：
ΔLi2＝vmaxT 式(12)
步骤2033)计算给定最大进给加速度约束：在给定最大进给加速度约束下，允许的
步长ΔLi3如式(13)所示，其中，进给加速度不能超过给定最大进给加速度amax；
ΔLi-1-amaxT2≤ΔLi3≤ΔLi-1+amaxT2 式(13)
其中，ΔLi-1表示第i-2插补点和第i-1插补点间的步长；
即步长满足式(14)：
其中，ΔLi表示第i-1插补点和第i插补点间的步长；
如果ΔLi3>ΔLi-1+amaxT2，则令ΔLi3＝ΔLi-1+amaxT2；如果ΔLi-1-amaxT2>ΔLi3，则令
ΔLi3＝ΔLi-1-amaxT2；
步骤2034)计算给定最大法向加速度约束：
首先根据NURBS插补过程中进给速度vi与法向加速度ani的关系，采用式(15)计算
法向加速度ani：
式中，ρi表示ΔLi的曲率半径；
设允许的最大法向加速度为anmax，采用式(16)获得最大法向加速度约束下允许的
最大步长ΔLi4：
根据式(15)和式(16)，得出即最大法向加速度约束下允许的最
大步长为ΔLi4，如式(17)所示：
步骤2035)步长自适应优化调节：在自适应步长规划过程中，当前段的期望步长Δ
Li满足式(18)：
通过上述自适应步长调节获得一系列期望步长序列，然后选取步长序列中的极小
值点作为曲率最大的点，即拐点，最后根据获得的拐点将期望步长序列分成若干段，并记录
每段的总长度，以及拐点处的期望步长。
步骤30)通过分段进行S型曲线位置插补，对自适应过程中的运动参数进行平滑处
理。
步骤30)可以实现柔性加减速。步骤30)具体包括：首先根据拐点步长和各段的长
度，对每段采用七段式S曲线速度规划，获得速度序列，从而得到各个周期的进给步长；然后
再次通过参数密化过程获得参数u序列；最后每隔插补周期T＇取参数u，获得在直角坐标系
下的位置插补序列p(u)。
步骤40)采用球面线性插值slerp方法完成姿态插补，得到的位置插补坐标和姿态
插补坐标。
步骤40)采用球面线性插值slerp(Spherical Linear Interpolation)完成姿态
插补，首先将起点、终点姿态以及中间插补姿态采用四元数的表示形式；然后将已完成位置
插补的总步长之和与NURBS曲线的总长作为姿态插补参数r；最后根据起点和终点的姿态采
用slerp插补方法完成NURBS姿态插补。如图8所述，所述的步骤40)具体包括步骤401)至步
骤403)：
步骤401)采用四元数表示姿态：将起点、终点姿态以及中间插补姿态采用四元数
的表示形式，记法如下：
[r,q]＝[r,(R,P,Y)]
其中，r为姿态插补系数，取值范围为[0，1]；q为姿态向量；R为插补点的a旋转轴的
姿态值，P为插补点的β旋转轴的姿态值，Y为插补点的γ旋转轴的姿态值；
步骤402)选定姿态插补系数r：将已完成位置插补的总步长之和与NURBS曲线的总
长作为姿态插补系数r；
步骤403)采用slerp插补方法完成姿态插补，如式(19)所示：
qr＝k0qbeg+k1qend 式(19)
其中，qr表示系数r的姿态向量，qbeg表示起始姿态向量，qend表示终止姿态向量，k0
表示起始点系数，k1表示终止点系数；
ω表示qbeg到qend弧所截的角；
采用式(20)所示slerp插补方法完成姿态插补：
步骤50)对步骤40)得到的位置插补坐标和姿态插补坐标，进行机器人正逆解计
算，得到关节角插补序列。
所述步骤50)具体为：首先根据位置插补坐标和姿态插补坐标，经机器人正逆解计
算得到对应的关节角插补序列，然后将该序列发送给伺服控制器，以控制机器人完成相应
的运动操作。
应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，
还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未
明确的各组成部分均可用现有技术加以实现。