CN115157262B - 关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及机器人 - Google Patents

Abstract

本发明提供一种关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及机器人，方法包括：步骤一，定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；步骤二，根据具体的机械臂构建其基于二次规划QP的障碍物躲避新方案；步骤三，基于非线性互补问题NCP函数将QP问题转化为非线性方程组；步骤四，设计零化神经网络ZNN求解器求解步骤三所得到的非线性方程组；步骤五，将求解结果传递给下位机以驱动机械臂运动。本发明较其他方法而言，能在不影响末端执行器期望运动的前提下，实现障碍物躲避和关节极限躲避功能，且求解精度高、速度快、无需训练和迭代计算，保障了机械臂运动的安全性和有效性，对机械臂的实际应用具有重要意义。

Description

关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及 机器人

技术领域

本发明涉及机器人的技术领域，具体涉及一种关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及机器人。

背景技术

冗余机械臂是一种自身自由度大于执行任务所需自由度的机械装置，广泛应用于如喷漆、焊接、组装、产品加工等国民经济生产活动中。冗余机械臂的一个运动学基本问题是逆运动学解析，逆运动学解析问题是指给定机械臂末端执行器的期望位姿，确定机械臂的关节构型。在实际应用中，可采用二次规划在速度层上对机械臂进行逆运动学解析，并将次目标如关节极限、障碍物躲避等作为约束条件加入到二次规划问题中。

在末端执行器执行任务期间，障碍物躲避功能可以避免机械臂与障碍物之间发生碰撞，保证机械臂安全可靠地运行。由于机械臂自身物理结构的限制，机械臂在实际运行过程中会受到关节极限约束，关节极限躲避功能可保证机械臂的安全性和可行性。

随着计算机的分布式和并行处理能力的提升，神经网络已经成为解决逆运动学解析问题的强大计算工具之一。零化神经网络ZNN求解器具有求解精度高、无需训练和迭代计算等优点，可广泛应用于时变优化问题的求解中。然而，目前尚没有基于零化神经网络适用于关节受限冗余机械臂的障碍物躲避与优化控制方法。

发明内容

本发明的目的在于克服现有技术的不足，提供一种关节受限冗余机械臂的障碍物躲避与优化控制方法、系统及机器人，本发明求解精度高、求解速度快、无需训练和迭代计算、且能实现关节极限躲避和障碍物躲避功能。

为了实现上述发明目的，采用的技术方案如下：

本发明一方面提供了一种关节受限冗余机械臂的障碍物躲避与优化控制方法，包括如下步骤：

步骤一、定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

步骤二、根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束/>

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标^T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

步骤三、基于非线性互补问题NCP函数和KKT条件，步骤二的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

步骤四、定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是步骤三得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

步骤五、将步骤四得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

作为优选的技术方案，所述机械臂由末端执行器和八个驱动关节θ₁…θ₈组成，该机械臂运动学方程为

约束方程为/>

其中/>

J表示雅可比矩阵，/>

表示机械臂末端执行器运动速度轨迹，/>

用于考虑障碍物躲避约束和关节极限约束。

作为优选的技术方案，所述机械臂障碍物躲避参数A和b分别定义为：

其中，

表示一种矩阵与向量的点乘运算，/>

表示障碍物点O到判据点C的方向，J_o表示判据点C的雅可比矩阵，/>

表示障碍物点O与判据点C之间的最短距离，l₁是内部安全距离，当l＜l₁时表示机械臂与障碍物发生碰撞，l₂是外部安全距离，当l＞l₂时表示机械臂不会与障碍物发生碰撞，当l≤l₂时表示机械臂可能与障碍物发生碰撞，此时不等式约束会施加到机械臂上，s(l)是一个连续可微的平滑函数，定义为：

作为优选的技术方案，所述判据点C定义为距离障碍物点O最近的机械臂连杆上的点，该点是对应的机械臂连杆上距离障碍物点O最近的点。

作为优选的技术方案，所述机械臂速度层运动学方程中，设计最小化性能指标

并考虑障碍物躲避约束和关节极限约束，建立冗余机械臂的障碍物躲避与优化控制方案；

经过等价转换，上述障碍物躲避与优化控制方案统一表征成具有一般形式的QP问题，其中最小化性能指标为

