(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211384049.2 (22)申请日 2022.11.07 (71)申请人 上海交通大 学 地址 200240 上海市闵行区东川路80 0号 (72)发明人 丁烨　陈永学　 (74)专利代理 机构 上海汉声知识产权代理有限 公司 3123 6 专利代理师 胡晶 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 六自由度机 器人侧铣 运动规划方法及系统 (57)摘要 本发明提供了一种六自由度机器人侧铣运 动规划方法及系统， 包括： 步骤1： 读取设计曲面 信息； 步骤2： 分别使用笛卡尔空间和李代数so (3)空间的B样条曲线表示刀具刀尖点位置和刀 具姿态； 步骤3： 建立机器人关节路径光顺性指 标； 步骤4： 以两 条B样条的控制点为优化变量， 以 路径全局光顺性能指标最小化为优化目标， 以关 节限位与加工误差为约束， 建立约束优化模型； 步骤5： 采用序列二次规划算法求解约束优化模 型， 输出机器人侧铣路径。 本方法可以解决标准 商用六自由度机器人进行侧铣加工时的刀路规 划问题， 生成的机器人铣削加工路径具有更加平 滑的关节路径， 同时优化过程具有极高的计算效 率。 权利要求书4页 说明书9页 附图3页 CN 115502984 A 2022.12.23 CN 115502984 A 1.一种六自由度机器人侧铣 运动规划方法， 其特 征在于， 包括： 步骤1： 读取设计曲面信息； 步骤2： 分别使用笛卡尔空间和李代数so(3)空间的B样条曲线表示刀具刀尖点位置和 刀具姿态； 步骤3： 建立机器人关节路径光 顺性指标； 步骤4： 以两条B样条的控制点为优化变量， 以路径全局光顺性能指标最小化为优化目 标， 以关节限位与加工误差为约束， 建立约束优化模型； 步骤5： 采用序列二次规划算法求 解约束优化模型， 输出机器人侧铣路径。 2.根据权利要求1所述的六自由度机器人侧铣运动规划方法， 其特征在于， 所述步骤2 包括： 在笛卡尔空间使用B样条曲线表示刀具刀尖点的位置， 将该B样条记为p(u)， 其中u∈ [0,1]为曲线参数； 将表示刀具姿态的旋转矩阵记为 其中R0为表示空间中任意一个固 定姿态的旋转矩阵， exp( …)为矩阵的指数映射， o(u)为三维欧氏空间中的B样条曲线， 为 o在李代数so(3)空间中对应的元 素。 3.根据权利要求2所述的六自由度机器人侧铣运动规划方法， 其特征在于， 所述步骤3 包括： 在刀具路径上进行等参数采样得到n个刀位， 通过使用机器人逆运动学求得各个刀位 对应的机器人关节角度 其中i∈{1, …,n}表示刀位序号； 使用数值差分公式， 获取机器人关节角度关于刀尖点路径弧长参数的一阶导数与二阶 导数， 令 表示第i个刀 位对应的第j个关节角度关于刀尖点路径弧长参数的一阶导数， 表示第i个刀位对应的第j个关节角度关于刀尖点路径弧长参数的二阶导数； 令Δsi表示第 i个刀尖点 位置与第i+1个刀尖点 位置的距离， 建立评价路径全局光 顺性能的指标： 其中， Φsmooth为路径全局光 顺性能指标， k1与k2为权重值。 4.根据权利要求3所述的六自由度机器人侧铣运动规划方法， 其特征在于， 所述步骤4 包括： 将步骤2中所述的刀 尖点曲线p(u)和姿态曲线o(u)的控制点记为向量w， 以路径全局光 顺性能指标最小化 为优化目标， 以关节限位与加工误差为约束， 建立约束优化模型： s.t.‑δ≤dj≤ δ θi＝f‑1(p(ui),o(ui))权　利　要　求　书 1/4 页 2 CN 115502984 A 2θmin≤θi≤θmax i＝1,2,…,n j＝1,2,…,nq 其中， δ为预设的最大加工误差， dj为设计曲面上第j个刀位点到刀具运动扫掠体包络面 的有向距离， f‑1(…)为机器人的逆运动学求解函数， ui为第i个刀位对应的运动参数值， θmin 与 θmax为机器人关节向量的下界与上界， nq为设计曲面上采样点的数量。 5.根据权利要求4所述的六自由度机器人侧铣运动规划方法， 其特征在于， 所述步骤5 包括： 步骤5.1： 令k＝0， 初 始化ζ ＝[1， …,1]T∈Rm,1， 其中m为向量w的长度； 对于i＝1,2, …,n， j＝1,2, …,nq， 计算初始控制点向量w0下θi， dj， dsi和Φsmooth的值； 令Φ0＝Φsmooth， 步骤5.2： 对于i＝ 1,…,n， j＝1, …,6， l＝1,2,…nq， γ＝1,2， 计算 和 通 过下列公式计算矩阵H和f： 步骤5.3： 求 解wk附近的二次规划子问题： ‑ζ ≤Δw≤ζ i＝1,2,…,n j＝1,2,…,nq 其中， wk为第k次迭代中控制点向量w的取值， Δw为控制点向量w的一阶增量， wl为w的第 l个分量， Δwl为Δw的第l个分量； 步骤5.4： 更新wk+1＝wk+Δw； 对于i＝1,2, …,n， j＝1,2, …,nq， 更新控制点向量wk+1下 θi， dj， dsi和Φsmooth的值； 令Φk+1＝Φsmooth， 步骤5.5： 如果Φk+1<Φk且βk+1≤ δ， 令k＝k+1， 执行步骤5.6； 否则， 执 行步骤5.7； 步骤5.6： 如果(Φk‑Φk+1)/|Φk|大于给定阈值τ2， 执行步骤5.2； 否则， 输出最优解为w* ＝wk； 步骤5.7： 如果ζ 的二范数大于给定阈值τ1， 令ζ＝0.5ζ， 执行步骤5.3； 否则， 输出最优解 为w*＝wk。权　利　要　求　书 2/4 页 3 CN 115502984 A 3