CN106737689B - 基于模式函数的超冗余机械臂混合逆向求解方法及系统 - Google Patents

Abstract

本发明公开了一种基于模式函数的超冗余机械臂混合逆向求解方法，包括：系统求得空间脊线；利用机械臂末端点与脊线末端点重合，求得末端万向节节点U_2N在空间的位置；系统根据求得末端万向节节点位置，确定点U_2N‑1位置；基于双段连杆长，依次确定各奇数万向节的节点位置；确定各臂型角，并求得各偶数万向节点位置；求解各关节的角度。本发明提供一种基于模式函数的超冗余机械臂混合逆向求解系统。首先通过关节点与脊线匹配的方式给定有限的约束条件，然后基于奇数万向节点与偶数万向节点，根据附加任务优化特定的臂型角参数值以达到求解关节角度，实现超冗余机械臂的空间合理规划的目标，可广泛应用于超冗余机器人的逆运动学领域。

Description

基于模式函数的超冗余机械臂混合逆向求解方法及系统

技术领域

本发明涉及超冗余机器人的逆运动学领域，具体为一种基于模式函数的超冗余机械臂混合逆向求解方法及系统。

背景技术

超冗余机械臂具有极强的灵活性，除了执行空间常规任务外，还特别适用于狭小空间开展作业，逐渐被应用于航天、核电站、医疗等诸多领域。但由于大量自由度的存在，该类机械臂的运动学逆解变得十分复杂，因此运动逆解及规划一直以来都是超冗余度机器人运动学研究中的一个重点和难点，然而超冗余机械臂的逆运动学求解却是运动规划的必要环节。通常冗余机械臂的逆运动学求解方法可以分为三类：(1)广义逆法(2)人工神经网络法(3)几何法。然而随着自由度的大量增加其运动学逆解及路径规划问题也变得更加复杂。人工神经网络法被广泛应用于工业机械臂的逆解，但是所需要训练集的大小会随着自由度数目的变化而显著变化，并且由于学习过程的延长可能无法满足实时性的要求。Chirikjian针对超冗余机械臂的运动学规划提出了脊线法的概念，奠定了超冗余机器人运动学的理论基础，他采用脊线法来描述超冗余机械臂的宏观构型，得到了许多重要的结论。在该方法中，脊线被定义为分段连续的曲线，用该曲线来表达超冗余机械臂的宏观几何特征。脊线被一系列本征的模式函数所拟合，这些模式函数可以根据需要任意选择并且可以在位置级得出有效的运动学逆解。一旦模式脊线针对假定的末端效应器位置所确定，就可以确定各种各样的拟合算法，求出机械臂在脊线上的关节点。Fahimi扩展了模式函数法在三维空间超冗余机械臂的应用，引入了新的模式函数扩展了工作空间，并采用了针对万向节关节构型的递归拟合算法求解每个连杆的非线性代数方程，从而避免了同时求大量非线性方程的弊端。为了降低超冗余机械臂大量自由度引起的高计算量问题，因此该技术有必要进行改进。

发明内容

为了解决上述技术问题，本发明的目的是提供一种基于模式函数的超冗余机械臂混合逆向求解方法及系统。

本发明所采用的技术方案是：

本发明提供一种基于模式函数的超冗余机械臂混合逆向求解方法，包括以下步骤；

系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

系统利用机械臂末端点与脊线末端点重合，并通过末段连杆匹配机械臂期望指向，进而求得末端万向节节点U_2N在空间的位置U_2N；

系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

系统根据所求得的各万向节节点位置，进而求解各关节的角度。

作为该技术方案的改进，所述空间脊线可表示为：

其中，u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)]，u(σ)是在σ处正切于曲线的单位向量；所述s∈[0,1]，其表示脊线的长度参数；l是脊线的实际长度。

作为该技术方案的改进，其还包括系统根据机械臂末端欧拉角α,β,γ求得末端空间方向向量k：

其中，O₀为坐标原点，T为机械臂末端点，L表示连杆长度。

作为该技术方案的改进，所述步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置，其包括：

其中，表示第(2N)个万向节(x_2N,y_2N,z_2N)构成的矢量，表示第(2N-1)个万向节(x_2N-1,y_2N-1,z_2N-1)构成的矢量；

表示第(2N-1)个连杆构成的矢量,且

作为该技术方案的改进，所述万向节节点U_2N-1在脊线上的空间位置可表示为：

其中，f(s_k)＝0，该式中s_k表示脊线起点到当前点的弧长，且s_k∈[0,1]。

作为该技术方案的改进，所述步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置，其包括：

其中，ρ_2i-1为等效连杆的长度，其表示第2i-1个万向节点与第2i+1个万向节点之间的等效臂长。

进一步地，所述步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置，其可表示为：

其中，在以为圆心且为半径的圆上，且与向量的夹角为臂型角ψ_2i-1。

进一步地，所述步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度，其包括俯仰-偏航型万向节求解和偏航-俯仰型万向节求解。

另一方面，本发明还提供一种基于模式函数的超冗余机械臂混合逆向求解系统，包括：

第一模块，用于执行步骤系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

第二模块，用于执行步骤系统利用机械臂末端点与脊线末端点重合，并通过末段连杆匹配机械臂期望指向，进而求得末端万向节节点U_2N在空间的位置U_2N；

第三模块，用于执行步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

第四模块，用于执行步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

第五模块，用于执行步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

第六模块，用于执行步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度。

本发明的有益效果是：本发明提供的基于模式函数的超冗余机械臂混合逆向求解方法及系统，该方案统筹考虑了超冗余机械臂的可解性与灵活性，首先通过关节点与脊线匹配的方式给定有限的约束条件，然后基于奇数万向节点与偶数万向节点，根据附加任务优化特定的臂型角参数值以达到求解关节角度，实现超冗余机械臂的空间合理规划的目标。

该混合逆向拟合法用独立的末端万向节匹配效应器的方向向量，不仅保证位置准确可达而且可以保证末端效应器的指向；其余关节优先通过双段拟合规则拟合到脊线上，这样既能保证超冗余机械臂在宏观构型上满足脊线要求，又将双段4DOF相对空间位置的一个冗余自由度转化为可直观调节的臂型角，在合理有效逆解的同时进一步释放了超冗余机械臂的冗余特性。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1是本发明一实施例的DH坐标系示意图；

图2是本发明第二实施例的整体原理图；

图3是本发明第三实施例的脊线整体构型及位置向量示意图；

图4是本发明第四实施例的位置求解原理示意图；

图5是本发明第五实施例的两段机械臂长度与转角关系示意图；

图6是本发明第六实施例的参考面臂型角示意图；

图7是本发明第七实施例的求各关节角度的示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

参照图1-7，是本发明实施例的示意图。本发明提供一种基于模式函数的超冗余机械臂混合逆向求解方法，包括以下步骤；

系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

系统利用机械臂末端点与脊线末端点重合，并通过末段连杆匹配机械臂期望指向，进而求得末端万向节节点U_2N在空间的位置U_2N；

系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

系统根据所求得的各万向节节点位置，进而求解各关节的角度。

作为该技术方案的改进，所述空间脊线可表示为：

其中，u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)]，u(σ)是在σ处正切于曲线的单位向量；所述s∈[0,1]，其表示脊线的长度参数；l是脊线的实际长度。

作为该技术方案的改进，其还包括系统根据机械臂末端欧拉角α,β,γ求得末端空间方向向量k：

其中，O₀为坐标原点，T为机械臂末端点，L表示连杆长度。

作为该技术方案的改进，所述步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置，其包括：

其中，表示第(2N)个万向节(x_2N,y_2N,z_2N)构成的矢量，表示第(2N-1)个万向节(x_2N-1,y_2N-1,z_2N-1)构成的矢量；表示第(2N-1)个连杆构成的矢量,且

作为该技术方案的改进，所述万向节节点U_2N-1在脊线上的空间位置可表示为：

其中，f(s_k)＝0，该式中s_k表示脊线起点到当前点的弧长，且s_k∈[0,1]。作为该技术方案的改进，所述步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置，其包括：

其中，ρ_2i-1为等效连杆的长度，其表示第2i-1个万向节点与第2i+1个万向节点之间的等效臂长。

进一步地，所述步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置，其可表示为：

其中，在以为圆心且为半径的圆上，且与向量的夹角为臂型角ψ_2i-1。

进一步地，所述步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度，其包括俯仰-偏航型万向节求解和偏航-俯仰型万向节求解。

为了解决由于超冗余的自由度存在，机械臂的逆解是无穷多的，本算法基于常规的期望约束(末端位置，工具指向、避障要求、避关节超限、穿越狭小空间等)提出的求解超冗余机械臂的方法，主要步骤如下：

第一步：求宏观脊线。根据期望的末端位置及中间的构型要求选定模式函数，通过对模式函数的数值积分求得脊线在空间的整体形态。

第二步：匹配末端方向向量。此时脊线的末端点也正是期望的位置，原则上可以采用脊线末端点的切线方向作为机械臂的指向，但由于臂杆有一定长度，当拟合到脊线上后，末端方向向量与脊线的切向向量并不相同，因此基于末端点引入末端方向向量k控制末端臂杆指向,求得U_2N点在空间位置，此时向量与方向向量k共线。

第三步：单段拟合到脊线。由于脊线有一定弧度，通常情况下以U_2N点为起点的在匹配方向向量后，U_2N点难以落在脊线上，本方案通过单段臂杆的逆向拟合将U_2N-1点拟合到脊线上。

第四步：双段拟合到脊线上求奇数节点位置。U₁到U_2N-2之间奇数万向节的节点通过双段逆向拟合的方法拟合到脊线上。

第五步：臂型角求偶数节点位置。当奇数节点确定后，可以默认设置臂型角或根据空间位置要求确定臂型角后，求解偶数万向节节点(U₂,U₄,U₆…U_2N-2)的位置。

第六步：求各关节角度。当所有万向节位置确定后，根据具体关节构型(Yaw-Pitch型，Pitch-Yaw型)求解组成万向节关节的角度。

(1)基于模式函数的整体构型

由于离散的分段机械臂有较多的连杆组成，这样它在空间的构型可以近似的由空间脊线来模拟，这条脊线可以认为是一条分段连续曲线——模式脊线，模式脊线法的基本原理是微分几何学，用它来描述超冗余机械臂的宏观几何特征。通过建立超冗余机械臂基于模式脊线的运动学模型，从而使的复杂的超冗余机械臂规划问题转化为模式脊线的运动学问题。基于模式脊线的空间宏观构型从而求得超冗余机械臂的各个关节的运动学解，因此可以方便有效的解决超冗余度机器人逆解问题，为空间超冗余度机器人的运动学规划提供了理论基础。模式脊线法的关键在于求得匹配脊线构型的模式参数，使得模式脊线合理表达超冗余度机器人的主要特征，同时，通过修正该模式函数法使其在起始点和末端点能够在切线方向，保证了该脊线是一条平滑的曲线，并且具有更合理的工作空间。

空间脊线上的任意点可以表示成末端在曲线上的空间位置向量，

u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)] (1)

因此空间脊线可以表达如下：

其中s∈[0,1]是曲线的长度参数，u(σ)是在σ处正切于曲线的单位向量，l是曲线的实际长度。

本方案选择三个模式函数来定义曲线参数：

φ(s)＝a₁sin(2πs)+a₂(1-cos(2πs))+b_1φ(1-sin(πs/2))+b_2φ(sin(πs/2)) (3)

ψ(s)＝a₃(1-cos(2πs))+b_1ψ(1-sin(πs/2))+b_2ψ(sin(πs/2)) (4)

其中b_1φ＝φ(0),b_1ψ＝ψ(0),b_2φ＝φ(1),b_2ψ＝ψ(1)分别与起始点和末端点的方位角子午角相关，表征了起始点和终止点的方向。公式(2)可表示为：

函数φ(s)和ψ(s)可以表达成如下模式函数的线性组合：

其中：f_i(s)是模式函数，a_i是模式协同参数，n₁是针对φ(s)模式函数的数目，g_i(s)以及b_iφ，b_iψ用来确定脊线起点和终点的方向。该模式函数法将逆运动学问题转化为求解满足任务要求的a_i的问题。参照图2-3，例如，x(1)＝x(T)，其中x(T)是脊线末端点的期望位置向量。

将式子(3)、(4)分别代入(6)、(7)，再统一代入式(5)，当s＝1时可以得到脊线的正运动学方程：

x(1)＝x(T) (8)

其中x(T)是脊线末端点的期望位置向量，式(8)可以根据模式协同参数a_i的值通过数值解得方式求解。

通过式(9)的迭代近似解可以求得模式协同参数向量：

其中α是控制收敛速度的常数，m表示迭代次数，3×3的模型雅克比矩阵J_a(a,s)取s＝1时的值。通过(3)、(4)、(5)可以求得x(1)各元素的偏微分，利用数值积分计算具体值；式(9)的数值解与机械臂关节的数目无关。

在求得空间脊线后，本方案提出了混合逆向拟合法将完全解放末端姿态，使得超冗余机械臂的末端完全匹配期望的空间位置及指向后，再将超冗余机械臂的节点以单段或双段逆向的方式拟合到空间脊线上。单段逆向拟合能保证超冗余机械臂的万向节拟合到脊线上，双段逆向拟合方法通过定义臂型角使得超冗余机械臂在空间运动状态，不再完全受限于空间脊线，而是在全局位置符合脊线位形的情况下，有了调整局部姿态的能力，既实现了末端效应器工作空间内任意位置及指向可达，又能够保证超冗余机械臂在符合期望脊线构型的情况下具备局部调整姿态的能力。

(2)方向向量拟合

k是根据末端欧拉角α,β,γ求得的空间方向向量：

(3)单段逆向拟合

参照图4，设定超冗余机械臂腕部末端与模式脊线的末端重合；在拟合方向向量k得到U_2N后为了保证臂杆长度，将递归计算脊线上的点使得临近关节点之间的空间距离满足杆长关系：

式(11)中

——第(2N)^th个万向节(x_2N,y_2N,z_2N)；

——第(2N-1)^th个万向节(x_2N-1,y_2N-1,z_2N-1)；

——第(2N-1)^th个连杆,

以上公式中，关节点的位置都是脊线弧长s_k的函数，保证点在空间脊线上，在区间[0,1]内搜索，分别以式f(s_k)＝0为判断条件，可以有效的得到U_2N-1关节点在脊线上的空间位置。

(4)双段逆向拟合

区别于传统的单段逆向拟合，本方案将其余关节采用双段逆向拟合的方式求解超冗余机械臂的逆解。传统单段逆向拟合可以保证很好的匹配空间脊线的形态，但由于脊线是通过模式函数定性确定的空间构型，往往难以满足实际工况需求，也因此受限于脊线的约束；双段逆向拟合的方法进一步将超冗余机械臂的冗余性与直观可控的参数(双段臂杆长度、臂型角)关联，相对于传统的单段逆向拟合法，在结合实际工况及机械臂构型及关节角度可解可控的情况下，更充分的利用了超冗余机械臂的冗余性。

由于万向节关节1关节2的运动不改变两节机械臂的起点和终点连线长度，仅改变终点在空间中的位置，因此假定θ₁＝θ₂＝0进行分析，参照图5所示，两节机械臂长度ρ仅与关节角θ₃,θ₄的变化有关；

其中，s_i＝sin(θ_i),c_i＝cos(θ_i)。

可得：

min(2L²(1+c₃c₄))≤ρ²≤4L² (15)

当θ₃＝θ₄＝90°时

1.42L≤ρ≤2.00L (16)

以n DOF超冗余机械臂为例，分为2N-1位置段，选定位置参数ρ_2i-1作为位置段长度调节参数。默认设定ρ₁＝...＝ρ_2N-2＝1.7L，将ρ_2N-1作为待确定值，当ρ_2N-1满足(16)时，则可以进行整体位置逆解。

设定超冗余机械臂的末端T与模式脊线的末端重合；为了满足4DOF等效关节臂杆长度，将递归计算脊线上的点使得临近关节点之间的空间距离满足杆长关系：

式(17)中

——第(2i+1)^th万向节构成的矢量，坐标为(x_2i+1,y_2i+1,z_2i+1)；

——第(2i-1)^th万向节构成的矢量，坐标为(x_2i-1,y_2i-1,z_2i-1)；

——第(2i-1)^th等效臂杆i.e.长度为

以上公式中，关节点的位置都是脊线弧长s_k的函数，保证点在空间脊线上，通过逐步扫描法和二分法在区间[0,1]内搜索，以式f(s_k)＝0为判断条件，可以有效的得到各个关节点的空间位置。

通过以上公式可以求得ρ_2i-1各万向节节节点在模式脊线上的空间位置；当逆向拟合到U₁～U₃万向节节点时需要对U₃节点的合理性进行判断：

当ρ_2N-1≥2L时，说明构型的实际空间结构的最大长度不能满足U₃的位置需求，因此需要调节前两段长度，将ρ₁＝ρ₁+△,...ρ_2N-2＝ρ_2N-2+△(△是一个可调节参数)后再次进行逆解计算，直到|ρ_2N-1-1.7L|≤δ(δ是一个小的阈值)。

当ρ_2N-1≤1.42L时，说明构型的实际空间结构的最小长度不能满足U₃位置需求，因此需要调节前两段长度，将ρ₁＝ρ₁-△,...ρ_2N-2＝ρ_2N-2-△(△是一个可调节参数)后再次进行逆解计算，直到|ρ_2N-1-1.7L|≤δ(δ是一个小的阈值)。

当确定合理的位置参数ρ_2i-1后，可以通过位置拟合，求得等效臂杆段节点的空间坐标。

(5)基于臂型角求解姿态

当确定机械臂分节点的位置后可以采用本方案提出的臂型角参数化求解各个关节的值，因为臂型角是可以调节的变量，因此可以有针对性的实现局部的姿态调整，例如通过调整局部姿态实现狭小空间的穿越或者实现障碍物的避障规划。

基于臂型角的位置求解

参照图6，以空间面U₁U_2i-1U_2i+1为参考平面，以为圆心，以矢量为Z轴，以为X轴，则按照右手定则可定义Y轴，在此坐标系中，X轴与Y轴正方向的夹角定义为臂型角ψ(ψ∈[0°,360°))。所以在平面上；到原点的距离为杆长L；在以为圆心且为半径的圆上与向量的夹角为臂型角ψ_2i-1。

a)在平面上：

b)到U_2i-1的距离为杆长L：

c)与向量的夹角为臂型角ψ_2i-1：

联立方程可得：

由(20)可以求得x,y,z的值。

(6)基于关节拟合点与角度求解

参照图7，是本发明一实施例的通过点与坐标系关系的正向递推求得各个关节角度示意图。

1)PY(pitch-yaw)型万向节求解

通过建立DH的关系可以求得

通过逆向拟合法可以求得U_Ji在0坐标系下的位置：

为了求得θ_4i-3,θ_4i-2的角度，需要将U_Ji的位置参考坐标转换到{4i-4}坐标系下，由于在逆向拟合时无法确定U_2i的姿态，因此仅用位置矩阵进行运算：当开始求解θ_4i-3,θ_4i-2时，关节θ₁到θ_4i-4的所有角度都已经求得了，因此⁰T_4i-4(⁰T_2i-4＝f(θ₁...θ_4i-4))的所有元素都是已知的，⁰T_4i-2仅位置部分已知，因此可以通过(23)求解^4i-4T_4i-2的位置部分，如下：

由(21)(23)可得：

解得：

θ_4i-3＝arctan2(y_4i-2/Lcos(θ_4i-2),x_4i-2/Lcos(θ_4i-2)) (26)

2)YP(yaw-pitch)型万向节求解

通过建立DH的关系可以求得

通过单段逆向拟合可以求得U_2i+1在{0}坐标系下的位置：

为了求得θ_4i-1,θ_4i的角度，需要将U_2i+1的位置坐标转换到{4i-2}坐标系下。当开始求解θ_4i-1,θ_4i时，关节θ₁到θ_4i-2的所有角度都已经求得了，因此⁰T_4i-2(⁰T_4i-2＝f(θ₁...θ_4i-2))的所有元素都是已知的，⁰T_4i仅位置部分已知，因此可以通过(29)求解^4i-2T_4i的位置部分，如下：

由(27)(29)可得：

θ_4i＝arctan2(y_4i/Lcos(θ_4i-1),x_4i/Lcos(θ_4i-1)) (31)

同理，根据DH坐标系YPPY…YPPY结构规则，可以依次求出各个关节的角度。

另一方面，本发明还提供一种基于模式函数的超冗余机械臂混合逆向求解系统，包括：

第一模块，用于执行步骤系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

第二模块，用于执行步骤系统利用机械臂末端点与脊线末端点重合，并通过末段连杆U_2NT匹配机械臂期望指向k，进而求得末端万向节节点U_2N在空间的位置U_2N；

第三模块，用于执行步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

第四模块，用于执行步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

第五模块，用于执行步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

第六模块，用于执行步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度。

本发明提供的基于模式函数的超冗余机械臂混合逆向拟合逆解方法，提出了基于模式函数的混合逆向拟合法解决超冗余机械臂在3D空间的逆运动学问题。在机械臂末端位置及指向确定后，通过该方法可以求得超冗余机械臂的合理关节角度，而且在求解过程中可以进一步考虑避障、避关节奇异等附加任务。

该方法基于末端点位姿信息，确定中间万向节节点的位置，在确定过程中可以考虑附加任务例如工具端指向，回避空间障碍物，回避机械臂内部奇异及边界奇异，回避关节运动超限等；中间万向节节点一旦确定就可以根据关节的配置类型求解当前位置下的关节角度。该方法分三大步骤：首先由根据整体空间环境确定的模式函数求得超冗余机械臂的空间脊线；然后基于脊线的宏观构型，将超冗余机械臂的各个臂杆按照末段拟合方向向量、次段拟合到脊线、其余双段拟合到脊线同时考虑附加任务的顺序，依次求解超冗余机械臂的万向节的空间节点位置；最后当各个万向节空间位置确定后，根据万向节的具体构型可求解相应的关节角度。该混合逆向拟合法用独立的末端万向节匹配效应器的方向向量，不仅保证位置准确可达而且可以保证末端效应器的指向；其余关节优先通过双段拟合规则拟合到脊线上，这样既能保证超冗余机械臂在宏观构型上满足脊线要求，又将双段4DOF相对空间位置的一个冗余自由度转化为可直观调节的臂型角，在合理有效逆解的同时进一步释放了超冗余机械臂的冗余特性。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (9)

1.一种基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，包括以下步骤；

系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

系统利用机械臂末端点与脊线末端点重合，并通过末段连杆匹配机械臂期望指向，进而求得末端万向节节点U_2N在空间的位置U_2N；

系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

系统根据所求得的各万向节节点位置，进而求解各关节的角度。

2.根据权利要求1所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述空间脊线可表示为：

其中，u(σ)＝[sinφ(σ)cosψ(σ),cosφ(σ)cosψ(σ),sinψ(σ)]，u(σ)是在σ处正切于曲线的单位向量；所述s∈[0,1]，其表示脊线的长度参数；l是脊线的实际长度。

3.根据权利要求2所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，其还包括系统根据机械臂末端欧拉角α,β,γ求得末端空间方向向量k：

其中，O₀为坐标原点，T为机械臂末端点，L表示连杆长度。

4.根据权利要求3所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置，其包括：

其中，表示第2N个万向节(x_2N,y_2N,z_2N)构成的矢量，表示第2N-1个万向节(x_2N-1,y_2N-1,z_2N-1)构成的矢量；表示第2N-1个连杆构成的矢量,且

5.根据权利要求4所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述万向节节点U_2N-1在脊线上的空间位置可表示为：

其中，f(s_k)＝0，该式中s_k表示脊线起点到当前点的弧长，且s_k∈[0,1]。

6.根据权利要求5所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置，其包括：

其中，ρ_2i-1为等效连杆的长度，其表示第2i-1个万向节点与第2i+1个万向节点之间的等效臂长。

7.根据权利要求6所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置，其可表示为：

其中，以空间面U₁U_2i-1U_2i+1为参考平面，为第2i个万向节节点的位置U_2i绕旋转轴U_2i-1U_2i+1旋转的中心点；为第2i个万向节节点的初始位置绕中心点旋转180度所对应的位置；

为第2i个万向节节点的初始位置绕中心点旋转ψ度所对应的位置，其中，以矢量为Z轴，以为X轴，按照右手定则定义Y轴，X轴与Y轴正方向的夹角定义为臂型角ψ(ψ∈[0°,360°))；在以为圆心且为半径的圆上，且与向量的夹角为臂型角ψ_2i-1。

8.根据权利要求7所述的基于模式函数的超冗余机械臂混合逆向求解方法，其特征在于，所述步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度，其包括俯仰-偏航型万向节求解和偏航-俯仰型万向节求解。

9.一种基于模式函数的超冗余机械臂混合逆向求解系统，其特征在于，包括：

第一模块，用于执行步骤系统根据期望的机械臂末端位置及各关节的构型确定模式函数，并求得空间脊线；

第二模块，用于执行步骤系统利用机械臂末端点与脊线末端点重合，并通过末段连杆匹配机械臂期望指向，进而求得末端万向节节点U_2N在空间的位置U_2N；

第三模块，用于执行步骤系统根据求得的末端万向节节点U_2N的位置，基于单段连杆长，将点U_2N-1拟合至所述空间脊线上，进而确定点U_2N-1的位置；

第四模块，用于执行步骤系统根据求得的点U_2N-1的位置，基于等效连杆长度，进而依次确定各奇数万向节的节点位置；

第五模块，用于执行步骤系统根据空间位置要求确定各臂型角，并求得各偶数万向节的节点位置；

第六模块，用于执行步骤系统根据所求得的各万向节节点位置，进而求解各关节的角度。