CN114055471A - 一种结合神经运动规划算法和人工势场法的机械臂在线运动规划方法 - Google Patents

Abstract

一种结合神经运动规划算法和人工势场法的机械臂在线运动规划方法，属于机器人运动规划领域。本发明为了解决基于强化学习的神经运动规划器当规划任务的精度要求高时训练困难的问题，以及人工势场法的斥力势场构建复杂、存在局部极值的问题。本发明包括：一：对人工势场法进行简化，仅保留机械臂受到的引力势场，在避免构建复杂的斥力势场的同时也使局部极值问题不再存在。二：提出将人工势场法和强化学习相结合进行规划的新思路，通过设计灵活的切换机制，在距离目标较远时采用强化学习进行规划、距离小于阈值时切换为人工势场进行规划，提高了强化学习的训练速度和运动规划成功率。通过在仿真引擎中对不同精度的规划任务进行训练和测试，验证了所提方法的有效性。本发明用于机器人运动规划技术领域。

Description

一种结合神经运动规划算法和人工势场法的机械臂在线运动 规划方法

技术领域

本发明涉及动态环境下的机械臂在线运动规划方法，属于机器人运动规划领域。

背景技术

机械臂运动规划算法可以分为离线运动规划算法和在线运动规划算法两类，离线运动规划算法的输入为规划目标、环境障碍物信息和运动学约束，输出为一条完整的轨迹。在线运动规划算法则接收规划目标和状态感知信息输出机械臂一步关节动作，机械臂执行一步运动之后将新的状态信息反馈给运动规划器输出下一步的动作。在线运动规划算法与离线运动规划算法相比具备闭环的规划能力，可以实现动态环境下的运动规划，因而具有更强的适应性。因此，机械臂在线运动规划非常重要。

现有专利文献CN113119114A公开了一种基于强化学习算法的工业机械臂运动规划方法，将强化学习Actor-Critic算法应用到机械臂运动规划中，使机械臂与环境建立起一种交互关系，通过与环境的实时交互进行训练，提高机械臂对环境的适应能力，从而实现自主学习控制；该文献首先搭建机械臂手眼系统的仿真环境，然后根据仿真环境建立强化学习算法模型，最终完成机械臂的运动规划训练，实现机械臂的智能控制。该现有专利文献采用迭代优化来优化路径，使用已有的强化学习中的一种算法来实现机械臂的运动规划任务。

神经运动规划算法(Neural Motion Planning,NMP)是一种在线运动规划算法，在高维规划空间中的规划效率与传统规划算法相比具有很大的优势，强化学习是训练神经运动规划器的有效手段，但当规划任务的精度提高时基于强化学习的神经运动规划器存在训练困难的问题。人工势场法同样为在线运动规划算法，其优点为计算简洁、高效，在具有高维规划空间的任务中适应性好，但人工势场法的斥力势场构建复杂，且存在局部极值的问题。

发明内容

本发明要解决的技术问题是：

本发明针对基于强化学习的神经运动规划器当规划任务的精度要求高时训练困难的问题，以及人工势场法的斥力势场构建复杂、存在局部极值的问题，提出了一种结合神经运动规划算法和人工势场法的机械臂在线运动规划方法。

本发明为解决上述技术问题所采用的技术方案为：

一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，包括以下步骤：

步骤一：在机械臂的工作空间中建立仅包含引力势场的人工势场；

步骤二：计算神经运动规划算法中智能体的观测信息s_t；

步骤三：利用算法切换规则从神经规划算法和人工势场法两者中选取一个算法来规划机械臂运动；

步骤四：使机械臂执行上一步规划得到的动作，重新计算神经运动规划算法中智能体的观测信息s_t+1；

步骤五：计算神经运动规划算法中智能体获得的奖励值；

步骤六：重复步骤三到步骤五，以收集神经运动规划算法中智能体与环境的交互数据，以观测信息、动作、下一步的观测信息、奖励值组成的元组的形式进行存储；当神经运动规划算法中智能体的交互数据达到一定的量时，开始以一定的频率训练神经运动规划算法中智能体的策略神经网络；当训练回合数达到设定的最大回合数时，停止收集交互数据和训练；

步骤七：重复步骤三到四，利用训练结束后神经运动规划算法和仅保留引力势场的人工势场法来进行机械臂的运动规划；当机械臂末端坐标系的位姿与目标位姿之间的位置距离和姿态距离分别小于距离阈值和姿态阈值时认为运动规划成功，否则重复步骤三到步骤四直到达到最大的规划步数限制。

进一步地，步骤一所述“仅包含引力势场的人工势场”的计算公式如下：

式中，K_a表示引力系数，p_end和at_end分别表示末端坐标系的位置和姿态，p_goal和at_goal分别表示目标位置和姿态，d_p、d_a分别为位置距离与姿态距离。

