申请/专利权人：香港理工大学深圳研究院

申请日：2017-01-20

公开（公告）日：2020-10-16

公开（公告）号：CN108326844B

主分类号：B25J9/16(20060101)

分类号：B25J9/16(20060101)

优先权：

专利状态码：有效-授权

法律状态：2020.10.16#授权;2018.08.21#实质审查的生效;2018.07.27#公开

摘要：一种冗余度机械臂的可操作度优化的运动规划方法包括：设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；将所述运动性能指标以及相应的约束关系转化为二次规划问题；通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；根据所述求解结果控制机械臂运动。本方法可以有效地躲避冗余度机械臂的各种物理约束，有效地扩大了冗余度机械臂的可操作度优化运动规划的应用空间。

主权项：1.一种冗余度机械臂的可操作度优化的运动规划方法，其特征在于，所述方法包括：设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；将所述运动性能指标以及相应的约束关系转化为二次规划问题；通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；根据所述求解结果控制机械臂运动；所述设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系步骤具体为：所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，即最小化所述与运动性能指标相应的约束关系包括：雅可比等式矩阵求逆等式障碍物躲避不等式关节角极限θ-≤θ≤θ+、关节速度极限其中θ∈Rk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度；fθ∈Rm表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度；rd与分别表示末端执行器期望路径以及期望速度；k0表示位置误差反馈系数；α表示矩阵JJT逆的向量化的值；表示机械臂的雅可比矩阵；表示机械臂可操作度，μi≥0表示矩阵JJT的第i个最大特征值；Im表示一个m*m维的单位矩阵；上标T表示矩阵和向量的转置；vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列；对于i＝1,2,...,k，表示克罗内克积运算；J□{H1,H2,…,Hk}定义为： θ+、θ-表示关节角上下限，表示关节速度上下限；A和b是障碍物躲避参数。

