一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法

摘要

本发明公开了一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法，包括根据机械臂的逆运动学模型和机械臂速度模型构建机械臂的可操纵椭球，其中可操纵椭球的半长轴沿酉矩阵U纵列的方向，且半长轴的长度对应Jacobian矩阵的奇异值，然后沿着可操纵椭球的第m个主轴方向修正末端执行器的路径，建立机械臂的避奇异路径。该方法可用于机械臂的自主避奇异路径规划，同时能够保持其跟踪性能，并且基于该方法能够实现以任务优先级为基础的多任务避奇异路径规划。

权利要求

1.一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，包括以下步骤：步骤S1，根据机械臂的逆运动学模型和机械臂速度模型构建机械臂的可操纵椭球，其中可操纵椭球的半长轴沿酉矩阵U纵列的方向，且半长轴的长度对应机械臂奇异值；通过对构建末端执行器速度 的Jacobian矩阵J的奇异值进行SVD分解，得到确定的最小奇异值σm以及U的m列为J的左奇异向量；

步骤S2，奇异任务的重构；包括沿着可操纵椭球第m个主轴方向的路径，建立机械臂避奇异路径为： 其中 表示给定的末端执行器速度 沿着酉矩阵U矢量um的投影。

2.根据权利要求1所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，所述步骤S1中可操纵椭球为 其中J为机械臂关节速度 到末端执行器速度 的Jacobian矩阵，JT为J的转置， 为 的转置。

3.根据权利要求2所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，所述步骤S2中对机械臂避奇异路径引入根据最小奇异值σm的大小而变化的权重系数αv，得到机械臂避奇异路径为 其中σif是奇异影响区域的预定义阈值，σuf是不安全区域的预定义阈值。

4.根据权利要求3所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，所述步骤S2中对机械臂避奇异路径引入变化系数αh，其中α0为避奇异路径的逃离增益，以便于在 和σm≤σif时进行奇异任务重构，得到机械臂新的避奇异路径为

5.根据权利要求4所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，机械臂在处理2个子任务时，任务变量 作为最高优先级子任务，任务变量作为低优先级子任务，J1和J2分别为两个任务对应的Jacobian矩阵；其运动学逆解为： 其中 代表J2到J1零空间的映射；然后对任务变量 进行奇异任务重构，得到任务变量 的避奇异路径为对任务变量 进行奇异任务重构，得到任务变量的避奇异路径为

6.根据权利要求5所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，机械臂在处理大于2个子任务时，建立如下模型： 其中代表第i个子任务相对于它的高优先级子任务的修正，当矩阵 接近奇异时，执行子任务 并建立其避奇异路径为

7.根据权利要求1所述的提高路径跟踪性能的机械臂自主鲁棒避奇异方法，其特征在于，所述步骤S1中末端执行器速度为 机械臂的运动学逆解为其中 是关节速度 映射到末端机械臂速度 的Jacobian矩阵； 是J的伪逆， 是单位矩阵， 是任意矢量，J的奇异值为判定机械臂是否发生奇异的标准， 表示实数域，其右上标表示实数域的维度。

说明书

一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法

技术领域

[0001] 本发明属于空间机器人路径规划技术领域；涉及一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法。

背景技术

[0002] 当机械臂的末端执行器自主跟踪任务空间的路径时，当在某些构型接近奇异时，任务空间微小的速度变化需要关节空间极大的速度去完成，因此，奇异会导致很大的跟踪偏差，同时对机械臂结构也有很大的伤害。因此，有必要对机械臂建立避奇异轨迹规划。

[0003] 一种处理奇异问题的著名方法是由C.A.Klein等提出的扩展Jacobian方法。扩展Jacobian可以应用于冗余机械臂，确保了闭环的关节空间轨迹对应于闭环的任务空间路径。然而，该方法只能用于冗余机械臂，不能保证扩展Jacobian矩阵逆的存在。对于非冗余机械臂，G.Schreiber等通过将机械臂的逆运动学问题转化为求解约束优化问题，从而实现奇异规避。然而，约束变换不易实现，且该方法不易扩展到一般的奇异性问题中。

[0004] 针对奇异性问题的另一种解决方法是由C.W.Wampler提出的阻尼最小二乘(DLS)法。该方法可容易地应用于冗余和非冗余机械臂，因此被广泛使用。该方法的缺点是引入了较大的路径跟踪误差。为克服这一缺点，在DLS方法中加入了可变阻尼系数和数值滤波。此外，权重DLS和带任务优先级的冗余逆运动学解也用于修正现有的DLS方法。这些方法相比于传统DLS方法显示出更好的性能，但是也面临阻尼系数的调节问题。近年来，G.Marani等提出了一种任务重构的方法来避免运动学和算法奇异性。给定的任务通过几何映射到给定的优化指标函数上进行修正。客观地，任务重构方法有较大的计算负担，尤其是考虑多个子任务时。相似地，C.Qiu等通过对任务空间速度的直接修正来实现冗余和非冗余机械臂的奇异规避，但没有分析该方法在多个子任务中的应用。

