一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法

摘要

本发明公开了一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法，包括以下步骤：S1：对七自由度空间机械臂进行几何建模，采用臂角ψ来参数化除肘关节之外的其余关节；S2：分析关节极限对臂角ψ范围的影响，分析除肘关节之外其余所有关节的运动范围对臂角ψ的影响，确定1个、2个或者3个臂角的可行域并限定算法的搜索空间；S3：分析机械臂的三种奇异：运动学奇异、算法奇异和半奇异，对不符合条件的臂角ψ进行排除；S4：建立空间机械臂的运动学方程的广义雅可比矩阵；S5：通过优化计算得出能够使空间机械臂载体卫星姿态变动最小的机械臂型位。该方法在保证计算量小的前提下减小基座扰动，并且通过优化算法来平衡扰动的大小和计算量。

说明书

技术领域

本发明属于机械臂控制领域，尤其涉及一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法。 

背景技术

在太空探索中空间机械臂技术成为各国研究的重点项目之一。随着研究的深入，七自由度冗余机械臂由于其运动学冗余的特性在避障方面有非常突出的作用，逐渐被各国研究者所重视。目前国内外现有技术方案中在解决空间机械臂基座最小扰动运动规划时使用的机械臂均为六自由度或者六自由度以下的机械臂。 

一般的6自由度空间机械臂的运动学控制方法是使用速度级雅可比进行计算，但这样只能得到唯一一个数值解，并且速度级控制器的精度不如位置级控制器的精度高，而求得其解析解是唯一获得位置级控制的唯一方法。这样的方案如国内的哈尔滨工业大学2008年发表的“The Cartesian Path Planning of Free-Floating Space Robot using Particle Swarm Optimization”等，以及国外东京大学2003年发表的“Engineering Test Satellite VII flight experiments for space robot dynamics and control Theories on laboratory test beds ten years ago, now in orbit”等。 

在冗余空间机械臂方面这个问题更具挑战性，其载体卫星的姿态和位置会受到机械臂运动的影响而发生变化，并且由于机械臂本身的冗余特性，我们不能得到确切的解，只能得到一个解的集合。比较完善的方法均为地面七自由度机械臂控制方案，如日本东京大学2008年发表的“Analytical Inverse Kinematic Computation for 7-DOF Redundant Manipulators With Joint Limits and Its Application to Redundancy Resolution”，国内哈尔滨工业大学2012年发表的“An Analytical Solution for Inverse Kinematic of 7-DOF Redundant Manipulators with Offset-Wrist”等，但这些方法仅能对地面上七自由度机械臂进行控制，不能对空间机械臂进行控制。 

发明内容

根据现有技术存在的问题，本发明公开了一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法：一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法，包括以下步骤： 

S1：对七自由度空间机械臂进行几何建模，采用臂角ψ来参数化除肘关节之外的其余关节； 

S2：分析关节极限对臂角ψ范围的影响，分析除肘关节之外其余所有关节的运动范围对臂角ψ的影响，确定1个、2个或者3个臂角的可行域并限定算法的搜索空间； 

S3：分析机械臂的三种奇异：运动学奇异、算法奇异和半奇异，对不符合条件的臂角ψ进行排除； 

S4：建立空间机械臂的运动学方程的广义雅可比矩阵； 

S5：通过优化计算得出能够使空间机械臂载体卫星姿态变动最小的机械臂型位。 

进一步的，步骤S5具体采用如下方式：首先我们采用四元数方程表示卫星基座的姿态,如下所示： 

$Q=\eta +q_1\overrightarrow{i}+q_2\overrightarrow{j}+q_3\overrightarrow{k}=\eta +q\in R^4$

此时，整个系统：包括卫星、基座和机械臂的状态方程为 

