1.本发明涉及机器人制造
技术领域：
：，尤其涉及一种机械臂动态避碰规划方法。
背景技术：
：：2.目前投入到工业生产中的工业机械臂大多数靠人工示教的方法完成路径规划，以完成焊接、喷涂、码垛、搬运、装配、加工等生产过程。人工示教方法在应对单一重复的特定任务时具有一定的作用。3.然而，随着生产任务的日益多样化与任务场景的日益复杂化，使现有的人工示教方法的诸如操作复杂繁琐、通用性差以及精度低等缺点渐渐暴露。且随着智能生产领域对机械臂的工作效率与工作精度要求越来越高，落后的人工示教方法已无法满足这种需求。为了解决机械臂的路径规划问题，许多算法及其变种算法被开发出来，但是这些算法大多数只支持离线规划，无法在动态障碍环境下运行，以至于机械臂不具备应对突发危险的能力。技术实现要素：4.本发明的目的是提供一种机械臂动态避碰规划方法，其控制效果好、鲁棒性强且支持在线优化。5.为了实现上有目的，本发明公开了一种机械臂动态避碰规划方法，其包括如下步骤：6.s1、构建机械臂的系统动态方程；7.s2、依据所述系统动态方程计算所述机械臂的原始预测函数；8.s3、构建环境惩罚模型；9.s4、依据所述原始预测函数和环境惩罚模型构建目标预测函数；10.s5、对所述目标预测函数进行优化，以获得控制序列；11.s6、依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。12.较佳地，所述环境惩罚模型以所述机械臂的关节状态量、控制量作为输入量，及以所述系统动态方程的环境惩罚量作为输出量。13.较佳地，所述步骤s6具体包括：14.s61、初始化所述目标预测函数中的环境惩罚量的权重；15.s62、在预设时间内以预设的连续控制量赋值所述目标预测函数，以获得多个状态量和环境惩罚量；16.s63、依据所述多个状态量和环境惩罚量优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。17.具体地，所述步骤s63具体为：18.依据所述多个状态量和环境惩罚量，以自然进化策略优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。19.较佳地，对所述目标预测函数赋值每一控制量获得对应的一个状态量和一个环境惩罚量。20.较佳地，所述步骤s1具体包括：21.s11、将所述系统动态方程进行离散化处理，得到离散化后的系统动态方程；22.s12、依据离散化后的系统动态方程计算得到所述机械臂的原始预测函数，所述原始预测函数包含有关于控制量的参数。23.较佳地，所述控制序列包括多个一一对应的新的状态量和控制增量，所述新的状态量为当前状态量与上一时刻的状态量的结合，所述控制增量为当前控制量与上一时刻的控制量的结合，所述环境惩罚模型用于预测所述机械臂在所述控制序列内的状态。24.相应地，本发明还公开了一种工业机械臂动态避碰规划装置，其包括：25.第一构建单元，被配置为用于构建机械臂的系统动态方程；26.计算单元，被配置为用于依据所述系统动态方程计算所述机械臂的原始预测函数；27.第二构建单元，被配置为用于构建环境惩罚模型；28.第三构建单元，被配置为用于依据所述原始预测函数和环境惩罚模型构建目标预测函数；29.优化单元，被配置为用于对所述目标预测函数进行优化，以获得控制序列；30.训练单元，被配置为用于依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。31.相应地，本发明还公开了一种存储介质，所述存储介质存储有多条指令，所述指令适于处理器进行加载，以执行如上所述的机械臂动态避碰规划方法中的步骤。32.相应地，本发明还公开了一种机器人，所述机器人上设有如上所述的工业机械臂动态避碰规划装置。33.与现有技术相比，本发明依据原始预测函数和环境惩罚模型构建目标预测函数，并依据控制序列训练环境惩罚模型，直至环境惩罚模型收敛，其目标预测函数结合了原始预测函数和环境惩罚模型，使目标预测函数更能贴合实际，通过控制序列训练环境惩罚模型，直至环境惩罚模型收敛，以使目标预测函数能够实时在线优化，并获得更好的控制效果、鲁棒性和稳定性。附图说明34.图1是本发明的机械臂动态避碰规划方法的流程框图；35.图2是本发明的环境惩罚模型的结构图；36.图3是自然进化策略算法的算法流程图；37.图4是本发明的工业机械臂动态避碰规划装置的结构框图。具体实施方式38.为详细说明本发明的技术内容、构造特征、所实现目的及效果，以下结合实施方式并配合附图详予说明。39.请参阅图1所示，本实施例的机械臂动态避碰规划方法，其包括如下步骤：40.s1、构建机械臂的系统动态方程；41.s2、依据所述系统动态方程计算所述机械臂的原始预测函数；42.s3、构建环境惩罚模型；43.s4、依据所述原始预测函数和环境惩罚模型构建目标预测函数；44.s5、对所述目标预测函数进行优化，以获得控制序列；45.s6、依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。46.较佳地，所述环境惩罚模型以所述机械臂的关节状态量、控制量作为输入量，及以所述系统动态方程的环境惩罚量作为输出量。47.较佳地，所述步骤s6具体包括：48.s61、初始化所述目标预测函数中的环境惩罚量的权重；49.s62、在预设时间内以预设的连续控制量赋值所述目标预测函数，以获得多个状态量和环境惩罚量；50.s63、依据所述多个状态量和环境惩罚量优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。51.具体地，所述步骤s63具体为：52.依据所述多个状态量和环境惩罚量，以自然进化策略优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。53.较佳地，对所述目标预测函数赋值每一控制量获得对应的一个状态量和一个环境惩罚量。54.较佳地，所述步骤s1具体包括：55.s11、将所述系统动态方程进行离散化处理，得到离散化后的系统动态方程；56.s12、依据离散化后的系统动态方程计算得到所述机械臂的原始预测函数，所述原始预测函数包含有关于控制量的参数。57.较佳地，所述控制序列包括多个一一对应的新的状态量和控制增量，所述新的状态量为当前状态量与上一时刻的状态量的结合，所述控制增量为当前控制量与上一时刻的控制量的结合，所述环境惩罚模型用于预测所述机械臂在所述控制序列内的状态。58.请参阅图1‑图3所示，下面以六轴工业机械臂为例，对本方法进行详细说明：59.1、系统动态方程和原始预测函数：60.设在t时刻的六轴工业机械臂构型向量为q(t)，速度向量为q’(t)，加速度向量为q”(t)，其中q(t)、q’(t)和q”(t)均为六维向量，分别代表机械臂六个关节的角度、角速度、角加速度。61.令机械臂的状态量x(t)＝[q(t)；q’(t)]，控制量u(t)＝q”(t)，则可以构建以下系统动态方程：[0062][0063]其中，[0064][0065]矩阵0m×n为m×n的全0矩阵，in×n为n维的单位矩阵。[0066]由于上述系统动态方程为连续时间系统，而连续时间系统无法直接作为预测控制器使用，因此，要先对连续时间的系统动态方程进行离散化，得到：[0067][0068]通过对上式进行移项可得离散化的系统动态方程：[0069][0070]其中，[0071][0072]在优化求解的过程中，还必须满足以下约束：[0073][0074]在预测时域np内进行递推得：[0075][0076]上式可以进一步表达为：[0077][0078]其中，[0079][0080]由此可得基于系统动态方程的原始预测函数为：[0081][0082]其中，xg为目标状态向量，qg、qu为相应的权重矩阵。通过优化每个k时刻的原始预测函数j，就能得到最优的控制量u(k)。下面上式改写成每一项都含有u(k)的形式：[0083][0084]上述原始预测函数可以用二次规划轻易求解。[0085]2、基于自然进化策略的静态/动态环境避障方法：[0086]在机械臂的优化问题含有碰撞距离、碰撞状态等硬约束，用传统的优化方法处理这些约束效率低下，或者说难以处理，因此本发明采用基于自然进化策略的静态/动态避障方法，并构造目标预测函数：[0087][0088]其中，f(x(k),u(k))为基于自然进化策略所训练的环境惩罚模型，fs与fd分别代表环境惩罚模型中的静态障碍网络与动态障碍网络，μ和φ均为系数，两种网络的结构与训练方法完全一致，唯一的区别在于动态障碍网络fd的状态量中还包含动态障碍物的状态量。该环境惩罚模型以系统动态方程的状态量x(k)与控制序列u(k)作为其输入，以环境惩罚量作为其输出。该网络的结构如图2所示。[0089]训练环境惩罚模型的步骤如下：[0090]1)初始化环境惩罚模型f的权重；[0091]2)一定时间内运行mpc控制器并优化函数jstable；[0092]3)得到一系列数据轨迹i(x(1)、x(2)、…x(k)，u(1)、u(2)、…u(k)、对应的环境惩罚量f1、f2、…、fk以及实际环境惩罚量f1real、f2real、…、fkreal)[0093]4)依次根据轨迹i中的数据，使用自然进化策略优化环境惩罚模型f中的权重参数；[0094]5)重复步骤2)‑步骤4)直到环境惩罚模型f收敛。[0095]在原始的自然进化策略算法中，需要更新的参数θ包含μ和σ，μ和σ是正态分布的两个参数，而本发明中使用的自然进化策略将参数σ固定而只专注与参数μ的更新，因此需要更新的参数θ也即是参数μ。[0096]在本发明中，使用的自然进化策略算法算法流程如图3所示，在图3示出的自然进化策略算法中，成本函数f的值依赖于环境，成本函数f可以设定为如下表达式：[0097][0098]模型预测的环境成本fk与实际成本fkreal的偏差越大梯度的模则越大，反之则越小。通过不断的迭代训练，最终可以使得成本函数f逼近现实的环境成本模型。[0099]请参阅图4所示,相应地，本发明还公开了一种工业机械臂动态避碰规划装置，其包括：[0100]第一构建单元10，被配置为用于构建机械臂的系统动态方程；[0101]计算单元20，被配置为用于依据所述系统动态方程计算所述机械臂的原始预测函数；[0102]第二构建单元30，被配置为用于构建环境惩罚模型；[0103]第三构建单元40，被配置为用于依据所述原始预测函数和环境惩罚模型构建目标预测函数；[0104]优化单元50，被配置为用于对所述目标预测函数进行优化，以获得控制序列；[0105]训练单元60，被配置为用于依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。[0106]相应地，本发明还公开了一种存储介质，所述存储介质存储有多条指令，所述指令适于处理器进行加载，以执行如上所述的机械臂动态避碰规划方法中的步骤。[0107]相应地，本发明还公开了一种机器人，所述机器人上设有如上所述的工业机械臂动态避碰规划装置。[0108]结合图1‑图4，本发明依据原始预测函数和环境惩罚模型构建目标预测函数，并依据控制序列训练环境惩罚模型，直至环境惩罚模型收敛，其目标预测函数结合了原始预测函数和环境惩罚模型，使目标预测函数更能贴合实际，通过控制序列训练环境惩罚模型，直至环境惩罚模型收敛，以使目标预测函数能够实时在线优化，并获得更好的控制效果、鲁棒性和稳定性，该方法的本质是使用最优解的梯度辅助下一步的搜索。[0109]以上所揭露的仅为本发明的优选实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明申请专利范围所作的等同变化，仍属本发明所涵盖的范围。当前第1页12当前第1页12
技术领域：
：，尤其涉及一种机械臂动态避碰规划方法。
背景技术：
：：2.目前投入到工业生产中的工业机械臂大多数靠人工示教的方法完成路径规划，以完成焊接、喷涂、码垛、搬运、装配、加工等生产过程。人工示教方法在应对单一重复的特定任务时具有一定的作用。3.然而，随着生产任务的日益多样化与任务场景的日益复杂化，使现有的人工示教方法的诸如操作复杂繁琐、通用性差以及精度低等缺点渐渐暴露。且随着智能生产领域对机械臂的工作效率与工作精度要求越来越高，落后的人工示教方法已无法满足这种需求。为了解决机械臂的路径规划问题，许多算法及其变种算法被开发出来，但是这些算法大多数只支持离线规划，无法在动态障碍环境下运行，以至于机械臂不具备应对突发危险的能力。技术实现要素：4.本发明的目的是提供一种机械臂动态避碰规划方法，其控制效果好、鲁棒性强且支持在线优化。5.为了实现上有目的，本发明公开了一种机械臂动态避碰规划方法，其包括如下步骤：6.s1、构建机械臂的系统动态方程；7.s2、依据所述系统动态方程计算所述机械臂的原始预测函数；8.s3、构建环境惩罚模型；9.s4、依据所述原始预测函数和环境惩罚模型构建目标预测函数；10.s5、对所述目标预测函数进行优化，以获得控制序列；11.s6、依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。12.较佳地，所述环境惩罚模型以所述机械臂的关节状态量、控制量作为输入量，及以所述系统动态方程的环境惩罚量作为输出量。13.较佳地，所述步骤s6具体包括：14.s61、初始化所述目标预测函数中的环境惩罚量的权重；15.s62、在预设时间内以预设的连续控制量赋值所述目标预测函数，以获得多个状态量和环境惩罚量；16.s63、依据所述多个状态量和环境惩罚量优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。17.具体地，所述步骤s63具体为：18.依据所述多个状态量和环境惩罚量，以自然进化策略优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。19.较佳地，对所述目标预测函数赋值每一控制量获得对应的一个状态量和一个环境惩罚量。20.较佳地，所述步骤s1具体包括：21.s11、将所述系统动态方程进行离散化处理，得到离散化后的系统动态方程；22.s12、依据离散化后的系统动态方程计算得到所述机械臂的原始预测函数，所述原始预测函数包含有关于控制量的参数。23.较佳地，所述控制序列包括多个一一对应的新的状态量和控制增量，所述新的状态量为当前状态量与上一时刻的状态量的结合，所述控制增量为当前控制量与上一时刻的控制量的结合，所述环境惩罚模型用于预测所述机械臂在所述控制序列内的状态。24.相应地，本发明还公开了一种工业机械臂动态避碰规划装置，其包括：25.第一构建单元，被配置为用于构建机械臂的系统动态方程；26.计算单元，被配置为用于依据所述系统动态方程计算所述机械臂的原始预测函数；27.第二构建单元，被配置为用于构建环境惩罚模型；28.第三构建单元，被配置为用于依据所述原始预测函数和环境惩罚模型构建目标预测函数；29.优化单元，被配置为用于对所述目标预测函数进行优化，以获得控制序列；30.训练单元，被配置为用于依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。31.相应地，本发明还公开了一种存储介质，所述存储介质存储有多条指令，所述指令适于处理器进行加载，以执行如上所述的机械臂动态避碰规划方法中的步骤。32.相应地，本发明还公开了一种机器人，所述机器人上设有如上所述的工业机械臂动态避碰规划装置。33.与现有技术相比，本发明依据原始预测函数和环境惩罚模型构建目标预测函数，并依据控制序列训练环境惩罚模型，直至环境惩罚模型收敛，其目标预测函数结合了原始预测函数和环境惩罚模型，使目标预测函数更能贴合实际，通过控制序列训练环境惩罚模型，直至环境惩罚模型收敛，以使目标预测函数能够实时在线优化，并获得更好的控制效果、鲁棒性和稳定性。附图说明34.图1是本发明的机械臂动态避碰规划方法的流程框图；35.图2是本发明的环境惩罚模型的结构图；36.图3是自然进化策略算法的算法流程图；37.图4是本发明的工业机械臂动态避碰规划装置的结构框图。具体实施方式38.为详细说明本发明的技术内容、构造特征、所实现目的及效果，以下结合实施方式并配合附图详予说明。39.请参阅图1所示，本实施例的机械臂动态避碰规划方法，其包括如下步骤：40.s1、构建机械臂的系统动态方程；41.s2、依据所述系统动态方程计算所述机械臂的原始预测函数；42.s3、构建环境惩罚模型；43.s4、依据所述原始预测函数和环境惩罚模型构建目标预测函数；44.s5、对所述目标预测函数进行优化，以获得控制序列；45.s6、依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。46.较佳地，所述环境惩罚模型以所述机械臂的关节状态量、控制量作为输入量，及以所述系统动态方程的环境惩罚量作为输出量。47.较佳地，所述步骤s6具体包括：48.s61、初始化所述目标预测函数中的环境惩罚量的权重；49.s62、在预设时间内以预设的连续控制量赋值所述目标预测函数，以获得多个状态量和环境惩罚量；50.s63、依据所述多个状态量和环境惩罚量优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。51.具体地，所述步骤s63具体为：52.依据所述多个状态量和环境惩罚量，以自然进化策略优化所述目标预测函数中的环境惩罚量的权重，直至所述环境惩罚模型收敛。53.较佳地，对所述目标预测函数赋值每一控制量获得对应的一个状态量和一个环境惩罚量。54.较佳地，所述步骤s1具体包括：55.s11、将所述系统动态方程进行离散化处理，得到离散化后的系统动态方程；56.s12、依据离散化后的系统动态方程计算得到所述机械臂的原始预测函数，所述原始预测函数包含有关于控制量的参数。57.较佳地，所述控制序列包括多个一一对应的新的状态量和控制增量，所述新的状态量为当前状态量与上一时刻的状态量的结合，所述控制增量为当前控制量与上一时刻的控制量的结合，所述环境惩罚模型用于预测所述机械臂在所述控制序列内的状态。58.请参阅图1‑图3所示，下面以六轴工业机械臂为例，对本方法进行详细说明：59.1、系统动态方程和原始预测函数：60.设在t时刻的六轴工业机械臂构型向量为q(t)，速度向量为q’(t)，加速度向量为q”(t)，其中q(t)、q’(t)和q”(t)均为六维向量，分别代表机械臂六个关节的角度、角速度、角加速度。61.令机械臂的状态量x(t)＝[q(t)；q’(t)]，控制量u(t)＝q”(t)，则可以构建以下系统动态方程：[0062][0063]其中，[0064][0065]矩阵0m×n为m×n的全0矩阵，in×n为n维的单位矩阵。[0066]由于上述系统动态方程为连续时间系统，而连续时间系统无法直接作为预测控制器使用，因此，要先对连续时间的系统动态方程进行离散化，得到：[0067][0068]通过对上式进行移项可得离散化的系统动态方程：[0069][0070]其中，[0071][0072]在优化求解的过程中，还必须满足以下约束：[0073][0074]在预测时域np内进行递推得：[0075][0076]上式可以进一步表达为：[0077][0078]其中，[0079][0080]由此可得基于系统动态方程的原始预测函数为：[0081][0082]其中，xg为目标状态向量，qg、qu为相应的权重矩阵。通过优化每个k时刻的原始预测函数j，就能得到最优的控制量u(k)。下面上式改写成每一项都含有u(k)的形式：[0083][0084]上述原始预测函数可以用二次规划轻易求解。[0085]2、基于自然进化策略的静态/动态环境避障方法：[0086]在机械臂的优化问题含有碰撞距离、碰撞状态等硬约束，用传统的优化方法处理这些约束效率低下，或者说难以处理，因此本发明采用基于自然进化策略的静态/动态避障方法，并构造目标预测函数：[0087][0088]其中，f(x(k),u(k))为基于自然进化策略所训练的环境惩罚模型，fs与fd分别代表环境惩罚模型中的静态障碍网络与动态障碍网络，μ和φ均为系数，两种网络的结构与训练方法完全一致，唯一的区别在于动态障碍网络fd的状态量中还包含动态障碍物的状态量。该环境惩罚模型以系统动态方程的状态量x(k)与控制序列u(k)作为其输入，以环境惩罚量作为其输出。该网络的结构如图2所示。[0089]训练环境惩罚模型的步骤如下：[0090]1)初始化环境惩罚模型f的权重；[0091]2)一定时间内运行mpc控制器并优化函数jstable；[0092]3)得到一系列数据轨迹i(x(1)、x(2)、…x(k)，u(1)、u(2)、…u(k)、对应的环境惩罚量f1、f2、…、fk以及实际环境惩罚量f1real、f2real、…、fkreal)[0093]4)依次根据轨迹i中的数据，使用自然进化策略优化环境惩罚模型f中的权重参数；[0094]5)重复步骤2)‑步骤4)直到环境惩罚模型f收敛。[0095]在原始的自然进化策略算法中，需要更新的参数θ包含μ和σ，μ和σ是正态分布的两个参数，而本发明中使用的自然进化策略将参数σ固定而只专注与参数μ的更新，因此需要更新的参数θ也即是参数μ。[0096]在本发明中，使用的自然进化策略算法算法流程如图3所示，在图3示出的自然进化策略算法中，成本函数f的值依赖于环境，成本函数f可以设定为如下表达式：[0097][0098]模型预测的环境成本fk与实际成本fkreal的偏差越大梯度的模则越大，反之则越小。通过不断的迭代训练，最终可以使得成本函数f逼近现实的环境成本模型。[0099]请参阅图4所示,相应地，本发明还公开了一种工业机械臂动态避碰规划装置，其包括：[0100]第一构建单元10，被配置为用于构建机械臂的系统动态方程；[0101]计算单元20，被配置为用于依据所述系统动态方程计算所述机械臂的原始预测函数；[0102]第二构建单元30，被配置为用于构建环境惩罚模型；[0103]第三构建单元40，被配置为用于依据所述原始预测函数和环境惩罚模型构建目标预测函数；[0104]优化单元50，被配置为用于对所述目标预测函数进行优化，以获得控制序列；[0105]训练单元60，被配置为用于依据所述控制序列训练所述环境惩罚模型，直至所述环境惩罚模型收敛。[0106]相应地，本发明还公开了一种存储介质，所述存储介质存储有多条指令，所述指令适于处理器进行加载，以执行如上所述的机械臂动态避碰规划方法中的步骤。[0107]相应地，本发明还公开了一种机器人，所述机器人上设有如上所述的工业机械臂动态避碰规划装置。[0108]结合图1‑图4，本发明依据原始预测函数和环境惩罚模型构建目标预测函数，并依据控制序列训练环境惩罚模型，直至环境惩罚模型收敛，其目标预测函数结合了原始预测函数和环境惩罚模型，使目标预测函数更能贴合实际，通过控制序列训练环境惩罚模型，直至环境惩罚模型收敛，以使目标预测函数能够实时在线优化，并获得更好的控制效果、鲁棒性和稳定性，该方法的本质是使用最优解的梯度辅助下一步的搜索。[0109]以上所揭露的仅为本发明的优选实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明申请专利范围所作的等同变化，仍属本发明所涵盖的范围。当前第1页12当前第1页12
再多了解一些
本文用于企业家、创业者技术爱好者查询，结果仅供参考。
