一种冗余自由度机械臂构型奇异快速判决方法技术

本发明专利技术公开了一种冗余自由度机械臂构型奇异快速判决方法，其特征在于，包括如下步骤：针对冗余自由度机械臂，其末端速度与各关节角速度之间存在如下映射关系：；表示末端速度，表示关节角速度，J表示雅可比矩阵；计算所述雅可比矩阵的最大奇异值与最小奇异值，以最大奇异值与最小奇异值的比值为条件数，当条件数大于设定数值时，则判定机械臂构型发生奇异；计算所述雅可比矩阵的条件数的过程中，采用基于广义逆计算中间结果迭代求取最大奇异值和最小奇异值的实时值，达到实时计算条件数的要求。该方法解决了通过奇异值分解求取雅克比矩阵条件数的复杂计算问题，同时加快计算效率。

【技术实现步骤摘要】

本专利技术属于机器人
，涉及一种冗余自由度机械臂的构型奇异快速判决方法。
技术介绍
当n自由度冗余机械臂达到某特定关节形位时，第i个关节产生终端线速度的方向(i＝1，2，…，n)或第i个关节产生终端角速度的方向(i＝l，2，…，n)共面，而在此平面的法线方向(或共直线的垂直方向)不能产生终端速度，则可以说相对于该平面的法线方向(或共直线的垂直方向)上，冗余机械臂产生了奇异。冗余自由度的机械臂有利于回避奇异形位。然而并不是所有的奇异形位都是可以回避的。因此对于奇异构型的分析判决很有必要。目前，许多研究都采用了比较复杂的微分几何或微分拓扑学的方法，且为研究运动奇异的全局性，代数拓扑学也曾作为机械臂奇异分析的研究工具。但是对于空间环境等的特殊应用场合，机械臂的控制器资源极其有限，且对速率有相当要求，文献已有方法均不适合。因此，需要一种可靠且快速的判决算法，其对于空间机械臂的操作安全预防至关重要。
技术实现思路
有鉴于此，本专利技术提供了一种冗余自由度机械臂构型奇异快速判决方法，解决了通过奇异值分解求取雅克比矩阵条件数的复杂计算问题，同时加快计算效率。为了解决上述技术问题，本专利技术是这样实现的：一种冗余自由度机械臂构型奇异快速判决方法，包括如下步骤：步骤一、针对冗余自由度机械臂，其末端速度与各关节角速度之间存在如下映射关系：其中，表示末端速度，表示关节角速度，J表示雅可比矩阵；步骤二、计算所述雅可比矩阵的最大奇异值与最小奇异值，以最大奇异值与最小奇异值的比值为条件数，当条件数大于设定数值时，则判定机械臂构型发生奇异；计算所述雅可比矩阵的条件数的过程中，采用基于广义逆计算中间结果迭代求取最大奇异值和最小奇异值的实时值，达到实时计算条件数的要求。进一步地，设定数值为1000。进一步地，基于广义逆计算中间结果包括A和B两个，其中A＝J·JT，B＝A-1，即A为6×6阶Hermite矩阵，B为A的逆矩阵；则最大奇异值δ0的实时值δ0,k采用如下迭代公式进行求解：δ0,k＝max(A·I0,k)I0,k+1＝A·I0,k/δ0,k其中，I0,k为最大奇异值求解的中间向量，I0,0为[1 1 1 1 1 1]T，k＝0,1,…,N，N为迭代次数。经过N次迭代之后，δ0＝δ0,N-1。最小奇异值δ5的实时值δ5,k采用如下迭代公式进行求解：δ5,k＝max(B·I5,k)I5,k+1＝B·I5,k/δ5,k其中，I5,k为最小奇异值求解的中间向量，I5,0为[1 1 1 1 1 1]T。经过N次迭代之后，δ5＝1/δ5,N-1。则条件数为Cond＝δ0/δ5。有益效果：本专利技术根据单冗余自由度机械臂的特殊性质，将其雅克比矩阵的广义逆计算公式分为两部分描述；再利用迭代法求取矩阵的最大奇异值和最小奇异值。解决了通过奇异值分解求取雅克比矩阵条件数的复杂计算问题，同时加快了计算效率，降低对控制器硬件(FPGA等处理芯片)的资源需求，有助于提高任务完成可靠性。附图说明图1不同迭代次数下最大条件数相对误差；图2不同迭代次数下最大条件数相对误差及拟合值；图3条件数理论值与计算值比较(迭代次数50)；图4条件数相对误差；图5本方法流程图。具体实施方式下面结合附图并举实施例，对本专利技术进行详细描述。在机械臂运动学计算过程中，其末端速度与各关节角速度之间存在一定的映射关系， x · = J · q · ]]>式中，表示末端速度，表示关节角速度，J表示雅可比矩阵。在判断机械臂构型奇异时，通常计算雅可比矩阵的条件数，当条件数趋于无穷大时，表示机械臂构型发生奇异。本实施例中具体设置为条件数大于100时即为无穷大。因此，判断构型奇异的关键在于计算雅可比矩阵的条件数。计算过程中，采用基于广义逆计算中间结果迭代求取最大和最小奇异值，达到实时计算条件数的要求。雅可比矩阵为6×7矩阵为J，其广义逆矩阵为J+。在广义逆计算过程中，有两个中间变量(1)A＝J·JT；(2)B＝A-1。本算法中利用这两个中间变量进行条件数的计算。其中，A为6×6阶Hermite矩阵，B为A的逆。最大奇异值δ0求解迭代公式：Vk＝A·I0,kδ0,k＝max(Vk)I0,k+1＝Vk/δ0,k其中，I0,0为初始向量[1 1 1 1 1 1]T，k＝0,1,…,N。N为迭代次数。经过N次迭代之后，δ0＝δ0,N-1。最小奇异值δ5求解迭代公式：Vk＝B·I5,kδ5,k＝max(Vk)I5,k+1＝Vk/δ5,k其中，I5,0为初始向量[1 1 1 1 1 1]T，k＝0,1,…,N。N为迭代次数。经过N次迭代之后，δ5＝1/δ5,N-1。按照定义，条件数为Cond＝δ0/δ5。下面结合附图对本专利技术的计算复杂度做进一步说明。奇异判决实时计算过程中，单次迭代浮点计算次数为78次(认为浮点除法是乘加的2倍)，则迭代次数为N时，计算时间为40N微秒。当迭代次数为50时，计算时间为2毫秒。由图1可以看到，迭代次数越多最大相对误差越小。迭代次数22次时，误差一个量级；迭代次数50次时，误差两个量级；迭代次数100次时，误差四个量级。在实际使用过程中，整臂构型奇异时条件数判决阈值应当根据实际测试情况进行设定，在最差情况下可以采用100次迭代进行计算，整个计算4毫秒。因此保证了条件数计算过程的实时性、有效性。图5为本方法流程图。综上所述，以上仅为本专利技术的较佳实施例而已，并非用于限定本专利技术的保护范围。凡在本专利技术的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本专利技术的保护范围之内。本文档来自技高网...