$\left(\begin{array}{c}\stackrel{·}{\eta }\\ \stackrel{.}{q}\end{array}\right)=\frac{1}{2}\left(\begin{array}{cc}0& -{\omega }^T\\ \omega & -\tilde{\omega }\end{array}\right)\left(\begin{array}{c}\eta \\ q\end{array}\right)=\frac{1}{2}\left(\begin{array}{c}-q^T\\ \mathrm{\eta E}-\tilde{q}\end{array}\right)\omega$

此时方向误差为： 

$\left(\begin{array}{c}\mathrm{\delta \eta }={\eta }_{b0}{\eta }_{\mathrm{bf}}+q_{b0}^T{q}_{\mathrm{bf}}\\ \mathrm{\delta q}={\eta }_{b0}{q}_{\mathrm{bf}}-{\eta }_{\mathrm{bf}}{q}_{b0}-{\tilde{q}}_{b0}{q}_{\mathrm{bf}}\end{array}\right)$

此处δq记为我们要优化的目标函数，其值越小，卫星的姿态变动就越小。 

进一步的，对七自由度空间机械臂进行几何建模时只使用臂角ψ一个参数来参数化其余关节。 

由于采用了上述技术方案，本发明提供的一种七自由度空间机械臂最小基座扰动解析逆运动学求解方法，通过对空间机械臂进行几何建模、采用臂角ψ来参数化除肘关节之外的其余关节，并且通过计算和筛选排除不符合条件的臂角ψ 值，从而得到够使空间机械臂载体卫星姿态变动最小的机械臂型位。该方法不仅提高了计算精度和节约了计算资源，同时达到了稳定航天器基座的目的，保证计算量小的前提下尽量减小基座扰动，并且通过优化算法来平衡扰动的大小和计算量。 

附图说明

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

图1为本发明中关节曲线和臂角可行域的示意图； 

图2为本发明中S-R-S机械臂结构简图； 

图3为本发明中机械臂结构简化图； 

图4(a)、图4(b)、图4(c)、图4(d)、图4(e)、图4(f)、图4(g)、和图4(h)为本发明中关节角θ_i与臂角ψ关系曲线图； 

图5为本发明中7-DOF空间机械臂的渲染图。 

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述： 

首先我们对本发明中出现的参数进行解释说明： 

其中R代表旋转矩阵，左上角标代表以该坐标系为基准，比如b则表示该旋转矩阵是在基座坐标系下的表示，右下角标则表示参与计算该旋转矩阵的关节数，比如s则代表组成肩关节关节1、2、3三个关节的合成。l代表一段距离的向量，同上左上角标代表参考坐标系，右下角标两个字母分别代表这段距离的起始点和终止点，比如^bl_bs代表在基座坐标系下从基座坐标系原点到肩关节坐标系原点的距离向量在基座坐标系下的表示。全部的字母含义如下所示： 

b 基座 s 肩关节 e 肘关节 w 腕关节 t 末端器

 

数字标号 对应数字的关节

S1：对七自由度空间机械臂进行几何建模，采用臂角来参数化除肘关节之外的其余关节,具体包括以下步骤： 

步骤1：通过机械臂末端器位姿计算机械臂手腕的位置，并且确定S-R-S机械臂的R关节的大小； 

步骤2：计算^bR_s|_ψ＝0，并且进行参数化； 

步骤3：识别关节类型； 

步骤4：计算每个关节对臂角可行域的限制； 

步骤5：将每个关节所求出的臂角可行域取交集，并且去掉奇异的点； 

步骤6：通过计算不同的解对于基座姿态的影响，选择可以将基座变化控制在最小幅度内的可行臂角，进而计算出机械臂的运动学逆解。计算过程中机械臂的运动速度均符合梯形规划法。 

S2：分析关节极限对臂角ψ范围的影响，分析除肘关节之外其余所有关节的运动范围对臂角ψ的影响，确定1个、2个或者3个臂角的可行域并限定算法的搜索空间。 

下面我们讨论为什么采用臂角这一个参数来参数化除肘关节之外的其余关节： 

