一种高阶复杂约束条件下高效的工业机器人加工节能轨迹规划方法与流程

1.本发明涉及工业机器人轨迹规划技术与数控加工技术领域，具体是一种高 阶复杂约束条件下高效的工业机器人加工节能轨迹规划方法。


背景技术：

2.随着制造业人力成本的上升及工人的短缺，作为自动化重要手段的工业机 器人得到了许多制造业的青睐。irf2019报告显示2018年全球有超过380000 台新机器人被安装在工厂中。工业机器人具有高效、低成本、灵活性强等优势， 未来会得到更多应用，因此，其能耗在制造总能耗中必然将占据一个更大比例。 然而，随着全球气候问题与能源短缺问题的日益严重以及能源价格的上涨，迫 切需要提高制造过程的可持续性、减少更多制造产生的能耗。由于机器人具有 较高的能耗水平和较大的节能空间，因此，在机器人制造系统中，能量最小化 对降低生产成本和总co2排放的作用是不言而喻的。
3.沿指定复杂轮廓路径加工是工业机器人应用的一个重要方向，如机器人焊 接、增材制造、抛光、去毛刺和铣削等。在这些加工过程中，若采用一般的保 守轨迹，将导致非常长的加工时间，且工业机器人制造系统会消耗大量电能量。 因此，机器人轨迹优化是降低工业机器人制造系统的能耗和提高加工效率的主 要方式之一，受到了广泛关注。
4.目前关于机器人轨迹的规划已有较多研究，但大多数是以最小化时间为目 标开发的方法。一些学者考虑时间和加速度最优，在关节空间中使用样条插值 法规划机器人点到点(ptp)运动的轨迹。一些学者基于梯形速度曲线近似(tvp) 实现沿指定短/线性路径的时间最优的机器人轨迹规划。一些学者采用伪速度方 式，处理了考虑非凸动力学下机器人沿指定轮廓路径的时间最优轨迹规划。
5.一些学者也关注了机器人制造系统的节能方面的研究。如通过优化布局来 减小机器人系统的能耗；利用机器人的沉余优化关节形位来减小能耗；通过整 个轨迹的线性时间缩放，研究机器人单元的能量最优调度；采用动态的时间缩 放因子获得近似全局能量最优，实现进一步节能。然而这些方法都是在不同轨 迹下使用工业机器人解析能耗模型计算机器人的加工能耗，并对这些轨迹能耗 进行比较，以寻找在约束条件下的能耗最小的轨迹。这些直接优化方法都需要 重复计算包含机器人动力学方程的能耗，导致计算时间成本过高，在高阶复杂 约束情况下尤其如此。因此，这些方法并不适用于沿指定长轮廓路径的工业机 器人节能轨迹规划。
6.近年来，通过修改样条进给速率曲线来优化指定加工路径轨迹的方法吸引 了学者的关注。但现有方法是基于考虑优化效率开发的，能耗优化并未被考虑 在内，因此现有方法并不能直接适用于节能轨迹规划。此外，现有方法几乎没 有考虑进给速率、路径加速度和路径加加速度之间的非线性耦合关系和b样条 进给速率曲线的变化性质对轨迹规划的不利影响。
7.因此，目前仍然缺乏实用的沿长复杂轮廓路径加工的工业机器人最小化能 量或
时间轨迹的规划方法。


技术实现要素：