【技术保护点】
一种冗余自由度机械臂构型奇异快速判决方法，其特征在于，包括如下步骤：步骤一、针对冗余自由度机械臂，其末端速度与各关节角速度之间存在如下映射关系：其中，表示末端速度，表示关节角速度，J表示雅可比矩阵；步骤二、计算所述雅可比矩阵的最大奇异值与最小奇异值，以最大奇异值与最小奇异值的比值为条件数，当条件数大于设定数值时，则判定机械臂构型发生奇异；计算所述雅可比矩阵的条件数的过程中，采用基于广义逆计算中间结果迭代求取最大奇异值和最小奇异值的实时值，达到实时计算条件数的要求。

【技术特征摘要】
1.一种冗余自由度机械臂构型奇异快速判决方法，其特征在于，包括如下步骤：步骤一、针对冗余自由度机械臂，其末端速度与各关节角速度之间存在如下映射关系：其中，表示末端速度，表示关节角速度，J表示雅可比矩阵；步骤二、计算所述雅可比矩阵的最大奇异值与最小奇异值，以最大奇异值与最小奇异值的比值为条件数，当条件数大于设定数值时，则判定机械臂构型发生奇异；计算所述雅可比矩阵的条件数的过程中，采用基于广义逆计算中间结果迭代求取最大奇异值和最小奇异值的实时值，达到实时计算条件数的要求。2.如权利要求1所述的一种冗余自由度机械臂构型奇异快速判决方法，其特征在于，所述设定数值为1000。3.如权利要求1所述的一种冗余自由度机械臂构型奇异快速判决方法，其特征在于，所述基于广义逆计算中...

【专利技术属性】
技术研发人员：梁常春，禹霁阳，饶炜，胡成威，唐自新，张晓东，刘鑫，潘冬，
申请(专利权)人：北京空间飞行器总体设计部，
类型：发明
国别省市：北京;11