CN107598929A - 一种单关节故障空间机械臂位姿可达空间求解方法 - Google Patents

Abstract

本发明实施例提供了一种单关节故障空间机械臂位姿可达空间求解方法，实现了任一关节故障锁定后空间机械臂可达位姿退化情况的量化分析，包括：基于关节人为限位求解单关节故障锁定的空间机械臂退化工作空间；构建姿态可达度指标，求解面向任务的空间机械臂期望姿态；综合期望姿态与退化工作空间内各散点坐标构建机械臂末端位姿集，筛选其中存在逆解的位姿，获得单关节故障的空间机械臂位姿可达空间。根据本发明实施例提供的技术方案，可以实现任一关节故障锁定的空间机械臂位姿可达空间求解。

Description

一种单关节故障空间机械臂位姿可达空间求解方法

【技术领域】

本发明涉及一种单关节故障空间机械臂位姿可达空间求解方法，属于空间机械臂容错控制技术领域。

【背景技术】

随着人类对太空探索的不断深入，具有跨度大、操作灵活、负载能力强等特点的空间机械臂应用日益广泛。但是工作环境的恶劣与关节结构的复杂，使得长期服役的空间机械臂发生关节故障的概率增大。机械臂在关节故障后仍能执行同时考虑末端位置和姿态可达的在轨空载/负载操作，对太空探索任务的高质量完成具有至关重要的作用。因此，开展关节故障的空间机械臂末端位姿可达性的相关研究，对加快人类探索空间进度具有非常大的理论价值和现实意义。

空间机械臂通常基于退化工作空间执行关节故障锁定后的空间操作任务，退化工作空间体积大小受故障关节锁定角度影响，当故障关节锁定至某些特殊角度时，工作空间严重退化导致空间机械臂可能无法完成后续任务。而且，退化工作空间不具有表征空间机械臂末端姿态可达性的特点，这将严重影响空间机械臂执行对末端位置和姿态都有要求的空间操作任务。因此，建立可满足后续任务位置可达性需求，且能够同时表征机械臂末端姿态可达性的工作空间，即机械臂位姿可达空间，对于空间机械臂关节故障后仍能继续完成后续操作任务具有至关重要的现实意义。

【发明内容】

有鉴于此，本发明实施例提供了单关节故障的空间机械臂位姿可达空间分析方法，所求位姿可达空间可为发生关节故障锁定的空间机械臂同时考虑末端位置和姿态可达性的后续操作提供保障。

上述单关节故障空间机械臂位姿可达空间的求解过程中，用到的方法至少包括：

依据故障关节锁定角度与空间机械臂退化工作空间体积之间的对应关系，以保证退化工作空间满足后续任务需求为目标，基于牛顿-拉夫逊法解出每一关节的人为限位，即在机械臂处于常态时人为限定关节转动范围，基于各关节所求人为限位范围，通过蒙特卡洛法求得单关节故障后空间机械臂的退化工作空间；

依据末端处于不同姿态时机械臂可达空间不同这一特点，构建姿态可达度指标，针对空间机械臂具体操作任务所对应的末端姿态变化范围，基于姿态可达度指标求出其中不同姿态所对应的可达空间体积，选取最大可达空间对应的姿态作为空间机械臂面向任务的期望姿态；

依据所求退化工作空间中各散点位置坐标和期望姿态的组合，构建空间机械臂末端位姿集，基于解析法筛选位姿集中存在逆运动学解的位姿，由这些位姿所对应的散点组成机械臂位姿可达空间，以实现单关节故障空间机械臂的位姿可达空间求解。

上述求解过程中，计算单关节故障空间机械臂退化工作空间的过程包括：

第一步，基于D-H参数法建立n自由度空间机械臂运动学模型，无故障状态下，令空间机械臂从基座到末端的各关节依次表示为J₁～J_n，各关节对应关节角度依次表示为θ₁～θ_n，各连杆依次表示为L₁～L_n；定义为连杆i-1和i之间的位姿转换矩阵，则机械臂正运动学方程可表示为：

当空间机械臂关节k发生故障并锁定在θ_k角度时，定义关节k后面各关节及各连杆编号在原有的基础上减1，并增加上标“～”；将连杆L_k-1与L_k视为一个连杆，即基于常态下空间机械臂正运动学方程可得单关节故障的空间机械臂正运动学方程为：

上式中，

第二步，基于蒙特卡洛法求解各关节在对应物理限位范围内转动时的空间机械臂工作空间W：

上式中，Q表示n自由度空间机械臂常态下的关节角序列集合；

设空间机械臂各关节人为限位为其中，和分别表示关节i的人为限位上下限，基于蒙特卡洛法及工作空间W的求解公式，可得各关节被人为限位时空间机械臂工作空间

