一种冗余度机械臂加速度层的容错规划方法
【技术领域】
[0001] 本发明涉及冗余度机械臂容错规划及控制领域，具体涉及一种冗余度机械臂的逆 运动学解析方法。
【背景技术】
[0002] 冗余度机械臂，具有关节自由度数目大于任务空间所需最少自由度的特性，即：完 成某一特定任务时，机械臂具有多余的自由度。其应用范围十分广泛，大到深海和宇宙空 间的探索，小到产品加工和家庭服务等不同领域都可以发现其身影。冗余度机械臂的逆运 动学问题是指已知机械臂末端位姿，确定机械臂的关节角度的问题。由于具备冗余的自由 度，冗余度机械臂的逆运动学问题具有无穷多解，即保证机械臂末端在完成所要求任务的 同时，还能根据不同的任务要求和工作环境选择不同的优化指标。
[0003] 以往的容错规划方法大多是在速度层上进行的，其得到结果为速度值，而这并不 能满足某些采用非速度控制的冗余度机械臂的要求。更重要的是，速度层上的求解方法未 考虑到加速度的极限，这使得机械臂在执行任务的过程中，存在超越加速度极限从而造成 物理损坏的可能性。
[0004] 见本发明人团队之前的中国发明专利，名称：《一种冗余度机械臂容错规划的方 法》，专利号=201010264141. 6,为一种速度层的容错解析方法，可以克服上述速度层控制的 冗余度机械臂的容错问题。但是，该发明的技术方案，仍在以下不足之处：（1)不能消除机 械臂超越加速度极限，从而造成物理损坏的可能性，而且无法实现障碍物躲避。
【发明内容】
[0005] 本发明的目的在于克服上述现有技术存在的不足，而提供一种新型的、计算量小 的、普遍用于加速度控制的冗余度机械臂容错规划方法，有效地消除机械臂超越加速度极 限而造成物理损坏的可能性，其推导过程从速度到加速度更深一层，且可以实现障碍物躲 避，因此具有较强的适用性、安全性和实用性。
[0006] 本发明的目的是采用以下技术措施解决。
[0007] -种冗余度机械臂的新型加速度层的容错规划方法包括如下步骤： 步骤一，通过上位机采用二次型优化方案在加速度层上对机械臂的容错进行逆运动学 解析，设计的最小化性能指标为容错，受约束于加速度雅可比等式、障碍物躲避不等式、关 节角度极限、关节速度极限和关节加速度极限； 步骤二，将步骤一的二次型优化方案转化为QP，即：二次规划； 步骤三，将步骤二的QP用QP求解器进行求解； 步骤四，将步骤三的求解结果传递给下位机控制器，以驱动机械臂运动。
[0008] 冗余度机械臂加速度层的容错方案设计为：最小化
，受约束于
，其中e表示关节角度，々表示机械 臂的关节速度，#表示机械臂的关节加速度，表示关节角度上下限，夕表示关节速度上 下限，表示关节加速度上下限，上标T表示矩阵和向量的转置，
，J是用来控 制收敛率的设计参数，g是容错矩阵（当第j个关节出错锁定时，其中je{X2,…，气丨，^中 对应对角线上的元素?赋值为1，其它元素赋值为0)，等式约束5对应机械臂末端运 动轨迹，/表示机械臂的雅可比矩阵，j是/的时间导数，
，卩表示机械臂末端执 行器加速度向量，d和&是障碍物躲避参数。
[0009] 上述二次型优化方案可以转化为一个QP，即最小化
，受约束于
分别表不合成双端 约束的上下限，它们第，个元素分别定义为
和
，其中i = U《表示关节序号，正常数 < 和心用来 调节关节加速度并保证关节加速度有足够大的可行域，巧为裕度。
[0010] 作为更具体的方案，上述QP可以用QP求解器求解。
[0011] 上述QP求解器，求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电 机使机械臂具有容错性能。
[0012] 本发明的有益效果如下。
[0013] 以往的冗余度机械臂不考虑加速度的极限，因而存在超越加速度极限从而造成物 理损坏的可能性，而且无法实现障碍物躲避，本发明给出了一个新型的加速度层的容错方 案，有效地消除了机械臂超越加速度极限从而造成物理损坏的可能性，同时其推导过程更 加深入严密，且能够实现障碍物躲避，因此，在实际应用中有非常重要的作用和广阔的前 景。
【附图说明】
[0014] 图1为本发明的流程图。
[0015] 图2为实现本发明的机械臂二维模型图。
[0016] 图3为实现本发明的机械臂容错示意图。
[0017] 图4为实现本发明的机械臂关节角度示意图。
[0018] 图5为实现本发明的机械臂关节速度示意图。
[0019] 图6为实现本发明的机械臂关节加速度示意图。
【具体实施方式】
[0020] 下面结合附图对本发明做进一步的说明。
[0021] 如图1所示，本发明冗余度机械臂容错规划方法，首先提出加速度层的容错性能 指标与约束；然后将其转化为相应的QP ;接着应用QP求解器求解；最后下位机控制器根据 求解结果控制机械臂的运动。
[0022] 见图2所示，实现本发明的机械臂为一个平面六连杆机械臂；该机械臂由六个连 杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成。
[0023] 见图3所示，为平面六连杆机械臂的容错示意图；给定末端任务为画出一个平面 圆形，当关节1和关节2都出错锁定时，其它关节及时调整到最佳运行轨迹，以保证机械臂 末端完成画圆的任务。
[0024]见图4所示，为平面六连杆机械臂的六个关节的关节角度示意图；其中，关节1、关 节2、关节3、关节4、关节5和关节6的关节角度分别是巧、马、色、巧、咚和乌。
[0025]见图5所示，为平面六连杆机械臂的六个关节的关节速度示意图；其中，关节1、关 节2、关节3、关节4、关节5和关节6的关节速度分别是戎、4、4、4、4和4。
[0026] 见图6所示，为平面六连杆机械臂的六个关节的关节加速度示意图；其中，关节1、 关节2、关节3、关节4、关节5和关节6的关节加速度分别是吴、兵、鸟、兵、马和4。
[0027] 本发明设计的加速度层容错规划方案为
其中1^/2+/备为新的加速度层的容错性能指标，$表示机械臂的关节角度，表示 机械臂的关节