8.本发明的目的是提供一种高阶复杂约束条件下高效的工业机器人加工节能 轨迹规划方法，包括以下步骤：
9.1)用非均匀有理b样条拟合刀具的现有路径曲线，得到nurbs刀具路径 曲线，步骤包括：
10.1.1)以归一化弧长u＝s/s
∑
为参数，对刀具的现有路径曲线进行nurbs 曲线拟合，得到nurbs刀具路径曲线；
11.1.2)将所述刀具路径曲线按等参数δu离散为d个微元路径获得d 1个采样点{u1，...，ui，...，ud}，所述采样点也作为约束判断点。
12.2)规划参考进给速率轨迹，步骤包括：
13.2.1)在速度约束条件下，计算d 1个路径采样点最大可行速率；
14.2.2)获得速度约束条件下时间最优进给速率轨迹，作为参考进给速率轨迹。
15.3)构造初始能量最优的b样条进给速率曲线，步骤包括：
16.3.1)计算参考轨迹下每个微元路径的三次多项式能耗特征模型，即：
[0017][0018]
式中，为平均进给速率；为参考平均进给速率；δe
∑，i
为能耗；μ
0，i
、μ
1，i
、 μ
2，i
、μ
3，i
、μ
4，i
为多项式系数；参数
[0019]
3.2)在速度约束下优化得到每个微元路径的初始的能量最优的平均进给速 率并令微元路径中间参数位置的进给速率
[0020]
3.3)以路径首尾点和所有微元路径中间参数位置点的进给速率作为拟合数 据点，以归一化弧长u为参数，构造初始能量最优的b样条进给速率曲线f(u)， 即：
[0021][0022]
式中，p为b样条进给速率曲线的次数，p≥4。fi为进给速率；n
i，p
(u)为b 样条基函数；
[0023]
4)对初始能量最优的b样条进给速率曲线进行优化，得到满足所有约束 条件的最优的进给速率轨迹，步骤包括：
[0024]
4.1)判断第k次循环修正的b样条进给速率曲线是否满足刀具加速度加 加速度约束条件和速度约束条件，若是，则进入步骤4.2)，否则，利用刀具加 速度、加加速度约束条件和速度约束条件修改b样条进给速率曲线，且令k＝k 1， 然后进入步骤4.2)；k初始值为0；k＝0时对应的b样条进给速率曲线为初始 能量最优的b样条进给速率曲线；
[0025]
利用刀具加速度加加速度约束条件和速度约束条件修改b样条进给速率曲 线，修改过程中，刀具加速度加加速度约束条件和速度约束条件被分别单独用 于曲线的调整，基于刀具加速度加加速度约束条件的曲线调整循环被嵌入每次 基于速度约束条件的曲线调整之前；修改的步骤包括：
[0026]
4.1.1)令违反点数num1＝0；
[0027]
4.1.2)令路径采样点下标i＝0，违反点数num2＝0，违反点下标l＝0；
[0028]
4.1.3)计算获取当前路径采样点ui的刀具加速度、加加速度；
[0029]
4.1.4)判断刀具加速度、加加速度是否满足刀具加速度、加加速度约束条 件，若不满足，则将路径采样点ui记为违反点，令l＝l 1，u
l
＝ui，num2＝num2 1， 并进入步骤4.1.5)，否则，进入步骤4.1.7)；
[0030]
4.1.5)采用b样条进给速率曲线对归一化弧长u的一阶导数的比例改变， 来减小违反点的刀具加速度、加加速度；修改后违反点u
l
处的一阶导数f
(1)
′
(u
l
) 如下所示：
[0031]f(1)
′
(u
l
)＝γf
(1)
(u
l
)
ꢀꢀ
(3)
[0032]
式中，γ为减小改变比例；γ＜1；f
(1)
(u
l
)为修改前违反点u
l
处的一阶导数；
[0033]
记录的违反点u
l
处的一阶导数减小量δf
(1)
(u
l
)如下所述：
[0034][0035]
式中，为第i个刀具加速度限制，为第i个刀具加加速度限制；ai(u
l
) 为第i个刀具加速度当前值，ji(u
l
)为第i个刀具加加速度当前值；λ为加速满 足约束的速率系数；nt为刀具加速度加加速度约束个数；
[0036]
获取b样条进给速率曲线的一阶导数曲线控制点变化量的计算关系式， 即：
[0037][0038]
式中，n
k，p-1
(u
l
)为b样条基函数；uk″
为b样条进给速率曲线一阶导数曲线 控制点的主影响区间；n为b样条进给速率曲线一阶导数曲线的控制点数，num2 为相应违反点数；此控制点为b样条进给速率曲线一阶导数曲线的形状约束点；
[0039]
引入回调机制的b样条进给速率曲线的控制点变化量δfk计算如下所示：
[0040][0041][0042]
式中，ζ为回调因子；分别表示第k、k-1个控制点变化量的一 阶导数；u
k p
、uk、u
k p 1
、u
k 1
表示控制点；为计算参数；
[0043]
4.1.6)判断i≥d是否成立，若是，则进入步骤4.1.8)，否则，更新路径点 为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回 步骤4.1.3)；
[0044]
4.1.7)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.1.3)；若 成立，则判断num2＝0是否成立，若num2＝0，则进入步骤9)，若num2≠0，进 入步骤8)；
[0045]
4.1.8)根据公式(6)和(7)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线；其中，b样条进给速率曲线控制点fk＝fk δfk，k＝1，...，n-1。
[0046]
判断max{δfk，k＝1，...，n-1}≤δ是否成立，若是，则进入步骤4.1.9)，否则， 返回步骤4.1.2)；δ为控制点变化量阈值；
[0047]
4.1.9)初始化下标i＝0，l＝0；
[0048]
4.1.10)计算获取当前路径采样点ui的进给速率和关节速度；
[0049]
4.1.11)判断当前路径采样点ui的进给速率和关节速度是否满足约束，若是， 则进入步骤4.1.12)，否则，记当前路径采样点ui为违反点，令num1＝num1 1， l＝l 1，u
l
＝ui，并进入步骤4.1.13)；
[0050]
4.1.12)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.1.10)； 若成立，则判断num1＝0是否成立，若num1＝0，则输出优化后的b样条进给速 率曲线，若num1≠0，进入步骤4.1.15)；
[0051]
4.1.13)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0052]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0053][0054]
式中，为第i个速度相关约束对应的进给速率限制，f(u
l
)为违反点u
l
处的当前进给速率，nv为速度约束个数；
[0055]
最终b样条进给速率曲线控制点的减小量通过求解下列线性方程组的最小 二乘解得到：
[0056][0057]
式中，n
k，p
(u
l
)为b样条基函数；n 1为b样条进给速率曲线的控制点数； num1为对应违反点数；
[0058]
4.1.14)判断i≥d是否成立，若是，则进入步骤4.1.15)，否则，更新路径 采样点为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并 返回步骤4.1.10)；
[0059]
4.1.15)根据公式(9)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲线， 并返回步骤4.1.1)；b样条进给速率曲线控制点fk＝fk δfk，k＝1，...，n-1。
[0060]
4.2)判断第k次循环修正的b样条进给速率曲线是否满足关节加速度加 加速度约束条件，若是，则进入步骤4.3)，否则，利用关节加速度、加加速度 约束条件修改b样条进给速率曲线，且令k＝k 1；判断是否k≤nk，若是，则 进入步骤4.3)，否则，返回步骤4.1)；nk为最大迭代次数；
[0061]
利用关节加速度加加速度约束条件修改b样条进给速率曲线，修改过程中 每次迭代调整依次经过两步曲线调整，修改步骤包括：
[0062]
4.2.1)令违反点数num3＝0；
[0063]
4.2.2)令路径采样点下标i＝0，违反点数num4＝0，违反点下标l＝0；
[0064]
4.2.3)计算获取当前路径采样点ui的各关节加速度、加加速度；
[0065]
4.2.4)判断各关节加速度、加加速度是否满足用各关节加速度、加加速度 约束条件，若不满足，则将路径采样点ui记为违反点，令l＝l 1，u
l
＝ui， num4＝num4 1，并进入步骤4.2.5)，否则，进入步骤4.2.7)；
[0066]
4.2.5)采用b样条进给速率曲线对归一化弧长u的一阶导数的比例改变， 来以减
小违反点的关节加速度、加加速度；修改后违反点u
l
处的一阶导数f
(1)
′
(u
l
) 如下所示：
[0067]f(1)
′
(u
l
)＝γf
(1)
(u
l
)
ꢀꢀ
(10)
[0068]
式中，γ为减小的改变比例；f
(1)
(u
l
)为修改前违反点u
l
处的一阶导数；
[0069]
违反点u
l
处的一阶导数减小量δf
(1)
(u
l
)如下所述：
[0070][0071]
式中，为第j个关节加速度限制，为第j个关节加加速度限制，为第j个关节加速度当前值，为第j个关节加加速度当前值；
[0072]
建立b样条进给速率曲线的一阶导数曲线控制点变化量的计算关系式， 即：
[0073][0074]
式中，n
k，p-1
(u
l
)为b样条基函数；uk″
为b样条进给速率曲线的一阶导数曲 线控制点主影响区间；n为b样条进给速率曲线的一阶导数曲线控制点数，num4 为相应的违反点数；
[0075]
引入回调机制的b样条进给速率曲线的一阶导数曲线控制点变化量的 计算式如下所示：
[0076][0077][0078]
式中，为回调因子；
[0079]
4.2.6)判断i≥d是否成立，若是，则进入步骤4.2.8)，否则，更新路径点 为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回 步骤4.2.3)；
[0080]
4.2.7)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.2.3)；若 成立，则判断num4＝0是否成立，若num4＝0，则进入步骤4.2.9)，若num4≠0， 进入步骤8)；
[0081]
4.2.8)根据公式(13)和(14)获得的b样条进给速率曲线的控制点变化 量δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率 曲线，并进入步骤4.2.9)；b样条进给速率曲线控制点fk＝fk δfk，k＝1，...，n-1；
[0082]
4.2.9)初始化i＝0，l＝0；
[0083]
4.2.10)计算获取当前路径采样点ui的各关节速度和加加速度；
[0084]
4.2.11)判断当前路径采样点ui的各关节速度和加加速度是否满足约束，若 是，则进入步骤4.2.12)，否则，记当前路径采样点ui为违反点，令i＝l 1，u
l
＝ui， num3＝num3 1，并进入步骤4.2.13)；
[0085]
4.2.12)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.2.10)； 若成立，则进入步骤4.2.16)；
[0086]
4.2.13)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0087]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0088]
δf(u
l
)＝(γ-1)f(u
l
)
ꢀꢀ
(15)
[0089]
式中，f(u
l
)为当前进给速率，γ可取0.99-0.999；
[0090]
最终b样条进给速率曲线控制点的减小量通过求解下列线性方程组的最小 二乘解得到：
[0091][0092]
式中，n
k，p
(u
l
)为b样条基函数；n 1为b样条进给速率曲线的控制点数； num3为对应的违反点数；
[0093]
4.2.14)判断i≥d是否成立，若是，则进入步骤4.2.15)，否则，更新i＝i 1， 并返回步骤4.2.10)；
[0094]
4.2.15)根据公式(16)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线，并返回步骤4.2.1)；b样条进给速率曲线控制点fk＝fk δfk，k＝1，...，n-1；
[0095]
4.2.16)判断num3＝0是否成立，若成立，则进入步骤17)，否则，返回步 骤15)；
[0096]
4.2.17)判断num4＝0是否成立，若成立，则输出优化后的b样条进给速率 曲线，否则，返回步骤4.2.1)。
[0097]
4.3)利用刀具加速度加加速度约束条件、速度约束条件、关节加速度加加 速度约束条件对当前b样条进给速率曲线进行微调，得到最终优化的进给速率 轨迹输出。
[0098]
基于所有的约束条件微调b样条进给速率曲线，得到并输出最终优化的进 给速率轨迹，微调过程中每次迭代调整依次经过两步曲线调整，微调步骤包括：
[0099]
4.3.1)令违反点数num5＝0；
[0100]
4.3.2)令路径采样点下标i＝0，违反点数num6＝0；
[0101]
4.3.3)计算获取当前路径采样点ui的刀具加速度、加加速度；
[0102]
4.3.4)判断刀具加速度、加加速度是否满足刀具加速度、加加速度约束条 件，若不满足，则按式(17)调节该违反点前后相邻的b样条进给速率曲线的 控制点；否则，进入步骤4.3.7)；
[0103][0104]
式中，fk为违反点前面相邻的b样条进给速率曲线的控制点，f
k 1
为违反 点后面相邻的b样条进给速率曲线的控制点；δ-、δ