为了描述机械臂运动学(即关节角大小与末端器位姿之间的关系)，我们需要定义坐标系。首先，Σ₀为基坐标系，Σ_i(i＝1,...,7)分别为各关节的坐标系。其次，Σ_i的坐标原点在关节i+1上，其z轴为该关节旋转轴，x轴为第i和i+1旋转轴的公垂线，y轴由“右手定则”来定义。最后，当关节角θ₁＝0时Σ₀与Σ₁方向相同，并且由于Σ₇位于机械臂的末端器上，当所有关节角均为零时其z轴方向与基坐标系z轴方向相同，此时机械臂所有关节排列成一条直线，末端器达到工作空间的最大边缘。尽管由于机械臂D-H参数会随着坐标系定义的不同而改变，但是我们仍然给出由上述定义坐标系方法所对应的S-R-S机械臂D-H参数，其中S:sphere；R:rotation；S-R-S代表球-旋转-球型结构。图2所示为本发明中S-R-S机械臂结构简图。如表一所示的7自由度空间机械臂的D-H参 数： 

表一 

为了描述七自由度机械臂的自旋，可以先将关节3设为零，让机械臂等效为一个6自由度机械臂，此时机械臂末端器位姿确定后的前臂和上臂所构成的平面的位置为自旋为零时刻，记为“臂角”为零或ψ＝0，这个平面被称为“参考平面”。当7-DOF机械臂发生自旋后，由机械臂上臂和前臂组成的平面被称为“臂平面”，此时臂角ψ为从末端器方向看向基座方向视角，由参考平面到臂平面需要转过的角度，如附图3。 

当机械臂简化成上述结构时，机械臂各关节之间的旋转矩阵也可以简化成如下形式： 

^bR_s＝⁰R₁·¹R₂·²R₃

^sR_e＝³R₄      (1) 

^eR_t＝⁴R₅·⁵R₆·⁶R₇

关节之间的连杆长度也可以用向量来表示： 

$\left(\begin{array}{cc}{}^{b}l_{\mathrm{bs}}={\left(\begin{array}{ccc}0& 0& d_{\mathrm{bs}}\end{array}\right)}^T& {}^{s}l_{\mathrm{se}}={\left(\begin{array}{ccc}0& -d_{\mathrm{se}}& 0\end{array}\right)}^T\\ {}^{e}l_{\mathrm{ew}}={\left(\begin{array}{ccc}0& 0& d_{\mathrm{ew}}\end{array}\right)}^T& {}^{w}l_{\mathrm{wt}}={\left(\begin{array}{ccc}0& 0& d_{\mathrm{wt}}\end{array}\right)}^T\end{array}\right)$

此时末端器的位置和姿态可以表示为： 

^bx_t＝^bl_bs+^bR_s·[^sl_se+^sR_e·(^el_ew+^eR_w·^wl_wt)]     (2) 

^bR_t＝^bR_s·^sR_e·^eR_t

由于上述内容中提到过的“自旋”可以知道当末端器的位姿确定后，机械臂的上臂和前臂可以围绕着肩关节和腕关节连接所成的轴旋转，则由式(2)第一行可以推出这个轴相对于基坐标系的表达式为： 

^bx_sw＝^bx_t-^bl_bs-^bR_t·^tl_wt     (3) 

＝^bR_s·(^sl_se+^sR_e·^el_ew) 

由于公式(3)中右手边第一行均为常量，所以可以证明当末端器位姿固定后腕关节的位置将不会改变，同时由于肩关节的位置也不会改变，我们可以通过由上臂、前臂以及肩关节和腕关节的连线所组成的三角形通过“余弦定理”计算肘关节的角度，当然我们知道肘关节只有一个关节θ₄： 

${\theta }_e={\theta }_4=a\cos \frac{{\left|\right|{}^{b}x_{\mathrm{sw}}\left|\right|}^2-d_{\mathrm{se}}^2-d_{\mathrm{ew}}^2}{2·d_{\mathrm{se}}·d_{\mathrm{ew}}}---\left(4\right)$

