一种无速度跳变的容错型冗余度机械臂运动规划方法与流程

本发明涉及一种冗余度机械臂运动的规划控制方法，尤其涉及一种无速度跳变的容错型冗余度机械臂运动规划方法。

背景技术：

冗余度机械臂是一种自由度大于任务空间所需最少自由度的末端能动机械装置，其运动任务包括焊接、油漆、组装、挖掘和绘图等，广泛应用于装备制造、产品加工、机器作业等国民经济生产活动中。

冗余度机械臂的逆运动学问题是指已知机械臂末端位姿，确定机械臂的关节角问题，当冗余度机械臂的某一关节发生故障导致关节发生退化时，为了使退化机械臂的末端能够继续根据原方案（即预定的冗余度解析方案）完成期望任务，需要采取容错方案（即机械臂容错型冗余度解析方案）替换原方案，存在的问题是：

已有技术提出的容错方案中有的没有考虑速度跳变，有的只是考虑减小速度跳变，虽然在关节故障发生时可减小关节速度跳变，但不能完全消除，因而无法从根本上避免由于速度跳变而带来的机械振动和电流冲击对机械臂的影响。

技术实现要素：

本发明的目的在于提供一种无速度跳变的容错型冗余度机械臂运动规划方法，以克服上述已有技术存在的不足。

本发明采取的技术方案是：

一种无速度跳变的容错型冗余度机械臂运动规划方法，包括如下步骤：

s1：根据机械臂关节出现故障的情况对退化机械臂重新配置退化方案，所述退化机械臂是指由于超负荷使用、异常操作、关节老化或磨损等原因导致机械臂单个或多个关节出现关节动作无响应、响应缓慢和其它响应失常故障而被锁定的机械臂；

重新配置退化方案的步骤包括：

s11：指定退化方案的初始关节角度

根据原方案在关节出现故障时刻机械臂关节所处的状态，确定退化方案中退化机械臂的初始关节状态即退化方案的初始关节角度；

s12：确定退化机械臂的连杆长度；

s13：根据步骤s11和s12所得的退化机械臂的初始关节角度和连杆长度计算雅可比矩阵，得到退化方案；

s2：通过上位机采用伪逆方法对退化方案进行求解，确定退化方案的过渡速度，包括：

s21：指定退化方案的初始关节速度

将故障时刻原方案机械臂的关节速度即原方案出现故障时刻的非故障关节速度作为退化方案的初始关节速度；

s22：重构机械臂末端速度

将机械臂末端速度重构为期望的末端速度与末端位置误差随时间的导数之和；

s23：采用基于雅克比矩阵伪逆的方法在速度层上对机械臂退化方案进行求解，得到退化方案的关节速度；

s24：确定退化方案的过渡速度

原方案出现故障时刻的非故障关节速度与退化方案的关节速度的加权和，即为关节故障后机械臂的关节速度即退化方案的过渡速度，其函数式为：

为故障时刻机械臂关节速度，也是指定的过渡速度的初始速度，

为故障关节锁定后退化方案中退化机械臂的关节速度，

为用于约束机械臂关节速度的时变函数，约束机械臂关节速度从故障时刻开始函数值由0到1变化的连续指数，为关节故障时刻；

s3：将步骤s2的结果传递给下位机控制器，驱动机械臂圆满地完成任务。

其进一步的技术方案是：

步骤s1中所述退化方案可以根据关节周期内出现故障的任意时刻重新配置。

更进一步：

步骤s1中所述的退化方案设计为：

，其中：

是给定机械臂结构和参数的可微非线性函数，

为关节空间的关节角向量对应的机械臂末端笛卡尔位置向量；

为实现容错操作过程中无关节速度跳变，设计退化方案中退化机械臂的关节速度为步骤s24确定的退化方案的过渡速度：

。

更进一步：

步骤s11所述初始关节角度和步骤s12所述退化机械臂的连杆长度，是通过三角形余弦定理计算得到。

由于采取上述技术方案，本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法具有以下有益效果：