为调整值；
[0105]
4.3.6)判断i≥d是否成立，若不成立，则更新路径采样点为当前控制点的 下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回步骤4.3.3)；若成 立，则进入步骤4.3.8)；
[0106]
4.3.7)判断i≥d是否成立，若是，则进入步骤4.3.8)，否则，更新i＝i 1， 并返回步骤4.3.3)；
[0107]
4.3.8)令i＝0，l＝0；
[0108]
4.3.9)计算获取当前路径采样点ui的刀具速度和关节速度、加速度和加加 速度；
[0109]
4.3.10)判断当前路径采样点ui的刀具速度和关节速度、加速度和加加速度 是否满足约束，若是，则进入步骤4.3.11)，否则，记当前路径采样点ui为违反 点，令num5＝num5 1，l＝l 1，u
l
＝ui，并进入步骤4.3.12)；
[0110]
4.3.11)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤9)；若成 立，则进入步骤4.3.14)；
[0111]
4.3.12)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0112]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0113]
δf(u
l
)＝(γ-1)f(u
l
)
ꢀꢀ
(18)
[0114]
式中，f(u
l
)为当前进给速率；
[0115]
最终b样条进给速率曲线控制点的减小量可通过求解下列线性方程组的最 小二乘解得到：
[0116][0117]
式中，n
k，p
(u
l
)为b样条基函数；n l为b样条进给速率曲线的控制点数； hum5为对应的违反点数；
[0118]
4.3.13)判断i≥d是否成立，若是，则进入步骤4.3.15)，否则，更新i＝i 1， 并返回步骤4.3.9)；
[0119]
4.3.14)判断num5＝0是否成立，若成立，则进入步骤4.3.16)，否则，进入 步骤4.3.15)；
[0120]
4.3.15)根据公式(19)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线，并返回步骤1)；b样条进给速率曲线控制点fk＝fk δfk，k＝1，...，n-1；
[0121]
4.3.16)判断num6＝0是否成立，若成立，则输出最终的优化后的b样条进 给速率曲线，否则，返回步骤4.3.1)。
[0122]
所述刀具进给速率的约束条件是刀具速度小于刀具进给速率阈值；关节速 度的约束条件是关节速度小于关节速度阈值；加速度的约束条件是加速度小于 加速度阈值；加加速度的约束条件是加加速度小于加加速度阈值。
[0123]
本发明的技术效果是毋庸置疑的，本发明基于初始最优的b样条进给速率 曲线的最小化修改，提出一种计算高效且实用的沿指定轮廓路径的机器人节能 轨迹规划方法。本发明基于不同特征的约束分步式地修改初始b样条进给速率 曲线，以满足要求的限制。本发明特别引入了进给速率曲线的回调机制，解决 了进给速率、刀具加速度和加加速度之间的非线性耦合关系和b样条进给速率 曲线的变化性质对轨迹规划带来的不利影响。因为本发明不用重复计算包含机 器人动力学方程的能耗模型，可以显著的减小节能轨迹规划时间，能获得工业 机器人沿复杂路径加工的复杂约束条件下的优化轨迹，实现系统的节能增效。
附图说明
[0124]
图1为工业机器人加工系统及沿指定路径运动示意图；
[0125]
图2为工业机器人加工节能轨迹规划方法原理图；
[0126]
图3为基于刀具加速度、加加速度约束和速度相关约束的b样条进 给速率曲线修改流程；
[0127]
图4为基于关节加速度、加加速度约束的b样条进给速率曲线修改 流程；
[0128]
图5为基于所有约束限制微调b样条进给速率曲线流程；
[0129]
图6为实施例3的路径曲线；
[0130]
图7为基于刀尖加速度、加加速度约束和速度相关的约束的b样条 进给速率曲线修改流程(实施例3)；
[0131]
图8为基于关节加速度、加加速度约束的b样条进给速率曲线修改 流程(实施例3)；
[0132]
图9为基于所有约束限制微调b样条进给速率曲线流程(实施例3)；
[0133]
图10为实施例3路径的节能轨迹；图10(a)为实施例3路径的节 能轨迹i；图10(b)为实施例3路径的节能轨迹ii；图10(c)为实施 例3路径的节能轨迹iii；
[0134]
图11为实施例4的路径曲线；
[0135]
图12为实施例5路径的节能轨迹；图12(a)为实施例5路径的节 能轨迹i；图12(b)为实施例5路径的节能轨迹ii；图12(c)为实施 例5路径的节能轨迹iii；
[0136]
图中，工业机器人1，刀具2，主轴系统3，刀具路径4。
具体实施方式
[0137]
下面结合实施例对本发明作说明，但不应该理解为本发明上述主题范围 仅限于下述实施例。在不脱离本发明上述技术思想的情况下，根据本领域普通 技术知识和惯用手段，做出各种替换和变更，均应包括在本发明的保护范围内。
[0138]
实施例1：
[0139]
一种高阶复杂约束条件下高效的工业机器人加工节能轨迹规划方法，包括 以下步骤：
[0140]
1)用非均匀有理b样条(nurbs)拟合原刀具路径曲线，并对所述刀具 路径曲线进行离散化，得到d 1个路径采样点，步骤包括：
[0141]
1.1)以归一化弧长u＝s/s
σ
为参数，对原刀具路径进行nurbs曲线拟合， 得到nurbs刀具路径曲线；
[0142]
1.2)将所述刀具路径曲线按等参数δu离散为d个微元路径获得d 1个路采样径点{u1，...，ui，...，ud}，也作为后面的约束判断点。
[0143]
2)规划参考进给速率轨迹，包括：
[0144]
2.1)在速度约束条件下，计算d 1个路径采样点最大可行速率；
[0145]
2.2)获取速度约束条件下时间最优进给速率轨迹，作为参考进给速率轨迹。
[0146]
3)获得初始能量最优的b样条进给速率曲线，步骤包括：
[0147]
3.1)计算参考轨迹下每个微元路径的三次多项式能耗特征模型，即：
[0148][0149]
式中，为平均进给速率；为参考平均进给速率；δe
∑，i
为能耗；μ
0，i
、μ
1，i
、 μ
2，i
、
μ
3，i
、μ
4，i
为多项式系数；参数
[0150]
3.2)在速度约束下优化得到每个微元路径的初始的能量最优的平均进给速 率并令微元路径中间参数位置的进给速率
[0151]
3.3)以路径首尾点和所有微元路径中间参数位置点的进给速率作为拟合数 据点，以归一化弧长u为参数，构造初始能量最优的b样条进给速率曲线f(u)， 即：
[0152][0153]
式中，p为b样条进给速率曲线的次数，要求p≥4。fi为进给速率；n
i，p
(u) 为b样条基函数；
[0154]
4)对初始能量最优的b样条进给速率曲线进行优化，得到满足约束条件 的最优进给速率轨迹。优化初始能量最优的b样条进给速率曲线含有多次循环 修正过程，步骤包括：
[0155]
4.1)判断第k次循环修正的b样条进给速率曲线是否满足刀具加速度加 加速度约束条件和速度约束条件，若是，则进入步骤4.2)，否则，利用刀具加 速度、加加速度约束条件和速度约束条件修改b样条进给速率曲线，且令k＝k 1， 然后进入步骤4.2)；k初始值为0；k＝0时对应的b样条进给速率曲线为初始 能量最优的b样条进给速率曲线；
[0156]
利用刀具加速度、加加速度约束条件和速度约束条件修改b样条进给速率 曲线的步骤包括(修正过程见图3)：
[0157]
4.1.1)令违反点数num1＝0；
[0158]
4.1.2)令路径采样点下标i＝0，违反点数num2＝0，违反点下标l＝0；
[0159]
4.1.3)计算获取当前路径采样点ui的刀具加速度、加加速度；
[0160]
4.1.4)判断刀具加速度、加加速度是否满足刀具加速度、加加速度约束条 件，若不满足，则将路径采样点ui记为违反点，令l＝l 1，u
l
＝ui，num2＝num2 1， 并进入步骤4.1.5)，否则，进入步骤4.1.7)；
[0161]
4.1.5)采用b样条进给速率曲线对归一化弧长u的一阶导数的比例改变， 来减小违反点的刀具加速度、加加速度；修改后违反点u
l
处的一阶导数f
(1)
′
(u
l
) 如下所示：
[0162]f(1)
′
(u
l
)＝γf
(1)
(u
l
)
ꢀꢀ
(3)
[0163]
式中，γ为减小改变比例(γ＜1)；f
(1)
(u
l
)为修改前违反点u
l
处的一阶导数；
[0164]
记录的违反点u
l
处的一阶导数减小量δf
(1)
(u
l
)如下所述：
[0165][0166]
式中，为第i个刀具加速度限制，为第i个刀具加加速度限制；ai(u
l
) 为第i个刀具加速度当前值，ji(u
l
)为第i个刀具加加速度当前值；λ为加速满 足约束的速率系数，可取0.99；nt为刀具加速度加加速度约束个数；
[0167]
获取b样条进给速率曲线的一阶导数曲线的控制点变化量的计算关系 式，即：
[0168][0169]
式中，n
k，p-1
(u
l
)为b样条基函数；uk″
为b样条进给速率曲线的一阶导数曲 线的控
制点主影响区间；n为b样条进给速率曲线的一阶导数曲线的控制点数， num2为相应的违反点数；
[0170]
引入回调机制的b样条进给速率曲线的控制点变化量δfk的计算式如下所 示：
[0171][0172][0173]
式中，为回调因子，可取0.45-0.499；分别表示第k、k-1个控 制点变化量的一阶导数；u
k p
、uk、u
k p 1
、u
k 1
表示控制点；为计算参数；
[0174]
4.1.6)判断i≥d是否成立，若是，则进入步骤4.1.8)，否则，更新路径点 为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回 步骤4.1.3)；
[0175]
4.1.7)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.1.3)；若 成立，则判断num2＝0是否成立，若num2＝0，则进入步骤4.1.9)，若num2≠0， 进入步骤4.1.8)；
[0176]
4.1.8)根据6)和(7)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线
[0177]fk
＝fx δfk，k＝1，...，n-1
[0178]
判断max{δfk，k＝1，...，n-1}≤δ是否成立，若是，则进入步骤4.1.9)，否则， 返回步骤4.1.2)；δ为控制点变化量阈值；
[0179]
4.1.9)初始化下标i＝0，l＝0；
[0180]
4.1.10)计算获取当前路径采样点ui的进给速率和关节速度；
[0181]
4.1.11)判断当前采样点ui的进给速率和关节速度是否满足约束，若是，则 进入步骤4.1.12)，否则，记当前路径采样点ui为违反点，令num1＝num1 1，l＝l 1， u
l
＝ui，并进入步骤13)；
[0182]
4.1.12)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.1.10)； 若成立，则判断num1＝0是否成立，若num1＝0，则输出优化后的b样条进给速 率曲线，若num1≠0，进入步骤4.1.15)；
[0183]
4.1.13)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0184]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0185][0186]
式中，为第i个速度相关约束对应的进给速率限制，f(u
l
)为违反点u
l
处的 当前进给速率，nv为速度约束个数；
[0187]
最终b样条进给速率曲线控制点的减小量可通过求解下列线性方程组的最 小二乘解得到：
[0188][0189]
式中，n
k，p
(u
l
)为b样条基函数；n 1为b样条进给速率曲线的控制点数； num1为对
应的违反点数；
[0190]
4.1.14)判断i≥d是否成立，若是，则进入步骤4.1.15)，否则，更新路径 点为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回 步骤4.1.10)；
[0191]
4.1.15)根据公式(9)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线
[0192]fk
＝fk δfk，k＝1，...，n-1
[0193]
，并返回步骤4.1.1)。
[0194]
4.2)判断第k次循环修正的b样条进给速率曲线是否满足关节加速度、 加加速度约束条件，若是，则进入步骤4.3)，否则，利用关节加速度、加加速 度约束条件修改b样条进给速率曲线，且令k＝k 1；判断是否k≤nk，若是， 则进入步骤4.3)，否则，返回步骤4.1)；nk为最大迭代次数；nk一般取4；
[0195]
利用关节加速度、加加速度约束条件修改b样条进给速率曲线的步骤包括 (修正过程见图4)：
[0196]
4.2.1)令违反点数num3＝0；
[0197]
4.2.2)令初始路径采样点下标i＝0，违反点数num4＝0，违反点下标l＝0；
[0198]
4.2.3)计算获取当前路径采样点ui的各关节加速度、加加速度；
[0199]
4.2.4)判断各关节加速度、加加速度是否满足各关节加速度、加加速度约 束条件，若不满足，则将路径采样点ui记为违反点，令l＝l 1，u
l
＝ui，num4＝num4 1 并进入步骤4.2.5)，否则，进入步骤4.2.7)；
[0200]
4.2.5)采用b样条进给速率曲线对归一化弧长u的一阶导数的比例改变， 来减小违反点的刀具加速度、加加速度；修改后违反点u
l
处的一阶导数f
(1)
′
(u
l
) 如下所示：
[0201]f(1)
′
(u
l
)＝γf
(1)
(u
l
)
ꢀꢀ
(10)
[0202]
式中，γ为减小的改变比例；f
(1)
(u
l
)为修改前违反点u
l
处的一阶导数；
[0203]
违反点u
l
处的一阶导数减小量δf
(1)
(u
l
)如下所述：
[0204][0205]
式中，为第j个关节加速度限制，为第j个关节加加速度限制，为第j个关节加速度当前值，为第j个关节加加速度当前值；
[0206]
建立b样条进给速率曲线的一阶导数曲线的控制点变化量的计算关系 式，即：
[0207][0208]
式中，n
k，p-1
(u
l
)为b样条基函数；uk″
为b样条进给速率曲线的一阶导数曲 线的控制点主影响区间；n为b样条进给速率曲线的一阶导数曲线的控制点数， num4为相应的违反点数；
[0209]
引入回调机制的b样条进给速率曲线的一阶导数曲线的控制点变化量的计算式如下所示：
[0210][0211][0212]
式中，为回调因子，可取0.45-0.499；
[0213]
4.2.6)判断i≥d是否成立，若是，则进入步骤4.2.8)，否则，更新路径点 为当前控制点的下一个控制点主影响区间内的第一个采样点(i＝ind)，并返回 步骤4.2.3)；
[0214]
4.2.7)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.2.3)；若 成立，则判断num4＝0是否成立，若num4＝0，则进入步骤4.2.9)，若num4≠0， 进入步骤4.2.8)；
[0215]
4.2.8)根据公式(13)和(14)获得的b样条进给速率曲线的控制点变化 量δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率 曲线
[0216]fk
＝fk δfk，k＝1，...，n-1
[0217]
，并进入步骤4.2.9)；
[0218]
4.2.9)初始化i＝0，l＝0；
[0219]
4.2.10)计算获取当前路径采样点ui的各关节速度和加加速度；
[0220]
4.2.11)判断当前路径采样点ui的个关节速度和加加速度是否满足约束，若 是，则进入步骤4.2.12)，否则，记当前路径采样点ui为违反点，令l＝l 1，u
l
＝ui， num3＝num3 1，并进入步骤4.2.13)；
[0221]
4.2.12)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.2.10)； 若成立，则进入步骤4.2.16)；
[0222]
4.2.13)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0223]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0224]
δf(u
l
)＝(γ-1)f(u
l
)
ꢀꢀ
(15)
[0225]
式中，f(u
l
)为当前进给速率，γ可取0.9-0.999；
[0226]
最终b样条进给速率曲线控制点的减小量可通过求解下列线性方程组的最 小二乘解得到：
[0227][0228]
式中，n
k，p
(u
l
)为b样条基函数；n 1为b样条进给速率曲线的控制点数； num3为对应的违反点数；
[0229]
4.2.14)判断i≥d是否成立，若是，则进入步骤4.2.15)，否则，更新i＝i 1， 并返回步骤4.2.10)；
[0230]
4.2.15)根据公式(16)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线
[0231]fk
＝fk δfk，k＝1，...，n-1
[0232]
，并返回步骤4.2.1)；
[0233]
4.2.16)判断num3＝0是否成立，若成立，则进入步骤4.2.17)，否则，返回 步骤4.2.15)；
[0234]
4.2.17)判断num4＝0是否成立，若成立，则输出优化后的b样条进给速率 曲线，否则，返回步骤4.2.1)。
[0235]
4.3)利用刀具加速度加加速度约束条件、速度约束条件、关节加速度加加 速度约束条件对b样条进给速率曲线进行微调，得到最终优化的进给速率轨迹。
[0236]
微调b样条进给速率曲线，得到并输出最终优化的进给速率轨迹的步骤包 括(修正过程见图5)：
[0237]
4.3.1)令违反点数num5＝0；
[0238]
4.3.2)令路径采样点下标i＝0，违反点数num6＝0；
[0239]
4.3.3)计算获取当前路径采样点ui的刀具加速度、加加速度；
[0240]
4.3.4)判断刀具加速度、加加速度是否满足刀具加速度、加加速度约束条 件，若不满足，则按式(17)调节该违反点前后相邻的b样条进给速率曲线的 控制点；否则，进入步骤4.3.7)；
[0241][0242]
式中，fk为违反点前面相邻的b样条进给速率曲线的控制点，f
k 1
为违反 点后面相邻的b样条进给速率曲线的控制点；δ-、δ

