速度，g表示机械臂的关节加速度，铲表示关节角度上下限，妒表示关节速 度上下限，占*表示关节加速度上下限，上标*表示矩阵和向量的转置，，a是用 来控制收敛率的设计参数，g是容错矩阵（当第j个关节出错锁定时，其中， g中对应对角线上的元素赋值为1，其它元素赋值为0)，等式约束《对应机械臂末 端运动轨迹，J表示机械臂的雅可比矩阵，j是J的时间导数，6 P表示机械臂末 端执行器加速度向量，d和&是障碍物躲避参数。
[0028]考虑到上述优化问题是在加速度层上求解，因此需将机械臂的关节角度约束（4)、 关节速度约束（5)和关节加速度约束（6)合并，从而可以得到基于加速度#的双端不等式 约束： <&<<;* , 其中f和f分别表示合成双端约束的上下限，它们第f个元素分别定义为
,其中 /= 1,2,…,rt表示关节序号，正常数&和&用来调节关节加速度并保证关节加速度有足够 大的可行域，爲为裕度。用Z表示机械臂的关节加速度邊，带物理约束的机械臂的新型二次 型优化方案（1)_(6)便可描述为如下的QP方案：
其中x=卢，C= J。上述QP问题求解等价于寻求一个向量
1表示由等式约束（8)和不 等式约束（9)得到的向量，1。和1,分别表示由1组成的?维和dm(句维向量，〇于数 值上替代无穷大，
其中J是单位矩阵。
[0029] 上述问题又可以等价为分段线性投影方程
，其中
设计其计算误差为
：当误差为零时，其对应的直便为分段 线性方程的解，其前《个元素组成QP的解x。给定初始值
，通过如下的迭代 来得到.F使得误差达到预设的精度，运行以下迭代：
其中
，迭代次数 i = 。通过算法不断迭代，得到分段线性投影方程
的解，从而 得到冗余度机械臂运动规划的最优解。
[0030] 通过上位机的QP求解器得到该QP的解后，再将结果转化为电机驱动所需要的控 制信号，并通过该冗余度机械臂的控制器来驱动机械臂各个关节电机运动，从而驱动各关 节电机使机械臂具有容错性能。
[0031] 现结合一个具体的实例操作对本发明的工作流程进行如下说明。
[0032] 在加速度层的容错方案实施过程中实现障碍物躲避，
机械臂初始位置关节1至6的角度设置为
弧度，角 度上限设置为
弧度，角度下限设置 为
弧度，速度上限设置为少
弧度/秒，速度下限设置为
弧度/秒，加速度上限设置为
弧度/秒2，加速度下限设置为孑
弧度/秒2。通过QP求解器求解，将计算得到的结果再传送给机械臂 控制器从而控制机械臂的运动。
[0033]平面六连杆机械臂的关节1和关节2出错锁定时，机械臂未出错的四个关节，即关 节3、关节4、关节5和关节6,开始执行任务，平面六连杆机械臂的末端最终成功画出一个 圆，也即，平面六连杆机械臂根据任务要求和工作环境，很好地展示出其容错性能。
【主权项】
1. 一种冗余度机械臂的加速度层的容错规划方法，其特征在于包括如下步骤： 步骤一，通过上位机采用新型二次型优化方案在加速度层上对机械臂的容错 进行冗余度解析，设计的新型最小化性能指标为容错，受约束于加速度雅可比等式、 障碍物躲避不等式、关节角度极限、关节速度极限和关节加速度极限，即为最小化 穸占/2.+ /吾，受约束于加速度雅可比等式g、障碍物躲避不等式约束，关节角 度极限r^沒sr、关节速度极限r ，关节加速度极限r^，其中s表示机 械臂的关节角度，^表示机械臂的关节速度，&表示机械臂的关节加速度，f表示关节角 度上下限，妒表示关节速度上下限，占1表示关节加速度上下限，上标T表示矩阵和向量的 转置，##，；!是用来控制收敛率的设计参数，g是容错矩阵（当第j个关节出错锁 定时，其中，g中对应对角线上的元素仏赋值为1，其它元素赋值为〇)，等 式约束?/#=《对应机械臂末端运动轨迹，j表示机械臂的雅可比矩阵，j是j的时间导数， & p表示机械臂末端执行器加速度向量，j和&是障碍物躲避参数； 步骤二，将步骤一的面向加速度层的容错的二次型优化方案转化为QP(二次规划）； 步骤三，将步骤二的QP用数值算法(也即QP求解器）进行求解； 步骤四，将步骤三的求解结果传递给下位机控制器驱动机械臂运动。2. 根据权利要求1所述的冗余度机械臂加速度层的容错规划方法，其特征在于所述 步骤二的新型二次型优化方案转化为一个QP，设计其性能指标为最小化rTx/2 + /x，受 约束于，其中x=#，和分别表示合成双 端约束的上下限，它们的第|个元素分别定义为和，其中1 = 1,2,《表示关节序号，正常数L和&用 来调节关节加速度并保证关节加速度有足够大的可行域，A为裕度。3. 根据权利要求2所述的冗余度机械臂加速度层的容错规划方法，其特征在于所述步 骤三的QP求解器求解。4. 根据权利要求3所述的冗余度机械臂加速度层的容错规划方法，其特征在于所述步 骤三的QP求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机 械臂具备容错性能。
【专利摘要】本发明提供了一种冗余度机械臂的加速度层的容错规划方法，包括如下步骤：步骤一，先设计二次型形式的加速度层的容错性能指标，生成二次型优化冗余度解析方案；步骤二，将步骤一的二次型优化冗余度解析方案转化为QP，即：二次规划；步骤三，将步骤二的QP运用QP求解器进行求解；步骤四，将步骤三的求解结果传递给下位机控制器驱动机械臂运动。本发明通过设计一种冗余度机械臂的新型加速度层的容错性能指标，保证了末端执行器在关节出错锁定的情况下仍能完成要求的任务，提高了冗余度机械臂的安全性和有效性，这在工程应用领域具有良好前景。
【IPC分类】B25J9/16
【公开号】CN104908040
【申请号】CN201510347924
【发明人】张雨浓, 王金金, 金龙, 晏小刚, 谭洪舟
【申请人】广东顺德中山大学卡内基梅隆大学国际联合研究院, 中山大学
【公开日】2015年9月16日
【申请日】2015年6月23日