CN115502984A - 六自由度机器人侧铣运动规划方法及系统 - Google Patents

Abstract

本发明提供了一种六自由度机器人侧铣运动规划方法及系统，包括：步骤1：读取设计曲面信息；步骤2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；步骤3：建立机器人关节路径光顺性指标；步骤4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；步骤5：采用序列二次规划算法求解约束优化模型，输出机器人侧铣路径。本方法可以解决标准商用六自由度机器人进行侧铣加工时的刀路规划问题，生成的机器人铣削加工路径具有更加平滑的关节路径，同时优化过程具有极高的计算效率。

Description

六自由度机器人侧铣运动规划方法及系统

技术领域

本发明涉及工业机器人加工制造技术领域，具体地，涉及一种六自由度机器人侧铣运动规划方法及系统。

背景技术

工业机器人侧铣加工系统具有工作空间大、灵活性好等优势，为大型复杂零件加工制造提供了一种新的技术方案。在工业机器人侧铣加工过程中，为生成机器人路径，需要在保证加工误差的基础上，对机器人末端刀具的五个自由度以及一个机器人功能冗余自由度进行规划。

专利文献CN112947298A(申请号：CN202110311129.4)公开了一种机器人曲面加工轨迹优化生成方法、系统及终端。该方法包括如下步骤：S1：获取当前待加工曲面的曲面类型，确定曲面类型为自由曲面或直纹曲面；S2：生成相应的机器人的加工刀具的扫掠轨迹；S3：计算机器人的刀轴的矢量约束空间；S4：获取概率路线图方法中优化性能最佳的版本作为路径优化器；S5：在加工刀具的扫掠轨迹和刀轴矢量约束空间的约束下，由路径优化器对有效路径进行优化，得到最优的加工轨迹。该系统包括：曲面类型获取模块、扫掠轨迹生成模块、矢量约束空间计算模块以及路径规划器模块。

目前一些针对五轴机床侧铣加工的运动规划方法可以在满足加工误差的基础上，通过优化刀具位姿获得机床旋转轴光顺性最优的刀具路径，但这一框架很难拓展到六自由度工业机器人的侧铣加工中，同时一些方法采用的启发式算法计算效率较低，鲁棒性较差。目前现有的机器人加工运动优化过程大多为对刀具的五自由度与机器人冗余自由度的分步求解，即首先在满足加工误差的基础上优化刀具的五个自由度，接下来以机器人性能最优为目标优化机器人冗余自由度。由于表示刀具位姿的五个自由度同样会对机器人构型造成影响，分步求解框架无法得到最优的机器人路径。

本发明提出能够对机器人末端六个自由度进行同步优化的机器人运动优化方法，对于提升机器人侧铣加工效率与加工表面质量具有十分重要的意义和背景。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种六自由度机器人侧铣运动规划方法及系统。

根据本发明提供的六自由度机器人侧铣运动规划方法，包括：

步骤1：读取设计曲面信息；

步骤2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；

步骤3：建立机器人关节路径光顺性指标；

步骤4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；

步骤5：采用序列二次规划算法求解约束优化模型，输出机器人侧铣路径。

优选的，所述步骤2包括：

在笛卡尔空间使用B样条曲线表示刀具刀尖点的位置，将该B样条记为p(u)，其中u∈[0,1]为曲线参数；

将表示刀具姿态的旋转矩阵记为

其中R₀为表示空间中任意一个固定姿态的旋转矩阵，exp(…)为矩阵的指数映射，o(u)为三维欧氏空间中的B样条曲线，

为o在李代数so(3)空间中对应的元素。

优选的，所述步骤3包括：

在刀具路径上进行等参数采样得到n个刀位，通过使用机器人逆运动学求得各个刀位对应的机器人关节角度

其中i∈{1,…,n}表示刀位序号；