为调整值；
[0243]
4.3.6)判断i≥d是否成立，若不成立，则更新路径点为当前控制点的下一 个控制点主影响区间内的第一个采样点(i＝ind)，并返回步骤4.3.3)；若成立， 则进入步骤4.3.8)；
[0244]
4.3.7)判断i≥d是否成立，若是，则进入步骤4.3.8)，否则，更新i＝i 1， 并返回步骤4.3.3)；
[0245]
4.3.8)初始化i＝0，l＝0；
[0246]
4.3.9)计算获取当前路径采样点ui的刀具速度和关节速度、加速度与加加 速度；
[0247]
4.3.10)判断当前路径采样点ui的刀具速度和关节速度、加速度与加加速度 是否满足约束，若是，则进入步骤4.3.11)，否则，记当前路径采样点ui为违反 点，令num5＝num5 1，l＝l 1，u
l
＝ui，并进入步骤4.3.12)；
[0248]
4.3.11)判断i≥d是否成立，若不成立，则令i＝i 1，并返回步骤4.3.9)； 若成立，则进入步骤4.3.14)；
[0249]
4.3.12)建立b样条进给速率曲线控制点的减小量计算关系式，计算b样 条进给速率曲线控制点的减小量δfk，k＝1，...，n-1；
[0250]
违反点u
l
处的进给速率减小量δf(u
l
)如下所示：
[0251]
δf(u
l
)＝(γ-1)f(u
l
)
ꢀꢀ
(18)
[0252]
式中，f(u
l
)为当前进给速率，γ可取0.95-0.999；
[0253]
最终b样条进给速率曲线控制点的减小量可通过求解下列线性方程组的最 小二乘解得到：
[0254]
[0255]
式中，n
k，p
(u
l
)为b样条基函数；n 1为b样条进给速率曲线的控制点数； hum5为对应的违反点数；
[0256]
4.3.13)判断i≥d是否成立，若是，则进入步骤4.3.15)，否则，更新i＝i 1， 并返回步骤4.3.9)；
[0257]
4.3.14)判断num5＝0是否成立，若成立，则进入步骤4.3.16)，否则，进入 步骤4.3.15)；
[0258]
4.3.15)根据公式(19)获得的b样条进给速率曲线的控制点变化量 δfk，k＝1，...，n-1，修改b样条进给速率曲线控制点fk以调整b样条进给速率曲 线
[0259]fk
＝fk δfk，k＝1，...，n-1
[0260]
，并返回步骤4.3.1)；
[0261]
4.3.16)判断num6＝0是否成立，若成立，则输出最终的优化后的b样条进 给速率曲线，否则，返回步骤4.3.1)。
[0262]
所述刀具速度的约束条件是刀具速度小于刀具速度阈值；关节速度的约束 条件是关节速度小于关节速度阈值；加速度的约束条件是加速度小于加速度阈 值；加加速度的约束条件是加加速度小于加加速度阈值；
[0263]
实施例2：
[0264]
一种高阶复杂约束条件下高效的工业机器人加工节能轨迹规划方法，包括 以下步骤(过程见图2)：
[0265]
1)刀具路径的nurbs曲线拟合：以归一化弧长u为参数，采用nurbs 曲线定义刀具路径曲线，并将路径曲线等参数δu离散为d个微元路径 (i＝1，...，d)，获得d 1个路径点。
[0266]
2)规划参考进给速率轨迹：在考虑速度相关的约束条件下，计算d 1个 路径点最大可行速率，获得速度约束条件下的时间最优进给速率轨迹，将其作 为参考轨迹。
[0267]
3)获得初始能量最优的b样条进给速率曲线：计算参考轨迹下每个微元 路径的三次多项式能耗特征模型 (为平均进给速率，为参 考平均进给速率)。然后，在速度约束下优化得到每个微元路径的初始的能量最 优平均进给速率并令微元路径中间参数位置的进给速率等于最后以路径首尾点和所有微元路径中间参数位置点的进给速率作为拟合数据点， 以归一化弧长u为参数，定义初始能量最优的b样条进给速率曲线：
[0268][0269]
所述b样条进给速率曲线的次数p大于等于4。
[0270]
4)修改初始能量最优的b样条进给速率曲线，得到满足所有约束的最终 优化的进给速率轨迹。所述初始能量最优的b样条进给速率曲线修改的约束一 般包括三类特征约束：速度相关约束、刀具加速度、加加速度约束和关节加速 度、加加速度约束；
[0271]
所述初始能量最优的b样条进给速率曲线修改，基于约束特征分为五步 (k＝4)：
[0272]
4.1)基于刀具加速度、加加速度约束和速度相关约束修改b样条进给速率 曲线，以满足刀具加速度、加加速度限制和仅与速度相关的相关限制，修改流 程见图3，其含有两
个曲线迭代调整循环：内部迭代调整循环和主迭代调整循 环。内部迭代调整循环被嵌入每次主迭代调整之前，即执行一次内部迭代调整 循环，旋即执行一次主迭代调整。
[0273]
所述内部迭代调整循环中，是修改b样条进给速率曲线以满足刀具加速度、 加加速度限制。为在b样条进给速率曲线修正中引入回调机制，采用b样条进 给速率曲线对归一化弧长u的一阶导数的比例改变f
(1)
′
(u
l
)＝γf
(1)
(u
l
)(γ＜1)(f
(1)
′
(u
l
) 为改变后新的导数，γ为减小改变比例)来减小违反点(违反相应约束限制的 路径点，此处为违反刀具加速度、加加速度限制的路径采样点)处的过大的刀 具加速度、加加速度。
[0274]
所述内部迭代调整循环的每次内部迭代调整，沿路径在b样条进给速率曲 线的一阶导数曲线的每个控制点主影响区间最多选取一个违反点。每个违反点 处的b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)为
[0275][0276]
式中，为第i个刀具加速度限制，为第i个刀具加加速度限制；ai(u
l
) 为第i个刀具加速度当前值，ji(u
l
)为第i个刀具加加速度当前值；λ为加速满 足约束的速率系数，可取0.99；nt为刀具加速度加加速度约束个数。
[0277]
根据所述违反点处b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)，可由下 式计算得到对应的b样条进给速率曲线一阶导数控制点的变化量
[0278][0279]
上式所述n
k，p-1
(u
l
)为b样条进给速率曲线一阶导数曲线的b样条基函数， uk″
为控制点主影响区间，n为b样条进给速率曲线一阶导数曲线的控制点数， num2为违反点数。
[0280]
所述基于回调机制的b样条进给速率曲线修正如下：
[0281][0282][0283]
δfk为b样条进给速率曲线的控制点改变量。为回调因子，可取0.45-0.499。
[0284]
所述内部迭代调整循环，为了避免逼近目标曲线之后，低的逼近速率带来 的长的计算时间，引入了额外的内部迭代调整循环结束条件：
[0285]
max{δfk，k＝0，...，n}≤δ
[0286]
上式所述δ可取0.1-0.25。
[0287]
所述主迭代调整循环中，是修改b样条进给速率曲线以满足所有仅与速度 相关的限制。采用相应违反点处进给速率的比例改变f
′
(u
l
)＝γf(u
l
)(γ＜1)(f
′
(u
l
)为 改变后新的进给速率，γ为减小改变比例)来减小违反点处的过大的进给速率。
[0288]
所述每次主迭代调整循，沿路径在b样条进给速率曲线的每个控制点主影 响区间最多选取一个违反点。每个违反点处的b样条进给速率曲线的减小量 δf(u
l
)为：
[0289]
[0290]
式中，为第i个速度相关约束对应的进给速率限制，f(u
l
)为违反点u
l
处的当前进给速率，nv为速度约束个数；
[0291]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到：
[0292][0293]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为b样条 进给速率曲线的控制点数，num1为违反点数。
[0294]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0295]
4.2)基于关节加速度、加加速度约束修改b样条进给速率曲线，以满足关 节加速度、加加速度限制，流程见图4。输入b样条进给速率曲线为步骤1) 的输出曲线。
[0296]
步骤4.2)所述的b样条进给速率曲线修改，含有一个曲线迭代调整循环， 每次迭代调整依次进过两步曲线修改：基于回调机制的b样条进给速率曲线修 改和无回调的b样条进给速率曲线修改。
[0297]
所述的基于回调机制的b样条进给速率曲线修改，使用b样条进给速率曲 线对归一化弧长u的一阶导数的比例改变f
(1)
′
(u
l
)＝γf
(1)
(u
l
)(γ＜1)(f
(1)
′
(u
l
)为改变 后新的导数，γ为减小改变比例)来减小违反点处的过大的关节加速度、加加 速度；
[0298]
所述的基于回调机制的b样条进给速率曲线修改，沿路径在b样条进给速 率曲线的一阶导数曲线的每个控制点主影响区间最多选取一个违反点。每个违 反点处的b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)为
[0299][0300]
式中，为第j个关节加速度限制，为第j个关节加加速度限制，为 第j个关节加速度当前值，为第j个关节加加速度当前值；
[0301]
根据所述的违反点处b样条进给速率曲线一阶导数的减小量af
(1)
(u
l
)，可由 下式计算得到对应的b样条进给速率曲线一阶导数控制点的变化量
[0302][0303]
上式所述n
k，p-1
(u
l
)为b样条进给速率曲线一阶导数曲线的b样条基函数， uk″
为控制点主影响区间，n为b样条进给速率曲线一阶导数曲线的控制点数， num2为违反点数。
[0304]
所述基于回调机制的b样条进给速率曲线修正如下：
[0305][0306][0307]
δfk为b样条进给速率曲线的控制点改变量。为回调因子，可取0.45-0.499。
[0308]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0309]
所述的无回调的b样条进给速率曲线修改，使用相应违反点处进给速率的 比例改
变f
′
(u
l
)＝γf(u
l
)(γ＜1)(f
′
(u
l
)为改变后新的进给速率，γ为减小改变比例) 来减小违反点处的过大的关节加速度、加加速度。
[0310]
所述的无回调的b样条进给速率曲线修改，沿路径考虑所有违反点。每个 违反点处的b样条进给速率曲线的减小量δf(u
l
)为：
[0311]
δf(u
l
)＝(γ-1)f(u
l
)
[0312]
上式所述f(u
l
)为当前进给速率，γ可取0.99-0.999。
[0313]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到：
[0314][0315]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为b样条 进给速率曲线的控制点数，num1为违反点数。
[0316]
4.3)再次基于刀具加速度、加加速度约束和速度相关约束修改b样条进给 速率曲线，以修正步骤4.2)的修改导致的刀具加速度、加加速度增大；
[0317]
步骤4.3)所述的b样条进给速率曲线修改的流程与步骤4.1)的流程完全 一致。输入b样条进给速率曲线为步骤4.2)的输出曲线。
[0318]
4.4)再次基于关节加速度、加加速度约束修改b样条进给速率曲线。
[0319]
步骤4.4)所述的b样条进给速率曲线修改的流程与步骤4.2)的流程完全 一致。输入的b样条进给速率曲线为步骤4.3)的输出曲线。
[0320]
4.5)最后考虑所有约束条件微调步骤4.4)获得的b样条进给速率曲线， 以获得最终满足所有约束限制的b样条进给速率曲线。
[0321]
步骤4.5)所述的b样条进给速率曲线微调的流程见图5。含有一个曲线迭 代调整循环，每次迭代调整依次径过两步曲线修改：基于刀具加速度、加加速 度约束的b样条进给速率曲线微调和基于速度相关约束及关节加速度、加加速 度约束的b样条进给速率曲线微调。
[0322]
所述的基于刀具加速度、加加速度约束的b样条进给速率曲线微调所采用 的方式是：按顺序逐个逐点检查b样条进给速率曲线一阶导数曲线控制点的主 影响区间，在每个主影响区间最多对一个违反点进行调整；当在当前主影响区 间对一个违反点调整后，则直接跳到相邻下一主影响区间继续检查相关约束是 否满足，直到所有采样点被检查或最后一个主影响区间内的一违反点被修正。
[0323]
所述每个主影响区间的违反点通过微调其前后相邻的b样条进给速率曲线 的控制点的方式进行修正，方式如下：
[0324][0325]
上式所述fk为违反点前面相邻的b样条进给速率曲线的控制点，f
k 1
为违 反点后面相邻的b样条进给速率曲线的控制点，δ

可取0.1-0.2，δ-可取0.05-0.1。
[0326]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0327]
所述的基于速度相关的约束和关节加速度、加加速度约束的b样条进给速 率曲线微调，使用违反点处进给速率的比例改变f
′
(u
l
)＝γf(u
l
)(γ＜1)(f
′
(u
l
)为改 变后新的进给速率，γ为减小改变比例)来减小违反点处的过大的相关约束量。
[0328]
沿路径考虑所有违反点，每个违反点处的b样条进给速率曲线的减小量 δf(u
l
)为：
[0329]
δf(u
l
)＝(γ-1)f(u
l
)
[0330]
上式所述f(u
l
)为当前进给速率，γ可取0.95-0.999。
[0331]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到：
[0332][0333]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为b样条 进给速率的控制点数，num1为违反点数。
[0334]
实施例3：
[0335]
本发明的流程原理图见图2，选取一工业机器人加工系统作为实验对象， 见图1(工业机器人加工系统包括工业机器人1、主轴系统3，刀具2)，该实验 对象的刀具路径4见图6所示，对该实验对象进行高效的高阶复杂约束条件下 工业机器人加工节能轨迹规划方法，步骤如下：
[0336]
首先，采用nurbs曲线将刀具路径曲线定义为归一化弧长u的函数，将 路径曲线等参数δu离散为d个微元路径获得d i个路径点。d＝551。
[0337]
第二步，在考虑速度相关约束条件下，计算d 1个路径点最大可行进给速 率，获得速度约束条件下的时间最优进给速率轨迹，将其作为参考轨迹。所述 速度相关约束条件如下：
[0338][0339]
所述ρ(u)为参数位置u处的路径曲率半径，ε
max
为路径曲线弦高误差限制， ts为插补周期，f
chord
(u)为参数位置u处ε
max
限制的最大可行进给速率，f
lim
为加 工最大进给速率，为关节j的最大关节速度。
[0340]
第三步，计算参考轨迹下每个微元路径的三次多项式能耗特征模型 (为平均进给速率，为参 考平均进给速率)。所述模型系数计算公式如下：
[0341][0342][0343]
[0344][0345][0346]
其中：
[0347][0348][0349][0350]
在速度约束下优化得到每个微元路径的初始的能量最优平均进给速率并令微元路径中间参数位置的进给速率等于优化模型如下：
[0351][0352]
约束：
[0353][0354]
最后，以路径首尾点和所有微元路径中间参数位置点的进给速率作为拟合 数据点，以归一化弧长u为参数，定义初始能量最优的b样条进给速率曲线：
[0355][0356]
所述b样条进给速率曲线的次数p可取4。
[0357]
第四步，修改初始能量最优的b样条进给速率曲线，得到满足所有约束的 最终优化的进给速率轨迹。
[0358]
所述初始能量最优的b样条进给速率曲线修改的约束一般包括三类特征约 束：速度相关约束、刀尖加速度和加加速度约束、关节加速度和加加速度约束。 具体约束如下：
[0359][0360]
其中：
[0361][0362]
[0363][0364][0365]
上式所述a
lim
为刀尖加速度限制，j
lim
为刀尖加加速度限制，为第j个 关节加速度限制，为第j个关节加加速度限制，su为刀具路径总弧长。具 体为：
[0366][0367]
所述初始能量最优的b样条进给速率曲线修改，基于约束特征分为五步：
[0368]
1)基于刀尖加速度和加加速度约束和速度相关约束修改b样条进给速率 曲线，以满足刀尖加速度、加加速度限制和速度相关相关限制；
[0369]
步骤1)所述b样条进给速率曲线的修改流程见图7，其含有两个曲线迭 代调整循环：内部迭代调整循环和主迭代调整循环。内部迭代调整循环被嵌入 每次主迭代调整之前，即执行一次内部迭代调整循环，旋即执行一次主迭代调 整。
[0370]
所述内部迭代调整循环中，是修改b样条进给速率曲线以满足刀尖加速度 和加加速度限制。为在b样条进给速率曲线修正中引入回调机制，采用b样条 进给速率曲线对归一化弧长u的一阶导数的比例改变f
(1)
′
(u
l
)＝γf
(1)
(u
l
)(γ＜1)(f (1)
′
(u
l
)为改变后新的导数，γ为减小改变比例)来减小违反点(违反相应约束限 制的路径点，此处为违反刀尖加速度或加加速度限制的路径点)处的过大的刀 尖加速度/加加速度。
[0371]
所述内部迭代调整循环的每次内部迭代调整，沿路径在b样条进给速率曲 线的一阶导数曲线的每个控制点主影响区间最多选取一个违反点。每个违反点 处的b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)为
[0372][0373]
上式所述ai(u
l
)为第i个刀具加速度当前值，ji(u
l
)为第i个刀具加加速度当 前值，λ为靠近约束的速率系数，可取0.99。
[0374]
根据所述违反点处b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)，可由下 式计算得到对应的b样条进给速率曲线一阶导数控制点的变化量
[0375][0376]
上式所述n
k，p-1
(u
l
)为b样条进给速率曲线一阶导数曲线的b样条基函数， uk″
为控制点主影响区间，n为b样条进给速率曲线一阶导数曲线的控制点数， num2为违反点数。
[0377]
所述基于回调机制的b样条进给速率曲线修正如下：
[0378][0379][0380]
上式所述δfk为b样条进给速率曲线的控制点改变量，为回调因子。
[0381]
所述回调因子可取0.475。
[0382]
所述内部迭代调整循环，为了避免逼近目标曲线之后，低的逼近速率带来 的长的计算时间，引入了额外的内部迭代调整循环结束条件：
[0383]
max{δfk，k＝0，...，n}≤δ
[0384]
上式所述δ可取0.1。
[0385]
所述主迭代调整循环中，是修改b样条进给速率曲线以满足所有与速度相 关的限制。采用相应违反点处进给速率的比例改变f
′
(u
l
)＝γf(u
l
)(γ＜1)(f
′
(u
l
)为 改变后新的进给速率，γ为减小改变比例)来减小违反点处的过大的进给速度。
[0386]
所述每次主迭代调整循，沿路径在b样条进给速率曲线的每个控制点主影 响区间最多选取一个违反点。每个违反点处的b样条进给速率曲线的减小量 δf(u
l
)为：
[0387][0388]
上式所述f(u
l
)为当前进给速率。
[0389]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到：
[0390][0391]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为b样条 进给速率曲线的控制点数，num1为违反点数。
[0392]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0393]
2)基于关节加速度、加加速度约束修改b样条进给速率曲线，以满足关 节加速度、加加速度限制；
[0394]
步骤2)所述的b样条进给速率曲线修改的流程见图8。输入b样条进给 速率曲线为步骤1)的输出曲线。
[0395]
步骤2)所述的b样条进给速率曲线修改，含有一个曲线迭代调整循环， 每次迭代调整依次径过两步曲线修改：基于回调机制的b样条进给速率曲线修 改和无回调的b样条进给速率曲线修改。
[0396]
所述的基于回调机制的b样条进给速率曲线修改，使用b样条进给速率曲 线对归一化弧长u的一阶导数的比例改变f
(1)
′
(u
l
)＝γf
(1)
(u
l
)(γ＜1)(f
(1)
′
(u
l
)为改变 后新的导数，γ为减小改变比例)来减小违反点处的过大的关节加速度、加加 速度；
[0397]
所述的基于回调机制的b样条进给速率曲线修改，沿路径在b样条进给速 率曲线的一阶导数曲线的每个控制点主影响区间最多选取一个违反点。每个违 反点处的b样条进给速率曲线一阶导数的减小量δf
(1)
(u
l
)为
[0398][0399]
上式所述为第j个关节加速度当前值，为第j个关节加加速度当 前值。
[0400]
根据所述的违反点处b样条进给速率曲线一阶导数的减小量δf
(1）
(u
l
)，可由 下式计算得到对应的b样条进给速率曲线一阶导数控制点的变化量
[0401][0402]
上式所述n
k，p-1
(u
l
)为b样条进给速率曲线一阶导数曲线的b样条基函数， uk″
为控制点主影响区间，n为b样条进给速率曲线一阶导数曲线的控制点数， num2为违反点数。
[0403]
所述基于回调机制的b样条进给速率曲线修正如下：
[0404][0405][0406]
上式所述δfk为b样条进给速率曲线的控制点改变量，为回调因子。
[0407]
所述回调因子可取0.475。
[0408]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0409]
所述的无回调的b样条进给速率曲线修改，使用相应违反点处进给速率的 比例改变f
′
(u
l
)＝γf(u
l
)(γ＜1)(f
′
(u
l
)为改变后新的进给速率，γ为减小改变比例) 来减小违反点处的过大的关节加速度、加加速度。
[0410]
所述的无回调的b样条进给速率曲线修改，沿路径考虑所有违反点。每个 违反点处的b样条进给速率曲线的减小量δf(u
l
)为：
[0411]
δf(u
l
)＝(γ-1)f(u
l
)
[0412]
上式所述f(u
l
)为当前进给速率，γ可取0.99。
[0413]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到：
[0414][0415]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为b样条 进给速率曲线的控制点数，num1为违反点数。
[0416]
3)再次基于刀尖加速度、加加速度约束和速度相关约束修改b样条进给 速率曲线，以修正步骤2)的修改导致的刀具加速度、加加速度增大；
[0417]
步骤3)所述的b样条进给速率曲线修改的流程与步骤1)的流程完全一 致。输入b样条进给速率曲线为步骤2)的输出曲线。
[0418]
4)再次基于关节加速度、加加速度约束修改b样条进给速率曲线。
[0419]
步骤4)所述的b样条进给速率曲线修改的流程与步骤2)的流程完全一 致。输入b样条进给速率曲线为步骤3)的输出曲线。
[0420]
5)最后考虑所有约束条件微调步骤4)获得的b样条进给速率曲线，以获 得最终满
足所有约束限制的b样条进给速率曲线。
[0421]
步骤5)所述的b样条进给速率曲线微调的流程见图9。含有一个曲线迭 代调整循环，每次迭代调整依次径过两步曲线修改：基于刀尖加速度、加加速 度约束的b样条进给速率曲线微调和基于速度相关约束和关节加速度、加加速 度约束的b样条进给速率曲线微调。
[0422]
所述的基于刀尖加速度、加加速度约束的b样条进给速率曲线微调所采用 的方式是：按顺序逐个逐点检查b样条进给速率曲线一阶导数曲线控制点的主 影响区间，在每个主影响区间最多对一个违反点进行调整；当在当前主影响区 间对一个违反点调整后，则直接跳到相邻下一主影响区间继续检查相关约束是 否满足，直到所有采样点被检查或最后一个主影响区间内的一违反点被修正。
[0423]
所述每个主影响区间的违反点通过微调其前后相邻的b样条进给速率曲线 的控制点的方式进行修正，方式如下：
[0424][0425]
上式所述fk为违反点前面相邻的b样条进给速率曲线的控制点，f
k 1
为违 反点后面相邻的b样条进给速率曲线的控制点，δ

