(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221079620 5.X (22)申请日 2022.07.06 (71)申请人 海南大学 地址 570100 海南省海口市人民大道58号 (72)发明人 郭东生　张卫东　陈卫星　张喜　 褚德英　熊明磊　史颂华　 (74)专利代理 机构 上海科盛知识产权代理有限 公司 312 25 专利代理师 杨宏泰 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/00(2006.01) (54)发明名称 一种具有抗噪特性的双臂机器人协调运动 规划方法 (57)摘要 本发明涉及一种具有抗噪特性的双臂机器 人协调运动规划方法， 包括以下步骤： 步骤1： 建 立基于相对雅克比矩 阵的双臂机器人运动学方 程； 步骤2： 结合双臂机器人的需求来构造噪声环 境中的协调运动规划方案； 步骤3： 采用多点差分 公式对上述方案进行离散化处理； 步骤4： 根据离 散后的方案设计具有抗噪特性的高精度协调运 动规划方案； 步骤5： 基于高精度协调运动规划方 案计算得到用于规划双臂机器人协调运动的关 节角度解； 步骤6： 机器人控制器根据关节角度解 驱动双臂各个 关节的运动， 从而使得双臂机器人 精确有效地完成设定的协调规划任务。 与现有技 术相比， 本发 明具有使 得双臂机器人在噪声干扰 的情况下仍可精确有效地完成设定的协调规划 任务等优点。 权利要求书3页 说明书6页 附图1页 CN 115179283 A 2022.10.14 CN 115179283 A 1.一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征在于， 该方法包括以下 步骤： 步骤1： 建立基于雅克比矩阵的双臂机器人的运动学 方程； 步骤2： 结合双臂机器人协调运动规划的实际需求， 通过引入末端执行器位姿误差及其 积分信息构造噪声环境中的协调运动规划方案； 步骤3： 采用多点差分公式对协调运动规划方案进行离 散化处理； 步骤4： 根据离散后的协调运动规划方案设计具有抗噪特性的高精度协调运动规划方 案； 步骤5： 基于高精度协调运动规划方案计算得到用以规划双臂机器人协调运动的关节 角度解； 步骤6： 机器人控制器根据关节角度解使得双臂机器人完成设定的协调规划任务。 2.根据权利要求1所述的一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征 在于， 所述的步骤1中， 建立双臂机器人的运动学 方程的过程具体为： 双臂机器人包括一个左臂a和一个右臂b， 基于单臂各自的雅克比矩阵通过旋转和平移 推导得到双臂机器人 的相对雅克比矩阵， 从而建立双臂机器人的运动学方程， 双臂机器人 的运动学 方程为： 其中， JR( θab(t))表示相 对雅克比矩阵， θab(t)表示双臂机器人的关节角度， 即包含a臂 和b臂关节角度的联合向量， 表示双臂机器人的关节速度， 即包含a臂和b臂关节速度 的联合向量， 表示双臂末端执 行器期望的速度向量， t 表示时间。 3.根据权利要求2所述的一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征 在于， 所述的步骤2中， 噪声环境中的协调运动规划方案具体为： 其中， 表示双臂机器人的相对雅可比矩阵的伪逆矩阵， α ＞0和β ＞0均为 设计 参数， 且两者在数值上满足α2＞β， e(t)＝r(t) ‑f( θab(t))表示双臂机器人在协调运动规划 过程中末端执行器的位姿误差， r(t)表示双臂机器人末端执行器期望的运动轨迹， 运动轨 迹包括位置信息和姿态信息， f( ·)表示非线性映射函 数， 表示双臂机器人 的末端执行器的位姿误差e(t )的积分信息， 表示z(t)的时间导数且在数值上等于e (t)， 即 σ 表示积分变量， δ(t)表示双臂机器人在运动规划过程中存在的噪声。 4.根据权利要求3所述的一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征 在于， 所述的步骤3中， 多点差分公式为： 权　利　要　求　书 1/3 页 2 CN 115179283 A 2其中， k表示迭代次数， 表示tk时刻双臂机器人的关节速度， θab,k＝ θab(tk＝kη)为tk时刻双臂机器人的关节角度， 表示zk的时间导数， 表示tk时刻双臂 机器人的末端执行器的位姿误差的积分信息， η表示采 样间隔。 5.根据权利要求4所述的一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征 在于， 所述的步骤4中， 具有抗噪特性的高精度协调运动规划方案具体为： 其中， 为tk时刻双臂机器人的相对雅可比矩阵的伪逆矩阵， 为tk时刻双臂末端执行器期望的速度向量， ek＝e(tk＝kη )为tk时刻双臂机 器人的末端 执行器的位姿误差， δk＝δ(tk＝k η )表示tk时刻双臂机器人在运动规划过程中存 在的噪声。 6.根据权利要求5所述的一种具有抗噪特性的双臂机器人协调运动规划方法， 其特征 在于， 所述的步骤5中， 基于高精度协调运动规划方案计算得到规划双臂机器人协调运动的 关节角度解的过程具体包括以下步骤： 步骤501： 通过七个数值对高精度协调运动规划方案进行初始化， 七个数值对分别为 { θab,0,z0}、 { θab,1,z1}、 { θab,2,z2}、 { θab,3,z3}、 { θab,4,z4}、 { θab,5,z5}和{ θab,6,z6}， 先设定一个 数值对{ θab,0,z0}， 再计算 其余六个数值对， 其 余的六个数值对的计算公式分别为： 权　利　要　求　书 2/3 页 3 CN 115179283 A 3