基于启发轨迹的灵巧手与机械臂强化学习协同控制方法

本发明属于机械臂及灵巧手控制领域，尤其涉及一种基于启发轨迹的灵巧手与机械臂强化学习协同控制方法。

背景技术：

1、当下生活中大部分机器人由机械臂与末端执行器构成，机械臂提供在空间范围中的移动灵活性与目标接近的功能，末端执行器与特定的对象或环境进行交互，进而完成用户设定的任务。目前大部分机械臂的末端执行器通常是二指夹爪或吸盘，该类执行器自由度受限，往往只能对具有大表面的坚硬物体进行平移操作，这对物体的种类和操作任务的扩展带来了很大的限制。

2、随着近年来机器人硬件平台和智能化技术的快速发展，机器人的结构，控制技术不断更新，其作业能力得到了迅猛提升，其中最具代表性的便是多关节，多自由度的末端执行器——多指灵巧手。灵巧手的指数≥3，自由度≥9，具有仿生结构，如同人类一般，能够适应各种形状材质的物体表面，调整广泛范围的姿态，并精细地完成伸展、抓取、夹持等操作，展现了优秀的精准性、通用性与泛化性。

3、目前，灵巧手最有前景的应用领域是例如家庭与航空航天等非结构化场景，凭借其对多样化物体表面的操作适应性及进行多样化操作任务的可能性，可以为房间整理、物品递送等家庭任务与设备修复，零件回收等空间任务带来了极大的便利，同时，这些任务也对其高精度末端操作的能力提出了要求，带来了更高的控制难度。

4、灵巧手由于其具有较高自由度，在与环境交互及物体操作时不仅需要考虑与对象的接触状态，还需要考虑自身的姿态合理性，上述两者与其高自由度的关节具有强耦合关系，在操作过程中难以通过偏导求梯度等方式进行数值上的优化，限制了该类基于模型的控制方法的表现。

5、如同上文所说，由于高维的动作空间和复杂的动力学模型，其控制方法的设计是一个具有挑战性的问题。目前主流可分为基于模型和基于学习与基于模型两类。基于模型的代表方法有：使用模型预测路径积分控制(mp pi)方法成功执行了操纵一个立方体的任务；改进的mp pi方法使两只手之间的交接任务变得可行。基于学习的方法均涉及强化学习，由于其简化了控制器的设计过程，无模型的方法配合大量探索在灵巧手控制领域取得了大量进展：基于强化学习的控制器成功控制灵巧手在手内重新定位一个块或魔方。由强化学习方法构成的学习系统成功实现在没有形状信息的情况下重新定位大量物体。通过来自遥操作或视频的示范学习进行灵巧操纵展示了良好的仿生效果。虽然当下研究表明强化学习能够在单手控制上进行高效且可扩展的学习，但对于无模型强化学习来说，在面向通用任务时灵巧手-机械臂的协同控制仍然是一个困难的问题。

6、由于灵巧手-机械臂系统相较灵巧手拥有更高的自由度，且两者存在异构的差异，各自由度发挥的效果不尽相同，在进行传统强化学习方法下的探索时，面临数据规模较大，有效样本难以选择，梯度有效性下降等问题，容易陷入局部陷阱导致在控制策略尚未合理化时就停止更新。尤其是面临多样化任务和多样化操作模式(平移、姿态调整、旋转等)，不同的操作与不同的对象对于位姿及接触力要求都不同，形成通用有效的控制策略变得极其困难。现有的方法需要收集专家数据加以引导，需要消耗极大的人力与时间成本，但仍然无法泛化至不同任务，极大限制了灵巧手作业能力的提升。

技术实现思路

1、本发明目的在于提供一种基于启发轨迹的灵巧手与机械臂强化学习协同控制方法，以解决上述的技术问题。

2、为解决上述技术问题，本发明的基于启发轨迹的灵巧手与机械臂强化学习协同控制方法的具体技术方案如下：

3、一种基于启发轨迹的灵巧手与机械臂强化学习协同控制方法，包括如下步骤：

4、步骤1：仿真环境与模型创建：建立基于gym库的mujocogym框架，建立灵巧手-机械臂一体化模型，构造两者相连关系及运动约束；

5、步骤2：在步骤1的基础上建立mujoco环境下的机械臂-灵巧手协同控制马尔科夫决策过程模型；

6、步骤3：针对环境及任务特点，进行分阶段奖励函数的设计，使其可以充分表达任务的完成情况，通过使用更丰富的信息奖励智能体；

7、步骤4：采用cs-ddp方法在动力学模型与约束基础上生成启发式轨迹样本，为强化学习训练过程提供引导；

8、步骤5：利用此前生成样本与智能体探索样本，以自适应权重的方式指导梯度更新。

9、进一步的，所述步骤1在mujoco环境中建立灵巧手-机械臂一体化模型，构造两者相连关系及运动约束，同时导入相关环境、物体、机器人、传感器的xml文件，形成仿真环境，并完成初始化状态、任务类型，环境边界及状态动作上下界的设定。

10、进一步的，对于建立的灵巧手-机械臂一体化模型，具有固定底座，在运动过程中各关节有明确耦合的动力学关系，并非仅在空间上进行考量的运动学关系。每一次环境的初始化，都会对其底座的三维位置进行一定范围的随机化。

11、进一步的，所述步骤2使用元组<s，a，p，r，γ>表示某一时刻的信息，其中s和a分别是状态和动作空间，强化学习算法学习一个策略π(a|s)，使得在该策略下获得的奖励r(st,at)的期望累积和最大化，起始于初始状态分布u并遵循过渡概率p(s(t+1)|st,at)，折扣因子γ用于平衡即时奖励和未来奖励之间的权衡，q(s,a,s′)表示状态转移后状态s(t+1)的估计值，本方法的目标由以下公式给出：

12、a＝argmax(r(s，a)+γ∑s′∈sq(s，a，s′)p(s(t+1)|st；at))。

13、进一步的，针对环境及任务特点，进行分阶段奖励函数的设计，采用不同阶段使用不同奖励函数的组合方法，具体如下所示：

14、对于平移问题，修改奖励函数，通过取对数放大的形式以激励机器人，这一阶段奖励函数设置为以下形式：

15、r＝r(s，g)+r(log(s，g))+r(path)

16、其中，r(s，g)的值为机器人手和目标之间距离的二范数的负值，在手和目标之间的距离越小时，获得的奖励越大，此外，对上述二范数进行对数变换得到r(log(s，g))，其中奖励随着手和目标之间的距离减小而以迅速增加，奖励函数还包括步数乘以负系数的r(path)，用于同样距离下惩罚更多的路径；

17、对于姿态调整问题，采用将灵巧手的姿态与对象的姿态进行比较并不断优化二者之间差异的方法，奖励函数用以下形式表示：

18、r＝r(s，g)+r(log(s，g))+r(path)+r(ori)+r(target)

19、其中前三项与平移阶段奖励函数的含义完全相同，第四项r(ori)表示灵巧手的姿态的四元数与目标的姿态的四元数之间的欧几里德距离，这一项用于激励机器人在使用灵巧手靠近目标时调整其姿态朝向预期的方向，最后，r(rarget))直接捕捉目标的旋转奖励，是由目标旋转角度乘以一定系数得到的函数，表示对灵巧手转动目标的奖励反馈；

20、在目标旋转到一定角度后，进行平移时的奖励函数设计：

21、r＝r(s，g)+r(log(s，g))+r(path)+r(ori)+r(target)+r(move)

22、这里新的r(move)奖励项直接与对象符合约束的平移距离相关，与先前表征其旋转的r(target)组合，完成对操作对象的完整表征。

