CN103901898A - 一种多自由度机器人的逆运动学通用求解方法 - Google Patents

Abstract

本发明提供的是一种多自由度机器人的逆运动学通用求解方法。本运用共性空间理论建立nR机器人的通用运动学方程，运用加权的空间矢量投影法分析空间矢量投影值与空间机器人转动关节角的关系，以各关节矢量在机器人末端矢量上投影加权值做为调整机器人末端位姿的依据，通过确定各关节矢量的投影加权值实现逆运动学的半解析求解。本发明提供的方法不仅仅实现了nR机器人运动学求解，同时还兼顾空间避障任务。该方法可广泛应用于串联式空间nR机器人，具有计算速度快、解算精度高的优点，为串联形式的机器人提供控制输入参数，满足工业现场对机器人运动学解算的作业需要。

Description

一种多自由度机器人的逆运动学通用求解方法

技术领域

本发明提供的是一种用于nR机器人运动学的半解析求解方法，特别是综合了机器人关节的建模方法、加权的空间矢量投影法。

背景技术

随着工业机器人技术的广泛的应用，空间nR串联机构的应用具有重要的意义。串联机构运动学逆解是串联机器人控制计算的先决条件，它直接关系到机器人离线编程、轨迹规划、实时控制等工作，在机器人学中占有重要地位，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机器人末端执行器按空间位姿进行编程控制(如直线轨迹和圆弧轨迹等)。

串联机构运动学中，空间6R串联机构的运动学逆解是最困难的，该问题与空间机构学中的单环7R机构运动学逆解属于同一问题，曾被喻为空间机构运动分析中的珠穆朗玛峰。各国学者对此开展很多有益的探索和研究。空间6R串联机构的运动学逆解求解方法分为解析形式和数值形式。一般6R串联操作臂运动学逆解因涉及结构参数多、解的非线性和耦合性以及需要求解代数方程等问题而变得难于得到解析解。解析解法适用于具有特殊几何结构参数的6R串联操作臂，可以应用矢量、螺旋或李代数方法得到理论解，这种方法具有计算结果准确、能够得到全部解等优点，但需要进行大量的代数和矩阵运算，推导过程比较复杂，并且有解的条件是操作臂的位置和姿态具有解耦特征或其特征多项式的次数小于等于4。廖启征将倍四元数引入空间串联机器人运动学研究当中，解决了一个经典的6R机器人的逆运动学问题。2006年，有学者提出把串联运动链拆成几个简单部分的组合，但该方法只适于某些解耦的特殊情况。以往用于串联机构位置逆解数学建模的方法主要有D-H矩阵法、球面三角法、实矩阵法、对偶数法等,得到了各不相同的逆解算法，不具有通用性。Raghavan和Roth通过矢量运算由6个逆运动学等式构造14个基础方程，消元运算后得到一元24次方程,求出最多16组逆运动学解，但存在8个增根.Manocha采用24阶矩阵特征分解方法对Raghavan的算法进行改进，提高了逆运动学解算的稳定性和精度。为解决空间7R机构的位移分析难题，分别采用复数方法和矩阵运算构造10个基础方程，进而得到一元16次方程，消除了增根。借鉴前期学者研究成果，将6R串联型机器人逆运动学求解问题分为两类：封闭解法求解满足Pieper准则的6R机器人的逆运动学问题；矢量计算和符号运算将Manocha得到的目标矩阵从24阶降低到16阶,并以矩阵特征分解方法提高一般6R机器人逆运动学求解的效率和稳定性，并组合牛顿-拉夫森迭代算法解决非Pieper准则的6R机器人的逆运动学问题。

而对于6R串联型机器人实际运动作业下，仅仅需要一种能够实时快速找到满足一定工作要求（如避障和动力学要求）和末端工作点位姿要求逆运动学解。为此产生了很多种数值形式的串联机器人逆运动学求解方法。一个常用的数值方法是将6R串联操作臂各关节的D-H参数中的径向参数ai、αi和轴向参数si、θi分离，运用双四元数方法或李代数方法将6R串联操作臂运动学正解矩阵构造成两个独立的齐次变换线性方程组，通过将两个方程组联立逐次迭代或消元而得到关于各关节转角的16组运动学逆解。如QIAO等运用双四元数理论得到了一般6R串联操作臂运动学逆解的数值解；ROCCO等运用李群、李代数等方法也得到了该问题的数值解。另一个比较常用的数值方法是将遗传算法和神经网络等工具引入6R操作臂的运动学逆解问题中，通过设定关节转角进给值等约束条件，以运动学正解和目标值之间的差值最小化为目标函数，采用上述算法求解最佳拟合的关节转角进给值。如CHIDDARWAR等比较了预测型与常规型神经网络算法对求解效率的影响；