进一步地，步骤二所述“神经运动规划算法中智能体的观测信息”包含如下内容：

机械臂神经运动规划算法中智能体的观测信息设定为：关节角度q_t、末端位姿T_t、目标位姿T_goal、规划精度ε；机械臂的末端位姿T_t可以由关节角度q_t和机械臂的正运动学模型计算得到。

进一步地，步骤三所述算法切换规则为：

计算机械臂末端坐标系的位置距离和姿态距离，并与位置距离阈值和姿态距离阈值进行比较，当位置距离和姿态距离均小于对应的阈值时采用仅保留引力势场的人工势场法进行规划，否则利用神经运动规划算法进行运动规划，具体形式如下：

式中，Δq_t为机械臂在t时刻的动作(含义为机械臂的关节角度增量)，

表示仅保留引力势场的人工势场法的规划结果，

表示神经运动规划算法的规划结果，

分别为强化学习神经运动规划器和人工势场法进行切换的位置距离阈值和姿态距离阈值。

进一步地，所述“仅保留引力势场的人工势场法进行运动规划”，其计算公式如下：

式中，J_p(q_t)为当前时刻的机械臂雅克比矩阵，Δq_t为机械臂的关节角度增量，Δt为时间步长。

进一步地，所述“利用神经运动规划算法进行运动规划”，其计算公式如下：

式中，π_φ(s_t)为神经运动规划器的输出值，ac为动作缩放参数将神经网络的输出缩放到合适的范围。

进一步地，步骤五所述“神经运动规划算法中智能体获得的奖励值”的计算公式如下：

上式中，d_a、d_p分别表示机械臂末端坐标系位姿与目标位置之间的姿态距离和位置距离，ε_a、ε_p分别表示判断是否规划成功的姿态距离阈值和位置距离阈值，f(d_a,d_p)的计算公式为：

f(d_a,d_p)＝0.02(0.2-(0.5d_a+0.15d_p))

一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现所述的一种结合神经规划算法和人工势场法的机械臂在线运动规划方法的步骤。

本发明至少具有以下有益技术效果：

本发明对人工势场法进行简化，仅保留机械臂受到的引力势场，在避免构建复杂的斥力势场的同时也使局部极值问题不再存在。本发明提出了将人工势场法和基于强化学习的神经运动规划算法这两种在线运动规划算法相结合来同时降低两者复杂度的技术途径，当机械臂末端坐标系的位姿与目标位姿的误差大于一定的阈值时采用基于强化学习的神经运动规划算法规划机械臂的运动，当机械臂末端坐标系的位姿与目标位姿的误差小于一定的阈值时采用仅保留引力势场的人工势场法规划机械臂的运动。提出将人工势场法和强化学习相结合进行规划的新思路，通过设计灵活的切换机制，在距离目标较远时采用强化学习进行规划、距离小于阈值时切换为人工势场进行规划，提高了强化学习的训练速度和运动规划成功率。通过在仿真引擎中对不同精度的规划任务进行训练和测试，验证了所提方法的有效性。

本发明将人工势场法和基于强化学习的神经运动规划算法这两种在线运动规划算法相结合，利用神经运动规划算法实现大范围的避障运动规划，利用简化后的人工势场法进行目标附近的小范围位姿调整，一方面解决了基于强化学习的神经运动规划算法在高精度规划任务中难以训练的问题，另一方面解决了人工势场法斥力势场建立复杂以及存在局部极值的问题。通过将两种在线运动规划算法相结合，在保留两者计算效率高、高维适应性好的优点的同时，降低两者的复杂度。本发明用于机器人运动规划技术领域。

附图说明

图1是机械臂运动规划任务场景；

图2是训练过程的成功率测试曲线；

图3是训练过程的累计交互步数曲线。

具体实施方式

具体实施方式一：

本实施方式所述的一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，包括以下步骤：

步骤一：在机械臂的工作空间中建立仅包含引力势场的人工势场，引力势场的建立方式如下式所示：

式中，K_a表示引力系数，p_end和at_end分别表示末端坐标系的位置和姿态，p_goal和at_goal分别表示目标位置和姿态，d_p、d_a分别为位置距离与姿态距离。利用仅包含引力势场的人工势场进行运动规划的公式如下：

式中，J_p(q_t)为当前时刻的机械臂雅克比矩阵，Δq_t为机械臂的关节角度增量，Δt为时间步长。

步骤二：计算神经运动规划算法中智能体的观测信息s_t，本发明中将机械臂运动规划任务智能体(神经网络)的观测信息设定为：关节角度q_t、末端位姿T_t、目标位姿T_goal、规划精度ε。机械臂的末端位姿T_t可以由关节角度q_t和机械臂的正运动学模型计算得到。