使用数值差分公式，获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶导数，令

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数；令Δs_i表示第i个刀尖点位置与第i+1个刀尖点位置的距离，建立评价路径全局光顺性能的指标：

其中，

Φ_smooth为路径全局光顺性能指标，k₁与k₂为权重值。

优选的，所述步骤4包括：

将步骤2中所述的刀尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型：

其中，δ为预设的最大加工误差，d_j为设计曲面上第j个刀位点到刀具运动扫掠体包络面的有向距离，f^-1(…)为机器人的逆运动学求解函数，u_i为第i个刀位对应的运动参数值，θ_min与θ_max为机器人关节向量的下界与上界，n_q为设计曲面上采样点的数量。

优选的，所述步骤5包括：

步骤5.1：令k＝0，初始化ζ＝[1，…,1]^T∈R^m,1，其中m为向量w的长度；对于i＝1,2,…,n，j＝1,2,…,n_q，计算初始控制点向量w₀下θ_i，d_j，ds_i和Φ_smooth的值；令Φ₀＝Φ_smooth，

步骤5.2：对于i＝1,…,n，j＝1,…,6，l＝1,2,…n_q，γ＝1,2，计算

和

通过下列公式计算矩阵H和f：

步骤5.3：求解w_k附近的二次规划子问题：

其中，w_k为第k次迭代中控制点向量w的取值，Δw为控制点向量w的一阶增量，w_l为w的第l个分量，Δw_l为Δw的第l个分量；

步骤5.4：更新w_k+1＝w_k+Δw；对于i＝1,2,…,n，j＝1,2,…,n_q，更新控制点向量w_k+1下θ_i，d_j，ds_i和Φ_smooth的值；令Φ_k+1＝Φ_smooth，

步骤5.5：如果Φ_k+1<Φ_k且β_k+1≤δ，令k＝k+1，执行步骤5.6；否则，执行步骤5.7；

步骤5.6：如果(Φ_k-Φ_k+1)/|Φ_k|大于给定阈值τ₂，执行步骤5.2；否则，输出最优解为w^*＝w_k；

步骤5.7：如果ζ的二范数大于给定阈值τ₁，令ζ＝0.5ζ，执行步骤5.3；否则，输出最优解为w^*＝w_k。

根据本发明提供的六自由度机器人侧铣运动规划系统，包括：

模块M1：读取设计曲面信息；

模块M2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；

模块M3：建立机器人关节路径光顺性指标；

模块M4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；

模块M5：采用序列二次规划算法求解约束优化模型，输出机器人侧铣路径。

优选的，所述模块M2包括：

在笛卡尔空间使用B样条曲线表示刀具刀尖点的位置，将该B样条记为p(u)，其中u∈[0,1]为曲线参数；

将表示刀具姿态的旋转矩阵记为

其中R₀为表示空间中任意一个固定姿态的旋转矩阵，exp(…)为矩阵的指数映射，o(u)为三维欧氏空间中的B样条曲线，

为o在李代数so(3)空间中对应的元素。

优选的，所述模块M3包括：

在刀具路径上进行等参数采样得到n个刀位，通过使用机器人逆运动学求得各个刀位对应的机器人关节角度

其中i∈{1,…,n}表示刀位序号；

使用数值差分公式，获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶导数，令

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数；令Δs_i表示第i个刀尖点位置与第i+1个刀尖点位置的距离，建立评价路径全局光顺性能的指标：

其中，

Φ_smooth为路径全局光顺性能指标，k₁与k₂为权重值。

优选的，所述模块M4包括：

将模块M2中所述的刀尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型：

其中，δ为预设的最大加工误差，d_j为设计曲面上第j个刀位点到刀具运动扫掠体包络面的有向距离，f^-1(…)为机器人的逆运动学求解函数，u_i为第i个刀位对应的运动参数值，θ_min与θ_max为机器人关节向量的下界与上界，n_q为设计曲面上采样点的数量。

优选的，所述模块M5包括：