等提出了一种考虑关节速度和加速度的3自由度机器人运动学逆解神经网络算法；KALRA等提出了一种基于遗传算法的6自由度工业机器人运动学逆解算法；HAMMOUR等采用连续传算法规划了6R操作臂的运动轨迹；ZHA[20]利用末端执行器位置和姿态矢量构成的曲面特征，通过遗传算法搜寻该曲面最小特征值而获得最优轨迹规划等。

发明内容

本发明的目的在于提供一种能够克服传统解析方法的求解机器人构形的局限性和专一性，也能克服通用的迭代方法非实时性和精度问题的多自由度机器人的逆运动学通用求解方法。

本发明的目的是这样实现的：

步骤1：向计算机输入nR机器人的关节参数：机器人关节类型、关节尺寸参数、运动范围、连接角度信息，对输入的多自由度关节进行分解处理，将其分解成多个单自由度关节，并建立运动学模型，同时输入空间的目标点位置和姿态矩阵；

步骤2：根据上一步骤建立的机器人关节运动学矩阵，按照共性几何空间理论建立nR机器人的一般运动学方程，同时建立关节矢量间的夹角表达式，以及矢量间夹角与矢量连接位置所在的关节运动量之间转换关系；

步骤3：根据上一步骤建立的nR机器人的运动学方程，按照机器人拓扑结构关系确定nR机器人空间矢量关系，进而确定每个关节的空间矢量在机器人末端矢量上的投影量；

步骤4：保持nR机器人关节之间的拓扑关系，以关节的空间矢量在机器人末端矢量上的投影量最大值为目标，通过转动机器人关节，确定关节的空间矢量在机器人末端矢量上的投影量最大值的机器人工作构形；

步骤5：以投影量最大值的机器人工作构形为初始构形，通过分解的方式，找到投影量为0的机器人工作构形，并用解析方法求解机器人各关节运动量；

步骤6：以投影量为0的机器人工作构形为基础，调整机器人构形，以满足机器人关节空间要求为前提，远离关节空间极限，使得机器人末端点达到目标空间点的位置要求，进而确定机器人位置要求的工作构形；

步骤7：修正并调整机器人构形的姿态，通过调整当前位置矢量与目标位置矢量的偏差角实现姿态的误差修正，实现机器人末端点同时满足空间的位置和姿态要求；

步骤8：计算最后得到的机器人构形的关节量，并将该组机器人关节广义运动量输入到机器人控制器，实现机器人运动控制。

本发明以多转动关节机器人为研究对象，运用共形几何空间理论和加权的空间矢量投影法相结合的方法，解决机器人逆运动学通用快速求解问题，为机器人关节控制提供运动参数。

本发明的主要特点在于：

1、按照共性几何空间理论建立nR机器人的一般运动学方程；

机器人末端的位置矢量为：

$p_n=a_0+\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{a}_k\left({\mathrm{\β}}_k\right)---\left(1\right)$

式中，a_i(i=0,1,2…n)为第i个转动关节中心指向第i+1个转动关节中心的矢量；矢量a_i-1与矢量a_i之间的夹角β_i与第i个转动关节转动角θ_i固定的数值关系；Pn是机器人末端位置矢量。

按照共性几何空间理论，可建立通用形式的nR机器人逆运动学方程如下：

$\cos {\mathrm{\β}}_i=\frac{a_{i-1}\·a_i}{\left|a_{i-1}\right|\left|a_i\right|}(i=\mathrm{1,2},...n)---\left(2\right)$

β_i与θ_i之间的关系可由空间几何关系表示，可得公式(3)。

$\cos {\mathrm{\θ}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\β}}_i}{{2h}_i{h}_{i-1}}---\left(3\right)$

式中h_i、k_i(i=1,2…n)是机器人固定的结构参数，可通过实际测量得到。