全文数据：冗余度机械臂的可操作度优化的运动规划方法和装置技术领域[0001]本发明属于智能控制领域，尤其涉及冗余度机械臂的可操作度优化的运动规划方法和装置。背景技术[0002]冗余度机械臂具有自由度大于任务空间所需最少自由度的特性，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中，为人们的生活和工作带来了便利。[0003]冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题。当冗余度机械臂末端执行任务时，如何使得机械臂的可操作性最大化，以最大限度地躲避奇异，这种问题叫做机械臂可操作度优化问题。当冗余度机械臂可操作度为〇时，将导致雅可比矩阵因不满秩而奇异。当可操作度接近0时，机械臂在特定方向的运动将显著受限，使得即使机械臂速度很大也难以完成任务。故在冗余度机械臂的运动规划中，在执行任务的同时都尽量使得可操作度最大化。[0004]然而，基于伪逆的可操作度的优化方法，一般难以嵌入机械臂的各种物理约束，限制了冗余度机械臂的可操作度。发明内容[0005]本发明的目的在于提供一种冗余度机械臂的可操作度优化的运动规划方法，以解决现有技术如果采用基于伪逆的可操作度的优化方法，一般难以嵌入机械臂的各种物理约束，限制了冗余度机械臂的可操作度的问题。[0006]第一方面，本发明实施例提供了一种冗余度机械臂的可操作度优化的运动规划方法，所述方法包括：[0007]设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；[0008]将所述运动性能指标以及相应的约束关系转化为二次规划问题；[0009]通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；[0010]根据所述求解结果控制机械臂运动。[0011]结合第一方面，在第一方面的第一种可能实现方式中，所述设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系步骤具体为：[0012]所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，最小化，所述与运动性能指标相应的约束关系包括:雅可比等式、矩阵求逆等式，障碍物躲避不等式，关节角极限关节速度极限；其中0eRk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度;f0er表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度;rd与分别表示末端执行器期望路径以及期望速度；k〇表示位置误差反馈系数；α表示矩阵JJt逆的向量化的值；表示机械臂的雅可比矩阵；表示机械臂可操作度，μθΟ表示矩阵JJt的第i个最大特征值;Im表示一个m*m维的单位矩阵;上标T表示矩阵和向量的转置;vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列;对于i=l，2，...，k:»表示克罗内克积运算定义为：、表示关节角上下限表示关节速度上下限;A和b是障碍物躲避参数。[0014]结合第一方面的第一种可能实现方式，在第一方面的第二种可能实现方式中，所述将所述运动性能指标以及相应的约束关系转化为二次规划问题步骤包括：[0015]将两个等式约束的范数嵌入进性能指标中，移除与决策变1无关的参数,加入运动学能量消耗指标，得到性能指标为:最小化[0016]约束关系包括：，其中，CQ0，C1M，C2〇与c30为性能指标中对应项的权重;ζ+和Γ分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和，其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。[0017]结合第一方面的第二种可能实现方式，在第一方面的第三种可能实现方式中，所述二次规划求解器为：[0018][0022]其中，所述二次规划求解器的初始值为：τ0表示采样间隔；下标j与j+Ι分别为对应项的第^时刻与第j+lT的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。[0023]结合第一方面，在第一方面的第四种可能实现方式中，所述根据所述求解结果控制机械臂运动步骤具体为：[0024]将所述二次规划问题的求解结果转化为各个关节电机所需要的控制信号，驱动各个关节电机使机械臂运动。[0025]第二方面，本发明实施例提供了一种冗余度机械臂的可操作度优化的运动规划装置，所述装置包括：[0026]指标设定单元，用于设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；[0027]转化单元，用于将所述运动性能指标以及相应的约束关系转化为二次规划问题；[0028]求解单元，用于通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；[0029]控制单元，用于根据所述求解结果控制机械臂运动。[0030]结合第二方面，在第二方面的第一种可能实现方式中，所述指标设定单元具体用于：[0031]所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，最小化，所述与运动性能指标相应的约束关系包括:雅可比等式、矩阵求逆等式，障碍物躲避不等式关节角极限关节速度极限其中OeRk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度;f0GRm表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度;rd与分别表示末端执行器期望路径以及期望速度；ko表示位置误差反馈系数；α表示矩阵JJt逆的向量化的值；表示机械臂的雅可比矩阵表示机械臂可操作度，μθΟ表示矩阵JJt的第i个最大特征值;Im表示一个m*m维的单位矩阵;上标T表示矩阵和向量的转置;vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列;对于i=l，2，...，k，涵表示克罗内克积运算：定义为19+、Θ表不关节角上下限表示关节速度上下限;A和b是障碍物躲避参数。[0032]结合第二方面的第一种可能实现方式，在第二方面的第二种可能实现方式中，所述转化单元具体用于：[0033]将两个等式约束的范数嵌入进性能指标中，移除与决策变量无关的参数加入运动学能量消耗指标，得到性能指标为:最小化[0034]约束关系包括：其中，co0，C10，c20与C30为性能指标中对应项的权重;ζ+和Γ分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和，其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。[0035]结合第二方面的第二种可能实现方式，在第二方面的第三种可能实现方式中，所述二次规划求解器为：[0036][0040]其中，所述二次规划求解器的初始值为：OO表示采样间隔；下标j与j+Ι分别为对应项的第jT时刻与第j+lT的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。[0041]结合第二方面，在第二方面的第四种可能实现方式中，所述控制单元具体用于：[0042]将所述二次规划问题的求解结果转化为各个关节电机所需要的控制信号，驱动各个关节电机使机械臂运动。[0043]在本发明中，将冗余度机械臂的可操作度导数最大化的运动性能指标以及对应的约束关系，转化为二次规划问题，采用二次规划求解器对所述二次规划问题进行求解，根据求解结果控制机械臂运动，本方法可以有效地躲避冗余度机械臂的各种物理约束，有效地扩大了冗余度机械臂的可操作度优化运动规划的应用空间。附图说明[0044]图1是本发明实施例提供的冗余度机械臂的可操作度优化的运动规划方法的实现流程图；[0045]图2是本发明实施例提供的机械臂的结构示意图；[0046]图3是本发明实施例提供的冗余度机械臂的可操作度优化的运动规划装置的结构示意图。具体实施方式[0047]为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。[0048]本发明实施例的目的在于提出一种冗余度机械臂的可操作度优化的运动规划方法，以解决现有技术中基于伪逆的可操作度的优化方法，一般难以嵌入机械臂的各种物理约束，限制了冗余度机械臂的可操作度的问题。下面结合附图，对本方案作进一步的说明。[0049]图1示出了本发明实施例提供的冗余度机械臂的可操作度优化的运动规划方法的实现流程，详述如下：[0050]在步骤SlOl中，设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系。[0051]具体的，所述设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系步骤具体为：[0052]所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，即为最小化：所述与运动性能指标相应的约束关系包括：[0053]雅可比等式[0054]矩阵求逆等式^[0055]障碍物躲避不等式[0056]关节角极限Θ-彡Θ彡θ+;[0057]关节速度极限[0058]其中OeRk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度;f⑼£1^表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度;rd与分别表示末端执行器期望路径以及期望速度;kQ表示位置误差反馈系数;α表示矩阵JJΤ逆的向量化的值；表示机械臂的雅可比矩阵；表示机械臂可操作度，μ90表示矩阵JJt的第i个最大特征值山表示一个m*m维的单位矩阵;上标T表示矩阵和向量的转置;vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列；对于i=1，2，...，k，表示克罗内克积运算；定义为ι9+、Θ表不关节角上下限表示关节速度上下限;A和b是障碍物躲避参数。[0059]在步骤S102中，将所述运动性能指标以及相应的约束关系转化为二次规划问题。[0060]具体的，所述转过过程可以具体为:将优化问题转化为受等式与不等式约束的凸二次规划问题，因考虑到原问题的优化指标的非凸性，将两个等式约束的范数嵌入到性能指标中以凸化性能指标;移除与决策变量无关的参数加入运动学能量消耗指标，得到性能指标为:最小化[0061]，约束关系包括：，其中，co0，C10，C20与C30为性能指标中对应项的权重;ζ+和Γ分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和,其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。[0062]在步骤S103中，通过二次规划求解器对所述二次规划问题进行求解，得到求解结果。[0063]所述二次规划求解器可以为：[0064][0068];:其中，所述二次规划求解器的初始值为：τ0表示采样间隔；下标j与j+Ι分别为对应项的第^时刻与第j+lT的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。[0069]通过将上述求解器多次迭代求解，得到求解结果。[0070]在步骤S104中，根据所述求解结果控制机械臂运动。[0071]所述二次规划求解器为：[0072][0076];其中，所述二次规划求解器的初始值为：τ0表示采样间隔；下标j与j+1分别为对应项的第ji时刻与第j+lT的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。[0077]如图2所示为本发明提供的机械臂的三维模型示意图，所述机械臂为一个空间六自由度的机械臂。该机械臂由六个连杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成。[0078]本发明设计的可操作度最优化运动规划方案为[0079]最小化[0080]约束条件[0081][0085]其中OeRk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度;f⑼£1^表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度;m表示机械臂末端执行器工作空间维度;rd与分别表示末端执行器期望路径以及期望速度;k〇表示位置误差反馈系数;α表示矩阵JJt逆的向量化的值表示机械臂的雅可比矩阵：表示机械臂可操作度，叫多〇表示矩阵JJT的第i个最大特征值;Im表示一个m*m维的单位矩阵;上标T表示矩阵和向量的转置;vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列；对于i=l，2,...，k，表示克罗内克积运算；定义为θ+、Θ表不关节角上下限：表示关节速度上下限;A和b是障碍物躲避参数。[0086]因考虑到上述问题的优化指标的非凸性，将两个等式约束的范数嵌入进性能指标中以凸化性能指标；与决策变量无关，故移除;此外，也加入运动学能量消耗指标；故设计性能指标为最小化受约束于其中，C〇0，C1M，C2〇与c30为性能指标中对应项的权重;ζ+和Γ分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和，其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。[0087]给定初始值通过如下的二次规划求解器求解：[0088][0092]其中，τ0表示采样间隔；下标j与j+1分别为对应项的第^时刻与第j+1τ的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。通过算法不断迭代，得到Θ的解，从而得到冗余度机械臂运动规划的最优解。[0093]通过上位机的QP求解器得到该QP的解后，再将结果转化为电机驱动所需要的控制信号，并通过该冗余度机械臂的控制器来驱动机械臂各个关节电机运动，从而驱动各关节电机使机械臂运动。[0094]图3为本发明实施例提供的冗余度机械臂的可操作度优化的运动规划装置的结构示意图，包括：[0095]指标设定单元301，用于设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；[0096]转化单元302,用于将所述运动性能指标以及相应的约束关系转化为二次规划问题；[0097]求解单元303,用于通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；[0098]控制单元304,用于根据所述求解结果控制机械臂运动。[0099]优选的，所述指标设定单元具体用于：[0100]所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，最小化，所述与运动性能指标相应的约束关系包括:雅可比等式、矩阵求逆等式，障碍物躲避不等式，关节角极限关节速度极限其中OeRk与分别表示机械臂关节角度与关节角速度，k表示机械臂的关节角度空间维度;f0er表示机械臂前向运动学函数，m表示机械臂末端执行器工作空间维度;rd与分别表示末端执行器期望路径以及期望速度；ko表示位置误差反馈系数；α表示矩阵JJt逆的向量化的值；表示机械臂的雅可比矩阵；表示机械臂可操作度，μθΟ表示矩阵JJt的第i个最大特征值;Im表示一个m*m维的单位矩阵;上标T表示矩阵和向量的转置;vec.表示对一个矩阵的向量化操作，即将目标矩阵所有列依次排为一列;对于i=l，2，...，k，表示克罗内克积运算；定义为：r表示关节角上下限：表示关节速度上下限;A和b是障碍物躲避参数。[0102]优选的，所述转化单元具体用于：[0103]将两个等式约束的范数嵌入进性能指标中，移除与决策变量g无关的参数，加入运动学能量消耗指标，得到性能指标为:最小化[0104]，约束关系包括：其中，co0，C10，c20与C30为性能指标中对应项的权重;ζ+和Γ分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和,其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。[0105]优选的，所述二次规划求解器为：[0106][0110]「其中，所述二次规划求解器的初始值为：τ0表示采样间隔；下标j与j+Ι分别为对应项的第时刻与第j+lT的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。[0111]优选的，所述控制单元具体用于：[0112]将所述二次规划问题的求解结果转化为各个关节电机所需要的控制信号，驱动各个关节电机使机械臂运动。[0113]图3所述冗余度机械臂的可操作度优化的运动规划装置，与图1所述冗余度机械臂的可操作度优化的运动规划方法对应。[0114]在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。[0115]所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。[0116]另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。[0117]所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备可以是个人计算机，服务器，或者网络设备等执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括:U盘、移动硬盘、只读存储器R0M，Read-0nlyMemory、随机存取存储器RAM，RandomAccessMemory、磁碟或者光盘等各种可以存储程序代码的介质。[0118]以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