步骤三：利用算法切换规则从神经规划算法和人工势场法两者中选取一个算法来规划机械臂运动。首先计算机械臂末端坐标系的位置距离和姿态距离，并与位置距离阈值和姿态距离阈值进行比较，当位置距离和姿态距离均小于对应的阈值时采用仅保留引力势场的人工势场法进行规划，否则利用神经运动规划算法进行运动规划。切换规则如下式所示：

式中，

分别为强化学习神经运动规划器和人工势场法进行切换的位置距离阈值和姿态距离阈值，π_φ(s_t)为神经运动规划器的输出值，ac为动作缩放参数将神经网络的输出缩放到合适的范围。

步骤四：使机械臂执行上一步规划得到的动作，重新计算神经运动规划算法中智能体的观测信息s_t+1；

步骤五：计算神经运动规划算法中智能体获得的奖励值。奖励函数设计为如下形式：

上式中，d_a、d_p分别表示机械臂末端坐标系位姿与目标位置之间的姿态距离和位置距离，ε_a、ε_p分别表示判断是否规划成功的姿态距离阈值和位置距离阈值，f(d_a,d_p)的计算公式为：

f(d_a,d_p)＝0.02(0.2-(0.5d_a+0.15d_p))

步骤六：重复步骤三到步骤五，以收集神经运动规划算法中智能体与环境的交互数据，以观测信息、动作、下一步的观测信息、奖励值组成的元组的形式进行存储。当神经运动规划算法中智能体的交互数据达到一定的量时，开始以一定的频率训练神经运动规划算法中智能体的策略神经网络。当训练回合数达到设定的最大回合数时，停止收集交互数据和训练。

步骤七：重复步骤三到四，利用训练结束后神经运动规划算法和仅保留引力势场的人工势场法来进行机械臂的运动规划。当机械臂末端坐标系的位姿与目标位姿之间的位置距离和姿态距离分别小于距离阈值和姿态阈值时认为运动规划成功，否则重复步骤三到步骤四直到达到最大的规划步数限制。

用以下实施例验证本发明的有益效果：

实施例：

1)实验任务

在真实机械臂上直接进行训练的速度很慢，且容易损坏机械臂，所以本发明首先在物理仿真引擎MuJoCo中建立机械臂的动力学和运动学模型，进行仿真训练以验证算法的有效性，最后将训练好的策略网络在真实环境下测试。本发明所采用的机械臂为jaco2协作机械臂，此机械臂有7个关节。如图1，机械臂强化学习运动规划的训练任务为在一个桌面的环境中使机械臂从初始位姿运动到目标位姿。桌面是机械臂的环境障碍物，当机械臂与桌面发生碰撞或机械臂自身连杆之间发生碰撞时训练回合结束。

2)训练参数

本发明选取在高维连续动作空间中表现较好的软策略评论家(SAC)强化学习算法来训练神经网络运动规划器，在SAC算法中有一个策略网络和两个价值网络，三个神经网络都为包含三个隐藏层的全连接神经网络。在策略网络中，三个隐藏层的节点数都为256，隐藏层的激活函数为elu函数，最后输出层的激活函数为tanh函数。策略网络的输出是机械臂各关节的关节角度增量，由于tanh函数的输出值在(-1,1)之间，需利用一个动作尺度参数，将策略网络的输出伸缩到合适的大小，本章中动作尺度参数的值为0.2，意味着每个关节每次的运动量不大于0.2rad。SAC算法的两个价值网络的结构相同，价值网络三个隐藏层的节点数分别为256、256、64，隐藏层的激活函数为elu函数，最后输出层的激活函数为线性函数。

人工势场法的参数为时间步长Δt，本发明中对Δt进行了分段设置。当位置距离大于5cm时Δt＝0.2，当位置距离小于5cm时Δt＝0.05。规划精度是所有神经运动规划器的重要参数，本发明设置了四组难度递增的精度参数，四组参数中姿态精度都为0.5rad，位置精度最大为0.2m，最小为0.01m，精度的编号如表2所示。本发明提出的结合了简化后的人工势场法和强化学习运动规划算法的融合运动规划算法为：SAC-APF算法，对于SAC-APF算法本发明仅在难度最大的规划精度下进行了实验，SAC算法分别测试了四种规划精度下的训练效果。

表2规划精度

3)实验结果

本发明从运动规划成功率和训练速度这两个方面来评估训练结果。运动规划成功率通过生成100组新的规划任务，并测试策略网络在这100组规划任务中的成功率来评估。每组实验训练了50000个回合，每组实验在同样的训练参数下训练3次。

表3成功率测试结果

