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

摘要

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

权利要求

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

步骤二，将步骤一的面向加速度层的容错的二次型优化方案转化为二次规划；

步骤三，将步骤二的二次规划用数值算法，二次规划求解器进行求解；

步骤四，将步骤三的求解结果传递给下位机控制器驱动机械臂运动；

所述步骤三的二次规划求解器求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂具备容错性能。

2.根据权利要求1所述的冗余度机械臂加速度层的容错规划方法，其特征在于所述步骤二的新型二次型优化方案转化为一个二次规划，设计其性能指标为最小化xTx/2+pTx，受约束于Cx＝d，Ax≤b， 其中 C＝J， 和 分别表示合成双端约束的上下限，它们的第i个元素分别定义为

和 其中i＝1,2,…,n表示关节序号，正常数kα和kβ用来

调节关节加速度并保证关节加速度有足够大的可行域， 为裕度。

说明书

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

技术领域

[0001] 本发明涉及冗余度机械臂容错规划及控制领域，具体涉及一种冗余度机械臂的逆运动学解析方法。

背景技术

[0002] 冗余度机械臂，具有关节自由度数目大于任务空间所需最少自由度的特性，即：完成某一特定任务时，机械臂具有多余的自由度。其应用范围十分广泛，大到深海和宇宙空间的探索，小到产品加工和家庭服务等不同领域都可以发现其身影。冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角度的问题。由于具备冗余的自由度，冗余度机械臂的逆运动学问题具有无穷多解，即保证机械臂末端在完成所要求任务的同时，还能根据不同的任务要求和工作环境选择不同的优化指标。

[0003] 以往的容错规划方法大多是在速度层上进行的，其得到结果为速度值，而这并不能满足某些采用非速度控制的冗余度机械臂的要求。更重要的是，速度层上的求解方法未考虑到加速度的极限，这使得机械臂在执行任务的过程中，存在超越加速度极限从而造成物理损坏的可能性。

[0004] 见本发明人团队之前的中国发明专利，名称：《一种冗余度机械臂容错规划的方法》，专利号：201010264141.6，为一种速度层的容错解析方法，可以克服上述速度层控制的冗余度机械臂的容错问题。但是，该发明的技术方案，仍在以下不足之处：(1)不能消除机械臂超越加速度极限，从而造成物理损坏的可能性，而且无法实现障碍物躲避。

发明内容

[0005] 本发明的目的在于克服上述现有技术存在的不足，而提供一种新型的、计算量小的、普遍用于加速度控制的冗余度机械臂容错规划方法，有效地消除机械臂超越加速度极限而造成物理损坏的可能性，其推导过程从速度到加速度更深一层，且可以实现障碍物躲避，因此具有较强的适用性、安全性和实用性。

[0006] 本发明的目的是采用以下技术措施解决：

[0007] 一种冗余度机械臂的新型加速度层的容错规划方法包括如下步骤：

[0008] 步骤一，通过上位机采用二次型优化方案在加速度层上对机械臂的容错进行逆运动学解析，设计的最小化性能指标为容错，受约束于加速度雅可比等式、障碍物躲避不等式、关节角度极限、关节速度极限和关节加速度极限；

[0009] 步骤二，将步骤一的二次型优化方案转化为QP，即：二次规划；

[0010] 步骤三，将步骤二的QP用QP求解器进行求解；

[0011] 步骤四，将步骤三的求解结果传递给下位机控制器，以驱动机械臂运动。

[0012] 冗余度机械臂加速度层的容错方案设计为：最小化 受约束于θ-≤θ≤θ+、 其中θ表示关节角度，表示机械臂
的关节速度，表示机械臂的关节加速度，θ±表示关节角度上下限， 表示关节速度上下限， 表示关节加速度上下限，上标T表示矩阵和向量的转置， λ是用来控制收敛率的设计参数，Q是容错矩阵(当第j个关节出错锁定时，其中j∈{1,2,…,n}，Q中对应对角线上的元素qjj赋值为1，其它元素赋值为0)，等式约束 对应机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是J的时间导数， 表示机械臂末端执行器加速度向量，A和b是障碍物躲避参数。

[0013] 上述二次型优化方案可以转化为一个QP，即最小化 受约束于Cx＝d，Ax≤b， 其中 C＝J， 和 分别表示合成双端约束的上下限，它们第i个 元 素 分 别 定 义 为 和
其中i＝1,2,…,n表示关节序号，正常数kα和kβ用来
调节关节加速度并保证关节加速度有足够大的可行域， 为裕度。

[0014] 作为更具体的方案，上述QP可以用QP求解器求解。

[0015] 上述QP求解器，求解结果转化为电机驱动所需要的控制信号，从而驱动各关节电机使机械臂具有容错性能。

[0016] 本发明的有益效果如下：

[0017] 以往的冗余度机械臂不考虑加速度的极限，因而存在超越加速度极限从而造成物理损坏的可能性，而且无法实现障碍物躲避，本发明给出了一个新型的加速度层的容错方案，有效地消除了机械臂超越加速度极限从而造成物理损坏的可能性，同时其推导过程更加深入严密，且能够实现障碍物躲避，因此，在实际应用中有非常重要的作用和广阔的前景。

附图说明

[0018] 图1为本发明的流程图；

[0019] 图2为实现本发明的机械臂二维模型图；

[0020] 图3为实现本发明的机械臂容错示意图；

[0021] 图4为实现本发明的机械臂关节角度示意图；

[0022] 图5为实现本发明的机械臂关节速度示意图；

[0023] 图6为实现本发明的机械臂关节加速度示意图。

具体实施方式