[0005] 上述方法虽能够避免奇异问题，但是会降低路径的跟踪性能，同时对于存在多个子任务的情形并未进行分析。为了满足机械臂对于轨迹规划中的避奇异及跟踪性能的要求，需要提出一种新的避奇异方法，同时保持跟踪性能。

发明内容

[0006] 本发明提供了一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法，该方法可用于机械臂避奇异的轨迹规划，同时能够保证其跟踪性能。

[0007] 本发明的技术方案是：一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法，包括以下步骤：

[0008] 步骤S1，根据机械臂的逆运动学模型和机械臂速度模型构建机械臂的可操纵椭球，其中可操纵椭球的半长轴沿酉矩阵U纵列的方向，且半长轴的长度对应机械臂奇异值；通过对构建末端执行器速度 的Jacobian矩阵J的奇异值进行SVD分解，得到确定的最小奇异值σm以及U的m列为J的左奇异向量；

[0009] 步骤S2，奇异任务的重构；包括沿着可操纵椭球第m个主轴方向的路径，建立机械臂避奇异路径为： 其中 表示给定的末端执行器任务 沿着酉矩阵U矢量um的投影。

[0010] 更进一步的，本发明的特点还在于：

[0011] 其中步骤S1中可操纵椭球为 其中J为机械臂关节速度 到末端执行器速度 的Jacobian矩阵，JT为J的转置， 为 的转置。

[0012] 其中步骤S2中对机械臂避奇异路径引入根据最小奇异值σm的大小而变化的权重系数αv，得到机械臂避奇异路径为 其中σuf是奇异影响区域的预定义阈值，σuf是不安全区域的预定义阈值。

[0013] 其中步骤S2中对机械臂避奇异路径引入变化系数αh，其中α0为避奇异路径的逃离增益，以便于在 和σm≤σif时进行奇异任务重构，得到机械臂新的避奇异路径为

[0014] 其中机械臂在处理2个子任务时，任务变量 作为最高优先级子任务，任务变量 作为低优先级子任务，其运动学逆解为：其中 代表J2到J1零空间的映射；然后对任务变量 进行奇异任务重构，得
到任务变量 的避奇异路径为 对任务变量 进
行奇异任务重构，得到任务变量 的避奇异路径为

[0015] 其中机械臂在处理大于2个子任务时，建立如下模型：其中 代表第i个子任务相对于它的高优先级子任务的修正，当矩阵 接近奇异时，执行子任务 并建立其避奇异路径为

[0016] 其中步骤S1中末端执行器速度为 机械臂的运动学逆解为其中 是关节速度 映射到末端机械臂速度 的Jacobian矩阵。 是J的伪
逆， 是单位矩阵， 是任意矢量，J的奇异值为判定机械臂是否发生奇异的标准。

[0017] 与现有技术相比，本发明的有益效果是：基于可操纵椭球概念的奇异任务重构(STR)方法来解决运动学和算法奇异问题。当机械臂接近其奇异构型时，通过映射期望任务到可操纵椭球的半长轴方向，修正后的任务路径能阻止机械臂进入奇异区域，与标准的阻尼最小二乘法相比，保证了路径跟踪性能的改进，该方法实现了机械臂的自主避奇异并保证了机械臂的跟踪性能，同时重构后的任务路径具有鲁棒性。

[0018] 更进一步地，引入参数αv避免了跟踪性能的下降，同时提高了避奇异的能力。

[0019] 更进一步地，引入参数αh能够产生逃逸，从而驱动机械臂离开奇异点。

[0020] 更进一步地，基于单任务的奇异任务重构避奇异方法，还可扩展到以任务优先级为基础的路径跟踪方法，提出了基于两个子任务或两个以上子任务的避奇异路径跟踪方法。

附图说明

[0021] 图1为本发明中奇异任务重构方法用于多个不同优先级子任务的流程图。

[0022] 图2为本发明中实施例1单任务避奇异的仿真结果示意图；

[0023] 图3为本发明中实施例2多个子任务避奇异的仿真结果示意图；

[0024] 图4为本发明中实施例2增加控制增益的仿真结果示意图。

[0025] 图1中：TM为人物变更；IK为逆运动学；STR为奇异任务重构。图2-4中Task in x-y Plane为在x-y平面的任务；Task-Space Error为任务空间误差；Minimum Singular Value为最小奇异值；Joint Velocity为关节速度；Primary Task-Space Error为主任务空间误差；Primary Task Minimum Singular Value为主任务最小奇异值；Secondary Task-sapce Error为次任务空间误差；Secondary Task Minimum Singular Value为次任务最小奇异值。