当关节k发生故障并锁定在θ_k角度时，空间机械臂正常关节的人为限位将会被释放至对应物理限位，即基于蒙特卡洛法及退化机械臂正运动学方程可得空间机械臂退化工作空间

基于工作空间体积权衡阈值ζ，建立空间机械臂退化工作空间与常态下工作空间之间的权衡依据如下：

上式中，表示空间机械臂退化工作空间的体积，表示常态下空间机械臂的工作空间的体积；

第三步，基于牛顿-拉夫逊法求解空间机械臂各关节的人为限位，具体求解步骤如下：

(1)令迭代次数j＝1，取空间机械臂关节k以外所有关节的人为限位为对应物理限位，即转至步骤(2)；

(2)基于工作空间求解公式求解各关节被人为限位时的空间机械臂工作空间令关节k故障锁定在并使以Δq为步长依次遍历关节k的转动范围[-π,π]，基于退化工作空间求解公式求解关节k锁定在不同角度时所对应的空间机械臂退化工作空间转至步骤(3)；

(3)基于所求退化工作空间筛选[-π,π]范围内满足退化工作空间与常态下工作空间权衡依据的关节角度区间，令其中最大区间为关节k的人为限位，转至步骤(4)；

(4)重复步骤(1)至(3)，求解空间机械臂所有关节的人为限位完成空间机械臂各关节人为限位的单次求解，转至步骤(5)；

(5)基于所求各关节的人为限位重新计算空间机械臂工作空间转至步骤(6)；

(6)若j＝1，则重复步骤(2)至(5)对机械臂各关节进行人为限位再求解；若j≤j_max(j_max为迭代次数上限)且相邻两次求解所得各关节人为限位上下限的误差满足下式：

上式中，υ表示最大允许误差；则输出此时对应的完成空间机械臂各关节人为限位求解；若j＝j_max时所求各关节人为限位仍不满足式(7)，则修改ζ和υ等阈值并重复步骤(1)至(6)，直至迭代次数j≤j_max时所求结果满足式(7)为止；

在完成空间机械臂各关节人为限位求解的基础上，当某关节故障锁定时，基于退化工作空间求解公式，即可求得满足后续任务位置可达性要求且体积不小于的空间机械臂退化工作空间

上述求解过程中，计算空间机械臂面向任务的期望姿态的过程包括：

第一步，基于末端姿态不同对应空间机械臂末端可达散点占空间散点总数的比值不同，进行空间机械臂姿态可达度指标的构造；

基于蒙特卡洛法随机遍历N组关节角序列可形成空间机械臂末端可达位置集合P：

P＝{(x_n,y_n,z_n)|n＝1,2,...,N} (8)

令欧拉角(α,β,γ)表示空间机械臂末端相对于基坐标系的姿态，任一姿态(α,β,γ)同位置集P中各位置向量(x_n,y_n,z_n)进行组合，可构成包含N个位姿的集合P_E：

P_E＝{(x_n,y_n,z_n,α,β,γ)|n＝1,2,...,N} (9)

基于解析法计算空间机械臂分别以位姿集P_E中各元素为末端位姿所对应的逆运动学解，即令机械臂末端处于P_E中某一位姿，然后求解此时对应的机械臂关节角序列，设可获得M个有逆解的位姿；定义具有逆解的位姿个数M同总位姿个数N的比值为姿态(α,β,γ)所对应的姿态可达度R：

R＝M/N (10)

第二步，基于空间机械臂后续任务要求末端姿态α,β,γ取值范围分别为α∈[α_min,α_max]，β∈[β_min,β_max]，γ∈[γ_min,γ_max]，以为步长依次遍历α,β,γ的取值范围，可获得空间机械臂末端的可达姿态集E：

上式中，η_β,η_γ取值类似于η_α；可达姿态集E中包含元素总个数可表示为：

令可达姿态集E中任一姿态同空间机械臂可达位置集P中各位置向量(x_n,y_n,z_n)进行组合，即可形成姿态所对应的包含N个位姿的空间机械臂位姿集基于姿态可达度指标计算公式求解姿态所对应的姿态可达度R_η；

第三步，基于任一姿态的姿态可达度R_η的计算方法，求解空间机械臂可达姿态集E中所有姿态所对应的姿态可达度R_η(η＝1,2,...,ε)；基于姿态可达度越大对应空间机械臂末端可达散点个数越多这一特征，选取空间机械臂可达姿态集E中姿态可达度R_η最高的姿态(α_exp,β_exp,γ_exp)，作为空间机械臂面向具体任务的期望姿态。

上述求解过程中，计算单关节故障的空间机械臂位姿可达空间的过程包括：