受限于等式约束/>

和不等式约束/>

基于NCP函数和KKT条件，将QP问题等价转换成非线性方程组f(t,y)＝0。

作为优选的技术方案，所述的NCP函数是连续可微的。

作为优选的技术方案，通过采用NCP函数

并定义误差监测函数ε(t):＝f(t,y)，基于演化法则/>

得到一种ZNN求解器

其中有：

并有Λ₁＝diag{e⊙g}，Λ₂＝diag{μ⊙g}均为对角矩阵，符号⊙表示哈达玛除法，所述

向量中的元素/>

分别是/>

λ、μ关于时间的导数，所述N矩阵中的元素

分别是W、J、C关于时间的导数，所述u向量中的元素/>

分别是q、/>

d关于时间的导数。

本发明另一方面提供了一种关节受限冗余机械臂的障碍物躲避与优化控制系统，包括轨迹规划模块、障碍物躲避方案规划模块、等价转换模块、二次规划问题求解模块以及机械臂驱动模块；

所述轨迹规划模块，用于定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

所述障碍物躲避方案规划模块，用于根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束/>

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标^T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

所述等价转换模块，用于基于非线性互补问题NCP函数和KKT条件，障碍物躲避方案规划模块的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

所述二次规划问题求解模块，用于定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是等价转换模块得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

所述机械臂驱动模块，用于将二次规划问题求解模块得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

本发明又一方面提供了一种计算机可读存储介质，存储有程序，所述程序被处理器执行时，实现所述的一种关节受限冗余机械臂的障碍物躲避与优化控制方法。

本发明再一方面提供了一种机器人，所述机器人包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的计算机程序指令，所述计算机程序指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行所述的一种关节受限冗余机械臂的障碍物躲避与优化控制方法。

本发明与现有技术相比，具有以下优点：

本发明能有效克服一般技术存在的不足，通过设计连续可微的障碍物躲避参数，建立一种新的障碍物躲避与优化控制方案，该方案数学上可表征成具有一般形式、含等式及不等式约束的QP问题，基于NCP函数的不等式约束处理策略，将QP问题等价转化成非线性方程组，结合演化法则和激活函数得到一种ZNN求解器，用以得到QP问题的最优解。所求得的最优解用于驱动冗余机械臂的运动，最终在完成指定的末端执行器任务的前提下实现障碍物躲避和关节极限躲避功能。本发明提供了一种求解精度高、速度快、无需训练和迭代计算、且可实现关节极限躲避和障碍物躲避功能的关节受限冗余机械臂的障碍物躲避与优化控制方法。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例关节受限冗余机械臂的障碍物躲避与优化控制方法的流程图。

图2为本发明实施例的仿真PA10机械臂的模型图。

图3为本发明实施例的仿真PA10机械臂在约束关节角位置情况下末端执行器的目标轨迹与实际轨迹图。

图4为本发明实施例的仿真PA10机械臂在约束关节角位置情况下的轨迹误差图。

图5为本发明实施例的仿真PA10机械臂在约束关节角位置情况下与障碍物的最短距离变化图。

图6为本发明实施例的仿真PA10机械臂在约束关节角位置情况下驱动关节角位置变化图。

图7为本发明实施例的仿真PA10机械臂在约束关节角位置情况下驱动关节角速度变化图。

图8为本发明实施例关节受限冗余机械臂的障碍物躲避与优化控制系统的结构示意图。

图9为本发明实施例机器人的结构示意图。

具体实施方式

为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请中的实施例及附图，对本发明的技术方案进行清楚、完整地描述，应当理解，附图仅用于示例性说明，不能理解为对本专利的限制。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本申请所描述的实施例可以与其它实施例相结合。

实施例

如图1所示，本实施例为一种关节受限冗余机械臂的障碍物躲避与优化控制方法，该方法包括以下步骤：

步骤一：定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

所述步骤一中，规划末端执行器的预期运动轨迹即设定末端执行器的理想速度

步骤二：根据具体的机械臂建立其基于QP的障碍物躲避新方案，最终得到的QP方案中最小化性能指标为

等式约束为/>

不等式约束为/>

所述步骤二中，通过该等式约束

可将末端执行器的理想速度/>

和其关节角速度/>

联系起来，从而可以达到对末端执行器的运动规划和控制。通过不等式约束/>

