CN114833848A - 一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法 - Google Patents

Abstract

本发明属于机器人加工技术领域，提出了一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法。基于机器人运动学及刚度性能指标，以刀轴矢量和冗余角为变量，沿既定刀具路径构造刚度空间；引入刚度阈值对所得刚度空间进行筛查，获得沿既定刀具路径的可行刚度空间；依据机器人各关节的运动特性，给出误差可控的机器人关节运动光顺算法；建立刚度约束的刀轴矢量和冗余度集成规划模型，并辅之以计算高效的轮换迭代求解策略，获取各离散点处的最优刀轴矢量和冗余角。相比于传统的顺序规划方法，本发明方法不仅能够大幅提高机器人的刚度，保证关节运动的光顺性，还能避免关节光顺造成的加工误差，可实现复杂曲面零件的机器人高质高效加工。

Description

一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划 方法

技术领域

本发明涉及机器人加工技术领域，特别涉及一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法。

背景技术

随着智能制造技术的快速发展，具备大操作空间、高操纵灵活性以及低投资成本等优点的6自由度工业机器人正被广泛地应用于航空航天、汽车船舶等领域中大型复杂曲面零件的加工制造。然而，与传统五轴数控机床相比，工业机器人存在绕刀轴方向转动的冗余自由度，当给定加工路径点和刀轴矢量时会有无数的机器人姿态与之对应；其次，机器人本体刚度较弱，在铣削加工中极易出现颤振现象，从而严重影响工件的表面质量和加工精度。因此，当前普遍采用顺序规划方法进行机器人铣削加工，即先利用CAM软件或者专门针对数控机床开发的方法规划刀轴矢量，然后依据不同的优化目标对冗余角进行调整。如文献“Xiong G,Ding Y,Zhu LM.Stiffness-based pose optimization of an industrialrobot for five-axis milling.Robotics and Computer-Integrated Manufacturing,2019,55:19-28.”首先利用CAM软件规划刀轴，然后以提升机器人整体刚度为目标，考虑关节极限以及运动灵巧性等约束条件建立优化模型，对机器人冗余角进行优化，从而获得最优的机器人加工姿态。文献“Chen C,Peng FY,Yan R,et al.Stiffness performanceindex based posture and feed orientation optimization in robotic millingprocess.Robotics and Computer-Integrated Manufacturing,2019,55:29-40.”对机器人不同冗余角的法向刚度分布进行研究，通过优化冗余角以获得最大法向刚度。上述顺序规划方法虽然可以有效地提高机器人的刚度，但忽略了刀轴矢量对刚度的影响，难以保证机器人在可行操作空间中达到最大刚度。此外，由于机器人刀轴矢量和冗余角与关节坐标之间的高度非线性运动学映射，最优的机器人刚度不一定能保证关节运动光顺。而不光顺的加减速运动不仅会加剧关节的磨损，还会激发机器人共振频率，导致表面加工质量急剧下降。

发明内容

为克服现有机器人铣削加工中顺序优化方法的技术缺陷，本发明提出一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法，对机器人刀轴矢量和冗余度进行同时优化，并给出计算高效的轮换迭代策略简化模型的求解。相比于传统的顺序规划方法，本方法不仅能大幅提高机器人的刚度，保证关节运动的光顺性，还能避免关节光顺造成的加工误差，可实现复杂曲面零件的机器人高质高效加工。

本发明采用的技术方案是：首先基于机器人运动学及刚度性能指标，以刀轴矢量和冗余角为变量，沿既定刀具路径构造刚度空间；进而引入刚度阈值对所得刚度空间进行筛查，获得沿既定刀具路径的可行刚度空间；然后依据机器人各关节的运动特性，在充分考虑加工误差的前提下，给出误差可控的机器人关节运动光顺算法；在此基础上，建立刚度约束的刀轴矢量和冗余度集成规划模型，并辅之以轮换迭代求解策略，获取各离散点处的最优刀轴矢量和冗余角，从而大幅提高机器人的刚度，保证关节运动的光顺性，同时避免关节光顺造成的加工误差。采用的具体步骤如下：

