一种冗余度机械臂加速度层的容错规划方法技术

本发明专利技术提供了一种冗余度机械臂的加速度层的容错规划方法，包括如下步骤：步骤一，先设计二次型形式的加速度层的容错性能指标，生成二次型优化冗余度解析方案；步骤二，将步骤一的二次型优化冗余度解析方案转化为QP，即：二次规划；步骤三，将步骤二的QP运用QP求解器进行求解；步骤四，将步骤三的求解结果传递给下位机控制器驱动机械臂运动。本发明专利技术通过设计一种冗余度机械臂的新型加速度层的容错性能指标，保证了末端执行器在关节出错锁定的情况下仍能完成要求的任务，提高了冗余度机械臂的安全性和有效性，这在工程应用领域具有良好前景。

【技术实现步骤摘要】
一种冗余度机械臂加速度层的容错规划方法
本专利技术涉及冗余度机械臂容错规划及控制领域，具体涉及一种冗余度机械臂的逆运动学解析方法。
技术介绍
冗余度机械臂，具有关节自由度数目大于任务空间所需最少自由度的特性，即：完成某一特定任务时，机械臂具有多余的自由度。其应用范围十分广泛，大到深海和宇宙空间的探索，小到产品加工和家庭服务等不同领域都可以发现其身影。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角度的问题。由于具备冗余的自由度，冗余度机械臂的逆运动学问题具有无穷多解，即保证机械臂末端在完成所要求任务的同时，还能根据不同的任务要求和工作环境选择不同的优化指标。以往的容错规划方法大多是在速度层上进行的，其得到结果为速度值，而这并不能满足某些采用非速度控制的冗余度机械臂的要求。更重要的是，速度层上的求解方法未考虑到加速度的极限，这使得机械臂在执行任务的过程中，存在超越加速度极限从而造成物理损坏的可能性。见本专利技术人团队之前的中国专利技术专利，名称：《一种冗余度机械臂容错规划的方法》，专利号：201010264141.6，为一种速度层的容错解析方法，可以克服上述速度层控制的冗余度机械臂的容错问题。但是，该专利技术的技术方案，仍在以下不足之处：(1)不能消除机械臂超越加速度极限，从而造成物理损坏的可能性，而且无法实现障碍物躲避。
技术实现思路
本专利技术的目的在于克服上述现有技术存在的不足，而提供一种新型的、计算量小的、普遍用于加速度控制的冗余度机械臂容错规划方法，有效地消除机械臂超越加速度极限而造成物理损坏的可能性，其推导过程从速度到加速度更深一层，且可以实现障碍物躲避，因此具有较强的适用性、安全性和实用性。本专利技术的目的是采用以下技术措施解决：一种冗余度机械臂的新型加速度层的容错规划方法包括如下步骤：步骤一，通过上位机采用二次型优化方案在加速度层上对机械臂的容错进行逆运动学解析，设计的最小化性能指标为容错，受约束于加速度雅可比等式、障碍物躲避不等式、关节角度极限、关节速度极限和关节加速度极限；步骤二，将步骤一的二次型优化方案转化为QP，即：二次规划；步骤三，将步骤二的QP用QP求解器进行求解；步骤四，将步骤三的求解结果传递给下位机控制器，以驱动机械臂运动。冗余度机械臂加速度层的容错方案设计为：最小化受约束于θ-≤θ≤θ+、其中θ表示关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，上标T表示矩阵和向量的转置，λ是用来控制收敛率的设计参数，Q是容错矩阵(当第j个关节出错锁定时，其中j∈{1,2,…,n}，Q中对应对角线上的元素qjj赋值为1，其它元素赋值为0)，等式约束对应机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是J的时间导数，表示机械臂末端执行器加速度向量，A和b是障碍物躲避参数。上述二次型优化方案可以转化为一个QP，即最小化受约束于Cx＝d，Ax≤b，其中C＝J，和分别表示合成双端约束的上下限，它们第i个元素分别定义为和其中i＝1,2,…,n表示关节序号，正常数kα和kβ用来调节关节加速度并保证关节加速度有足够大的可行域，为裕度。作为更具体的方案，上述QP可以用QP求解器求解。上述QP求解器，求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂具有容错性能。本专利技术的有益效果如下：以往的冗余度机械臂不考虑加速度的极限，因而存在超越加速度极限从而造成物理损坏的可能性，而且无法实现障碍物躲避，本专利技术给出了一个新型的加速度层的容错方案，有效地消除了机械臂超越加速度极限从而造成物理损坏的可能性，同时其推导过程更加深入严密，且能够实现障碍物躲避，因此，在实际应用中有非常重要的作用和广阔的前景。附图说明图1为本专利技术的流程图；图2为实现本专利技术的机械臂二维模型图；图3为实现本专利技术的机械臂容错示意图；图4为实现本专利技术的机械臂关节角度示意图；图5为实现本专利技术的机械臂关节速度示意图；图6为实现本专利技术的机械臂关节加速度示意图。具体实施方式下面结合附图对本专利技术做进一步的说明。如图1所示，本专利技术冗余度机械臂容错规划方法，首先提出加速度层的容错性能指标与约束；然后将其转化为相应的QP；接着应用QP求解器求解；最后下位机控制器根据求解结果控制机械臂的运动。见图2所示，实现本专利技术的机械臂为一个平面六连杆机械臂；该机械臂由六个连杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成。见图3所示，为平面六连杆机械臂的容错示意图；给定末端任务为画出一个平面圆形，当关节1和关节2都出错锁定时，其它关节及时调整到最佳运行轨迹，以保证机械臂末端完成画圆的任务。见图4所示，为平面六连杆机械臂的六个关节的关节角度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节角度分别是θ1、θ2、θ3、θ4、θ5和θ6。见图5所示，为平面六连杆机械臂的六个关节的关节速度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节速度分别是和见图6所示，为平面六连杆机械臂的六个关节的关节加速度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节加速度分别是和本专利技术设计的加速度层容错规划方案为最小化：约束条件：θ-≤θ≤θ+(4)其中为新的加速度层的容错性能指标，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，上标T表示矩阵和向量的转置，λ是用来控制收敛率的设计参数，Q是容错矩阵(当第j个关节出错锁定时，其中j∈{1,2,…,n}，Q中对应对角线上的元素qjj赋值为1，其它元素赋值为0)，等式约束对应机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是J的时间导数，表示机械臂末端执行器加速度向量，A和b是障碍物躲避参数。考虑到上述优化问题是在加速度层上求解，因此需将机械臂的关节角度约束(4)、关节速度约束(5)和关节加速度约束(6)合并，从而可以得到基于加速度的双端不等式约束：其中和分别表示合成双端约束的上下限，它们第i个元素分别定义为和其中i＝1,2,…,n表示关节序号，正常数kα和kβ用来调节关节加速度并保证关节加速度有足够大的可行域，为裕度。用x表示机械臂的关节加速度带物理约束的机械臂的新型二次型优化方案(1)-(6)便可描述为如下的QP方案：最小化：xTx/2+pTx(7)约束条件：Cx＝d(8)Ax≤b(9)其中C＝J，上述QP问题求解等价于寻求一个向量使之满足(y-y*)T(Hy*+g)≥0，其中y＝[x,u,v]T，u∈Rm和v∈Rdim(b)表示由等式约束(8)和不等式约束(9)得到的向量，1u和1v分别表示由1组成的m维和dim(b)维向量，ω＞＞0于数值上替代无穷大+∞，其中I是单位矩阵。上述问题又可以等价为分段线性投影方程PΩ(y-(Hy+g))-y＝0，其中设计其计算误差为z(y)＝y-PΩ(y-(Hy+g))：当误差为零时，其对应的y值便为分段线性方程的解，其前n个元素组成QP的解x。给定初始值y0∈Rn+m+dim(b)，通过如下的迭代来得到y使得误差z(y)达到预设的精度，运行以下迭代：yk+1＝PΩ(yk-ρ(yk)本文档来自技高网...

【技术保护点】
一种冗余度机械臂的加速度层的容错规划方法，其特征在于包括如下步骤：步骤一，通过上位机采用新型二次型优化方案在加速度层上对机械臂的容错进行冗余度解析，设计的新型最小化性能指标为容错，受约束于加速度雅可比等式、障碍物躲避不等式、关节角度极限、关节速度极限和关节加速度极限，即为最小化，受约束于加速度雅可比等式、障碍物躲避不等式约束，关节角度极限、关节速度极限，关节加速度极限，其中表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，上标表示矩阵和向量的转置，，是用来控制收敛率的设计参数，是容错矩阵（当第个关节出错锁定时，其中，中对应对角线上的元素赋值为1，其它元素赋值为0），等式约束对应机械臂末端运动轨迹，表示机械臂的雅可比矩阵，是的时间导数，，表示机械臂末端执行器加速度向量，和是障碍物躲避参数；步骤二，将步骤一的面向加速度层的容错的二次型优化方案转化为QP（二次规划）；    步骤三，将步骤二的QP用数值算法（也即QP求解器）进行求解；    步骤四，将步骤三的求解结果传递给下位机控制器驱动机械臂运动。

【技术特征摘要】
1.一种冗余度机械臂的加速度层的容错规划方法，其特征在于包括如下步骤：步骤一，通过上位机采用新型二次型优化方案在加速度层上对机械臂的容错进行冗余度解析，设计的新型最小化性能指标为容错，受约束于加速度雅可比等式、障碍物躲避不等式、关节角度极限、关节速度极限和关节加速度极限，即为最小化受约束于加速度雅可比等式障碍物躲避不等式约束关节角度极限θ-≤θ≤θ+、关节速度极限关节加速度极限其中θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ±表示关节角度上下限，表示关节速度上下限，表示关节加速度上下限，上标T表示矩阵和向量的转置，λ是用来控制收敛率的设计参数，Q是容错矩阵，当第j个关节出错锁定时，其中j∈{1,2,…,n}，Q中对应对角线上的元素qjj赋值为1，其它元素赋值为0，等式约束对应机械臂末端运动轨迹，J表示机械臂的雅可...

【专利技术属性】
技术研发人员：张雨浓，王金金，金龙，晏小刚，谭洪舟，
申请(专利权)人：广东顺德中山大学卡内基梅隆大学国际联合研究院，中山大学，
类型：发明
国别省市：广东;44