本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法，可以对出现问题的关节进行容错处理，消除容错时刻的速度跳变而带来的机械振动和电流冲击，减小故障时刻出现的位置误差，使得机械臂能够圆满地完成指定轨迹任务：

1.本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法，可以在故障时刻通过原方案与退化方案的转换来消除机械臂的关节速度跳变，从而避免了故障时刻关节速度跳变带来的机械振动和电流冲击；

（其原理为：采用过渡速度进行速度过渡，根据故障时刻机械臂的速度即故障时刻原方案的关节速度来指定退化方案的初始速度，使得在故障时刻原方案与退化方案进行转换瞬间机械臂无关节速度跳变，从而避免了容错时刻出现速度跳变)）

2.本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法，通过将机械臂末端速度重构为：期望的末端速度与末端位置误差随时间的导数之和，可以减小由于关节故障后关节速度跳变带来的末端位置误差，使得机械臂能够圆满地完成指定轨迹跟踪任务；

3.本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法，利用基于雅克比矩阵伪逆的方法对机械臂容错型冗余度解析方案进行求解，较其他的冗余度求解方法（比如：二次规划数值方法、构造神经网络求解器法），该方法更易于理解，且计算量小，容易实现。

下面结合附图和实施例对本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法的技术特征作进一步的说明。

附图说明

图1为本发明之一种无速度跳变的容错型冗余度机械臂运动规划方法流程图。

具体实施方式

一种无速度跳变的容错型冗余度机械臂运动规划方法，包括如下步骤：

s1：根据机械臂关节出现故障的情况对退化机械臂重新配置退化方案，所述退化机械臂是指由于超负荷使用、异常操作、关节老化或磨损等原因导致机械臂单个或多个关节出现关节动作无响应、响应缓慢和其它响应失常故障而被锁定的机械臂；

重新配置退化方案的步骤包括：

s11：指定退化方案的初始关节角度

根据原方案在关节出现故障时刻机械臂关节所处的状态，确定退化方案中退化机械臂的初始关节状态即退化方案的初始关节角度；

s12：确定退化机械臂的连杆长度；

s13：根据步骤s11和s12所得的退化机械臂的初始关节角度和连杆长度计算雅可比矩阵，得到退化方案；

s2：通过上位机采用伪逆方法对退化方案进行求解，以确定退化方案的过渡速度，包括：

s21：指定退化方案的初始关节速度

将故障时刻原方案机械臂的关节速度即原方案出现故障时刻的非故障关节速度作为退化方案的初始关节速度；

s22：重构机械臂末端速度

将机械臂末端速度重构为期望的末端速度与末端位置误差随时间的导数之和；

s23：采用基于雅克比矩阵伪逆的方法在速度层上对机械臂退化方案进行求解，得到退化方案的关节速度；

s24：确定退化方案的过渡速度

原方案出现故障时刻的非故障关节速度与退化方案的关节速度的加权和，即为关节故障后机械臂的关节速度即退化方案的过渡速度，其函数式为：

为故障时刻机械臂关节速度，也是指定的过渡速度的初始速度，

为故障关节锁定后退化方案中退化机械臂的关节速度，

为用于约束机械臂关节速度的时变函数，约束机械臂关节速度从故障时刻开始函数值由0到1变化的连续指数，为关节故障时刻；

s3：将步骤s2的结果传递给下位机控制器，驱动机械臂圆满地完成任务。

上述步骤s1中所述退化方案可以根据关节周期内出现故障的任意时刻重新配置。

上述步骤s1中所述的退化方案设计为：

，其中：

是给定机械臂结构和参数的可微非线性函数，

为关节空间的关节角向量对应的机械臂末端笛卡尔位置向量；

为实现容错操作过程中无关节速度跳变，设计退化方案中退化机械臂的关节速度为步骤s24确定的退化方案的过渡速度：

。

上述步骤s11所述初始关节角度和步骤s12所述退化机械臂的连杆长度，是通过三角形余弦定理计算得到。

作为实施例，该机械臂期望初始位置关节角度设置为：