可以处理机械臂驱动杆所受到的障碍物躲避约束和关节极限约束。

步骤三：基于NCP函数和KKT条件，将QP问题等价转化成非线性方程组。

所述步骤三中，通过为QP问题定义相应的拉格朗日函数，在此基础上求导得到KKT条件，利用NCP函数

对KKT条件进行处理，最终将QP问题等价转化成非线性方程组f(t,y)＝0。

步骤四：针对步骤三的非线性方程组，利用误差监测函数设计零化神经网络ZNN求解器，以此求得非线性方程组以及QP问题的最优解。

基于步骤三的非线性方程组，定义误差监测函数ε(t):＝f(t,y)，使用演化法则

和激活函数ψ(·)，推导得到一种ZNN求解器。通过最终得到的ZNN求解器可以求解非线性方程组的解也即QP问题的最优解/>

步骤五：将步骤四得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

如图2所示，该机械臂模型是由第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5、第六驱动杆6、第七驱动杆7、第七驱动杆8以及末端执行器9组成的，该机械臂速度层运动学关系式为

其中J表示雅可比矩阵，/>

表示机械臂末端执行器运动速度轨迹。

如图3所示，实线为仿真PA10机械臂末端执行器的实际轨迹，虚线为仿真PA10机械臂末端执行器的目标轨迹。从图中可以看出目标轨迹和实际轨迹几乎重合，说明该方案可以实现对机械臂的运动实现精确控制，误差极小。

如图4所示，其中虚线e_x、点线e_y、实线e_z分别表示仿真PA10机械臂末端执行器在X方向、Y方向、Z方向上的误差。在末端执行器执行任务期间，三个方向的误差都小于6×10^-5米，定位精度高于亚毫米级精度需求。

如图5所示，其中l₁表示内部安全距离，l₂表示外部安全距离，l表示机械臂与障碍物的最短距离。在末端执行器执行任务期间，机械臂与障碍物之间的最短距离始终大于内部安全距离，表示机械臂没有与障碍物发生碰撞，从而可以说明本发明对于QP问题中障碍物躲避的有效性。

如图6所示，其中θ₁,θ₂,θ₃,θ₄,θ₅,θ₆,θ₇,θ₈分别表示仿真PA10机械臂的第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5、第六驱动杆6、第七驱动杆7和第八驱动杆8的关节角位置，对第四驱动杆的关节角位置的上极限设置为其初始关节角位置π/2弧度。在末端执行器执行任务期间，各个驱动杆的关节角位置不断变化，对应于机械臂出现各种不同的关节构型及末端执行器位姿，其中第四驱动杆的关节角位置始终不大于π/2弧度，从而可以说明本发明对于QP问题中关节极限躲避的有效性。

如图7所示，其中

分别表示仿真PA10机械臂的第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5、第六驱动杆6、第七驱动杆7和第八驱动杆8的关节角速度，关节角速度上极限设置为/>

弧度/秒，关节角速度下极限设置为/>

弧度/秒。在执行任务期间，各个驱动杆的关节角速度可以保证在设定范围内变化，从而可以说明本发明对于QP问题中不等式约束处理和关节极限躲避的有效性。

如图8所示，在本申请的另一个实施例中，提供了一种关节受限冗余机械臂的障碍物躲避与优化控制系统100，包括轨迹规划模块101、障碍物躲避方案规划模块102、等价转换模块103、二次规划问题求解模块104以及机械臂驱动模块105；

所述轨迹规划模块101，用于定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

所述障碍物躲避方案规划模块102，用于根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标^T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

所述等价转换模块103，用于基于非线性互补问题NCP函数和KKT条件，障碍物躲避方案规划模块的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

所述二次规划问题求解模块104，用于定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是等价转换模块得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

所述机械臂驱动模块105，用于将二次规划问题求解模块得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。需要说明的是，本发明的关节受限冗余机械臂的障碍物躲避与优化控制系统与本发明的关节受限冗余机械臂的障碍物躲避与优化控制方法一一对应，在上述关节受限冗余机械臂的障碍物躲避与优化控制方法的实施例阐述的技术特征及其有益效果均适用于关节受限冗余机械臂的障碍物躲避与优化控制方法的实施例中，具体内容可参见本发明方法实施例中的叙述，此处不再赘述，特此声明。