a.构建离散路径点处的刚度空间；

a.1沿既定刀具路径进行离散，获得一系列的离散路径点p_i，以p_i为原点建立局部坐标系，坐标系中x_i表示加工进给方向，z_i是p_i点处曲面的单位法向量，y_i是z_i和x_i的向量积；考虑机器人冗余自由度来构建机器人的运动学模型，确定刀轴矢量o(α_i,β_i)和冗余角μ_i与机器人六关节角θ_i之间的一一映射关系；

机器人的运动学模型基于多个坐标系建立，分别为机器人基坐标系{B}、机器人刀具末端坐标系{E}、工件坐标系{W}以及关节坐标系；其中，关节坐标系{1}，{2}，{3}，{4}，{5}和{6}分别基于机器人的6个关节，即控制本体回旋的关节a(1)、控制大臂运动的关节b(2)、控制小臂运动的关节c(3)、控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)、控制手腕圆周运动的关节f(6)而建立的；

机器人的运动学模型具体为：

其中，

表示从机器人基坐标系{B}到控制手腕圆周运动的关节f(6)坐标系{6}的齐次变换，

表示从控制手腕圆周运动的关节f(6)坐标系{6}到机器人刀具末端坐标系{E}的齐次变换，

表示从机器人基坐标系{B}到工件坐标系{W}的齐次变换，T(x_i,π)表示绕x_i轴旋转π弧度的齐次旋转矩阵，用于对齐机器人刀具末端坐标系{E}与工件坐标系{W}中的位姿；H_i(z_i,μ)＝H_i,0·T(z_i,μ)，其中H_i,0表示机器人在离散路径点p_i处的初始姿态，T(z_i,μ)表示绕z_i轴旋转角度μ的齐次旋转矩阵；

a.2以机器人关节限位、运动奇异避免及控制大臂运动的关节b(2)、控制小臂运动的关节c(3)的运动耦合为约束条件，建立固定刀轴矢量下以冗余角为变量的机器人刚度计算模型：

其中，K表示与机器人六关节角θ_i相关的整体刚度指标，θ²和θ³分别表示控制大臂运动的关节b(2)、控制小臂运动的关节c(3)的关节角，[θ^min,θ^max]表示机器人各关节的物理极限，J表示雅克比矩阵，η表示运动奇异避免阈值，‖·‖表示Frobenius范数；

a.3根据公式(2)求得任意刀轴矢量下机器人的刚度范围，然后对离散路径点p_i点处所有刀轴矢量对应的刚度范围求并集，获得p_i点的刚度空间；遍历所有离散路径点，求得整条路径的刚度空间；

b.引入刚度阈值对刚度空间进行筛查，获得可行刚度空间；

b.1对所得刚度空间进行离散，获得均匀采样点；引入刚度阈值，构建刚度约束条件；

b.2结合避免加工碰撞、局部干涉的基本切削条件约束以及b.1构造的刚度约束对所有采样点进行筛查，得到可行采样点；对所有可行采样点构成的可行立方体求并集，获得可行刚度空间；

c.依据机器人各关节的运动特性，在充分考虑加工误差的前提下，构建误差可控的机器人关节运动光顺算法；

c.1对运动加减速相对频繁且关节刚度较小的控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)和控制手腕圆周运动的关节f(6)进行运动光顺，将上述三个关节的运动表示为五次B样条曲线形式：

其中，*表示关节序号，*＝4,5,6；k表示B样条曲线的次数，k＝5；N_j,k(u)表示定义在节点矢量U上的第j个B样条基函数；

表示控制系数，j＝0,1,2…,n；

以控制系数为变量，以各关节角加速度最小为目标建立最小二乘优化模型：

其中，f表示切削进给速度；Δs_i表示两个相邻离散路径点p_i和p_i+1的距离；

对优化目标Ω^*取极值，