训练过程中的成功率测试结果如图2所示，图中的实线表示三次训练的成功率均值，实线背后的阴影表示三次训练中成功率最大、最小值之间的覆盖范围。为了定量的评估不同算法在不同精度下的规划成功率，将每组训练中测试成功率最高时的策略网络参数进行了保存，等到训练结束后用保存的策略网络参数来评估算法能够达到的最好性能。成功率测试结果如表3所示。由成功率的测试结果可以得出，将强化学习算法与人工势场法相结合可以明显提升规划的成功率。

本发明通过计算训练过程中的交互步数来评估训练速度，每隔100个训练回合存储一次到此回合为止智能体与环境交互的总步数，训练过程的累计交互步数如图3所示。图中的实线表示三次训练的累计交互步数均值，实线背后的阴影表示三次训练中累计交互步数最大、最小值之间的覆盖范围。

表4训练速度统计结果

训练结束后的总交互步数如表4所示，可见将强化学习算法与人工势场法相结合不仅可以提高规划任务的成功率，同样也可以明显的加快训练的速度。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明做出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (8)

1.一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于：包括以下步骤：

步骤一：在机械臂的工作空间中建立仅包含引力势场的人工势场；

步骤二：计算神经运动规划算法中智能体的观测信息s_t；

步骤三：利用算法切换规则从神经规划算法和人工势场法两者中选取一个算法来规划机械臂运动；

步骤四：使机械臂执行上一步规划得到的动作，重新计算神经运动规划算法中智能体的观测信息s_t+1；

步骤五：计算神经运动规划算法中智能体获得的奖励值；

步骤六：重复步骤三到步骤五，以收集神经运动规划算法中智能体与环境的交互数据，以观测信息、动作、下一步的观测信息、奖励值组成的元组的形式进行存储；当神经运动规划算法中智能体的交互数据达到一定的量时，开始以一定的频率训练神经运动规划算法中智能体的策略神经网络；当训练回合数达到设定的最大回合数时，停止收集交互数据和训练；

步骤七：重复步骤三到四，利用训练结束后神经运动规划算法和仅保留引力势场的人工势场法来进行机械臂的运动规划；当机械臂末端坐标系的位姿与目标位姿之间的位置距离和姿态距离分别小于距离阈值和姿态阈值时认为运动规划成功，否则重复步骤三到步骤四直到达到最大的规划步数限制。

2.根据权利要求1的所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，步骤一所述“仅包含引力势场的人工势场”的计算公式如下：

式中，K_a表示引力系数，p_end和at_end分别表示末端坐标系的位置和姿态，p_goal和at_goal分别表示目标位置和姿态，d_p、d_a分别为位置距离与姿态距离。

3.根据权利要求1的所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，步骤二所述“神经运动规划算法中智能体的观测信息”包含如下内容：

机械臂神经运动规划算法中智能体的观测信息设定为：关节角度q_t、末端位姿T_t、目标位姿T_goal、规划精度ε；机械臂的末端位姿T_t可以由关节角度q_t和机械臂的正运动学模型计算得到。

4.根据权利要求1所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，步骤三所述算法切换规则为：

计算机械臂末端坐标系的位置距离和姿态距离，并与位置距离阈值和姿态距离阈值进行比较，当位置距离和姿态距离均小于对应的阈值时采用仅保留引力势场的人工势场法进行规划，否则利用神经运动规划算法进行运动规划，具体形式如下：

式中，Δq_t为机械臂在t时刻的动作(含义为机械臂的关节角度增量)，

表示仅保留引力势场的人工势场法的规划结果，

表示神经运动规划算法的规划结果，

分别为强化学习神经运动规划器和人工势场法进行切换的位置距离阈值和姿态距离阈值。

5.根据权利要求4所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，所述“仅保留引力势场的人工势场法进行运动规划”，其计算公式如下：

式中，J_p(q_t)为当前时刻的机械臂雅克比矩阵，Δq_t为机械臂的关节角度增量，Δt为时间步长。

6.根据权利要求4所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，所述“利用神经运动规划算法进行运动规划”，其计算公式如下：

式中，π_φ(s_t)为神经运动规划器的输出值，ac为动作缩放参数将神经网络的输出缩放到合适的范围。

7.根据权利要求1的所述一种结合神经规划算法和人工势场法的机械臂在线运动规划方法，其特征在于，步骤五所述“神经运动规划算法中智能体获得的奖励值”的计算公式如下：

上式中，d_a、d_p分别表示机械臂末端坐标系位姿与目标位置之间的姿态距离和位置距离，ε_a、ε_p分别表示判断是否规划成功的姿态距离阈值和位置距离阈值，f(d_a,d_p)的计算公式为：

f(d_a,d_p)＝0.02(0.2-(0.5d_a+0.15d_p))

8.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-7中任一项所述的一种结合神经规划算法和人工势场法的机械臂在线运动规划方法的步骤。

Images