此外，上述实施例的关节受限冗余机械臂的障碍物躲避与优化控制系统的实施方式中，各程序模块的逻辑划分仅是举例说明，实际应用中可以根据需要，例如出于相应硬件的配置要求或者软件的实现的便利考虑，将上述功能分配由不同的程序模块完成，即将所述关节受限冗余机械臂的障碍物躲避与优化控制系统的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分功能。

如图9所示，在一个实施例中，提供了一种机器人200，所述机器人200可以包括第一处理器201、第一存储器202和总线，还可以包括存储在所述第一存储器202中并可在所述第一处理器201上运行的计算机程序，如关节受限冗余机械臂的障碍物躲避与优化控制程序203。

其中，所述第一存储器202至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、移动硬盘、多媒体卡、卡型存储器(例如：SD或DX存储器等)、磁性存储器、磁盘、光盘等。所述第一存储器202在一些实施例中可以是机器人200的内部存储单元，例如该机器人200的移动硬盘。所述第一存储器202在另一些实施例中也可以是机器人200的外部存储设备，例如机器人200上配备的插接式移动硬盘、智能存储卡(Smart Media Card，SMC)、安全数字(Secure Digital，SD)卡、闪存卡(Flash Card)等。进一步地，所述第一存储器202还可以既包括机器人200的内部存储单元也包括外部存储设备。所述第一存储器202不仅可以用于存储安装于机器人200的应用软件及各类数据，例如关节受限冗余机械臂的障碍物躲避与优化控制程序203的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。

所述第一处理器201在一些实施例中可以由集成电路组成，例如可以由单个封装的集成电路所组成，也可以是由多个相同功能或不同功能封装的集成电路所组成，包括一个或者多个中央处理器(Central Processing unit，CPU)、微处理器、数字处理芯片、图形处理器及各种控制芯片的组合等。所述第一处理器201是所述电子设备的控制核心(Control Unit)，利用各种接口和线路连接整个电子设备的各个部件，通过运行或执行存储在所述第一存储器202内的程序或者模块，以及调用存储在所述第一存储器202内的数据，以执行机器人200的各种功能和处理数据。

图9仅示出了具有部件的机器人，本领域技术人员可以理解的是，图9示出的结构并不构成对所述机器人200的限定，可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。

所述机器人200中的所述第一存储器202存储的关节受限冗余机械臂的障碍物躲避与优化控制程序203是多个指令的组合，在所述第一处理器201中运行时，可以实现：

步骤一、定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

步骤二、根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束/>

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标^T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

步骤三、基于非线性互补问题NCP函数和KKT条件，步骤二的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

步骤四、定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是步骤三得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

步骤五、将步骤四得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

进一步地，所述机器人200集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非易失性计算机可读取存储介质中。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。

以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，包括如下步骤：

步骤一、定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

步骤二、根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束/>

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

步骤三、基于非线性互补问题NCP函数和KKT条件，步骤二的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

步骤四、定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是步骤三得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

步骤五、将步骤四得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

2.根据权利要求1所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，所述机械臂由末端执行器和八个驱动关节θ₁…θ₈组成，该机械臂运动学方程为

约束方程为/>

其中/>

J表示雅可比矩阵，/>

表示机械臂末端执行器运动速度轨迹，/>

用于考虑障碍物躲避约束和关节极限约束。

3.根据权利要求1所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，所述机械臂障碍物躲避参数A和b分别定义为：

其中，

表示一种矩阵与向量的点乘运算，/>

表示障碍物点O到判据点C的方向，J_o表示判据点C的雅可比矩阵，/>

表示障碍物点O与判据点C之间的最短距离，l₁是内部安全距离，当l＜l₁时表示机械臂与障碍物发生碰撞，l₂是外部安全距离，当l＞l₂时表示机械臂不会与障碍物发生碰撞，当l≤l₂时表示机械臂可能与障碍物发生碰撞，此时不等式约束会施加到机械臂上，s(l)是一个连续可微的平滑函数，定义为：

4.根据权利要求3所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，所述判据点C定义为距离障碍物点O最近的机械臂连杆上的点，该点是对应的机械臂连杆上距离障碍物点O最近的点。

