(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210621679.0 (22)申请日 2022.06.01 (71)申请人 浙江大学 地址 310058 浙江省杭州市西湖区余杭塘 路866号 申请人 浙江钱塘机 器人及智能装备研究有 限公司 (72)发明人 方梓仰　王进　张海运　李小飞　 翟安邦　陆国栋　 (74)专利代理 机构 杭州求是专利事务所有限公 司 33200 专利代理师 万尾甜　韩介梅 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于强化学习与动力学前馈融合的机 器人轨迹跟踪控制器设计方法 (57)摘要 本发明属于机器人控制技术领域， 公开了一 种基于强化学习与动力学前馈融合的机器人轨 迹跟踪控制器 设计方法.该方法可在被控系统中 存在动力学不确定性、 未建模非线性因素、 参数 摄动和未知外部干扰等条件下， 能够自主准确跟 踪参考轨迹， 并自适应减小轨迹误差以实现提高 轨迹跟踪精度。 本发明运用了动力学前馈控制和 人工蜂群算法对动力学参数进行辨识补偿作为 控制方法的基础， 通过二者的融合， 加之针对未 建模的非线性因素、 外部干扰等偏 差的强化学习 TD3补偿算法， 实现了控制方法和控制器的设计。 该控制方法能对不同结构参数机械臂的动力学 不确定性所产生的轨迹误差进行自适应反馈调 整和补偿， 有效提高轨 迹跟踪精度和性能。 权利要求书3页 说明书7页 附图3页 CN 114952849 A 2022.08.30 CN 114952849 A 1.一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器设计方法， 其特征在 于， 主要包含以下几个步骤： A.动力学参数辨识模型构建 构建机械臂动力学参数辨识模型， 通过设计的激励轨迹， 采集机械臂运动信息与已知 的动力学模型 方程相结合， 计算出对应的偏差； B.基于人工蜂群算法动力学参数辨识 针对机械臂动力学模型参数的辨识， 求解过程通过人工蜂群算法循环迭代更新， 找到 满足的“蜜源”， 即找到满足精度的解， 最小化偏差e， 从而得到 机械臂的真实动力学参数； C.设计动力学 前馈控制器 通过建立动态 的惯量模型， 给出系统内部控制所需的控制量， 减小目标关节角度与实 际关节角度的偏差， 减小位置误差； D.对未建模的误差采用强化学习TD3进一 步补偿。 2.如权利要求1所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器 设计方法， 其特 征在于， 步骤A具体为： 采用牛顿‑欧拉迭代构建机 械臂动力学参数辨识模型： 公式(1)中， q是机械臂关节角度， 为关节角速度， 为关节角加速度， τ 是各个关节轴驱 动力矩， D(q)是对称正定的n ×n质量矩阵， 是科里奥利和离心力向量， 维度为n ×1， G (q)是包含重力矩的向量； Fcv为摩擦力项， 具体展开 为： 公式(2)中Fcv为摩擦力， fc是库伦摩擦力矩， fv是粘性摩擦系数， sign为符号函数； 通过 公式(1)对串联机械臂进 行递推， 得到机械臂动力学模型； 然后， 通过设计的激励轨迹， 采集 到机械臂的各个关节力矩、 关节角度、 角速度、 角加速度信息和已知的动力学模 型方程相结 合， 计算出对应的偏差: 公式(3)中e代表理论值和真实值的偏差； τt为通过公式(1)计算得出 的关节力矩， Ft为 传感器采集到的各个关节力矩信息， m代表关节轴数， n代表采样数量； wi为第i关节轴力矩 残差的权 重。 3.如权利要求2所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器 设计方法， 其特 征在于， 步骤B具体为： 针对机械臂动力学模型参数的辨识， 求解过程可以通过人工蜂群算法循环迭代更新， 找到满足的 “蜜源”， 即找到满足精度的解； 要求 解的m个动力学参数， 如下： X＝[x1,x2,…,xm]                   (4) 其中， x代 表动力学模型中待辨识的参数， 第n次迭代求 解得到的第i个蜜源表达为： 对第i个蜜源在搜索约束的范围内， 进行随机初始化：权　利　要　求　书 1/3 页 2 CN 114952849 A 2Xi＝Xmin+rand(0,1)(Xmax‑Xmin)               (6) 公式(6)中rand(0,1)表示生成范围为[01]的随机数， Xmax为辨识参数上限， Xmin为辨识 参数下限， Xi为第i次搜索 初始化值； 然后， 蜜蜂更新蜜源的位置信息： vij＝xij+δ(xij‑xkj)                   (7) 式中， v代表更新的位置， j∈{1,2,3...,m ‑1,m}代表更新的是第几个参数， i和k代表第 几次更新， 但i≠k， δ∈[ ‑1,1]， δ(xij‑xkj)表示扰动大小； 当新的蜜源Vi带入公式(3)的求解 小于Xi时， 其中Vi＝[vi1,vi2,…,vim]， 采用贪心的方法用Vi替代Xi； 所有的引领蜂根据公式(7)更新后， 跟随蜂按照引领蜂提供的蜜源信息， 进行更新， 经 过迭代， 如果蜜源Xi达到阈值但没找到更好的蜜源， 则Xi将会被放弃， 并将引领蜂转换为侦 查蜂。 最终， 判断求得的解是否满足 终止条件， 或者是否 达到了迭代的上限。 通过人工蜂群算法对参数进行辨识， 最小化偏差e， 从而得到机械臂的真实动力学参 数。 4.如权利要求3所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器 设计方法， 其特 征在于， 步骤C具体为： 采集关节角度、 角速度和加速度， 通过公式(1)和B中计算出的真机模型参数， 计算得到 动力学力矩 τd， 然后采用双环PID控制， 其中 内环的控制律 为： 其中 为目标关节角速度， 为机械臂真实关节角速度， 为两者偏差值， 为比例控 制， TI为积分控制参数， 为微分控制参数； 外环的控制律 为： e(t)＝qd‑q    (11) 其中qd为目标关节角度， q为机械臂真实关节角度， e为两者偏差值， KP为比例控制， TI为 积分控制参数， TD为微分控制参数； 前馈控制的控制律 为： τ ＝ τPID+τd    (12) 其中τ为动力学前馈输出力矩， τPID为双环PID力矩， τd为动力学参数辨识后计算出的动 力学力矩。 5.如权利要求4所述的一种基于强化学习与动力学前馈融合的机器人轨迹跟踪控制器 设计方法， 其特 征在于， 步骤D具体为： 构建机械臂输入输出模型： xt+1＝f(xt, τt)    (13) 式中f函数代表机械臂非线性动力系 统， t时刻输入为关节力矩τt， xt为t时刻 机械臂状态， xt+1为t+1时刻机 械臂状态， 智能体输出为关节力矩值。权　利　要　求　书 2/3 页 3 CN 114952849 A 3