2、运用加权的空间矢量投影法确定nR机器人关节角，各关节广义关节角，进而实现机器人逆运动学求解；

由空间矢量投影关系可以建立下式：

$\left|\right|p_n\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+{\mathrm{\κ}}_n\left|\right|a_n\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\κ}}_{n-1}\left|\right|a_k\left|\right|---\left(4\right)$

式中κ_i(i=0,1,…n)是矢量a_i(i=0,1,…n)在空间p_n矢量上的投影与矢量a_i(i=0,1,…n)长度的比值；当机器人结构和工作位姿确定后，κ_i(i=1,2,…n)是与β_i(i=1,…n)和κ_i-1相关，故可由下式表示：

κ_i=f(β_i,κ_i-1)(i=1,2…n)               (5)

运用加权的空间矢量投影法的加权值确定过程如下：

步骤1：极大值的空间矢量投影值确定

确定每个关节矢量在矢量P_n方向上的max(κ_i)(i=1,2…n-1)，这样可以得到位姿矢量上的新的投影值，如下式所示：

$\left|\right|P^{\′}\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\max \left({\mathrm{\κ}}_{n-1}\right)\left|\right|a_k\left|\right|---\left(6\right)$

矢量P'与矢量P_n在矢量的方向上相同，关系上有如下关系：

||P'||≥||P_n||-κ_n||a_n||                (7)

定义δ为关节矢量在位姿矢量上投影的总余量，则有：

δ=||P'||-||P_n||+κ_n||a_n||                    (8)

步骤2：δ→0工作构形确定

为杆件矢量a_i在机器人位姿矢量P_n上极大值投影。采用均分分配总余量的方法，须将a_i在机器人位姿矢量P_n上投影由原来的b_i变为b_i-δ/n，假如关节i的关节角由于关节运动范围，无法将其在机器人位姿矢量P_n上投影变为b_i-δ/n（该值可为负数），将a_i在机器人位姿矢量P_n上最小投影记为b'_i，则有：

Δ_i=b'_i-b_i+δ/n                     (9)

步骤3：姿态补偿修正

P'_n-1是δ→0工作构形中第n个关节中心的空间矢量，P"'_n-1是目标构形的第n个关节中心的空间矢量，由于两个矢量在P_n方向上投影值相同，因此在空间上矢量P"'_n-1为矢量P'_n-1绕空间矢量轴P_n旋转φ得到的。根据这种关系，以δ→0工作构形为初始构形，寻找φ→0的工作构形。在确定δ→0工作构形时，采用均分δ的方法，因此此时大多数机器人关节存在运动余量。

本发明的一种通用的快速进行nR机器人逆运动学求解方法，该方法克服传统解析方法的求解机器人构形的局限性和专一性，也克服了通用的迭代方法非实时性和精度问题，能够解决nR机器人逆运动学求解问题。

附图说明

图1是求解过程示意图。

图2是nR型串联机器人的运动学模型。

图3β_i与θ_i之间的关系。

图4空间矢量的投影关系。

图5矢量投影与关节角关系。

图6相邻关节投影补偿。

图7姿态角补偿。

图8φ→0工作构形与目标工作构形关系示意图。

图9一般6R机器人模型。

具体实施方式

下面结合附图对本发明做更详细的描述。

主要求解过程如图1所示。

采用的nR机器人的运动学通用模型附图2所示，由n个自由度的转动关节和刚体连杆组成。附图2中，J_i(i=1,2…n)为第i个转动关节中心；L_i(i=0,1,2…n)为第i个刚体；Σ_i(i=1,2…n)为L_i固连坐标系，Σ₀为基础坐标系；a_i(i=0,1,2…n)为第i个转动关节中心指向第i+1个转动关节中心的矢量，由于关节参数固定，矢量a_i-1与矢量a_i之间的夹角β_i与第i个转动关节转动角θ_i固定的数值关系，矢量a_i的范数由刚体L_i形状决定是一个固定的数，其变化与转动关节角有关，故可表示为a_i(β_i)。

β_i与θ_i之间的关系可由附图3所示空间几何关系表示，可得公式(3)。

$\cos {\mathrm{\θ}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\β}}_i}{{2h}_i{h}_{i-1}}---\left(3\right)$

