(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210488157.8 (22)申请日 2022.05.06 (71)申请人 武汉科技大 学 地址 430000 湖北省武汉市青山区和平大 道947号 (72)发明人 孙瑛　宓乐圆　李公法　江都　 陶波　肖帆　云俊童　刘鑫　 蒋国璋　熊禾根　郑祖嘉　刘颖　 赵国军　 (74)专利代理 机构 北京哌智科创知识产权代理 事务所(普通 合伙) 11745 专利代理师 张元媛 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于参考轨迹实时修正的机器人轨迹 跟踪控制方法 (57)摘要 本发明公开了一种基于参考轨迹实时修正 的机器人轨迹跟踪控制方法， 以实现对动力学不 确定机器人的高性能轨迹跟踪控制。 该方法主要 通过对轨迹跟踪点的误差经过增益后进行累加， 然后将累加值实时补偿至参考轨迹上下一个将 被跟踪的点， 生成一系 列与期望轨迹不同的新指 令， 从而控制机器人跟踪期望轨迹。 该方法的本 质是通过修改参考轨迹来控制机器人的每个关 节符合期望的轨迹， 即当机器人工作过程中参考 轨迹和实际轨迹之间存在误差时， 只需实际轨迹 高精度地跟踪期望轨迹， 而不需要参考轨迹与实 际轨迹相符。 该方法具有结构简单、 易于在硬件 上应用的特点。 权利要求书3页 说明书8页 附图3页 CN 114888797 A 2022.08.12 CN 114888797 A 1.一种基于参 考轨迹实时修 正的机器人轨 迹跟踪控制方法， 所述机器人包括控制板、 静工作平台、 4个旋转关节的自由度和1个夹爪的自由度， 其特征在于， 所述控制方法包括如下步骤： 步骤1： 为机器人的控制器设计迭代前馈补偿， 并且指定合理的迭代前馈补偿方案： 所述迭代前馈补偿方案具体为： 机器人各关节的参考轨迹与当前实际位置作差产生的 误差值与前馈补偿增益内积， 内积所得值在累加器里面进行累加； 然后， 将累加器的输出加 到参考轨迹上从而得到新的指令； 接着， 新的指 令与当前实际位置作差产生指 令误差值， 所 得到的指令误差值作为控制器的输入， 从而调节机器人的关节运动； 步骤2： 根据 所设计的迭代前馈补偿方案推导得到机器人的跟踪误差e和指令误差δq之 间的关系方程： 步骤3： 迭代展开机器人运动过程当中跟踪误差e和指令误差δq之间的关系方程并分析 跟踪误差e的收敛性： 在步骤2中整理得到的关系方程的基础上进行迭代展开可以得到： 步骤4： 根据P D控制律验证机器人跟踪误差en+1的收敛性： 步骤5： 设计适当参数 方案， 通过机器人进行 试验对该控制策略的有效性进行验证。 2.如权利要求1所述的一种基于参考轨迹实时修正的机器人轨迹跟踪控制方法， 其特 征在于， 所述机器人驱动器采用Dynamixel  XM430‑W350‑T， 机器人的最大负载为500g， 重复 定位精度为 ±0.2mm， 最大关节转速为46RPM， 总重量为0.7kg； 该驱动器的分辨率为每脉冲 0.088°。 3.如权利要求1所述的一种基于参考轨迹实时修正的机器人轨迹跟踪控制方法， 其特 征在于， 步骤2具体为： 首先根据所设计的前馈补偿方案推导机器人运动过程中第一个期望点对应的当前关 节跟踪误差e1与第一次补偿后指令误差 δ q1之间的关系； 将上式进行整理可以得到： 上式当中q1是第一个期望点机器人各关节的当前位置关节矢量， qd1是第一个期望点机 器人各关节期望位置的关节矢量， qc1是第一个期望点机器人修正后的参考轨迹， e1表示机 器人第一个期望点与其当前关节位置之间的跟踪误差， 即q1和qd1之间的误差， δ q1表示机器 人第一个期望点的指令误差， Kc1是控制过程当中第一个期望点的前馈补偿增益， 是一个对 角矩阵；权　利　要　求　书 1/3 页 2 CN 114888797 A 2同样可以类推出运动过程中机器人的第二个期望点以至于第n个和第n+1和期望点的 跟踪误差e和指令误差 δ q之间的关系方程： 上式中q2是机器人第二个期望点各关节的当前位置关节矢量， qd2是机器人第二个期望 点各关节期望位置的关节矢量， qc2是第二个期望点机器人修正后的参考轨迹， e2表示机器 人第二个期望点的跟踪误差， 即q2和qd2之间的误差， δ q2表示机器人第二个期望点的指令误 差， Kc2是机器人控制过程当中第二个期望点的前馈补偿增益； 将上式进行整理可以得到 机器人第二个期望点的跟踪误差表达式： 按照上述推导过程依次类 推可以得到： 上式中qn、 qn+1分别表示机器人第n个期望点和第n+1个期望点各关节的当前位置关节 矢 量， en‑1、 en、 en+1分别表示机器人第n ‑1个期望点、 第n个期望点和第n+1个期望点的跟踪误 差， δ qn、 δ qn+1表示机器人第n个期望点和第n+1个期望点的指令误差， Kcn‑1、 Kcn、 Kcn+1分别表示 机器人控制过程当中第n ‑1个期望点、 第n个期望点和第n+1个期望点的前馈补偿增益； 进一步整理得到： 上式中I为单位对角矩阵， En为机器人运动过程中指令误差的差分， 即En＝ δ qn+1‑δ qn。 4.如权利要求1所述的一种基于参考轨迹实时修正的机器人轨迹跟踪控制方法， 其特 征在于， 步骤4具体为： 所述机器人动力学模型建立如下： 上式当中 代表机器人的关节加速度向量； M(q)∈Rm×m表示机器人的惯性矩阵； 表示机器人的离心矩阵； g(q)∈Rm表示机器人的重力量； 表示机器 人的关节粘度/ 静摩擦力矩； τd∈Rm表示外部干扰； u∈Rm是作用在机器人关节上的总控制输 入； 为方便后述证明f代 表上式左边； 机器人控制器采用的P D控制律如下： 上式当中KP和KD分别表示比例增益和微 分增益， 其都是正定对称矩阵； 表示机器人运 动过程中的指令 速度误差； 将上式进行整理， 并且考虑Lyapun ov函数V＝ δ qTKPδ q/2后可以得到：权　利　要　求　书 2/3 页 3 CN 114888797 A 3