模块M5.1：令k＝0，初始化ζ＝[1，…,1]^T∈R^m,1，其中m为向量w的长度；对于i＝1,2,…,n，j＝1,2,…,n_q，计算初始控制点向量w₀下θ_i，d_j，ds_i和Φ_smooth的值；令Φ₀＝Φ_smooth，

模块M5.2：对于i＝1,…,n，j＝1,…,6，l＝1,2,…n_q，γ＝1,2，计算

和

通过下列公式计算矩阵H和f：

模块M5.3：求解w_k附近的二次规划子问题：

其中，w_k为第k次迭代中控制点向量w的取值，Δw为控制点向量w的一阶增量，w_l为w的第l个分量，Δw_l为Δw的第l个分量；

模块M5.4：更新w_k+1＝w_k+Δw；对于i＝1,2,…,n，j＝1,2,…,n_q，更新控制点向量w_k+1下θ_i，d_j，ds_i和Φ_smooth的值；令Φ_k+1＝Φ_smooth，

模块M5.5：如果Φ_k+1<Φ_k且β_k+1≤δ，令k＝k+1，执行模块M5.6；否则，执行模块M5.7；

模块M5.6：如果(Φ_k-Φ_k+1)/|Φ_k|大于给定阈值τ₂，执行模块M5.2；否则，输出最优解为w^*＝w_k；

模块M5.7：如果ζ的二范数大于给定阈值τ₁，令ζ＝0.5ζ，执行模块M5.3；否则，输出最优解为w＝w_k。

与现有技术相比，本发明具有如下的有益效果：

本发明可以对机器人侧铣加工过程中机器人末端六自由度进行同步优化，且具有较高的计算效率；采用机器人关节路径的全局光顺指标，对所有刀位对应的机器人关节角度进行同步优化；优化后的机器人铣削路径具有更加平滑的关节路径，进而能够有效提升机器人铣削的加工效率和加工表面质量。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明的流程示意图；

图2为所加工的叶轮叶片曲面；

图3为优化后的加工误差分布图；

图4为优化前后的机器人六个关节的关节路径曲线图；

图5为优化过程光顺指标的变化图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例：

本发明提供了一种六自由度机器人侧铣运动规划方法，如图1所示，具体包括如下步骤：

步骤1：读取设计曲面信息；

步骤2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；

步骤3：建立机器人关节路径光顺性指标；

步骤4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；

步骤5：采用序列二次规划算法求解优化模型，根据具体的机器人系统生成机器人运动程序。

在所述步骤1中：获取所要加工的设计曲面S(u,v)，对设计曲面进行离散化，得到n_q个离散点。

在所述步骤2中：根据设计曲面S(u,v)计算一系列初始刀位点，对这些刀位点的刀尖点位置进行B样条插值，得到曲线p(u)，其中u∈[0,1]为曲线参数。

使用图优化方法以机器人关节路径最短为优化目标获取上述刀位点对应的机器人冗余角度，从而计算得到上述刀位点对应的旋转矩阵。令这些刀位点中第一个刀位点对应的姿态旋转矩阵为R₀，依据公式

得到上述刀位点对应的so(3)空间中的元素

利用B样条对so(3)中的元素进行插值，得到三维曲线o(u)＝[o₁(u)；o₂(u)；o₃(u)]，同时

成立。

在所述步骤3中：沿着刀具路径上进行等参数采样得到n个刀位，通过使用机器人逆运动学求得各个刀位对应的机器人关节角度

其中i∈{1,…,n}表示刀位序号。

使用数值差分公式，获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶导数：

其中，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数。

令Δs_i表示第i个刀尖点位置与第i+1个刀尖点位置的距离，建立评价路径全局光顺性能的指标：

其中，

Φ_smooth为路径全局光顺性能指标，k₁与k₂为权重值。

具体地，在所述步骤4中：将步骤2中所述的刀尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型：