23、进一步的，所述步骤4：提出结合约束强化的微分动态规划方法cs-ddp，在微分动态规划ddp的基础上，使得原本最小化偏离期望轨迹的目标函数额外加入紧约束项，

24、设xt是机械臂在时刻t的状态，ut是时刻t的控制输入，系统的动力学描述为：xt+1＝f(xt,ut)，其中f是机械臂的非线性动力学函数，目标是找到一系列控制输入ut，使得最小化目标函数j，在系统的动力学和约束条件下：

25、

26、

27、其中l是阶段目标函数，惩罚偏离期望轨迹，φ是末状态目标函数，惩罚偏离最终状态，是期望的初始状态，是期望的最终状态，g是强制系统约束的约束函数，约束函数被更新为：

28、g(x，u)＝g0(x，u)+λv*gv(x，u)+λe*ge(x，u)

29、其中g0(x，u)是没有约束的原始目标函数，λv和λe是速度和位置约束的惩罚系数，gv(x，u)和ge(x，u)是速度和位置约束的惩罚函数，这些惩罚函数定义为

30、gv(x，u)＝max(0，||vk||-vmax)2

31、ge(x，u)＝max(0，||pk-pdes||-emax)2

32、在算法执行过程中利用仿真环境对目标对象姿态四元数进行多次采样，以提供准确的位置和姿态范围，利用这些信息确定操作所需的灵巧手位置和姿态的最佳范围。，即最后将得到最大误差emax和最佳灵巧手位姿

33、进一步的，所述步骤5在考虑灵巧手动力学与操作姿态约束的前提下，将马尔可夫决策框架更新为以下：

34、a＝argmax(r(s，a)+γ∑s′∈sq(s，a，s′)p(s(t+1)|st；at))

35、

36、上式中r(s，a)表示在状态s中执行动作a获得的奖励，q(s，a，s′)表示通过执行动作a从状态s过渡到状态s′时获得的q值，p表示状态转移的概率，测量函数∈loc量化当前距离向量与理想距离向量之间的距离差异，确保小于δ，测量函数∈pos捕捉当前姿势与理想姿势之间的差异，差异越小，性能越好。

37、进一步的，所述步骤5首先使用cs-ddp方法在mujoco环境中根据灵巧手与机械臂动力学模型及目标物姿态约束生成启发式轨迹样本，建立并行训练环境与样本缓冲区，并在训练环境中随机初始化策略πθ与参数λ，其次设定循环次数niter，在每一个循环中，设定交互步数nstep，在并行环境1中使用启发式轨迹样本填充样本缓冲区1，并以此建立<s，a，p，r，γ>的马尔可夫链，在并行环境2中使用πθ作为探索策略进行探索性样本收集，并填充进入样本缓冲区2，以此循环nstep步完成样本收集，之后依据ppo算法梯度更新方式，以λ与1-λ的比重分别将缓冲区1的样本计算所得梯度与缓冲区2的样本计算所得梯度更新至策略πθ上，完成梯度更新后根据缓冲区1贡献的梯度更新幅度减小λ的值，至此完成一个循环，重复循环niter次，直至机器人在仿真中获得合理的操作策略。

38、本发明的基于启发轨迹的灵巧手与机械臂强化学习协同控制方法具有以下优点：

39、本方法的优势在于考虑到了灵巧手-机械臂协同控制的难题，着重关注了其中灵巧手的姿态调整与有效的启发式样本收集两大挑战，通过阶段式奖励函数的创新式设计强化了灵巧手姿态的调整优化；通过cs-ddp方法建立约束及生成样本启发灵巧手-机械臂系统进行更合理的尝试，提高了整体学习效率；通过混合梯度更新的方法将探索和启发共同作用在机器人系统上，使得既具有高效合理的行为，又具备强化学习自由探索的泛化性。整体框架无需手工收集专家数据，模型约束由仿真环境推导、采样获得，保证了整体框架的轻量与灵活。