然后给定一组或多组θ^*作为已知条件对式(4)展开求解，获得控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)和控制手腕圆周运动的关节f(6)的B样条曲线方程，通过插值获得各离散路径点处的关节角θ⁴，θ⁵和θ⁶；

c.2依据离散路径点坐标以及插值得到θ⁴、θ⁵和θ⁶，结合机器人的运动学模型，考虑加工误差对刀具进行重定位，得到关于未知关节角θ¹、θ²和θ³的方程组：

其中，

表示基坐标系下的离散路径点坐标；s_v和c_v分别表示第v个关节角的正弦和余弦值，v＝1,2,3；w₁，w₂和w₃表示不含未知数的系数，由下式计算：

a₂,a₃,a₄,d₁,d₄,d₆,l₁和l₂为机器人的Denavit-Hartenberg参数；对式(5)进行联立求解，获得未知关节角θ¹、θ²和θ³的表达式：

其中，

c.3将机器人的六个关节角代入机器人的运动学模型，生成各离散路径点处对应的刀轴矢量和冗余角((α,β),μ)，α表示后跟角，β表示侧偏角；

d.建立刚度约束的刀轴矢量和冗余度集成规划模型并利用轮换迭代策略进行求解；

d.1在首末路径点p_first和p_end的可行刚度空间中搜索刚度最大的刀轴矢量和冗余角，通过逆向运动学求得对应关节角θ_first和θ_end，作为初始已知条件；

d.2将初始已知条件带入式(4)中，解得其余离散路径点处的关节角θ⁴、θ⁵和θ⁶；然后利用式(6)求得关节角θ¹、θ²和θ³，并生成其余离散路径点处对应的刀轴矢量和冗余角((α,β),μ)；

d.3将所得刀轴矢量和冗余角((α,β),μ)分别代入各离散路径点处的可行刚度空间中进行检查，对于不满足可行刚度空间约束的刀轴矢量和冗余角，指定该点处刚度最大的刀轴矢量和冗余角对其进行替换，并设为新增已知条件，再次执行步骤d.2；

d.4交替迭代执行上述步骤，直到所有刀轴矢量和冗余角均满足可行刚度空间的约束。

与现有的技术相比，本发明的有益效果：本发明不仅能大幅提高铣削加工过程中机器人的刚度，还能保证关节运动的光顺性，同时避免了关节光顺造成的加工误差，从而实现复杂曲面零件的机器人高质高效加工。

附图说明

图1是一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法的流程图。

图2是ABB IRB 6660工业机器的运动学模型。

图3是刚度空间示意图。

图4是可行刚度空间示意图。

图5是空间曲线加工路径示意图。

图6是传统方法与本发明方法的机器人刚度对比曲线。

图7是传统方法与本发明方法的关节6角加速度对比曲线。

图8是刀具重定位前后加工误差对比曲线。

图中：1-控制本体回旋的关节a；2-控制大臂运动的关节b；3-控制小臂运动的关节c；4-控制手腕旋转的关节d；5-控制手腕摆动的关节e；6-控制手腕圆周运动的关节f；7-末端执行器；8-待加工曲面；9-空间曲线加工路径。

具体实施方式

本发明一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法的流程图如图1所示。下面以ABB IRB 6660工业机器为例，结合附图和实施步骤对本发明的具体实施过程进行详细描述：

步骤1，构建刀具路径离散点处的刚度空间。

步骤1.1：沿既定刀具路径进行离散，获得一系列的离散路径点p_i。

步骤1.2：建立机器人运动学模型，如图2所示。图中{B}表示机器人基坐标系，{E}表示机器人刀具末端坐标系，{W}表示工件坐标系，关节坐标系{1}，{2}，{3}，{4}，{5}和{6}分别基于机器人的6个关节，即控制本体回旋的关节a1、控制大臂运动的关节b2、控制小臂运动的关节c3、控制手腕旋转的关节d4、控制手腕摆动的关节e5、控制手腕圆周运动的关节f6。由于冗余自由度的存在，机器人的末端执行器7可以绕着z_i轴自由旋转，因而在不改变刀具位姿的情况下存在无数的机器人姿态H_i(z_i,μ)＝H_i,0·T(z_i,μ)，其中H_i,0表示机器人在路径点p_i处的初始姿态，μ表示冗余角且μ∈[-π,π]，T(z_i,μ)表示绕z_i轴旋转角度μ的齐次旋转矩阵。因此，考虑了冗余角的机器人运动学模型可表示如下：