，四个连杆长度设置为，任务周期设置为。

机械臂的任务是跟踪一条直线，而在任务执行到时间时，机械臂关节出现了故障，导致机械臂末端轨迹偏离期望路径。

在机械臂关节故障时刻重新配置一个机械臂退化方案，即：

根据故障时刻退化机械臂相应的初始关节角度和退化后机械臂的连杆长度设置退化方案的关节速度；

运用基于雅克比矩阵伪逆的方法进行求解，将计算得到的结果再传送给机械臂控制器，从而控制机械臂完成任务。

下面是本发明关于容错型冗余机械臂运动规划的一般设计方案：

原方案：其中：

是给定机械臂结构和参数的可微非线性函数，

为关节空间的关节角向量，

为关节空间的关节角向量对应的机械臂末端的笛卡尔位置向量。

其中：

表示关节速度向量，

表示对应于的关节速度的机械臂末端笛卡尔速度，

表示机械臂的雅可比矩阵。

四连杆机械臂的关节速度通过基于雅克比矩阵伪逆的方法求解：

退化方案：

在时，第二关节出现了故障，四连杆机械臂退化成三连杆机械臂，

新的连杆长度和退化方案的初始关节角度可以通过三角形余弦定理求得。

退化后三连杆机械臂的关节速度同样也是通过基于雅克比矩阵伪逆的方法求解：

其中，是雅克比矩阵的伪逆。

由于更换退化方案时刻，机械臂末端执行器的实际位置和期望位置之间可能会出现偏差，为了减小位置误差，提高机械臂的任务执行精度，将机臂末端执行器速度设计为：

其中：

表示机械臂末端执行器优化后的速度矢量，

表示期望的机械臂末端执行器的笛卡尔轨迹速度，

表示实际和期望的机械臂末端执行器轨迹之间的向量值偏差随时间的导数，

设置参数，用来调节收敛速度。

接着，通过故障时刻处原方案和退化方案之间的转换，运用基于雅克比矩阵伪逆的方法在速度层上解决了冗余度机械臂在容错操作中关节速度的跳变问题：

其中：

为故障时刻关节速度，

为退化后三连杆机械臂关节速度，

为一个时变函数，用来约束机械臂的关节速度。

通过算法随时间进行迭代求解，便可得到最优的机械臂末端的笛卡尔轨迹速度，从而得到无速度跳变的容错型冗余机械臂运动规划的解。

术语解释：

（1）退化机械臂：是指正常工作的机械臂发生故障致使局部关节被锁定后的机械臂；

退化机械臂跟正常工作的机械臂是同一个机械臂，只是由于超负荷使用、异常操作、关节老化或磨损等原因导致机械臂单个或多个关节出现关节动作无响应、响应缓慢和其它响应失常等故障，导致机械臂有一个或多个关节被锁定；

（2）机械臂容错型冗余度解析方案：原方案和退化方案的统称，发生关节故障前用原方案，发生故障后启用退化方案；

原方案：机械臂不发生关节故障的机械臂容错型冗余度解析方案；

退化方案：机械臂出现故障后的机械臂容错型冗余度解析方案；

（3）关节角度：机械臂关节转动的角度；

（4）关节速度：机械臂关节移动或转动的速度；

（5）初始关节速度：即公式

中的，也是指定的过渡速度的初始速度，因为是一个从故障时刻函数值开始由0到1变化的连续指数时变函数；

（6）末端速度：是指机械臂末端的速度，如机械臂的手指指尖速度，又如机械臂拿焊枪进行焊接工作，末端速度就是焊枪最末端跟踪焊缝的速度；

（7）过渡速度：机械臂关节发生故障后原方案过渡到退化方案的速度，过渡速度为原方案出现故障时刻的非故障关节速度与退化方案的关节速度的加权和，即；因此，需要一定的过渡时间从原方案的关节速度过渡到退化方案的关节速度，该过渡时间可根据实际情况需要进行调节或设置，通过上位机采用基于雅克比矩阵伪逆的方法对机械臂容错型冗余度解析方案进行求解。