其中，δ为预设的最大加工误差，d_j为设计曲面上第j个刀位点到刀具运动扫掠体包络面的有向距离，f^-1(…)为机器人的逆运动学求解函数，u_i为第i个刀位对应的运动参数值，θ_min与θ_max为机器人关节向量的下界与上界，n_q为设计曲面上采样点的数量。

具体地，在所述步骤5中：

步骤5.1：令k＝0，初始化ζ＝[1，…,1]^T∈R^m,1，其中m为向量w的长度；对于i＝1,2,…,n，j＝1,2,…,n_q，计算初始控制点向量w₀下θ_i，d_j，ds_i和Φ_smooth的值；令Φ₀＝Φ_smooth，

步骤5.2：对于i＝1,…,n，j＝1,…,6，l＝1,2,…n_q，γ＝1,2，计算

和

通过下列公式计算矩阵H和f：

步骤5.3：求解w_k附近的二次规划子问题：

其中，w_k为第k次迭代中控制点向量w的取值，Δw为控制点向量w的一阶增量，w_l为w的第l个分量，Δw_l为Δw的第l个分量。

步骤5.4：更新w_k+1＝w_k+Δw；对于i＝1,2,…,n，j＝1,2,…,n_q，更新控制点向量w_k+1下θ_i,d_j，ds_i和Φ_smooth的值；令Φ_k+1＝Φ_smooth，

步骤5.5：如果Φ_k+1<Φ_k且β_k+1≤δ，令k＝k+1，执行步骤5.6；否则，执行步骤5.7。

步骤5.6：如果(Φ_k-Φ_k+1)/|Φ_k|大于给定阈值τ₂，执行步骤5.2；否则，输出最优解为w^*＝w_k。

步骤5.7：如果ζ的二范数大于给定阈值τ₁，令ζ＝0.5ζ，执行步骤5.3；否则，输出最优解为w^*＝w_k，并根据最优解生成机器人运动程序。

下面结合具体加工实例说明本发明的具体实施方案，如图2所示，实例加工设计曲面为叶轮叶片曲面，该曲面为由两条B样条曲线作为导线定义的直纹面，两条曲线的控制点分别为[585.4813,455.9559,639.0092；592.2912,450.36,635.0837；606.2763,441.2605,625.7919；627.9718,429.0474,611.1412；654.2096,422.963,599.7476；682.3795,414.5691,600.6242；698.4081,404.1829,599.99；706.4338,398.8462,600.292]和[608.1091,465.6494,656.1561；614.4314,460.3823,652.6947；626.2854,451.1126,646.0888；649.4551,432.5134,632.2094；672.4973,420.2748,623.7455；695.5706,408.5445,627.1174；706.5466,399.7385,628.7624；711.9363,395.9552,629.6409]，两条B样条的节点向量均为[0,0,0,0,0.2,0.4,0.6,0.8,1,1,1,1]，设计曲面上采样点数量n_q＝5000。

使用的工业机器人型号为ABB IRB4600-60，使用的刀具为锥形刀，刀底半径为6.25mm，半锥角为10°，刀具高度为30mm。刀位采样点数量为n＝50。预设的最大加工误差为δ＝0.085mm。优化过程的权重系数为k₁＝1，k₂＝1000，两个阈值设定为τ₁＝10^-5，τ₂＝10^-3。

将已知参数代入发明内容中的步骤1-步骤6中。

利用序列二次规划算法获得的结果如图3-5所示，其中，图3为优化后的加工误差分布情况，图4为优化前后的机器人六个关节的关节路径曲线，图5为优化过程光顺指标的变化情况。结果显示对于本发明获得的机器人姿态，在满足加工误差的基础上，关节路径更加平滑，进而可以提高加工效率和加工表面质量；同时优化方法具有很快的收敛速度。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种六自由度机器人侧铣运动规划方法，其特征在于，包括：

步骤1：读取设计曲面信息；

步骤2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；