其中，

表示从机器人基坐标系{B}到控制手腕圆周运动的关节f(6)坐标系{6}的齐次变换，

表示从控制手腕圆周运动的关节f6坐标系{6}到机器人刀具末端坐标系{E}的齐次变换，

表示从机器人基坐标系{B}到工件坐标系{W}的齐次变换，T(x_i,π)表示绕x_i轴旋转π弧度的齐次旋转矩阵，用于对齐机器人刀具末端坐标系{E}与工件坐标系{W}中的位姿；H_i(z_i,μ)＝H_i,0·T(z_i,μ)，其中H_i,0表示机器人在离散路径点p_i处的初始姿态，T(z_i,μ)表示绕z_i轴旋转角度μ的齐次旋转矩阵；

步骤1.3：依据公式(1)以及机器人逆向运动学，求机器人关节角θ_i：

对于普通工业机器人，θ_i的解与三个构型指标(臂、肘、腕)有关，因此，式(2)最多存在8组解，对应8种机器人姿态。但本型号机器人存在并连杆机构，仅腕关节指标对姿态求解构成影响，故只存在两组可能解。引入角位移增量最小化准则从两组可能解中得到唯一可行的机器人姿态，从而建立刀轴矢量和冗余角与机器人六关节角之间的一一映射关系。显然，刀轴矢量或冗余角的改变会导致机器人姿态的变化，同时影响机器人的刚度。

步骤1.4：计算固定刀轴矢量下，以冗余角为变量的机器人刚度。引入文献“GuoYJ,Dong HY,Ke YL.Stiffness-oriented posture optimization in robotic machiningapplications.Robotics and Computer-Integrated Manufacturing,2015,35:69-76.”中的刚度指标

来量化机器人在某一姿态下的整体刚度，其中C_tt表示柔度子矩阵。刚度计算要满足关节限位约束，即θ∈[θ^min,θ^max]，其中[θ^min,θ^max]表示机器人各关节物理极限。同时机器人必须远离奇异点，即满足‖J(θ)‖‖J^-1(θ)‖≤η，其中，J表示雅克比矩阵，η表示奇异避免阈值，‖·‖表示Frobenius范数。由于机器人存在并连杆，控制大臂运动的关节b2、控制小臂运动的关节c3的运动存在耦合，除了满足各自关节的物理极限外，还要满足约束

其中θ²和θ³分别表示，控制大臂运动的关节b2、控制小臂运动的关节c3的角度。综上，固定刀轴矢量下以冗余角为变量的刚度计算模型可表示为：

步骤1.5：构造离散路径点处的刚度空间。根据公式(3)求得任意刀轴矢量下机器人的刚度范围，对p_i点处所有刀轴矢量对应的机器人刚度范围求并集，即可得到该点的机器人刚度空间，如图3所示。刚度空间中的每一个数据点都对应着一组刀轴矢量和冗余角((α,β),μ)，其中(α,β)表示刀轴矢量，α表示后跟角，β表示侧偏角。

步骤2，引入刚度阈值对刚度空间进行筛选，构造可行刚度空间。

步骤2.1：各离散点处的可行刚度空间是刚度空间的一个子空间，其必须满足两个约束条件。一是刀轴矢量要满足基本切削条件，避免加工过程中出现干涉、碰撞等；二是满足刚度约束，即

其中

表示当前路径点处的最大刚度，δ表示刚度阈值且δ＜1。

步骤2.2：将p_i点处的刚度空间进行均匀离散，获得均匀采样点如图4所示。利用步骤2.1中两个约束条件对所有采样点进行检查，如果一个采样立方体的八个角点均为可行采样点，则该立方体标记为可行刚度空间的子集。所有可行刚度空间的子集都构成p_i点处的可行刚度空间。遍历所有离散点，获得整条刀具路径的可行刚度空间。

步骤3，依据机器人各关节的运动特性，在充分考虑加工误差的前提下，给出误差可控的机器人关节运动光顺算法。

步骤3.1：首先对运动加减速相对频繁且关节刚度较小的控制手腕旋转的关节d4、控制手腕摆动的关节e5和控制手腕圆周运动的关节f6进行运动光顺，将上述三个关节的运动表示为五次B样条曲线形式：

其中，*表示关节序号，*＝4,5,6；k表示B样条曲线的次数，k＝5；N_j,k(u)表示定义在节点矢量U上的B样条基函数，可由Cox-de Boor递推公式得出；

表示控制系数。然后，以控制系数为变量，以各关节角加速度最小为目标建立最小二乘优化模型：

其中，f表示切削进给速度；Δs_i表示两个相邻路径点p_i和p_i+1的距离。优化目标Ω^*取极值的条件为

通过化简可得到线性方程组cφ＝b。当给定一组或多组θ^*作为已知条件，即可求解该线性方程组为φ＝(c^Tc)^-1(c^Tb)，其中c表示系数矩阵；φ表示待求的控制系数向量；b表示已知条件组成的向量。至此，控制手腕旋转的关节d4、控制手腕摆动的关节e5和控制手腕圆周运动的关节f6的关节角θ^*(u)可用B样条曲线方程表示，通过插值即可获得各离散点处的关节角θ⁴，θ⁵和θ⁶。

步骤3.2：根据上述获得的关节角θ⁴，θ⁵和θ⁶以及给定的路径点坐标p，结合机器人运动学，在充分考虑加工误差的前提下，对刀具进行重定位，进而求得关节角θ¹，θ²和θ³。

由式(1)的机器人运动学模型可知，机器人刀具末端坐标系在基坐标系{B}中可以表示为:

其中，x^B，y^B，z^B分别表示刀具末端坐标系的x轴，y轴和z轴；

表示基坐标系{B}下的离散路径点坐标，可通过坐标变换

求得。将p^B和上一步插值得到的关节角θ⁴，θ⁵和θ⁶代入式(6)中，得到关于未知关节角θ¹，θ²和θ³的方程组：

其中，s_v和c_v分别表示第v个关节角的正弦和余弦值，v＝1,2,3；w₁,w₂和w₃表示不含未知数的系数，可由下式计算：

a₂,a₃,a₄,d₁,d₄,d₆,l₁和l₂为机器人的Denavit-Hartenberg参数。将式(7)中方程的部分项进行整合，引入中间变量w₄和w₅，可实现未知数θ²,θ³和θ¹之间的解耦：

至此，式(7)可进行进一步简化为：

结合三角函数的基本性质对式(9)进行联立求解，即可获得中间变量w₄和w₅以及未知关节角θ¹的表达式：

将中间变量w₄和w₅代入式(8)中可求得未知关节角θ²和θ³：

利用上述方法求得的机器人六个关节角不仅能保证机器人运动光顺，同时还能避免关节角光顺过程中带来的加工误差，保证加工精度。将六个关节角代入式(1)，可自动生成各离散点处对应的刀轴矢量和冗余角((α,β),μ)。

步骤4，建立刚度约束的刀轴矢量和冗余度集成规划模型并给出轮换迭代策略求解。

步骤4.1：以图5给出的待加工曲面8上的空间曲线加工路径9即刀具路径为例。将曲线离散为900个路径点，设进给速度f＝300mm/min，奇异避免阈值η＝1.5，刚度阈值δ＝0.9。在首末路径点p_first和p_end的可行刚度空间中搜索刚度最大的刀轴矢量和冗余角，通过式(2)求得对应关节角θ_first和θ_end，作为求解式(5)的初始已知条件。