第一步，基于满足后续任务位置可达性要求且体积不小于的空间机械臂退化工作空间中各散点位置坐标(x_n,y_n,z_n)同所求期望姿态(α_exp,β_exp,γ_exp)的组合，构建面向任务的空间机械臂末端位姿集P_{E_ini}：

P_{E_ini}＝{(x_n,y_n,z_n,α_exp,β_exp,γ_exp)|n＝1,2,...,N}(13)

第二步，基于解析法求解空间机械臂末端处于位姿集P_{E_ini}中不同位姿的逆运动学解，选取其中存在逆运动学解的位姿组成优选位姿集P_{E_opt}，基于P_{E_opt}中各位姿所对应的散点即可组成满足后续任务位置和姿态可达性要求的空间机械臂位姿可达空间W_PE。

本发明实施例的技术方案具有以下有益效果：

(1)本发明基于牛顿-拉夫逊法进行空间机械臂各关节人为限位的求解，实现了空间机械臂满足后续任务要求的退化工作空间建立，解决了机械臂关节人为限位求解过程中所存在的耦合问题和关节故障可能导致机械臂工作空间退化严重的问题；

(2)本发明依据末端处于不同姿态时机械臂可达空间不同这一特点，构造了针对空间机械臂的姿态可达度指标，通过判断不同姿态所对应的姿态可达度指标求得空间机械臂面向任务的期望姿态，解决了空间机械臂末端姿态可达情况的量化问题与面向任务期望姿态的求解问题；

(3)本发明提出了单关节故障的空间机械臂位姿可达空间的概念，通过组合退化工作空间中各散点位置坐标和期望姿态，实现了空间机械臂位姿可达空间的求解，解决了任一关节故障锁定后空间机械臂可达位姿退化情况的量化问题；

根据本发明的优点，能够利用期望姿态与基于关节人为限位求得的退化工作空间的组合，获得空间机械臂任一关节故障锁定后的位姿可达空间，该技术可应用于对位置和姿态都有要求的空间机械臂轨迹搜索过程。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施方式所提供的单关节故障空间机械臂位姿可达空间分析方法的流程示意图；

图2是本发明实施例中研究对象的运动学模型图；

图3是本发明实施例中关节2故障锁定角度与空间机械臂退化工作空间体积的对应关系图；

图4是本发明实施例中关节4故障锁定角度与空间机械臂退化工作空间体积的对应关系图；

图5是本发明实施例中空间机械臂姿态可达度的三维表示图；

图6是本发明实施例中单关节故障的空间机械臂位姿可达空间图。

【具体实施方式】

1、建立七自由度空间机械臂的运动学模型如图2所示，其对应D-H参数如表1所示。

表1七自由度空间机械臂初始构型对应D-H参数

2、基于牛顿-拉夫逊法求解空间机械臂各关节人为限位，令υ＝1°，取与的权衡阈值ζ＝40％；设空间机械臂各关节初始人为限位为对应的物理限位则基于蒙特卡洛法通过生成N＝200000个空间散点，可求得各关节分别锁定在不同角度时空间机械臂的退化工作空间体积进而通过式(6)可完成各关节人为限位的初次求解，基于初次求解结果继续迭代求解直至υ值满足要求为止，求解结果如表2所示。

表2七自由度空间机械臂关节人为限位求解结果

基于表2可知空间机械臂各关节人为限位求解结果如表3所示。

表3空间机械臂各关节人为限位求解结果

由表3可知空间机械臂的关节2和关节4需被人为限位，且基于蒙特卡洛法可求得关节人为限位的空间机械臂工作空间体积分别求解关节2和关节4故障锁定在不同角度时空间机械臂退化工作空间体积求解结果如图3和图4所示。

由图3可知，当关节2故障锁定在人为限位上限79°(或下限-79°)时，对应退化工作空间体积空间机械臂未进行关节人为限位时关节2故障锁定后对应的最小退化工作空间体积由图4可知，当关节4故障锁定在人为限位上限99°(或下限-99°)时，对应的退化工作空间体积空间机械臂未进行关节人为限位时关节4故障锁定后对应的最小退化工作空间体积

综上所述可知各关节未进行人为限位时，空间机械臂退化工作空间体积的最小值为对应关节4故障锁定在180°(或-180°)；关节进行人为限位时，退化工作空间体积的最小值为对应关节4故障锁定在99°(或-99°)。