通过反余弦计算式(4)会得到正负两个结果，但是由于“自旋”，当ψ＝π时会让其中一个结果与ψ＝0时另外一个结果相同，所以我们只保留一个正的结果就可以。 

由于S-R-S机械臂的R关节的大小唯一确定，此时机械臂手腕的位置只跟^bR_s有关，又已知当末端器位姿固定时手腕位置不变，可以推出^bR_s不变，而其中只包含组成肩关节的θ₁,θ₂,θ₃。为了使用冗余来参数化关节角θ₁,θ₂,θ₃，我们用 

${}^{b}R_{\psi }=I_3+\sin \psi ·{}^b\tilde{u}_{\mathrm{sw}}+(1-\cos \psi )·{}^b\tilde{u}_{\mathrm{sw}}^2---\left(5\right)$

来表达围绕着上的轴转过了ψ所产生的旋转矩阵。而上臂的方向可以表示成： 

^bR_s＝^bR_ψ·^bR_s|_ψ＝0     (6) 

^bR_s|_ψ＝0表示当机械臂为前述的等效6-DOF机械臂的情况下上臂的方向，此时θ₃＝0，我们可以通过左乘式(5)的旋转矩阵来得到S-R-S机械臂的上臂方向。我们可以将式(6)代入式(3)右手边第二行来计算^bR_s|_ψ＝0。 

为了计算组成肩关节的三个关节的角度，我们可以将式(5)代入式(6)化简得到： 

^bR_s＝A_ssinψ+B_scosψ+C_s      (7) 

其中：$A_s={}^b\tilde{u}_{\mathrm{sw}}·{}^{b}R_s{|}_{\psi =0},B_s=-{}^b\tilde{u}_{\mathrm{sw}}^2·{}^{b}R_s{|}_{\psi =0},C_s={}^{b}u_{\mathrm{sw}}·{}^{b}u_{\mathrm{sw}}^T·{}^{b}R_s{|}_{\psi =0}$

注意：如果是^bu_sw的tilde则有同时由符号表中旋转矩阵公式可由式(1)第一行计算出： 

${}^{b}R_s=\left(\begin{array}{ccc}*& -\cos {\theta }_1·\sin {\theta }_2& *\\ *& -\sin {\theta }_1·\sin {\theta }_2& *\\ -\sin {\theta }_2·\cos {\theta }_3& -\cos {\theta }_2& \sin {\theta }_2·\sin {\theta }_3\end{array}\right)$

我们可以通过分别计算tanθ₁＝^bR_s(2,2)/^bR_s(1,2)、cosθ₂＝-^bR_s(3,2)、tanθ₃＝-^bR_s(3,3)/^bR_s(3,1)来得到： 

θ₁＝atan2(-a_s22·sinψ-b_s22·cosψ-c_s22,-a_s12·sinψ-b_s12·cosψ-c_s12) 

θ₂＝acos(-a_s32·sinψ-b_s32·cosψ-c_s32)      (8) 

θ₃＝atan2(a_s33·sinψ+b_s33·cosψ+c_s33,-a_s31·sinψ-b_s31·cosψ-c_s31) 

其中，a_sij,b_sij,c_sij分别为A_s，B_s，C_s的第(i,j)个元素。 

同理我们可以将式(7)和^sR_e代入式(2)的第二行，得到： 

^eR_t＝A_wsinψ+B_wcosψ+C_w

其中：$A_w={}^{s}R_e^T·A_s^T·{}^{b}R_t,B_w={}^{s}R_e^T·B_s^T·{}^{b}R_t,C_w={}^{s}R_e^T·C_s^T·{}^{b}R_t$

则由式(1)第三行可以得到： 

${}^{e}R_t=\left(\begin{array}{ccc}*& *& \cos {\theta }_5·\sin {\theta }_6\\ *& *& \sin {\theta }_5·\sin {\theta }_6\\ -\sin {\theta }_6·\cos {\theta }_7& \sin {\theta }_6·\sin {\theta }_7& \cos {\theta }_6\end{array}\right)$

则有： 

θ₅＝atan2(a_w23·sinψ+b_w23·cosψ+c_w23,a_w13·sinψ+b_w13·cosψ+c_w13) 

θ₆＝acos(a_w33·sinψ+b_w33·cosψ+c_w33)      (9) 

θ₇＝atan2(a_w32·sinψ+b_w32·cosψ+c_w32,-a_w31·sinψ-b_w31·cosψ-c_w31) 

至此我们仅用臂角ψ这一个变量参数化了θ₁,θ₂,θ₃,θ₅,θ₆,θ₇，而θ₄则与ψ无关，只与末端器位姿有关。 

由于某些原因，比如机械臂机械结构、电器设备或导线的障碍等的影响机械臂各关节会有一个极限，并且又由于S-R-S机械臂的自旋，即使末端器位姿固定其逆运动学解仍然不唯一。为了排除掉在关节极限外的逆运动学解，得到全部都在关节极限内的逆运动学解，下面我们将讨论这些关节极限将如何影响关节参数化时的唯一参数——臂角ψ。 

除了θ₄不受ψ影响之外，我们通过观察式(8)和(9)可以看出θ_i与ψ之间的关 系只有两种情况，即tan型和cos型，下面我们将分情况进行讨论。 

Tan Type:为了讨论方便我们可以把θ₁,θ₃,θ₅,θ₇的表达式写成隐函数的形式： 

$\tan {\theta }_i=\frac{a_n·\sin \psi +b_n·\cos \psi +c_n}{a_d·\sin \psi +b_d·\cos \psi +c_d}---\left(10\right)$

我们将上面的隐函数用凑微分方程进行取微，得到： 

$\frac{d{\theta }_i}{\mathrm{d\psi }}=\frac{a_t·\sin \psi +b_t·\cos \psi +c_t}{f_n^2\left(\psi \right)+f_d^2\left(\psi \right)}---\left(11\right)$

其中：a_t＝b_dc_n-b_nc_d、b_t＝a_nc_d-a_dc_n、c_t＝a_nb_d-a_db_n、and分别是式(10)的分子和分母。 

因为式(11)是函数(10)的微分式，其可以表示函数(10)的特性。下面我们讨论θ_i与ψ可以有周期关系和单调关系两种情况。根据导数定义，当我们令其分子为零时可以计算出能够使与θ_i静止点对应的ψ值： 

很明显如果则θ_i(ψ)没有极值点。这种情况下函数θ_i(ψ)为单调函数如附图4(a)。 

如果则θ_i(ψ)必然会在上式的两个结果之一处达到极大值，而另一处达到极小值，如附图4(e)。我们分别将对应的θ_i极大值和极小值的ψ值命名为ψ^max和ψ^min，不要混淆的是ψ为在[-π,π]区间的值，其本身没有极大值和极小值的概念。 

如果则θ_i(ψ)只有一个极值点，如图4(c)和图4(d)，我们可以注意到当ψ位于该极值点时θ_i是不能唯一确定的，此时为tan型计算奇异，同样以下我们会对S-R-S机械臂的各种奇异进行专门的讨论。 

Cos类型:同tan类型我们先将θ₂,θ₄写成隐函数的形式： 

cosθ_i＝a·sinψ+b·cosψ+c 

接下来我们对隐函数求导得到： 

$\frac{{\mathrm{d\theta }}_i}{\mathrm{d\psi }}=-\frac{1}{\sin {\theta }_i}(a·\cos \psi -b·\sin \psi )$

当sinθ_i≠0时，另上式等于零我们可以解出： 