[0024] 下面结合附图对本发明做进一步的说明。

[0025] 如图1所示，本发明冗余度机械臂容错规划方法，首先提出加速度层的容错性能指标与约束；然后将其转化为相应的QP；接着应用QP求解器求解；最后下位机控制器根据求解结果控制机械臂的运动。

[0026] 见图2所示，实现本发明的机械臂为一个平面六连杆机械臂；该机械臂由六个连杆组成，通过关节1、关节2、关节3、关节4、关节5和关节6组成。

[0027] 见图3所示，为平面六连杆机械臂的容错示意图；给定末端任务为画出一个平面圆形，当关节1和关节2都出错锁定时，其它关节及时调整到最佳运行轨迹，以保证机械臂末端完成画圆的任务。

[0028] 见图4所示，为平面六连杆机械臂的六个关节的关节角度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节角度分别是θ1、θ2、θ3、θ4、θ5和θ6。

[0029] 见图5所示，为平面六连杆机械臂的六个关节的关节速度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节速度分别是 和

[0030] 见图6所示，为平面六连杆机械臂的六个关节的关节加速度示意图；其中，关节1、关节2、关节3、关节4、关节5和关节6的关节加速度分别是 和

[0031] 本发明设计的加速度层容错规划方案为

[0032] 最小化：

[0033] 约束条件：

[0034]

[0035] θ-≤θ≤θ+                     (4)

[0036]

[0037]

[0038] 其中 为新的加速度层的容错性能指标，θ表示机械臂的关节角度，表示机械臂的关节速度，表示机械臂的关节加速度，θ±表示关节角度上下限， 表示关节速度上下限， 表示关节加速度上下限，上标T表示矩阵和向量的转置， λ是用来控制收敛率的设计参数，Q是容错矩阵(当第j个关节出错锁定时，其中j∈{1,2,…,n}，Q中对应对角线上的元素qjj赋值为1，其它元素赋值为0)，等式约束 对应机械臂末端运动轨迹，J表示机械臂的雅可比矩阵，是J的时间导数， 表示机械臂末端执行器加速度向量，A和b是障碍物躲避参数。

[0039] 考虑到上述优化问题是在加速度层上求解，因此需将机械臂的关节角度约束(4)、关节速度约束(5)和关节加速度约束(6)合并，从而可以得到基于加速度 的双端不等式约束：

[0040]

[0041] 其中 和 分别表示合成双端约束的上下限，它们第i个元素分别定义为和 其中i＝1,2,…,n表示关节序号，正常数kα和kβ用来调节关节加速度并保证关节加速度有足够大的可行域， 为裕度。用x表示机械臂的关节加速度 带物理约束的机械臂的新型二次型优化方案(1)-(6)便可描述为如下的QP方案：

[0042] 最小化：xTx/2+pTx                   (7)

[0043] 约束条件：Cx＝d                      (8)

[0044] Ax≤b                       (9)

[0045]

[0046] 其中 C＝ J ， 上述 QP问 题求解 等价于 寻求一 个向 量* T *
使之满足(y-y) (Hy+g)≥0， 其中y＝[x,u,
v]T， u∈Rm和v∈Rdim(b)表示由等式约束(8)和不
等式约束(9)得到的向量，1u和1v分别表示由1组成的m维和dim(b)维向量，ω＞＞0于数值上替代无穷大+∞，

[0047]

[0048] 其中I是单位矩阵。

[0049] 上述问题又可以等价为分段线性投影方程PΩ(y-(Hy+g))-y＝0，其中[0050]

[0051] 设计其计算误差为z(y)＝y-PΩ(y-(Hy+g))：当误差为零时，其对应的y值便为分段0 n+m+dim(b)
线性方程的解，其前n个元素组成QP的解x。给定初始值y ∈R ，通过如下的迭代来得到y使得误差z(y)达到预设的精度，运行以下迭代：

[0052] yk+1＝PΩ(yk-ρ(yk)f(yk))，

[0053] 其中f(yk)＝HTz(yk)+Hyk+g和 迭代次数k＝0,1,2,…。通过算法不断迭代，得到分段线性投影方程PΩ(y-(Hy+g))-y＝0的解，从而得到冗余度机械臂运动规划的最优解。

[0054] 通过上位机的QP求解器得到该QP的解后，再将结果转化为电机驱动所需要的控制信号，并通过该冗余度机械臂的控制器来驱动机械臂各个关节电机运动，从而驱动各关节电机使机械臂具有容错性能。

[0055] 现结合一个具体的实例操作对本发明的工作流程进行如下说明。

[0056] 在加速度层的容错方案实施过程中实现障碍物躲避，

[0057]

[0058] 机械臂初始位置关节1至6的角度设置为[0,-π/4,0,π/2,0,-π/4,0]T弧度，角度上限设置为θ+＝[2.775,0.750,4.049,2.967,0.035,3.142]T弧度，角度下限设置为θ-＝[-2.775,-3.892,-0.905,-1.919,-1.745,-4.625]T弧度，速度上限设置为
弧 度 / 秒 ， 速 度 下 限 设 置 为
弧度/秒，加速度上限设置为 弧度/
2 2
秒 ，加速度下限设置为 弧度/秒。通过QP求解器求解，将计算得到
的结果再传送给机械臂控制器从而控制机械臂的运动。

[0059] 平面六连杆机械臂的关节1和关节2出错锁定时，机械臂未出错的四个关节，即关节3、关节4、关节5和关节6，开始执行任务，平面六连杆机械臂的末端最终成功画出一个圆，也即，平面六连杆机械臂根据任务要求和工作环境，很好地展示出其容错性能。