式中h_i、k_i(i=1,2…n)是机器人固定的结构参数，可通过实际测量得到，这样空间nR机器人的逆运动学问题就是如何确定空间的a_i(i=1,2…n-1)的矢量角，其空间的范数是已知的。

κ_i(i=1,2,…n)在β_i(i=0,1,…n)约束下存在一种迭代关系，如图4所示。当a_i-1空间矢量确定后，其在P_n的投影κ_i-1||a_i-1||也就随之确定，第i个关节的旋转轴矢量也同样可知了，κ_i||a_i||的变化范围也是已知了，这样就根据κ_i||a_i||的范围，在满足约束条件情况下，确定a_i，进而得到β_i和θ_i，实现逆运动学的求解。

矢量投影与关节角关系如图5所示。图中平面τ是关节矢量a_i绕关节轴J_i旋转一定角度后与初始关节矢量

形成的平面（关节矢量a_i绕关节轴J_i旋转形成的是圆锥面），由于机器人的刚体杆件不一定垂直关节轴J_i，因此设B点、F点在关节轴J_i上的共同的投影点A，AB与AF的夹角θ就是所要求解的关节角；O_iB、O_iF与关节轴J_i夹角固定，因此AB与AF值是已知；

为关节矢量a_i在关节角为0时的矢量，即O_iB；B点、F点在位姿矢量P_n的投影点分别为D点和G点，过D点做GF的平行线，得到交点C，易知DG是a_i在矢量P'_n上的投影值与

在矢量P'_n上的投影值的差值Δ；位姿矢量P_n在平面τ投影交BC与E点；由于O_iB、P_n、平面τ是已知的，可由共形几何空间理论求的O_iB与P_n夹角α、P_n与平面τ夹角γ。

当Δ已知，求解夹角θ时，由图示关系可建立下列等式：

$O_{i}D=a_i^0\cos \mathrm{\α}---\left(10\right)$

$O_{i}F=a_i^0---\left(11\right)$

$O_{i}C=\frac{O_{i}D*O_{i}F}{O_{i}D+\mathrm{\Δ}}---\left(12\right)$

$O_{i}E=\frac{O_{i}D}{\cos \mathrm{\γ}}---\left(13\right)$

$\mathrm{BC}=\sqrt{{\left(O_{i}B\right)}^2-{\left(O_{i}E\right)}^2}+\sqrt{{\left(O_{i}C\right)}^2-{\left(O_{i}E\right)}^2}---\left(14\right)$

设∠FO_iB为θ'_i，则有：

${\mathrm{\θ}}_i^{\′}=\arccos \left(\frac{O_{i}E}{O_{i}B}\right)+\arccos \left(\frac{O_{i}E}{O_{i}C}\right)---\left(15\right)$

${\left|\right|a_i^0\left|\right|}^2+{\left|\right|a_i\left|\right|}^2-2\left|\right|a_i^0\left|\right|*\left|\right|a_i\left|\right|*\cos {\mathrm{\θ}}_i^{\′}={\left(\mathrm{BF}\right)}^2---\left(16\right)$

(AB)²+(AF)²-2(AB)*(AF)*cosθ=(BF)²               (17)

通过式（10）-（17），可建立Δ与θ关系，反之当θ已知，求解Δ，也可由图5所示的关系求解。

如图6所示为杆件矢量a_i在机器人位姿矢量P_n上极大值投影。按照前面介绍的均分分配总余量的方法，须将a_i在机器人位姿矢量P_n上投影由原来的b_i变为b_i-δ/n，假如关节i的关节角由于关节运动范围，无法将其在机器人位姿矢量P_n上投影变为b_i-δ/n（该值可为负数），将a_i在机器人位姿矢量P_n上最小投影记为b'_i，则有：

Δ_i=b'_i-b_i+δ/n                (18)

这样第i个连杆的相邻连杆a_i-1和a_i+1在机器人位姿矢量P_n上投影值分别减少δ/n+Δ_i/2，按照该方式依次求得位姿矢量上的新的投影值，使得δ→0，从而由初始构形得到新的机器人工作构形。