权利要求：1.一种冗余度机械臂的可操作度优化的运动规划方法，其特征在于，所述方法包括：设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；将所述运动性能指标以及相应的约束关系转化为二次规划问题；通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；根据所述求解结果控制机械臂运动。2.根据权利要求1所述方法，其特征在于，所述设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系步骤具体为：所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，最小化，所述与运动性能指标相应的约束关系包括：雅可比等式.、矩阵求逆等式，障碍物躲避不等式，关节角极限ίΓ0，C1M，C20与c30为性能指标中对应项的权重；分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和,其中为裕度，i表示关节序号，i=l，2,...，k，ka0为用来调节和保证关节速度的可行域的常数。4.根据权利要求3所述方法，其特征在于，所述二次规划求解器为：:;其中，所述二次规划求解器的初始值为：表示采样间隔;下标j与j+Ι分别为对应项的第ji时刻与第j+li的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。5.根据权利要求1所述方法，其特征在于，所述根据所述求解结果控制机械臂运动步骤具体为：将所述二次规划问题的求解结果转化为各个关节电机所需要的控制信号，驱动各个关节电机使机械臂运动。6.—种冗余度机械臂的可操作度优化的运动规划装置，其特征在于，所述装置包括：指标设定单元，用于设定冗余度机械臂的可操作度导数最大化的优化运动性能指标，以及与所述运动性能指标相应的约束关系；转化单元，用于将所述运动性能指标以及相应的约束关系转化为二次规划问题；求解单元，用于通过二次规划求解器对所述二次规划问题进行求解，得到求解结果；控制单元，用于根据所述求解结果控制机械臂运动。7.根据权利要求6所述装置，其特征在于，所述指标设定单元具体用于：所述冗余度机械臂的可操作度导数最大化的优化运动性能指标为可操作度导数最大化，最小化4所述与运动性能指标相应的约束关系包括：雅可比等式，、矩阵求逆等式I，障碍物躲避不等式，关节角极限ίΓ0，C1M，C20与c30为性能指标中对应项的权重；分别表示合成双端约束的上下极限，它们的第i个元素分别定义为和，其中为裕度，i表示关节序号，i=l，2,···，k，ka0为用来调节和保证关节速度的可行域的常数。9.根据权利要求8所述装置，其特征在于，所述二次规划求解器为：;其中，所述二次规划求解器的初始值为：表示采样间隔;下标j与j+Ι分别为对应项的第ji时刻与第j+li的值;h0表示算法迭代步长;Ρω表示由双端约束与不等式约束构成的投影映射。10.根据权利要求6所述装置，其特征在于，所述控制单元具体用于：将所述二次规划问题的求解结果转化为各个关节电机所需要的控制信号，驱动各个关节电机使机械臂运动。

百度查询： 香港理工大学深圳研究院 冗余度机械臂的可操作度优化的运动规划方法和装置

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD