(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211300005.7 (22)申请日 2022.10.24 (71)申请人 广州市微眸医疗器 械有限公司 地址 511466 广东省广州市南沙区珠 江街 南江二路6号自编8栋 （9#楼） 8层80 3 (72)发明人 林生智　刘晴　晏丕松　 (74)专利代理 机构 广州粤高专利商标代理有限 公司 44102 专利代理师 黄志铖 (51)Int.Cl. B25J 9/16(2006.01) B25J 13/00(2006.01) (54)发明名称 一种机械臂控制方法及其控制系统和计算 机可读存 储介质 (57)摘要 本发明涉及一种机械臂控制方法， 步骤一： 获取机械臂上各电机的当前状态； 步骤二： 设定 机械臂运动轨迹； 步骤三： 根据运动轨迹设定机 械臂的运动指令； 步骤四： 将设定的运动指令发 送至机械臂， 令机械臂响应并运动； 步骤五： 设定 机械臂运动停止条件， 并进行条件判断， 满足条 件后机械臂停止运动。 还包括实现该方法的机械 臂控制系统和计算机可读存储介质。 该方法对机 器人运动直接执行原始状态约束的控制； 并设计 了补偿控制， 使得机器人关节能够以渐进稳定的 误差跟随给出的设定位置轨迹以及设定速度， 同 时保证控制闭环内所有的信号都有界， 即所有关 节状态都不会超 过限制边界， 使得机械臂在运动 过程中不会 出现超调的问题， 提高定位精度和跟 踪精度。 权利要求书5页 说明书20页 附图2页 CN 115366115 A 2022.11.22 CN 115366115 A 1.一种机 械臂控制方法， 其特 征在于， 包括如下步骤： 步骤一： 获取机 械臂上各电机的当前状态； 步骤二： 设定 机械臂的运动轨 迹， 设定机械臂各关节的位置 轨迹 和速度轨 迹 ； 步骤三： 根据运动轨迹设定机械臂的运动指令； 根据位置轨迹 和速度轨迹 ， 利 用编码器读取机械臂各关节实际位置 计算位置追踪误差 ， 利用位置追踪误差 对速度轨迹 进行修正， 并接受速度信号反馈， 最终输出速度跟随误差 ； 反馈得到的 位置追踪误差 及速度跟随误差 构成了控制输出 、 速度反馈输出 ， 关节的位置 及速度状态 和运动控制器的速度输出构成了模型补偿控制输出 ， 最终利用 得到控制输入 ， 得到控制电机进行运动的运动指令， 具体表 示形式如下： 式中， 为一个微小的正常数； ， 使 得 为正数形式的速度控制增益参数； 是一组正定的控制增益参数， 可以有效镇定位置及速度跟随误差， 得 到系统的指数 稳定； 是速度跟随误差的转置矩阵； 将机械臂各关节电机的输出位置及输出速度跟随误差进行 范围的限定， 具体如下： 式中， 表示矩阵 的最大特征值， 表示为李雅普若夫函数 在初始时刻 t=0时的值；权　利　要　求　书 1/5 页 2 CN 115366115 A 2步骤四： 将设定的运动指令发送至 机械臂， 令机械臂响应并运动； 步骤五： 设定机械臂运动停止条件， 并在机械臂运动过程中进行条件判断， 满足条件后 机械臂停止运动。 2.根据权利要求1所述的一种机械臂控制方法， 其特征在于， 在所述步骤二中， 机械臂 以位置模式运动， 各关节的位置 轨迹 的获取流 程为： S1.1： 获取当前末端执 行器的坐标信息； S1.2： 设定末端的运动轨 迹， 逆推值各关节电机的运动轨 迹。 3.根据权利要求2所述的一种机 械臂控制方法， 其特 征在于， S1.1的具体流 程为： S1.1.1:记录当前机 械臂1至6号电机的编码器信息： S1.1.2:1至 6号电机的编码器数值 转换成各电机当前的运动位置： 式中， 为1号电机在z轴上移动的位移量； 分别为2号、 3号、 4 号、 5号、 6号电机的旋转角度； 编码器数值 转运动角度的计算方法如下： S1.1.3:利用正 运动学计算当前坐标系位置矩阵， 具体形式如下： 式中， 为1号电机在z轴上移动的位移量； 分别为2号、 3号、 4 号、 5号、 6号电机的旋转角度； 分别为2号电机与3号电机的距离、 3号电机4号 电机、 4号电机距离 5号电机；权　利　要　求　书 2/5 页 3 CN 115366115 A 3