3、设七自由度空间机械臂的关节4故障并锁定在θ₄＝30°，则其余正常关节的人为限位将释放至对应物理限位。利用欧拉角α,β,γ表示机械臂末端姿态，设α,β,γ取值范围为(-180°,180°)，考虑到步长越小会导致计算量越大，故以为步长分别遍历欧拉角α,β,γ的取值范围，总共可得1331个末端姿态。令每个姿态同机械臂退化工作空间中200000个散点的位置坐标进行组合，以所得组合为机械臂末端位姿并进行逆运动学求解，可获得每个姿态的姿态可达度R_η，如图5所示，其中各散点表示各姿态，散点颜色的深浅表示对应姿态可达度R_η的高低，颜色越深(浅)表示姿态可达度越高(低)。设空间机械臂关节故障后的操作任务要求末端姿态α,β,γ的取值范围为[100°,150°；-50°,50°；0°,60°]，基于图5可求得空间机械臂面向该任务的期望姿态为(144°,0°,36°)。

4、基于所求期望姿态(144°,0°,36)结合空间散点位置坐标，可构造包含200000个位姿的空间机械臂末端位姿集，对末端处于该位姿集中不同位姿时的空间机械臂进行逆运动学求解，将具有逆解的位姿所对应的散点进行组合，即可获得如图6所示的单关节故障空间机械臂位姿可达空间，其中，黑色散点表示位姿可达散点，灰色散点表示位姿不可达散点。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。

Claims (4)

1.一种单关节故障空间机械臂位姿可达空间求解方法，其特征在于，该方法包括以下步骤：

(1)基于牛顿-拉夫逊法求解空间机械臂各关节人为限位，当机械臂任一关节故障锁定后，释放其他关节的人为限位至物理限位，通过蒙特卡洛法求解单关节故障后空间机械臂的退化工作空间；

(2)依据末端处于不同姿态时机械臂可达空间不同这一特点，构建空间机械臂姿态可达度指标，求解空间机械臂末端处于不同姿态时的姿态可达度，选取最大姿态可达度对应的姿态作为空间机械臂面向任务的期望姿态；

(3)依据所求退化工作空间中各散点位置坐标和期望姿态的组合，构建空间机械臂末端位姿集，基于解析法筛选位姿集中存在逆运动学解的位姿，由这些位姿所对应的散点组成空间机械臂位姿可达空间。

2.根据权利要求1所述的方法，其特征在于，所述单关节故障空间机械臂的退化工作空间的求解至少包括：

(1)基于工作空间体积权衡阈值ζ，建立空间机械臂退化工作空间与常态下工作空间之间的权衡依据

(2)设空间机械臂各关节的初始人为限位为对应的物理限位依次判断各关节锁定在不同角度是空间机械臂退化工作空间是否满足步骤(1)中的权衡依据，完成空间机械臂各关节人为限位的初次求解；

(3)在完成各关节人为限位的初次求解之后，基于牛顿-拉夫逊法迭代求解各关节人为限位，直至相邻两次所求各关节人为限位之差满足要求为止；

(4)将常态下空间机械臂的各关节转动范围限制在所求人为限位内，当某关节故障锁定后，释放其余关节转动范围至对应物理限位，基于退化工作空间求解公式，完成体积不小于的空间机械臂退化工作空间的求解。

3.根据权利要求1所述的方法，其特征在于，所述空间机械臂面向任务的期望姿态的计算至少包括：

(1)基于蒙特卡洛法随机遍历N组关节角序列，获得包含N个元素的位置集合P＝{(x_n,y_n,z_n)|n＝1,2,...,N}，令集合P中各位置与欧拉角(α,β,γ)组合以获得空间机械臂末端位姿集合P_E＝{(x_n,y_n,z_n,α,β,γ)|n＝1,2,...,N}；

(2)对末端分别处于位姿集合P_E中各位姿处时的机械臂进行逆运动学求解，定义存在逆解的位姿个数M同总位姿个数N的比值为姿态(α,β,γ)所对应的姿态可达度R；

(3)以为步长依次遍历末端位姿α,β,γ的取值范围，获得空间机械臂可达姿态集

(4)求解空间机械臂可达姿态集E中所有姿态所对应的姿态可达度R_η(η＝1,2,...,ε)；

(5)选取最高位姿可达度R_η所对应的姿态(α_exp,β_exp,γ_exp)，作为空间机械臂面向具体任务的期望姿态。

4.根据权利要求1所述的方法，其特征在于，所述单关节故障的空间机械臂位姿可达空间的求解至少包括：

(1)组合所求退化工作空间中各散点位置坐标(x_n,y_n,z_n)同期望姿态(α_exp,β_exp,γ_exp)，构建面向任务的空间机械臂末端位姿集P_{E_ini}＝{(x_n,y_n,z_n,α_exp,β_exp,γ_exp)|n＝1,2,...,N}；

(2)选取位姿集P_{E_ini}中存在逆运动学解的位姿组成优选位姿集P_{E_opt}，由优选位姿集P_{E_opt}中各位姿所对应的散点组成空间机械臂位姿可达空间