可以证明θ_i分别在上式中的一个解中达到极大值，另外一个解中达到极小值，如图4(f)。当sinθ_i＝0，我们无法确定cos型时的θ_i(ψ)梯度，我们可以方便的通过判断a²+b²-(c-1)²＝0或者a²+b²-(c+1)²＝0是否被满足来判断此种情况。并且我们可以通过图4(g)图4(h)看出其规律，此时为cos型计算奇异，同样我们将于后文详细讨论奇异。 

值得注意的是图4(b)，从图中可以看出，该曲线并不绕点(0,0)中心对称，这跟图4(a)略有不同。实际上这张图是S-R-S机械臂第5和第7个关节可能产生的结果，当然无论是周期型还是单调型或者是奇异型，机械臂最后3个关节都可能会产生其相对于ψ的运动轨迹不围绕ψ＝0时刻对称的情况(周期型时是相对于ψ＝0左右对称，单调型和奇异型都是相对于(0,0)中心对称)，而前3个关节永远是围绕ψ＝0时刻对称。实际上这个对称点只跟末端器的姿态有关，或者更严格的说只跟姿态中z轴与基坐标系z轴夹角有关，我们可以通过下面的公式方便的确定后三个关节角相对于ψ的曲线的对称轴或者对称中心所在的ψ的值： 

eul＝dc2eul(^bR_t) 

$\mathrm{axis}=-\frac{\mathrm{eul}\left(2\right)}{\mathrm{abs}\left(\mathrm{eul}\left(2\right)\right)}·(\pi -\mathrm{eul}\left(2\right))$

我们先将末端器姿态转化成用Z-Y-Z欧拉角表示的形式，然后再计算绕y轴转过的角度即可。 

通过观察图4(a)、图4(b)、图4(c)、图4(d)、图4(e)、图4(f)、图4(g)、和图4(h)我们把受臂角ψ影响的关节角θ₁,θ₂,θ₃,θ₅,θ₆,θ₇与ψ的关系大体上分为了3类，即单调型(仅包含tan型)、周期型(包含tan和cos型)和奇异型(包含tan和cos型)。图4(a)为单调1型、图4(b)为单调2型、图4(c)为正切奇异1型、图4(d)为正切奇异2型、图4(e)为正切周期1型、图4(f)为余弦周期1型、图4(g)为余弦奇异1型、图4(h)为余弦奇异2型下面我们分别讨论关节极限在这三种情况下会对这些关节的唯一参数ψ 的影响。 

单调型：此种情况下θ_i与ψ唯一对应，所以当关节极限确定时，可以将其上限和下限分别代入方程(10)中即可求出与之唯一对应的ψ上限ψ^u和下限ψ^l，当然这只是单调增的情况，当函数为单调减时对应ψ^l，对应ψ^u。 

周期型：此种情况虽然包含了tan型和cos型两种关系类型，但他们没有区别，我们一并进行讨论。我们从图4(e)图4(f)中可以看到，当θ_i存在极限时，如果或者时能够使θ_i达到极值点的ψ值只有ψ^uandψ^l。但是当 或者时我们有两个ψ值对应ψ^u或者有两个ψ值对应ψ^l，这使得确定可行的ψ范围变得困难，明白了这点之后我们针对5种不同的情况进行分析： 

Case1:or

此时不存在可行的臂角ψ 

Case2:and

此时ψ^max仍然对应但对应的ψ^min却不在可行臂角ψ的范围，我们需要将替换下式的θ_limit： 

${\psi }^{\mathrm{lu}}=2·a\tan \frac{-b_{\theta }\pm \sqrt{{b_{\theta }}^2-4·a_{\theta }·c_{\theta }}}{2·a_{\theta }}---\left(13\right)$

其中，a_θ＝tan(θ_limit)·(c_d-b_d)-c_n+b_n、b_θ＝2·tan(θ_limit)·a_d-a_n、c_θ＝tan(θ_limit)·(c_d+b_d)-c_n-b_n。得到and两个解，且则

Case3:and

此时ψ^min仍然对应但对应的ψ^max却不在可行臂角ψ的范围，我们需要将替换式(13)中的θ_limit，得到and两个解，且则 $\psi \in [-\pi ,{\psi }_1^u]\cup [{\psi }_2^u,\pi ].$

Case4:or

此时对应的ψ^min和对应的ψ^max都不在可行臂角ψ的范围，我们需要分别将和替换式(13)中的θ_limit，分别得到and两个解和and则ψ的可行范围分为两种情况：(1)当与分离的情况下$\psi \in [-\pi ,{\psi }_1^l]\cup [{\psi }_2^l,{\psi }_1^u]\cup [{\psi }_2^u,\pi ];$(2)当$[{\psi }_1^l,{\psi }_2^l]\in [{\psi }_1^u,{\psi }_2^u]$or$[{\psi }_1^u,{\psi }_2^u]\in [{\psi }_1^l,{\psi }_2^l]$情况下 $\psi \in [{\psi }_1^u,{\psi }_1^l]\cup [{\psi }_2^l,{\psi }_2^u]$or$\psi \in [{\psi }_1^l,{\psi }_1^u]\cup [{\psi }_2^u,{\psi }_2^l].$

Case5:and

此时ψ^max对应ψ^min对应则ψ∈[-π,π]。 

S3：分析机械臂的三种奇异：运动学奇异、算法奇异和半奇异 

奇异型：这里我们统一处理各种奇异的问题。我们根据不同的问题会将奇异大体分为3类——运动学奇异：此时机械臂会损失某个方向上的自由度、算法奇异：此时某些公式无法得出结果、半奇异：由关节极限导致的奇异。 

(1)运动学奇异又分为肩部奇异、肘部奇异和腕部奇异，其中肘部奇异和腕部奇异不会产生任何影响但是肩部奇异发生时，即当手腕的位置在连杆1的延长线上，关节1的大小无法唯一确定，我们可以通过将此时的θ₁直接置为零来解决肩部奇异。 

(2)算法奇异：我们可以通过判断a²+b²-(c-1)²＝0或者a²+b²-(c+1)²＝0来判断cos型达到奇异，通过来判断tan型达到计算奇异。 

通过图4(g),图4(h)我们可以看出即使cos达到奇异点ψ＝0(计算前3个关节)或ψ＝axis(计算后3个关节)时刻我们仍然能够唯一确定该点的θ_i值，只不过由于sinθ_i＝0我们无法通过式(13)来计算该点的θ_i值，但是我们可以通过计算其左右极限来确定θ_i的值，并且cos奇异型仍然为周期型函数，所以其关于臂角ψ的计算规则与周期型通用。但tan奇异型有所不同，我们可以从图4(c)、图4(d)观察到，当ψ处于奇异点时我们不能确定θ_i的值，所以tan奇异型除了类似于单调型之外(很容易就可以证明除奇异点之外tan奇异型是单调的函数)，我们还要从整个可行臂角区间内减去奇异点的ψ值。 

(3)半奇异：半奇异是当机械臂关节达到关节极限位置时发生的一种奇异，这种奇异要比运动学奇异解决起来困难的多，我们使用一个简单的一元优化算法来使得机械臂最终的运动学逆解远离每一个关节极限来避免半奇异问题。 

S4：建立空间机械臂的运动学方程的广义雅可比矩阵，该雅可比矩阵方程 

为：$\left(\begin{array}{c}{v}_e\\ {\omega }_e\end{array}\right)=[J_m-J_b{I}_b^{-1}{I}_{\mathrm{bm}}]\stackrel{·}{\Theta }=J^{*}({\psi }_b,\Theta ,m_i,I_i)\stackrel{·}{\Theta }$

此处，v_e和ω_e分别为末端执行器的线速度和角速度，J_m和J_b分别为机械臂 和基座的雅可比矩阵，I_b和I_bm分别为基座的惯性矩阵和基座与机械臂之间的耦合惯性矩阵，为机械臂关节速度矢量。J^*为广义雅可比的符号，Ψ_b为基座的姿态，m_i和I_i分别为机械臂每个连杆的质量和惯性矩阵。 

S5：通过优化计算得出能够使空间机械臂载体卫星姿态变动最小的机械臂型位。首先我们使用四元数方程来表达卫星基座的姿态： 

$Q=\eta +q_1\overrightarrow{i}+q_2\overrightarrow{j}+q_3\overrightarrow{k}=\eta +q\in R^4$

此时整个系统(包括卫星、基座和机械臂)的状态方程为： 

$\left(\begin{array}{c}\stackrel{·}{\eta }\\ \stackrel{.}{q}\end{array}\right)=\frac{1}{2}\left(\begin{array}{cc}0& -{\omega }^T\\ \omega & -\tilde{\omega }\end{array}\right)\left(\begin{array}{c}\eta \\ q\end{array}\right)=\frac{1}{2}\left(\begin{array}{c}-q^T\\ \mathrm{\eta E}-\tilde{q}\end{array}\right)\omega$

此时方向误差为： 

$\left(\begin{array}{c}\mathrm{\delta \eta }={\eta }_{b0}{\eta }_{\mathrm{bf}}+q_{b0}^T{q}_{\mathrm{bf}}\\ \mathrm{\delta q}={\eta }_{b0}{q}_{\mathrm{bf}}-{\eta }_{\mathrm{bf}}{q}_{b0}-{\tilde{q}}_{b0}{q}_{\mathrm{bf}}\end{array}\right)$

此处δq记为我们要优化的目标函数，其值约小，卫星的姿态变动就越小。 

实施例： 

空间机械臂的连杆参数可以如下表示： 

d_bs＝0.13md_se＝1.62md_ew＝1.46md_wt＝0.08m, 

表二为空间机械臂个关节的约束条件： 

表二 

表三为空间机械臂各连杆的质量特性： 

本体包括机械臂基座总质量为1146.347kg，基座相对于卫星几何中心的位置为[0 0.850 0.476](m)，卫星和基座的总惯性矩阵为： 

$I_0=\left(\begin{array}{ccc}291.1& 10.16& -25.15\\ 10.16& 536.3& -4.100\\ -25.15& -5.000& 669.6\end{array}\right)$

假定机械臂总共运行时间20秒，加速和减速的时间均为3秒，末端器相对于卫星几何中心的期望位姿为： 

$T_d=\left(\begin{array}{cccc}0.105& 0.824& 0.557& 1.500\\ 0.653& 0.365& -0.663& 1.200\\ -0.750& 0.433& -0.500& 0.700\\ 0& 0& 0& 1\end{array}\right)$

我们可以从每个关节的限制中得到其对应臂角的范围： 

ψ₁∈[-180 180]deg 

ψ₂∈[-180 180]deg 

ψ₃∈[-159.454 159.454]deg 

ψ₄∈[-180 180]deg 

ψ₅∈[-180 116.769]deg 

ψ₆∈[-180 180]deg 

ψ₇∈[-180 -38.236]∪[32.140 180]deg. 

将上述可行域取交集，我们得到 

[-159.454 -38.216]∪[32.143 116.769]deg 

其可视化的表现可以看图1所示，此时最优的臂角为 

ψ^opt＝-96.452. 

这个臂角对应的每个关节的大小为 

[70.389 92.328 -97.949 121.281 89.096 38.009 28.5831]deg. 

当ψ＝0时，我们有另外一组关节位置的值 

[15.479 29.437 0 121.282 -95.001 52.2726 175.764]deg. 

我们可以注意到上面的结果关节七根本不在其关节限制内，并且经过优化算法的结果也使空间机械臂的基座尽可能保持稳定，此时空间机械臂基座的姿态变化用欧拉角表示为： 

ω_e＝[17.029 5.658 1.632]^Tdeg. 

很明显，除了x轴方向变化较大之外，y轴和z轴方向变化均较小，这正符合我们方法的思想。 

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。