步骤3：建立机器人关节路径光顺性指标；

步骤4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；

步骤5：采用序列二次规划算法求解约束优化模型，输出机器人侧铣路径。

2.根据权利要求1所述的六自由度机器人侧铣运动规划方法，其特征在于，所述步骤2包括：

在笛卡尔空间使用B样条曲线表示刀具刀尖点的位置，将该B样条记为p(u)，其中u∈[0,1]为曲线参数；

将表示刀具姿态的旋转矩阵记为

其中R₀为表示空间中任意一个固定姿态的旋转矩阵，exp(…)为矩阵的指数映射，o(u)为三维欧氏空间中的B样条曲线，

为o在李代数so(3)空间中对应的元素。

3.根据权利要求2所述的六自由度机器人侧铣运动规划方法，其特征在于，所述步骤3包括：

在刀具路径上进行等参数采样得到n个刀位，通过使用机器人逆运动学求得各个刀位对应的机器人关节角度

其中i∈{1,…,n}表示刀位序号；

使用数值差分公式，获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶导数，令

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数；令Δs_i表示第i个刀尖点位置与第i+1个刀尖点位置的距离，建立评价路径全局光顺性能的指标：

其中，

Φ_smooth为路径全局光顺性能指标，k₁与k₂为权重值。

4.根据权利要求3所述的六自由度机器人侧铣运动规划方法，其特征在于，所述步骤4包括：

将步骤2中所述的刀尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型：

s.t.-δ≤d_j≤δ

θ_i＝f^-1(p(u_i),o(u_i))

θ_min≤θ_i≤θ_max

i＝1,2,…,n

j＝1,2,…,n_q

其中，δ为预设的最大加工误差，d_j为设计曲面上第j个刀位点到刀具运动扫掠体包络面的有向距离，f^-1(…)为机器人的逆运动学求解函数，u_i为第i个刀位对应的运动参数值，θ_min与θ_max为机器人关节向量的下界与上界，n_q为设计曲面上采样点的数量。

5.根据权利要求4所述的六自由度机器人侧铣运动规划方法，其特征在于，所述步骤5包括：

步骤5.1：令k＝0，初始化ζ＝[1，…,1]^T∈R^m,1，其中m为向量w的长度；对于i＝1,2,…,n，j＝1,2,…,n_q，计算初始控制点向量w₀下θ_i，d_j，ds_i和Φ_smooth的值；令Φ₀＝Φ_smooth，

步骤5.2：对于i＝1,…,n，j＝1,…,6，l＝1,2,…n_q，γ＝1,2，计算

和

通过下列公式计算矩阵H和f：

步骤5.3：求解w_k附近的二次规划子问题：

-ζ≤Δw≤ζ

i＝1,2,…,n

j＝1,2,…,n_q

其中，w_k为第k次迭代中控制点向量w的取值，Δw为控制点向量w的一阶增量，w_l为w的第l个分量，Δw_l为Δw的第l个分量；

步骤5.4：更新w_k+1＝w_k+Δw；对于i＝1,2,…,n，j＝1,2,…,n_q，更新控制点向量w_k+1下θ_i，d_j，ds_i和Φ_smooth的值；令Φ_k+1＝Φ_smooth，

步骤5.5：如果Φ_k+1<Φ_k且β_k+1≤δ，令k＝k+1，执行步骤5.6；否则，执行步骤5.7；

步骤5.6：如果(Φ_k-Φ_k+1)/|Φ_k|大于给定阈值τ₂，执行步骤5.2；否则，输出最优解为w^*＝w_k；

步骤5.7：如果ζ的二范数大于给定阈值τ₁，令ζ＝0.5ζ，执行步骤5.3；否则，输出最优解为w^*＝w_k。

6.一种六自由度机器人侧铣运动规划系统，其特征在于，包括：

模块M1：读取设计曲面信息；

模块M2：分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和刀具姿态；

模块M3：建立机器人关节路径光顺性指标；

模块M4：以两条B样条的控制点为优化变量，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型；

模块M5：采用序列二次规划算法求解约束优化模型，输出机器人侧铣路径。

7.根据权利要求6所述的六自由度机器人侧铣运动规划系统，其特征在于，所述模块M2包括：

在笛卡尔空间使用B样条曲线表示刀具刀尖点的位置，将该B样条记为p(u)，其中u∈[0,1]为曲线参数；

将表示刀具姿态的旋转矩阵记为

其中R₀为表示空间中任意一个固定姿态的旋转矩阵，exp(…)为矩阵的指数映射，o(u)为三维欧氏空间中的B样条曲线，

为o在李代数so(3)空间中对应的元素。

8.根据权利要求7所述的六自由度机器人侧铣运动规划系统，其特征在于，所述模块M3包括：

在刀具路径上进行等参数采样得到n个刀位，通过使用机器人逆运动学求得各个刀位对应的机器人关节角度

其中i∈{1,…,n}表示刀位序号；

使用数值差分公式，获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶导数，令

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数，

表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数；令Δs_i表示第i个刀尖点位置与第i+1个刀尖点位置的距离，建立评价路径全局光顺性能的指标：

其中，

Φ_smooth为路径全局光顺性能指标，k₁与k₂为权重值。

9.根据权利要求8所述的六自由度机器人侧铣运动规划系统，其特征在于，所述模块M4包括：

将模块M2中所述的刀尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w，以路径全局光顺性能指标最小化为优化目标，以关节限位与加工误差为约束，建立约束优化模型：

s.t.-δ≤d_j≤δ

θ_i＝f^-1(p(u_i),o(u_i))

θ_min≤θ_i≤θ_max

i＝1,2,…,n

j＝1,2,…,n_q

其中，δ为预设的最大加工误差，d_j为设计曲面上第j个刀位点到刀具运动扫掠体包络面的有向距离，f^-1(…)为机器人的逆运动学求解函数，u_i为第i个刀位对应的运动参数值，θ_min与θ_max为机器人关节向量的下界与上界，n_q为设计曲面上采样点的数量。

10.根据权利要求9所述的六自由度机器人侧铣运动规划系统，其特征在于，所述模块M5包括：

模块M5.1：令k＝0，初始化ζ＝[1，…,1]^T∈R^m,1，其中m为向量w的长度；对于i＝1,2,…,n，j＝1,2,…,n_q，计算初始控制点向量w₀下θ_i，d_j，ds_i和Φ_smooth的值；令Φ₀＝Φ_smooth，

模块M5.2：对于i＝1,…,n，j＝1,…,6，l＝1,2,…n_q，γ＝1,2，计算

和

通过下列公式计算矩阵H和f：

模块M5.3：求解w_k附近的二次规划子问题：

-ζ≤Δw≤ζ

i＝1,2,…,n

j＝1,2,…,n_q

其中，w_k为第k次迭代中控制点向量w的取值，Δw为控制点向量w的一阶增量，w_l为w的第l个分量，Δw_l为Δw的第l个分量；

模块M5.4：更新w_k+1＝w_k+Δw；对于i＝1,2,…,n，j＝1,2,…,n_q，更新控制点向量w_k+1下θ_i，d_j，ds_i和Φ_smooth的值；令Φ_k+1＝Φ_smooth，

模块M5.5：如果Φ_k+1<Φ_k且β_k+1≤δ，令k＝k+1，执行模块M5.6；否则，执行模块M5.7；

模块M5.6：如果(Φ_k-Φ_k+1)/|Φ_k|大于给定阈值τ₂，执行模块M5.2；否则，输出最优解为w^*＝w_k；

模块M5.7：如果ζ的二范数大于给定阈值τ₁，令ζ＝0.5ζ，执行模块M5.3；否则，输出最优解为w^*＝w_k。

Images