5.根据权利要求2所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，所述机械臂速度层运动学方程中，设计最小化性能指标

并考虑障碍物躲避约束和关节极限约束，建立冗余机械臂的障碍物躲避与优化控制方案；

经过等价转换，上述障碍物躲避与优化控制方案统一表征成具有一般形式的QP问题，其中最小化性能指标为

受限于等式约束/>

和不等式约束/>

基于NCP函数和KKT条件，将QP问题等价转换成非线性方程组f(t,y)＝0。

6.根据权利要求5所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，所述的NCP函数是连续可微的。

7.根据权利要求1所述的关节受限冗余机械臂的障碍物躲避与优化控制方法，其特征在于，通过采用NCP函数

并定义误差监测函数ε(t):＝f(t,y)，基于演化法则/>

得到一种ZNN求解器/>

其中有：

并有Λ₁＝diag{e⊙g}，Λ₂＝diag{μ⊙g}均为对角矩阵，符号⊙表示哈达玛除法，所述

向量中的元素/>

分别是/>

λ、μ关于时间的导数，所述N矩阵中的元素/>

分别是W、J、C关于时间的导数，所述u向量中的元素/>

分别是q、/>

d关于时间的导数。

8.一种关节受限冗余机械臂的障碍物躲避与优化控制系统，其特征在于，包括轨迹规划模块、障碍物躲避方案规划模块、等价转换模块、二次规划问题求解模块以及机械臂驱动模块；

所述轨迹规划模块，用于定义机械臂与障碍物之间的安全距离，规划机械臂末端执行器的预期运动轨迹；

所述障碍物躲避方案规划模块，用于根据具体的机械臂构建其基于二次规划QP的障碍物躲避方案，设计的最小化性能指标为

受限于等式约束/>

不等式约束/>

双端约束θ^-≤θ≤θ⁺和/>

其中θ表示关节角位置，/>

表示关节角速度，θ⁺和θ^-分别表示关节角位置上下限，/>

和/>

分别表示关节角速度上下限，W表示合适维数的正定矩阵，q表示为某一任务而设计的向量，上标^T表示矩阵和向量的转置，J表示雅可比矩阵，/>

表示机械臂末端执行器的运动速度轨迹，A和b是障碍物躲避参数；

通过等价转化，将上述不等式约束和双端约束整理成一个不等式约束

用以考虑障碍物躲避约束和关节极限约束，其中C＝[A,I,-I]^T和d＝[b,ξ⁺,-ξ^-]^T，ξ⁺和ξ^-分别表示合成双端约束上下限，其定义为：

其中，h₁(θ)和h₂(θ)分别定义为：

并有c₁＝α₁θ^-，c₂＝θ^--c₁，c₃＝α₂θ⁺，c₄＝θ⁺-c₃，参数α₁∈(0,1)和α₂∈(0,1)用于调节关节角速度的可行域；

所述等价转换模块，用于基于非线性互补问题NCP函数和KKT条件，障碍物躲避方案规划模块的QP问题等价转化为非线性方程组f(t,y)＝H(t)y(t)+v(t)＝0，其中NCP函数的具体表达式定义为

δ→0⁺是实现NCP函数连续可微的扰动项，符号/>

是哈达玛乘积，λ和μ分别是等式约束/>

和不等式约束/>

对应的拉格朗日乘子，并有：

所述二次规划问题求解模块，用于定义误差监测函数ε(t):＝f(t,y)，所述误差监测函数是等价转换模块得到的非线性方程组，使用演化法则

推导得到一种ZNN求解器，其中γ是收敛参数，ψ(·)是激活函数阵列，通过该ZNN求解器求解得到QP问题的最优解，进而获得机械臂的关节角速度/>

所述机械臂驱动模块，用于将二次规划问题求解模块得到的求解结果

或其积分θ传递给下位机，以驱动机械臂运动。

9.一种计算机可读存储介质，存储有程序，其特征在于，所述程序被处理器执行时，实现权利要求1-7任一项所述的一种关节受限冗余机械臂的障碍物躲避与优化控制方法。

10.一种机器人，其特征在于，所述机器人包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的计算机程序指令，所述计算机程序指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1-7中任意一项所述的一种关节受限冗余机械臂的障碍物躲避与优化控制方法。

Images