具体实施方式

[0026] 下面结合附图和具体实施例对本发明的技术方案进一步说明。

[0027] 当机械臂关节构型θ*使Jacobian矩阵不满秩时会导致奇异问题，奇异问题不仅在工作空间的边界发生，也有可能在工作空间中发生。在这种构型下不能确定J(θ*)的伪逆，奇异问题限制了使用逆运动学求解关节速度；此外，在奇异点附近，任务空间微小的速度变化需要关节空间极大的速度去完成，，导致很大的跟踪偏差，同时对机械臂结构也有很大的伤害。

[0028] 本发明提供了一种提高路径跟踪性能的机械臂自主鲁棒避奇异方法，包括以下步骤：

[0029] 步骤S1，构建可操纵椭球；首先获得末端执行器的速度和其逆运动学解分别为：

[0030]

[0031]

[0032] 其中 是关节速度 映射到末端执行器速度 的Jacobian矩阵。是J的伪逆， 是单位矩阵， 是任意矢量，J的奇异值为判定机械臂是否发生奇异的标准；通过对J进行SVD分解确定最小奇异值：

[0033] J＝U∑VT    (3)

[0034]

[0035] 其中U＝[u1,…,um]是m×m酉矩阵，σ1,…,σm是J的奇异值，满足σ1≥σ2≥…≥σm≥0，V＝[v1,…,vn]的转置VT是n×n酉矩阵。U的m列和V的n列分别称作J的左奇异向量和右奇异向量。由于J是从关节空间到任务空间的线性映射，若关节速度 可操纵椭球可定义如下：

[0036]

[0037] 该可操纵椭球的半长轴沿着酉矩阵U纵列的方向，且半长轴的长度对应J的奇异值。

[0038] 步骤S2，奇异任务重构：从公式(5)的可操纵椭球可以看出，当沿第i个主轴方向的σi＝0时出现奇异。J的最小奇异值σm具有特殊的意义，由于它是接近奇异的唯一精确测量，σm描述了任务空间速度和关节空间速度在某一方向上的比例，该方向由左奇异向量um给出。因此，如果修正沿第m个主轴方向的路径，可得到末端执行器避奇异路径如下：

[0039]

[0040] 其中 代表给定的任务 沿着酉矢量um的投影，方程(6)保证了机械臂规避奇异区域。为了提升避奇异的性能，对方程(6)引入权重系数αv，得到新的避奇异路径为：

[0041]

[0042] 其中αv是根据最小奇异值σm的大小而调整的参数。如果σif是奇异影响区域的预定义阈值，σuf是不安全区域的预定义阈值，αv可表示为：

[0043]

[0044] 当σm小于不安全阈值，方程(7)不能保证逃离奇异区域，导致很大的跟踪偏差。为了克服该缺陷，在方程(7)中加入第三项，得到新的避奇异路径为：

[0045]

[0046] 新加入的项将会产生逃逸作用并驱动机械臂脱离奇异点，系数αh定义为：

[0047]

[0048] 其中α0避奇异的逃离增益。事实上，只有当 和σm≤σif时才需要考虑奇异任务重构，因此避奇异路径可重新描述为：

[0049]

[0050] 公式(11)显示，无论机械臂的初始构型是在不安全区域或接近奇异区域，避奇异路径的重新规划能够使系统逃离奇异区域。

[0051] 忽略零空间运动，即z＝0的情况下，公式(2)变为：

[0052]

[0053] 上述过程实现了单任务的奇异任务重构及避奇异路径规划，将该方法扩展到以任务优先级为基础的方法中，能够实现对两个任务或两个以上任务的避奇异路径规划。

[0054] 首先，假设有两个子任务，将任务变量 的作为最高优先级子任务，将任务变量 作为低优先级子任务；其中任务变量和机械臂关节变量之间的运动学关系为：

[0055]

[0056]

[0057] 其中 和 为对应任务变量 的Jacobian矩阵。考虑任务优先级，其逆运动学解为：

[0058]

[0059] 其中 代表J2到J1零空间的映射。使用公式(15)，第二个子任务的执行不会影响到第一个子任务。然而当 不能被确定时，也就是说当第一个子任务 发生运动学奇异，或者 遇到奇异时，将导致奇异的发生。将上述单任务奇异路径规划和递归方法应用到两个子任务的情形中，J1和 的运动学和算法奇异可以自动避免。使用本发明的STR方法，首先需要利用SVD算法获取最小奇异值σm和相应的左奇异矢量um。

[0060] 在公式(15)中，忽略零空间运动，关节空间速度可以分为两部分：

