(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210517409.5 (22)申请日 2022.05.13 (71)申请人 中山大学 地址 510275 广东省广州市海珠区新港西 路135号 (72)发明人 黎卫兵　邹艳英　潘永平　黄凯　 (74)专利代理 机构 广州市华学知识产权代理有 限公司 4 4245 专利代理师 李斌 (51)Int.Cl. B25J 9/00(2006.01) B25J 9/16(2006.01) B25J 13/00(2006.01) (54)发明名称 关节受限冗余并联机械臂的运动规划和控 制方法、 系统及机 器人 (57)摘要 本发明公开了一种关节受限冗余并联机械 臂的运动规划和控制方法、 系统及机器人， 包括 如下步骤： 步骤一， 规划关节受限冗余并联机械 臂末端执行器的预期运动轨迹； 步骤二， 根据具 体的并联机械臂建立其相应的约束优化控制方 案； 步骤三， 基于非线性互补 问题NCP函数， 将约 束优化问题等价转化成非线性方程组； 步骤四， 针对步骤三的非线性方程组， 构建误差监测函数 并设计零化神经网络ZNN求解器， 以此求得非线 性方程组以及约束优化问题的最优解； 步骤五， 将步骤四所得结果发送给下位机， 驱动并联机械 臂运动。 本发明相比其他方法而言， 求解精度高、 无需训练和迭代 计算、 并可实现关节极限躲避功 能， 保障并联机械臂的安全性及其末端执行器的 高精度定位。 权利要求书3页 说明书8页 附图6页 CN 114851168 A 2022.08.05 CN 114851168 A 1.一种关节受限冗余并联机 械臂的运动规划 和控制方法， 其特 征在于， 包括下述 步骤： 步骤一、 规划关节受限冗余并联机械臂末端执行器的预期运动轨迹， 即并联机械臂末 端执行器的理想 速度 的设定； 步骤二、 根据具体的并联机械臂建立其相应的约束优化控制方案， 设计性能指标为 等式约束为 和 以及双端约束为q‑≤q≤q+和 其中 表示末端执行器的速度， W1和W2表示加权矩阵， q和 分别代表驱动关节长度和长度变 化率， q‑和q+分别代表驱动关节长度的上下限， 和 分别代表驱动关节长度变化率的上 下限， J表 示雅可比矩阵， K为一定 常矩阵； 通过等价转化方法可以将驱动关节长度及其长度 变化率所受到的两个双端约束整理成一个不等式约束 其中L＝[I, ‑I]T， 再通过引入 矩阵W＝W1+JTW2J∈R6×6以及N＝LJ∈R12×6将约束优化控制方案， 统一表征成具有一般形式的 二次规划问题， 其中性能指标为 等式约束为 不等式约束为 步骤三、 基于非线性互补问题NCP函数ψ(a,b)， 其中NCP函数的具体表达式定义为 基于KKT条件， 将约束优化问题等价转化成非线性方 程组g(t,x)＝0； 步骤四、 定义误差监测函数e(t):＝ g(t,x)， 所述误差监测函数是步骤三得到的非线性 方程组， 使用演化法则 其中α 为收敛参数， Φ( ·)表示激活函数阵列， 推导 得到一种全新的ZNN求解器， 通过ZNN求解器求解得到约束优化问题的最优解， 进而获得并 联机械臂驱动关节的长度变化 率 步骤五、 将步骤四的所 得结果 或其积分q发送给 下位机， 驱动并联机 械臂运动。 2.根据权利要求1所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 所述并联机械臂由固定平台、 移动平台、 末端执行器和六个驱动关节q1～q6组成， 该并联机 械臂速度层运动学 方程为 其中 J表示雅可比矩阵。 3.根据权利要求2所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 所述雅克比矩阵具体为： 其中 表示末端执行器相 对于固定基座的旋转矩阵， 表示末端执行器相对于固定基座的位置向量， bi和pi分别表示 驱动关节与静平台和动平台的连接点。 4.根据权利要求2所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 所述并联机械臂速度层运动学方程中， 设计优化性能指标 并考虑驱动 关节长度及其变化 率的上下限约束， 建立冗余并联机 械臂的约束优化控制方案； 经过等价整理， 数学上可表征为具有一般形式的二次规划问题， 其中性能指标为权　利　要　求　书 1/3 页 2 CN 114851168 A 2等式约束为 以及不等式约束为 基于NCP函数和K KT条件， 将二次规划问题等 价转化成非线性方程组g(t,x)＝0 。 5.根据权利要求4所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 所述NCP函数是处处可微的。 6.根据权利要求1所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 通过采用NCP函数 并定义误差监测函数e(t):＝g(t, x)， 基于演化法则 可得到一种全新的ZNN求解器 其中有： 并有Λ1＝diag{ ν }, Λ3＝diag{min{0, ν }}, 均为对角矩阵。 7.根据权利要求6所述关节 受限冗余并联机械臂的运动 规划和控制方法， 其特征在于， 所述ZNN求解器无需训练和迭代计算， 可高精度求解得到二次规划问题的最优解， 进而获得 并联机械臂驱动关节的长度变化率 可将其或其积分q发送给下位机， 以驱动并联机械臂 运动。 8.关节受限冗余并联机械臂的运动规划和控制系统， 其特征在于， 应用于权利要求1 ‑7 中任一项所述的关节受 限冗余并联机械臂的运动规划和控制方法， 其特征在于， 包括预期 运动轨迹规划模块、 运动学控制方案构建模块、 等价转换模块、 最优求解模块以及驱动模 块； 所述预期运动轨迹规划模块， 用于规划关节 受限冗余并联机械臂末端执行器的预期运 动轨迹， 即并联机 械臂末端执 行器的理想 速度 的设定； 所述运动学控制方案构建模块， 用于根据 具体的并联机械臂建立其相应的约束优化控 制方案， 设计性 能指标为 等式约束为 和 以及双端约束为 q‑≤q≤q+和 其中 表示末端执行器的速度， W1和W2表示加权矩阵， q和 分别代 表驱动关节长度和长度变化率， q‑和q+分别代表驱动关节长度的上下限， 和 分别代表 驱动关节长度变化率的上下限， J表 示雅可比矩阵， K为一定 常矩阵； 通过等价转化方法可以 将驱动关节长度及其长度变化率所受到的两个双端约束整理成一个不等式约束 其 中L＝[I, ‑I]T， 再通过引入矩阵W＝W1+JTW2J∈R6×6以及N＝LJ∈R12×6将约束优化控制方案，权　利　要　求　书 2/3 页 3 CN 114851168 A 3