关节受限冗余并联机械臂的运动规划和控制方法、系统及机器人

1.本发明涉及机器人的技术领域，具体涉及一种关节受限冗余并联机械臂的运动规划和控制方法、系统及机器人。


背景技术：

2.并联机械臂广泛应用于工业和医疗领域，它由移动平台、固定平台、末端执行器以及连接这两个平台的线性驱动关节组成，是一个多路闭环结构。在执行任务过程中所需要的自由度比其可用自由度小的时候，并联机器人在运动学上就成为了冗余并联机器人。冗余度解析是运动学冗余机械臂的基本问题之一。
3.由于分布式和并行计算能力，神经网络已经成为解决冗余问题的强大计算工具之一。实际上，机器人的每个关节都拥有自身极限，在任务执行过程中如果超出机器人的关节极限，将可能损坏机器人。为了处理冗余机器人的关节约束，可将其逆运动学问题表示成约束优化问题，如二次规划问题。求解约束优化问题的求解器有动态神经网络(dynamic neural network,dnn)求解器、基于梯度的神经网络(gradient-based neural network,gnn)求解器以及znn求解器和数值算法等。znn求解器广泛应用时变优化问题的求解，具有求解精度高、无需训练和迭代运算等优点。


技术实现要素：

4.本发明的主要目的在于克服现有技术的缺点与不足，提供一种关节受限冗余并联机械臂的运动规划和控制方法、系统及机器人，本发明可以处理关节受限冗余并联机械臂的运动学控制问题，并可以确保机器人在执行任务时的安全性。
5.为了达到上述目的，本发明采用以下技术方案：
6.本发明一方面提供了一种关节受限冗余并联机械臂的运动规划和控制方法，包括下述步骤：
7.步骤一、规划关节受限冗余并联机械臂末端执行器的预期运动轨迹，即并联机械臂末端执行器的理想速度的设定；
8.步骤二、根据具体的并联机械臂建立其相应的约束优化控制方案，设计性能指标为等式约束为和以及双端约束为q-≤q≤q
+
和其中表示末端执行器的速度，w1和w2表示加权矩阵，q和分别代表驱动关节长度和长度变化率，q-和q
+
分别代表驱动关节长度的上下限，和分别代表驱动关节长度变化率的上下限，j表示雅可比矩阵，k为一定常矩阵；通过等价转化方法可以将驱动关节长度及其长度变化率所受到的两个双端约束整理成一个不等式约束其中l＝[i,-i]
t
，再通过引入矩阵w＝w1+j
t
w2j∈r6×6以及n＝lj∈r
12
×6将约束优化控制方案，统一表征成具有一般形式的二次规划问题，其中性能指标为等式约束为不等式约束为
[0009]
步骤三、基于非线性互补问题ncp函数ψ(a,b)，其中ncp函数的具体表达式定义为基于kkt条件，将约束优化问题等价转化成非线性方程组g(t,x)＝0；
[0010]
步骤四、定义误差监测函数e(t):＝g(t,x)，所述误差监测函数是步骤三得到的非线性方程组，使用演化法则其中α为收敛参数，φ(
·
)表示激活函数阵列，推导得到一种全新的znn求解器，通过znn求解器求解得到约束优化问题的最优解，进而获得并联机械臂驱动关节的长度变化率
[0011]
步骤五、将步骤四的所得结果或其积分q发送给下位机，驱动并联机械臂运动。
[0012]
作为优选的技术方案，所述并联机械臂由固定平台、移动平台、末端执行器和六个驱动关节q1～q6组成，该并联机械臂速度层运动学方程为其中j表示雅可比矩阵。
[0013]
作为优选的技术方案，所述雅克比矩阵具体为：
[0014][0015]
其中其中表示末端执行器相对于固定基座的旋转矩阵，表示末端执行器相对于固定基座的位置向量，bi和pi分别表示驱动关节与静平台和动平台的连接点。
[0016]
作为优选的技术方案，所述并联机械臂速度层运动学方程中，设计优化性能指标并考虑驱动关节长度及其变化率的上下限约束，建立冗余并联机械臂的约束优化控制方案；
[0017]
经过等价整理，数学上可表征为具有一般形式的二次规划问题，其中性能指标为等式约束为以及不等式约束为
[0018]
基于ncp函数和kkt条件，将二次规划问题等价转化成非线性方程组g(t,x)＝0。
[0019]
作为优选的技术方案，所述ncp函数是处处可微的。
[0020]
作为优选的技术方案，通过采用ncp函数并定义误差监测函数e(t):＝g(t,x)，基于演化法则可得到一种全新的znn求解器其中有：
[0021]
[0022][0023][0024]
并有均为对角矩阵。
[0025]
作为优选的技术方案，所述znn求解器无需训练和迭代计算，可高精度求解得到二次规划问题的最优解，进而获得并联机械臂驱动关节的长度变化率可将其或其积分q发送给下位机，以驱动并联机械臂运动。
[0026]
本发明另一方面提供了一种关节受限冗余并联机械臂的运动规划和控制系统，包括预期运动轨迹规划模块、运动学控制方案构建模块、等价转换模块、最优求解模块以及驱动模块；
[0027]
所述预期运动轨迹规划模块，用于规划关节受限冗余并联机械臂末端执行器的预期运动轨迹，即并联机械臂末端执行器的理想速度的设定；
[0028]
所述运动学控制方案构建模块，用于根据具体的并联机械臂建立其相应的约束优化控制方案，设计性能指标为等式约束为和以及双端约束为q-≤q≤q
+
和其中表示末端执行器的速度，w1和w2表示加权矩阵，q和分别代表驱动关节长度和长度变化率，q-和q
+
分别代表驱动关节长度的上下限，和分别代表驱动关节长度变化率的上下限，j表示雅可比矩阵，k为一定常矩阵；通过等价转化方法可以将驱动关节长度及其长度变化率所受到的两个双端约束整理成一个不等式约束其中l＝[i,-i]
t
，再通过引入矩阵w＝w1+j
t
w2j∈r6×6以及n＝lj∈r
12
×6将约束优化控制方案，统一表征成具有一般形式的二次规划问题，其中性能指标为等式约束为不等式约束为
[0029]
所述等价转换模块，用于基于非线性互补问题ncp函数ψ(a,b)，其中ncp函数的具体表达式定义为基于kkt条件，将约束优化问题等价转化成非线性方程组g(t,x)＝0；
[0030]
所述最优求解模块，用于定义误差监测函数e(t):＝g(t,x)，所述误差监测函数是将得到的非线性方程组，使用演化法则其中α为收敛参数，φ(
·
)表示激活函数阵列，推导得到一种全新的znn求解器，通过znn求解器求解得到约束优化问题的最优解，进而获得并联机械臂驱动关节的长度变化率
[0031]
所述驱动模块，用于将所得结果或其积分q发送给下位机，驱动并联机械臂运动。
[0032]
本发明又一方面提供了一种计算机可读存储介质，存储有程序，所述程序被处理器执行时，实现所述的受关节约束的冗余并联机械臂的运动规划和控制的方法。
[0033]
本发明再一方面提供了一种机器人，所述机器人包括：
[0034]
至少一个处理器；以及，
[0035]
与所述至少一个处理器通信连接的存储器；其中，
[0036]
所述存储器存储有可被所述至少一个处理器执行的计算机程序指令，所述计算机程序指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行所述的受关节约束的冗余并联机械臂的运动规划和控制的方法。
[0037]
本发明与现有技术相比，具有如下优点和有益效果：
[0038]
本发明能有效地于克服一般技术存在的不足，在这之前所提出的znn求解器只考虑对二次规划问题中的等式约束的处理而没有考虑不等式约束的处理，因此无法实现冗余并联机械臂的关节极限躲避。本发明考虑了二次规划问题中的不等式约束如关节极限约束，并利用一种ncp函数进行不等式约束处理和二次规划问题的等价转化，结合演化法则以及激活函数得到了一种全新的znn求解器，该znn求解器可以求得非线性方程组的解和二次规划问题的最优解。得到二次规划问题的最优解之后，即可驱动冗余并联机械臂运动，实现关节躲避功能并完成给定的末端执行器任务。本发明提供了一种操作方便、无需训练和迭代计算、作业规范且是关节受限并联机械臂的运动规划方法。
附图说明
[0039]
为了更清楚地说明本技术实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0040]
图1为本发明实施例关节受限冗余并联机械臂的运动规划和控制方法的流程图；
[0041]
图2为本发明实施例的仿真stewart并联机械臂的模型图。
[0042]
图3为本发明实施例的仿真stewart并联机械臂末端执行器的目标轨迹与实际轨迹图。
[0043]
图4为本发明实施例的仿真stewart并联机械臂的轨迹误差图。
[0044]
图5为本发明实施例的仿真stewart并联机械臂的驱动关节长度变化图。
[0045]
图6为本发明实施例的仿真stewart并联机械臂的驱动关节长度变化率图。
[0046]
图7是本发明实施例关节受限冗余并联机械臂的运动规划和控制的系统的结构示意图；
[0047]
图8是本发明实施例机器人的结构示意图。
具体实施方式
[0048]
为了使本技术领域的人员更好地理解本技术方案，下面将结合本技术中的实施例及附图，对本发明的技术方案进行清楚、完整地描述，应当理解，附图仅用于示例性说明，不能理解为对本专利的限制。显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
[0049]
在本技术中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本技术的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相
同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本技术所描述的实施例可以与其它实施例相结合。
[0050]
实施例
[0051]
如图1所示，本实施例为一种关节受限冗余并联机械臂的运动规划和控制方法，该方法包括以下步骤：
[0052]
步骤一：规划关节受限冗余并联机械臂末端执行器的预期运动轨迹，其中包括并联机械臂的末端执行器理想的速度的设定；
[0053]
所述步骤一中，通过并联机械臂的末端执行器理想的速度的设定，即可以确定并联机械臂末端执行器的预期运动轨迹。
[0054]
步骤二：根据具体的并联机械臂建立其相应的约束优化控制方案，最终得到的约束优化控制方案中性能指标为等式约束为以及不等式约束为
[0055]
所述步骤二中，通过该等式约束可将末端执行器的理想速度和其实际速度联系起来，从而可以达到对末端执行器的运动规划和控制。通过不等式约束可以处理并联机械臂驱动杆所受到的关节极限约束。
[0056]
步骤三：基于ncp函数，将约束优化问题等价转化成非线性方程组。
[0057]
所述步骤三中，通过为约束优化问题定义相应的拉格朗日函数，在此基础上，进行求导得到kkt条件，对得到的kkt条件使用ncp函数进行处理，最终可将约束优化问题等价转化成非线性方程组。
[0058]
步骤四：针对步骤三的非线性方程组，构建误差函数并设计znn求解器，以此求得非线性方程组以及约束优化问题的最优解，进而获得并联机械臂各个关节的长度变化率。
[0059]
基于步骤三的非线性方程组，构建误差监测函数e(t):＝g(t,x)，使用演变法则和激活函数φ(
·
)，推导出一种全新的znn求解器。通过最终所得到的znn求解器可以同时求得非线性方程组的解和约束优化问题的最优解进而基于关系式可得到并联机械臂驱动关节的长度变化率
[0060]
步骤五：将步骤四的所得结果或其积分q发送给下位机，驱动并联机械臂运动。
[0061]
如图2所示，该并联机械臂的模型是由静平台8、动平台7、末端执行器9、第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5以及第六驱动杆6组成的，该并联机械臂速度层运动学关系式为其中其中表示末端执行器的速度，雅可比矩阵j具体为：
[0062][0063]
如图3所示，虚线为仿真stewart并联机械臂末端执行器的实际轨迹，而实线则为仿真stewart并联机械臂末端执行器的目标轨迹。从图中可以看出目标轨迹和实际轨迹几乎完全重合，说明利用该方法可以实现对机械臂的运动实现精确控制，极少出现偏差。
[0064]
如图4所示，其中虚线e
x
表示仿真stewart并联机械臂末端执行器在x方向上的误
差，点线ey表示仿真stewart并联机械臂末端执行器在y方向上的误差，实线ez表示仿真stewart并联机械臂末端执行器在z方向上的误差。其中在任务执行期间，三个方向的误差皆小于等于1.5
×
10-4
米，具有亚毫米级高定位精度。
[0065]
如图5所示，其中，q1,q2,q3,q4,q5,q6分别表示仿真stewart并联的第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5以及第六驱动杆6长度。在任务执行期间，各个驱动杆的长度不断变化，对应机械臂出现各种不同的运动。
[0066]
如图6所示，其中，分别表示仿真stewart并联的第一驱动杆1、第二驱动杆2、第三驱动杆3、第四驱动杆4、第五驱动杆5以及第六驱动杆6的长度变化率。从图中可以看到，在任务执行的过程中，各个驱动杆的长度变化率可以保证在一定的范围内变化，从而可以说明本发明对于二次规划问题中不等式约束处理和关节极限躲避的有效性。
[0067]
如图7所示，在本技术的另一个实施例中，提供了一种关节受限冗余并联机械臂的运动规划和控制系统100，包括预期运动轨迹规划模块101、运动学控制方案构建模块102、等价转换模块103、最优求解模块104以及驱动模块105；
[0068]
所述预期运动轨迹规划模块101，用于规划关节受限冗余并联机械臂末端执行器的预期运动轨迹，即并联机械臂末端执行器的理想速度的设定；
[0069]
所述运动学控制方案构建模块102，用于根据具体的并联机械臂建立其相应的约束优化控制方案，设计性能指标为等式约束为和以及双端约束为q-≤q≤q
+
和其中表示末端执行器的速度，w1和w2表示加权矩阵，q和分别代表驱动关节长度和长度变化率，q-和q
+
分别代表驱动关节长度的上下限，和分别代表驱动关节长度变化率的上下限，j表示雅可比矩阵，k为一定常矩阵；通过等价转化方法可以将驱动关节长度及其长度变化率所受到的两个双端约束整理成一个不等式约束其中l＝[i,-i]
t
，再通过引入矩阵w＝w1+j
t
w2j∈r6×6以及n＝lj∈r
12
×6将约束优化控制方案，统一表征成具有一般形式的二次规划问题，其中性能指标为等式约束为不等式约束为
[0070]
所述等价转换模块103，用于基于非线性互补问题ncp函数ψ(a,b)，其中ncp函数的具体表达式定义为基于kkt条件，将约束优化问题等价转化成非线性方程组g(t,x)＝0；
[0071]
所述最优求解模块104，用于定义误差监测函数e(t):＝g(t,x)，所述误差监测函数是将得到的非线性方程组，使用演化法则其中α为收敛参数，φ(
·
)表示激活函数阵列，推导得到一种全新的znn求解器，通过znn求解器求解得到约束优化问题的最优解，进而获得并联机械臂驱动关节的长度变化率
[0072]
所述驱动模块105，用于将所得结果或其积分q发送给下位机，驱动并联机械臂运动。需要说明的是，本发明的关节受限冗余并联机械臂的运动规划和控制的系统与本发明的关节受限冗余并联机械臂的运动规划和控制的方法一一对应，在上述关节受限冗余并联机械臂的运动规划和控制的方法的实施例阐述的技术特征及其有益效果均适用于关节受限冗余并联机械臂的运动规划和控制的方法的实施例中，具体内容可参见本发明方法实施例中的叙述，此处不再赘述，特此声明。
[0073]
此外，上述实施例的关节受限冗余并联机械臂的运动规划和控制的系统的实施方式中，各程序模块的逻辑划分仅是举例说明，实际应用中可以根据需要，例如出于相应硬件的配置要求或者软件的实现的便利考虑，将上述功能分配由不同的程序模块完成，即将所述关节受限冗余并联机械臂的运动规划和控制的系统的内部结构划分成不同的程序模块，以完成以上描述的全部或者部分功能。
[0074]
如图8所示，在一个实施例中，提供了一种机器人200，所述机器人200可以包括第一处理器201、第一存储器202和总线，还可以包括存储在所述第一存储器202中并可在所述第一处理器201上运行的计算机程序，如关节受限冗余并联机械臂的运动规划和控制程序203。
[0075]
其中，所述第一存储器202至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、移动硬盘、多媒体卡、卡型存储器(例如：sd或dx存储器等)、磁性存储器、磁盘、光盘等。所述第一存储器202在一些实施例中可以是机器人200的内部存储单元，例如该机器人200的移动硬盘。所述第一存储器202在另一些实施例中也可以是机器人200的外部存储设备，例如机器人200上配备的插接式移动硬盘、智能存储卡(smart media card，smc)、安全数字(securedigital，sd)卡、闪存卡(flash card)等。进一步地，所述第一存储器202还可以既包括机器人200的内部存储单元也包括外部存储设备。所述第一存储器202不仅可以用于存储安装于机器人200的应用软件及各类数据，例如关节受限冗余并联机械臂的运动规划和控制程序203的代码等，还可以用于暂时地存储已经输出或者将要输出的数据。
[0076]
所述第一处理器201在一些实施例中可以由集成电路组成，例如可以由单个封装的集成电路所组成，也可以是由多个相同功能或不同功能封装的集成电路所组成，包括一个或者多个中央处理器(central processing unit，cpu)、微处理器、数字处理芯片、图形处理器及各种控制芯片的组合等。所述第一处理器201是所述电子设备的控制核心(control unit)，利用各种接口和线路连接整个电子设备的各个部件，通过运行或执行存储在所述第一存储器202内的程序或者模块，以及调用存储在所述第一存储器202内的数据，以执行机器人200的各种功能和处理数据。
[0077]
图8仅示出了具有部件的机器人，本领域技术人员可以理解的是，图8示出的结构并不构成对所述机器人200的限定，可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。
[0078]
所述机器人200中的所述第一存储器202存储的关节受限冗余并联机械臂的运动规划和控制程序203是多个指令的组合，在所述第一处理器201中运行时，可以实现：
[0079]
规划关节受限冗余并联机械臂末端执行器的预期运动轨迹，其中包括并联机械臂末端执行器的理想速度r的设定；
[0080]
根据具体的并联机械臂建立其相应的约束优化控制方案，其中包括性能指标、等式约束和不等式约束；
[0081]
基于ncp函数可将约束优化问题转化成非线性方程组；
[0082]
根据所得到的非线性方程组，构建误差监测函数并设计znn求解器，以此求得非线性方程组以及约束优化问题的最优解，进而获得并联机械臂各个关节的长度变化率。
[0083]
将得到的求解结果发送给下位机，驱动并联机械臂运动。
[0084]
进一步地，所述机器人200集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非易失性计算机可读取存储介质中。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(rom，read-only memory)。
[0085]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一非易失性计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(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)等。
[0086]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0087]
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。