[0061]

[0062] 其中 描述了由于第一任务子空间的存在对第二子空间的修正，第一子任务 可以由 使用STR方法进行重构，通过SVD分解，进行奇异任务重构规划得到避奇异路径：

[0063] J1＝U1ΣV1T    (17)

[0064]

[0065] 其中公式(18)为第一子任务 规划后的避奇异路径，公式(17)通过SVD分解确定最小奇异值σm和相应的左奇异矢量um。

[0066] 对于两个以上的子任务，如图1所示，将单任务的STR方法扩充到具有多个子任务中：

[0067]

[0068] 其中 代表第i个子任务对于它的高优先级子任务的修正；当在第i个子任务的 中检测到奇异时，STR方法将执行子任务 进行奇异任务重构规划得到避奇异路径：

[0069]

[0070]

[0071] 公式(23)为第i个子任务 规划后的避奇异路径，公式(22)通过SVD分解确定最小奇异值σm和相应的左奇异矢量um。

[0072] 相应地，在多个子任务的处理过程中，无论是运动学或者算法的奇异值，均能够通过递归调用STR方法来处理。

[0073] 本发明的具体实施例为：

[0074] 实施例1

[0075] 单任务避奇异：该实施例中期望的任务为跟踪一个在工作空间的圆形，时间为10s。

[0076] 其中α0＝20,σif＝0.05,σuf＝0.02，K＝diag(100,100)作为调节参数，使用本发明的STR方法得到的仿真如图2所示。

[0077] 图2显示了3自由度平面机械臂利用本发明STR方法在任务空间跟踪圆形轨迹的仿真结果。图(a)显示了x-y平面的跟踪情况，图(b)显示了任务空间的跟踪误差，图(c)显示了跟踪过程中最小奇异值的变化曲线，图(d)显示了关节速度的变化曲线。可以看出，如果不使用避奇异策略，在4-5s之间将发生奇异问题，采用STR方法，新生成的路径有效地逃离了奇异区域，并保证了最小奇异值一直处在其不安全区域之外。

[0078] 实施例2

[0079] 多个子任务避奇异：增加第二个任务x2，保持末端执行器的方向为90°，并且验证该方法对于具有任务优先级的子任务的鲁棒性和避奇异性。

[0080] 控制参数为K1＝diag(1.5,1.5),K2＝0.5；得到的仿真结果如图3所示。

[0081] 图3显示了3自由度平面机械臂利用STR方法在任务空间跟踪圆形轨迹并同时保持末端执行器方向的仿真结果。图(a)显示了x-y平面的跟踪情况，图(b)显示了关节速度的变化曲线，图(c)显示了第一任务的跟踪误差，图(d)显示了跟踪过程中第一任务相关的Jacobian矩阵最小奇异值的变化曲线，图(e)显示了第二任务的跟踪误差，图(f)显示了跟踪过程中第二任务相关的Jacobian矩阵最小奇异值的变化曲线。可以看出，采用STR方法，新生成的路径有效地逃离了第一任务和第二任务相关的Jacobian矩阵的奇异区域，并保证了最小奇异值一直处在其不安全区域之外。

[0082] 为了测试本方法的鲁棒性，与上以仿真选取同样的参数，增加控制增益为K1＝diag(4,4),K2＝0.5。得到仿真结果如图4所示。

[0083] 图4显示了3自由度平面机械臂利用STR方法在任务空间跟踪圆形轨迹并同时保持末端执行器方向的仿真结果。图(a)显示了x-y平面的跟踪情况，图(b)显示了关节速度的变化曲线，图(c)显示了第一任务的跟踪误差，图(d)显示了跟踪过程中第一任务相关的Jacobian矩阵最小奇异值的变化曲线，图(e)显示了第二任务的跟踪误差，图(f)显示了跟踪过程中第二任务相关的Jacobian矩阵最小奇异值的变化曲线。可以看出，即便调整为不同的控制增益，采用STR方法，新生成的路径仍可以有效地逃离第一任务和第二任务相关的Jacobian矩阵的奇异区域，并保证了最小奇异值一直处在其不安全区域之外。

[0084] STR方法相较于DLS方法，其跟踪误差更小，且对于最小奇异值的控制更为有效。对于多任务情形，STR方法的关节速度更加平滑，此外，第一任务的跟踪偏差也更小。通过仿真，可以表明，STR方法具有有效性和鲁棒性。

[0085] 本发明分别针对单任务和多个子任务进行了仿真，对于多个子任务选取不同的控制增益，仿真结果表明了所提出方法的鲁棒性。对于多任务，STR方法的关节速度更加平滑，此外，第一个任务空间的偏差也更小。通过仿真，可以表明，STR方法具有有效性和鲁棒性。