如图7所示，P'_n-1是δ→0工作构形中第n个关节中心的空间矢量，P"'_n-1是目标构形的第n个关节中心的空间矢量，由于两个矢量在P_n方向上投影值相同，因此在空间上矢量P"'_n-1为矢量P'_n-1绕空间矢量轴P_n旋转φ得到的。根据这种关系，以δ→0工作构形为初始构形，寻找φ→0的工作构形。在确定δ→0工作构形时，采用均分δ的方法，因此此时大多数机器人关节存在运动余量。

以β_i(i=1,2,…n-1)为输入量，调整机器人杆件空间矢量，使φ→0。

由于第n个杆件的空间矢量a_n是确定的，则第n个关节旋转矢量J_n也是确定的，这样决定了空间矢量a_n-1所在平面矢量是确定的。而φ→0工作构形中的杆件矢量a"_n-1可能不在旋转矢量J_n平面矢量确定的平面中，此时矢量a"_n-1与该平面仅存在一个交点，为在第n个关节中心，因此φ→0工作构形还不是实际所需要的目标构形，还不满足机器人姿态需要，为此需要进一步进行姿态修正。

如图8所示，φ→0工作构形与目标工作构形关系示意图。在图中，φ→0工作构形保证了其第n个关节中心与实际工作构形的第n个关节中心重合，但是φ→0工作构形中的J"_n与目标工作构形中的J_n不重合，从而使得机器人末端姿态不满足工作要求。

J_n已知，根据机器人拓扑关系能够确定a_n-1所在的平面η_n-1，这样姿态修正的问题就是在φ→0工作构形基础上，通过调整β_i(i=1,2,…n-1)值，按照机器人杆件的空间约束关系，在η_n-1平面找到合适的a_n-1。

如图9所示，建立空间6R机器人一般运动模型，通过MATLAB进行计算，按照表1确定机器人结构参数模型。

表1机器人结构参数表

给定机器人目标点的末端姿态矩阵：

$T=\left[\begin{array}{cccc}-0.8673& 0.4894& -0.0908& 50.0142\\ 0.2884& 0.3456& -0.8929& -1003\\ -0.4056& -0.8007& -0.4409& -471.074\\ 0& 0& 0& 1\end{array}\right]$

首先根据机器人运动模型，按照加权的空间矢量投影法步骤1的方法确定投影最大工作构形的空间矢量，该矢量为1822mm，该构形的工作构形关节角如表2所示。

表2投影最大工作构形的关节角

按照加权的空间矢量投影法步骤2的方法确定δ→0工作构形，该工作构形的关节角如表3所示。

表3δ→0工作构形的关节角

在不改变机器人关节间的约束条件下，调整空间矢量投影的加权值，使得机器人末端点满足机器人末端位置要求，得到的机器人关节角如表4所示。

表4满足位置要求工作构形的关节角

机器人工作构形不仅要满足工作的末端点的位置要求，还需满足其姿态要求。为此还需进行工作构形姿态修正。按照加权的空间矢量投影法步骤3的方法，得到最后机器人工作构形，其机器人关节角如表5所示。

表5最后机器人关节角

将表5中的各关节角数值输入到机器人控制器中，控制器就可以控制机器人关节运动，实现机器人末端满足目标点位置和姿态的要求。

Claims (3)

1.一种多自由度机器人的逆运动学通用求解方法，其特征是：

步骤1：向计算机输入nR机器人的关节参数：机器人关节类型、关节尺寸参数、运动范围、连接角度信息，对输入的多自由度关节进行分解处理，将其分解成多个单自由度关节，并建立运动学模型，同时输入空间的目标点位置和姿态矩阵；

步骤2：根据上一步骤建立的机器人关节运动学矩阵，按照共性几何空间理论建立nR机器人的一般运动学方程，同时建立关节矢量间的夹角表达式，以及矢量间夹角与矢量连接位置所在的关节运动量之间转换关系；

步骤3：根据上一步骤建立的nR机器人的运动学方程，按照机器人拓扑结构关系确定nR机器人空间矢量关系，进而确定每个关节的空间矢量在机器人末端矢量上的投影量；

步骤4：保持nR机器人关节之间的拓扑关系，以关节的空间矢量在机器人末端矢量上的投影量最大值为目标，通过转动机器人关节，确定关节的空间矢量在机器人末端矢量上的投影量最大值的机器人工作构形；

步骤5：以投影量最大值的机器人工作构形为初始构形，通过分解的方式，找到投影量为0的机器人工作构形，并用解析方法求解机器人各关节运动量；

步骤6：以投影量为0的机器人工作构形为基础，调整机器人构形，以满足机器人关节空间要求为前提，远离关节空间极限，使得机器人末端点达到目标空间点的位置要求，进而确定机器人位置要求的工作构形；

步骤7：修正并调整机器人构形的姿态，通过调整当前位置矢量与目标位置矢量的偏差角实现姿态的误差修正，实现机器人末端点同时满足空间的位置和姿态要求；

步骤8：计算最后得到的机器人构形的关节量，并将该组机器人关节广义运动量输入到机器人控制器，实现机器人运动控制。

2.根据权利要求1所述的多自由度机器人的逆运动学通用求解方法，其特征是所述按照共性几何空间理论建立nR机器人的一般运动学方程包括：

机器人末端的位置矢量为：

$p_n=a_0+\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{a}_k\left({\mathrm{\β}}_k\right)$

式中，a_i，i=0,1,2…n为第i个转动关节中心指向第i+1个转动关节中心的矢量；矢量a_i-1与矢量a_i之间的夹角β_i与第i个转动关节转动角θ_i固定的数值关系；Pn是机器人末端位置矢量；

建立通用形式的nR机器人逆运动学方程如下：

$\cos {\mathrm{\β}}_i=\frac{a_{i-1}\·a_i}{\left|a_{i-1}\right|\left|a_i\right|}(i=\mathrm{1,2},...n)$

β_i与θ_i之间的关系由空间几何关系表示，得公式

$\cos {\mathrm{\θ}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\β}}_i}{{2h}_i{h}_{i-1}}$

式中h_i、k_i,i=1,2…n是机器人固定的结构参数，通过实际测量得到。

3.根据权利要求2所述的多自由度机器人的逆运动学通用求解方法，其特征是所述用解析方法求解机器人各关节运动量具体包括：

运用加权的空间矢量投影法确定nR机器人关节角，各关节广义关节角，进而实现机器人逆运动学求解；

由空间矢量投影关系建立下式：

$\left|\right|p_n\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+{\mathrm{\κ}}_n\left|\right|a_n\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\κ}}_{n-1}\left|\right|a_k\left|\right|$

式中κ_i,i=0,1,…n是矢量a_i,i=0,1,…n在空间p_n矢量上的投影与矢量a_i,i=0,1,…n长度的比值；当机器人结构和工作位姿确定后，κ_i,i=1,2,…n是与β_i,i=1,…n和κ_i-1相关，故由下式表示：

κ_i=f(β_i,κ_i-1)(i=1,2…n)

运用加权的空间矢量投影法的加权值确定过程如下：

步骤1：极大值的空间矢量投影值确定

确定每个关节矢量在矢量P_n方向上的max(κ_i),i=1,2…n-1，这样可以得到位姿矢量上的新的投影值，如下式所示：

$\left|\right|P^{\′}\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\max \left({\mathrm{\κ}}_{n-1}\right)\left|\right|a_k\left|\right|$

矢量P'与矢量P_n在矢量的方向上相同，关系上有如下关系：

||P'||≥||P_n||-κ_n||a_n||

定义δ为关节矢量在位姿矢量上投影的总余量，则有：

δ=||P'||-||P_n||+κ_n||a_n||

步骤2：δ→0工作构形确定

为杆件矢量a_i在机器人位姿矢量P_n上极大值投影，采用均分分配总余量的方法，将a_i在机器人位姿矢量P_n上投影由原来的b_i变为b_i-δ/n，假如关节i的关节角由于关节运动范围，无法将其在机器人位姿矢量P_n上投影变为b_i-δ/n，将a_i在机器人位姿矢量P_n上最小投影记为b'_i，则有：

Δ_i=b'_i-b_i+δ/n

步骤3：姿态补偿修正

P'_n-1是δ→0工作构形中第n个关节中心的空间矢量，P’”_n-1是目标构形的第n个关节中心的空间矢量，两个矢量在P_n方向上投影值相同，在空间上矢量P’”_n-1为矢量P'_n-1绕空间矢量轴P_n旋转φ得到，根据这种关系，以δ→0工作构形为初始构形，寻找φ→0的工作构形，在确定δ→0工作构形时，采用均分δ的方法。

Images