步骤4.2：将已知条件带入式(5)中，执行步骤3.1，求得其余离散路径点处的关节角θ⁴，θ⁵和θ⁶。然后执行步骤3.2，求解关节角θ¹，θ²和θ³，并生成其余离散路径点处的刀轴矢量和冗余角((α,β),μ)。

步骤4.3：将所得刀轴矢量和冗余角分别代入各离散路径点处的可行刚度空间中进行检查，对于不满足可行刚度空间约束的刀轴矢量和冗余角，则指定该点处刚度最大的刀轴矢量和冗余角对其进行替换，并设为新增已知条件，再次执行步骤4.2。

步骤4.4：交替迭代执行上述步骤，直到生成的所有刀轴矢量和冗余角均满足可行刚度空间的约束。图6为传统顺序优化方法和本发明方法的机器人刚度对比，图7为两种方法的机器人关节6角加速对比，图8为关节光顺过程中刀具重定位前后的加工误差对比。从图中可以看出，采用本发明的方法不仅能大幅提升的机器人刚度，保证关节运动更加光顺，还能有效地避免了关节光顺造成的加工误差，进而实现复杂曲面零件的机器人高质高效加工。

Claims (1)

1.一种刚度约束的机器人铣削加工刀轴矢量和冗余度集成规划方法，其特征在于，该方法首先基于机器人运动学及刚度性能指标，以刀轴矢量和冗余角为变量，沿既定刀具路径构造刚度空间；进而引入刚度阈值对所得刚度空间进行筛查，获得沿既定刀具路径的可行刚度空间；然后依据机器人各关节的运动特性，在充分考虑加工误差的前提下，给出误差可控的机器人关节运动光顺算法；在此基础上，建立刚度约束的刀轴矢量和冗余度集成规划模型，并辅之以轮换迭代求解策略，获取各离散路径点处的最优刀轴矢量和冗余角；

具体步骤为：

a.构建离散路径点处的刚度空间；

a.1沿既定刀具路径进行离散，获得一系列的离散路径点p_i，以p_i为原点建立局部坐标系，坐标系中x_i表示加工进给方向，z_i是p_i点处曲面的单位法向量，y_i是z_i和x_i的向量积；考虑机器人冗余自由度来构建机器人的运动学模型，确定刀轴矢量o(α_i,β_i)和冗余角μ_i与机器人六关节角θ_i之间的一一映射关系；

机器人的运动学模型基于多个坐标系建立，分别为机器人基坐标系{B}、机器人刀具末端坐标系{E}、工件坐标系{W}以及关节坐标系；其中，关节坐标系{1}，{2}，{3}，{4}，{5}和{6}分别基于机器人的6个关节，即控制本体回旋的关节a(1)、控制大臂运动的关节b(2)、控制小臂运动的关节c(3)、控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)、控制手腕圆周运动的关节f(6)而建立的；

机器人的运动学模型具体为：

其中，

表示从机器人基坐标系{B}到控制手腕圆周运动的关节f(6)坐标系{6}的齐次变换，

表示从控制手腕圆周运动的关节f(6)坐标系{6}到机器人刀具末端坐标系{E}的齐次变换，

表示从机器人基坐标系{B}到工件坐标系{W}的齐次变换，T(x_i,π)表示绕x_i轴旋转π弧度的齐次旋转矩阵，用于对齐机器人刀具末端坐标系{E}与工件坐标系{W}中的位姿；H_i(z_i,μ)＝H_i,0·T(z_i,μ)，其中H_i,0表示机器人在离散路径点p_i处的初始姿态，T(z_i,μ)表示绕z_i轴旋转角度μ的齐次旋转矩阵；

a.2以机器人关节限位、运动奇异避免及控制大臂运动的关节b(2)、控制小臂运动的关节c(3)的运动耦合为约束条件，建立固定刀轴矢量下以冗余角为变量的机器人刚度计算模型：