可取0.1，δ-可取0.05。
[0426]
图中所述ind为当前控制点的下一个控制点主影响区间内的第一个采样点。
[0427]
所述的基于速度相关约束和关节加速度、加加速度约束的b样条进给速率 曲线微调，使用违反点处进给速率的比例改变f
′
(u
l
)＝γf(ul)(γ＜1)(f
′
(u
l
)为改变 后新的进给速率，γ为减小改变比例)来减小违反点处的过大的相关约束量。
[0428]
沿路径考虑所有违反点，每个违反点处的b样条进给速率曲线的减小量 δf(u
l
)为：
[0429]
δf(u
l
)＝(γ-1)f(u
l
)
[0430]
上式所述f(u
l
)为当前进给速率，γ可取0.99。
[0431]
获得所有违反点处的b样条进给速率曲线的减小量后，用于b样条进给速 率曲线调整的控制点最小改变可通过求解以下线性方程组的最小二乘解得到。
[0432][0433]
上式所述n
k，p
(u
l
)为b样条进给速率曲线的b样条基函数，n 1为bfc的 控制点数，num1为违反点数。所述n可取100。所得路径的节能轨迹见图10。
[0434]
实施例4：
[0435]
本实施例的实验对象为实施例3的工业机器人加工系统。本案例的刀具路 径见图11所示。本实施例的过程与实施例3完全一样，所使用的约束种类和数 量与实施例1相同，所有符号及相关定义也一样。本实施例的相关参数和约束 推荐如下：
[0436][0437]
其余未列举的相关参数按实施例2的处置。所得路径的节能轨迹见图12。