其中，K表示与机器人六关节角θ_i相关的整体刚度指标，θ²和θ³分别表示控制大臂运动的关节b(2)、控制小臂运动的关节c(3)的关节角，[θ^min,θ^max]表示机器人各关节的物理极限，J表示雅克比矩阵，η表示运动奇异避免阈值，‖·‖表示Frobenius范数；

a.3根据公式(2)求得任意刀轴矢量下机器人的刚度范围，然后对离散路径点p_i点处所有刀轴矢量对应的刚度范围求并集，获得p_i点的刚度空间；遍历所有离散路径点，求得整条路径的刚度空间；

b.引入刚度阈值对刚度空间进行筛查，获得可行刚度空间；

b.1对所得刚度空间进行离散，获得均匀采样点；引入刚度阈值，构建刚度约束条件；

b.2结合避免加工碰撞、局部干涉的基本切削条件约束以及b.1构造的刚度约束对所有采样点进行筛查，得到可行采样点；对所有可行采样点构成的可行立方体求并集，获得可行刚度空间；

c.依据机器人各关节的运动特性，考虑加工误差，构建误差可控的机器人关节运动光顺算法；

c.1对控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)和控制手腕圆周运动的关节f(6)进行运动光顺，将上述三个关节的运动表示为五次B样条曲线形式：

其中，*表示关节序号，*＝4,5,6；k表示B样条曲线的次数，k＝5；N_j,k(u)表示定义在节点矢量U上的第j个B样条基函数；

表示控制系数，j＝0,1,2…,n；

以控制系数为变量，以各关节角加速度最小为目标建立最小二乘优化模型：

其中，f表示切削进给速度；Δs_i表示两个相邻离散路径点p_i和p_i+1的距离；

对优化目标Ω^*取极值，

然后给定一组或多组θ^*作为已知条件对式(4)展开求解，获得控制手腕旋转的关节d(4)、控制手腕摆动的关节e(5)和控制手腕圆周运动的关节f(6)的B样条曲线方程，通过插值获得各离散路径点处的关节角θ⁴，θ⁵和θ⁶；

c.2依据离散路径点坐标以及插值得到θ⁴、θ⁵和θ⁶，结合机器人的运动学模型，考虑加工误差对刀具进行重定位，得到关于未知关节角θ¹、θ²和θ³的方程组：

其中，

表示基坐标系下的离散路径点坐标；s_v和c_v分别表示第v个关节角的正弦和余弦值，v＝1,2,3；w₁，w₂和w₃表示不含未知数的系数，由下式计算：

a₂,a₃,a₄,d₁,d₄,d₆,l₁和l₂为机器人的Denavit-Hartenberg参数；对式(5)进行联立求解，获得未知关节角θ¹、θ²和θ³的表达式：

其中，

c.3将机器人的六个关节角代入机器人的运动学模型，生成各离散路径点处对应的刀轴矢量和冗余角((α,β),μ)，α表示后跟角，β表示侧偏角；

d.建立刚度约束的刀轴矢量和冗余度集成规划模型并利用轮换迭代策略进行求解；

d.1在首末路径点p_first和p_end的可行刚度空间中搜索刚度最大的刀轴矢量和冗余角，通过逆向运动学求得对应关节角θ_first和θ_end，作为初始已知条件；

d.2将初始已知条件带入式(4)中，解得其余离散路径点处的关节角θ⁴、θ⁵和θ⁶；然后利用式(6)求得关节角θ¹、θ²和θ³，并生成其余离散路径点处对应的刀轴矢量和冗余角((α,β),μ)；

d.3将所得刀轴矢量和冗余角((α,β),μ)分别代入各离散路径点处的可行刚度空间中进行检查，对于不满足可行刚度空间约束的刀轴矢量和冗余角，指定该点处刚度最大的刀轴矢量和冗余角对其进行替换，并设为新增已知条件，再次执行步骤d.2；

d.4交替迭代执行上述步骤，直到所有刀轴矢量和冗余角均满足可行刚度空间的约束。

Images