CN118003322A - 一种基于改进臂角法的ssrms机械臂位置级逆解算法 - Google Patents

Abstract

一种基于改进臂角法的SSRMS机械臂位置级逆解算法，涉及机械臂运动能力分析技术领域。建立SSRMS构型机械臂关节运动旋量坐标；定义臂平面和臂角；等效SRS机械臂关节角解析表达式；臂角处于零位状态的等效SRS机械臂运动学解析；给定臂角时的等效SRS机械臂运动学分析；机械臂等效原理分析；臂角与关节角的映射关系；臂角法逆运动学求解结果数值校正。通过给定期望末端位姿和臂角参数求解位置级逆运动学，保证末端位姿不变的情况下调整臂角取值，建立臂角与关节角的映射关系，分析机械臂处于奇异构型时关节角与臂角的联系，使用臂角阈值避免奇异，将臂角法求解得到关节角作为初始条件进行数值迭代，采用牛顿下山法提高求解精度。

Description

一种基于改进臂角法的SSRMS机械臂位置级逆解算法

技术领域

本发明涉及机械臂运动能力分析技术领域，具体是一种基于改进臂角法的SSRMS机械臂位置级逆解算法。

背景技术

SSRMS构型机械臂是与国际空间站遥操作系统(Space Station RemoteManipulator System)关节配置一致、结构相同的七自由度空间机械臂的统称，其由于冗余自由度的存在极大地提高了机械臂的操作灵活性，在完成预期操作任务的同时可以实现避奇异、避障、避关节限位、关节力矩优化等方面的性能优化，因此在空间站建设与维护、辅助航天员完成舱外活动、空间载荷转移、空间结构维修等方面具有广泛的应用。

对于同一个期望末端位姿，冗余机械臂存在无数组关节组合，即操作构型用于完成指定任务。但与此相反，在已知末端位姿的情况下求解冗余机械臂的各关节角，即逆运动学求解过程也会存在无数组解，增加了求解困难程度。为了实现SSRMS构型机械臂的位置级逆运动学求解，徐文福等人(徐文福，等。偏置式冗余空间机械臂逆运动学求解的参数化方法[J]，2015)采用的关节角参数化法通过给定某一关节的参考角度值，然后根据预期任务的末端位姿和机械臂结构求解其余关节角，但该方法给定的参考角度值具有随机性，容易造成求解失败。赵智远等人(赵智远，等。求解SSRMS构型空间机械臂逆运动学的方法[J]，2022)在关节角参数化法的基础上提出了CCDJAP-IK方法，采用循环坐标下降法求得的近似解作为参考角度值，减少了参数取值的盲目性。但上述两种方法通过预先给定参考角度值减少了冗余机械臂一个自由度，使七自由度机械臂“降级”为六自由度机械臂，无法充分发挥冗余机械臂的灵活性。徐文福的方法虽然也将臂型角参数化法(简称“臂角法”)应用于SSRMS构型机械臂的逆运动学求解，通过给定末端位姿和臂角参数得到机械臂各关节角，但该方法求解得到的实际臂角与给定的期望臂角存在误差，在实际使用中容易出现机械臂碰撞等意外情况。

因此，为适应空间操作环境的工作载荷多、分布密集等机械臂活动区域受限的特点，亟需一种求解精度高同时能够发挥SSRMS构型机械臂冗余特性的逆运动学算法用于机械臂的运动控制和路径规划，为空间站建设及空间探索任务等在轨操作技术提供重要保障。

发明内容

为解决背景技术存在的不足，本发明提供一种基于改进臂角法的SSRMS机械臂位置级逆解算法，它通过给定期望末端位姿和臂角参数求解位置级逆运动学，在保证末端位姿不变的情况下通过机械臂的自运动调整臂角取值，通过建立臂角与关节角之间的映射关系，分析机械臂处于奇异构型时关节角与臂角之间的联系，并使用臂角阈值避免因臂角取值不当造成机械臂奇异，将臂角法求解得到机械臂关节角作为初始条件进行数值迭代，采用牛顿下山法提高求解精度。

为实现上述目的，本发明采取下述技术方案：一种基于改进臂角法的SSRMS机械臂位置级逆解算法，包括以下步骤：

步骤一：建立SSRMS构型机械臂的关节运动旋量坐标

以{X_bY_bZ_b}和{X_eY_eZ_e}分别表示机械臂的基坐标系和末端坐标系，以S_i{i＝1,…,7}表示在基坐标系中的各关节运动旋量，其中S₇与末端坐标系的X_e轴重合且方向一致，S_i＝(w_i,v_i)，w_i为运动旋量的单位方向向量，v_i＝-w_i×q_i为与基坐标系原点重合的点相对运动旋量的速度，以q_i表示位于各关节运动旋量上的参考点，以a_k(k＝0,…,8)表示臂杆长度，从而建立七自由度的SSRMS构型机械臂的关节运动旋量坐标，根据各关节运动旋量S_i转过一定角度θ_i后的矩阵指数得到SSRMS构型机械臂的正运动学指数积公式如下：

式中，^bT_e为各关节转过一定角度后末端坐标系相对基坐标系的位姿矩阵，M为机械臂处于零位构型时末端坐标系相对基坐标系的位姿矩阵；

步骤二：SSRMS构型机械臂的臂平面和臂角定义

2.1、臂平面、参考平面和臂角定义

点P_s为关节运动旋量S₂与S₃轴线的交点，点P_w为关节运动旋量S₅与S₆轴线的交点，点P_e为关节运动旋量S₄与臂杆a₅轴线的交点，w为由点P_s指向点P_w的向量，v为过点P_s且方向沿关节运动旋量S₁正向的向量，e为由点P_s指向点P_e的向量，向量w与v构成参考平面，点P_e绕向量w转动过程中，当其位于参考平面上时定义其为P_e0，过点P_s、P_w和P_e的平面为臂平面，与参考平面之间的夹角定义为臂角ψ，向量d为向量e在向量w上的投影，向量k为过点P_e0且与w垂直的向量，为臂平面的法向量，向量p为过点P_e且与w垂直的向量，为参考平面的法向量，臂角ψ也是向量k与向量p之间的夹角；

2.2、臂角解析表达式

得到臂角ψ的解析表达式为：

式中，为向量w的单位向量，为向量k与向量w垂直的单位向量，为向量p与向量w垂直的单位向量；

步骤三：等效SRS机械臂的关节角θ₄解析表达式

设SSRMS构型机械臂的臂杆a₁和a₇为0时，即肩部和腕部偏置退化为SRS机械臂，其余臂杆保持不变，令等效SRS机械臂关节1、2和3的三轴交点为P_s0，关节5、6和7的三轴交点为P_w0，关节1、2和3的旋转不影响点P_s0的位置，即关节5、6和7的旋转不影响点P_w0的位置，即各关节旋转任意角度后变为P_s和P_w，截取以P_s为起点且以P_w为终点的机械臂部分结构为参考，根据余弦定理得到关节角θ₄存在的两组解为：

式中，d_sw为点P_s与P_w之间的距离；

步骤四：臂角处于零位状态的等效SRS机械臂运动学解析

臂角ψ处于零位时表明点P_e位于参考平面内，此时P_e与P_e0重合，臂角ψ处于零位时关节2的正切值为：

式中，s_i＝sinθ_i，c_i＝cosθ_i，c_im＝cos(θ_i+θ_m)，m＝1,…,7且i≠m，从而能够计算得到其它关节角不变的情况下，臂角ψ处于零位时关节角2所对应的角度值⁰θ₂；

根据机械臂的臂杆参数定义，能够得到点P_s与P_e0之间的距离

根据几何关系，存在下列关系式：

式中，e₀为由点P_s指向点P_e0的向量，R(l,α)表示绕向量l旋转角度α的旋转矩阵，通过下式计算：

l＝w×v (27)

R(l,α)＝I+[u_l×]sinα+[u_l×]²(1-cosα) (28)

式中，I表示单位向量，[u_l×]表示向量u_l的反对称矩阵，u_l表示向量l的单位向量；

建立姿态与机械臂基坐标系保持一致的连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}，其中，方向轴Z₃与臂杆a₃方向平行，方向轴X₃与关节4的向量w₄平行，连杆坐标系{X₃Y₃Z₃}表示与关节1、2和3相关的参考坐标系，方向轴Z₄与臂杆a₅方向平行，方向轴X₄与关节5的向量w₅平行，连杆坐标系{X₄Y₄Z₄}表示与关节1、2、3和4相关的参考坐标系，从而能够得到连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄，将各关节旋转一定角度后，连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄分别简写为：

式中，³T₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的位姿矩阵，M₃为初始构型下连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵，M₄为初始构型下连杆坐标系相对基坐标系{X_bY_bZ_b}的位姿矩阵，^bxi、^byi和^bzi为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}的各方向轴在基坐标系各方向轴的投影，^bti为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}原点在基坐标系中的位置，i取3,4；

联立公式(31)-(33)，得到如下关系式：

进而得到^bx₃、^by₃和^by₃的表达式为：

因此，臂角ψ处于零位状态时，可以求解连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵表达式为：

步骤五：给定臂角时的等效SRS机械臂运动学分析

5.1、关节角θ₁、θ₂和θ₃的解析表达式

根据臂角ψ不为0时，根据连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态，得到如下关系式：

^bR₃＝A_esinψ+B_ecosψ+C_e (42)

式中，[u_w×]表示向量u_w的反对称矩阵，u_w表示向量w的单位向量，为臂角ψ处于零位状态时连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态矩阵；

当s₂≠0时关节角θ₁、θ₂和θ₃的三角函数值分别为：

c₂＝A_e(1,1)sinψ+B_e(1,1)cosψ+C_e(1,1) (45)

取θ₁＝θ₂＝0，关节角θ₃的约束关系式为：

5.2、关节角θ₅、θ₆和θ₇的解析表达式

同理，根据连杆坐标系{X₄Y₄Z₄}相对末端坐标系的姿态，得到如下关系式：

^eR₄＝A_wsinψ+B_wcosψ+C_w (53)

式中， ³R₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的姿态；

进一步得到包含待求关节角θ₅、θ₆和θ₇的关系式：

当s₆≠0时关节角θ₅、θ₆和θ₇的三角函数值表达式分别为：

c₆＝-[A_w(1,1)sinψ+B_w(1,1)cosψ+C_w(1,1)] (56)

令θ₆＝θ₇＝0，关节角θ₅的约束关系式为：

步骤六：SSRMS构型机械臂与等效SRS机械臂原理分析

利用臂角ψ和期望末端位姿求解SSRMS构型机械臂位置级逆运动学过程如下：

1)利用得到的等效SRS机械臂逆解模型，求解对应期望位姿和臂角ψ的位置级逆解；

2)对于每一组逆解，利用SSRMS构型机械臂的逆运动学求解模型，参考文献“偏置式冗余空间机械臂逆运动学求解的参数化方法”中的公式(27)至公式(36)，代入关节角θ₁、θ₂、θ₆和θ₇的角度值，得到关节角θ₃、θ₄和θ₅的约束关系方程，逐步求解θ₃、θ₄和θ₅的实际角度值；

步骤七：臂角ψ与关节角θ_i的映射关系

7.1、臂角ψ与关节角θ₁、θ₃、θ₅、θ₇的映射关系

关节1、3、5和7的角度正切值tanθ_j与臂角ψ存在以下关系：

利用正切函数和复合函数求导公式，对公式(62)求微分得到公式：

式中，a_t＝b_dc_n-b_nc_d，b_t＝a_nc_d-a_dc_n，c_t＝a_nb_d-a_db_n；

对公式(62)的分子项进一步开展极限分析得到：

式中，δ为机械臂奇异时的臂角阈值；

分析臂角取值为ψ₀时公式(62)的极限，得到关系式：

对应左右极限分别为：

说明从机械臂奇异的臂角ψ₀左右两侧靠近时关节角的正切值tanθ_j相等，但存在180°的跳变，对应机械臂的肩或肘关节奇异；

7.2、臂角ψ与关节角θ₂和θ₆的映射关系

关节2和6的余弦值cosθ_r与臂角ψ存在以下关系：

cosθ_r＝F(ψ)＝asinψ+bcosψ+c,(r＝2,6) (69)

同样利用余弦函数和复合函数的求导公式对公式(69)求微分可以得到：

当sinθ_r≠0时，公式(69)存在驻点的条件如下：

当sinθ_r＝0时，cosθ_r＝±1，对应臂角ψ的解如下：

机械臂的奇异构型即分别对应公式(73)的临界状态，此时臂角ψ取值如下：

奇异构型处导数左右不连续，但函数值连续，对应机械臂的肩或肘关节奇异；

步骤八：臂角法逆运动学求解结果数值校正

定义误差函数如下：

f(θ₁)＝ψ(θ₁)-ψ_d (75)

式中，ψ(θ₁)为关节1角度为θ₁时的实际臂角，ψ_d为期望臂角；

为此，公式(75)的数值校正问题转换为以下方程的求解问题：

f(θ₁)＝ψ(θ₁)-ψ_d＝ε (76)

式中，ε为满足条件的精度阈值；

为得到满足期望臂角的角度值θ₁，对公式(76)采用牛顿下山法进行数值校正，即：

式中，θ_1(q)为当前关节1的角度值，θ_1(q+1)为迭代得到的角度值，μ(τ)为下山因子，f′(θ_1(q))为函数f(θ_1(q))在θ_1(q)处的一阶导数；

迭代过程中，设置动态下山因子μ(τ)，表达式如下所示：

μ(τ)＝e^-0.5τ (78)

同时对公式(77)设置迭代次数上限，当通过公式(77)迭代得到的θ_1(q+1)满足公式(76)中给定的精度阈值或达到迭代次数上限时，求解算法结束。

与现有技术相比，本发明的有益效果是：

1、通过建立SSRMS构型机械臂与等效SRS机械臂之间的关系，在给定期望末端位姿和臂角取值时进行SSRMS构型机械臂的位置级逆运动学求解；

2、建立机械臂各关节角与臂角参数之间的映射关系，并通过该映射关系分析机械臂处于奇异状态时臂角取值与关节角的对应关系，提出通过臂角阈值避免因臂角取值不当造成的构型奇异；

3、以臂角法求解得到的机械臂关节角作为初始构型，采用牛顿下山法并设置动态下山因子，通过数值迭代降低臂角法的求解误差，使机械臂实际臂角尽可能靠近期望臂角。

附图说明

图1是本发明的SSRMS构型机械臂的关节运动旋量坐标示意图；

图2是本发明的SSRMS构型机械臂定义的臂角参数示意图；

图3是本发明的SSRMS构型机械臂退化为等效SRS机械臂的示意图；

图4是本发明截取的机械臂部分结构的示意图；

图5是本发明在等效SRS机械臂运动学解析中臂角零位状态的示意图；

图6是本发明建立的连杆坐标系的示意图；

图7是本发明设置的下山因子的变化曲线图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1～图7所示，一种基于改进臂角法的SSRMS机械臂位置级逆解算法，包括以下步骤：

步骤一：建立SSRMS构型机械臂的关节运动旋量坐标

结合图1所示，机械臂的七个关节均为旋转关节，其中关节1、2、3为肩关节，关节4为肘关节，关节5、6、7为腕关节，以{X_bY_bZ_b}和{X_eY_eZ_e}分别表示机械臂的基坐标系和末端坐标系，以S_i{i＝1,…,7}表示在基坐标系中的各关节运动旋量，其中S₇与末端坐标系的Xe轴重合且方向一致，以q_i表示位于各关节运动旋量上的参考点，以a_k(k＝0,…,8)表示臂杆长度，其中a₁、a₄和a₇均不为0表明肩部、肘部和腕部存在偏置，a₃和a₅对应臂杆为长臂杆，从而建立七自由度的SSRMS构型机械臂的关节运动旋量坐标，S_i＝(w_i,v_i)的运动旋量参数结合表1所示：

表1 SSRMS构型机械臂的关节运动旋量参数

其中，w_i为运动旋量的单位方向向量，v_i＝-w_i×q_i为与基坐标系原点重合的点相对运动旋量的速度。

根据各关节运动旋量S_i转过一定角度θ_i后的矩阵指数可以得到SSRMS构型机械臂的正运动学指数积公式如下：

式中，^bT_e为各关节转过一定角度后末端坐标系相对基坐标系的位姿矩阵，M为机械臂处于零位构型(图1所示)时末端坐标系相对基坐标系的位姿矩阵；

步骤二：SSRMS构型机械臂的臂平面和臂角定义

2.1、臂平面、参考平面和臂角定义

结合图2所示，点P_s为关节运动旋量S₂与S₃轴线的交点，点P_w为关节运动旋量S₅与S₆轴线的交点，点P_e为关节运动旋量S₄与臂杆a₅轴线的交点，w为由点P_s指向点P_w的向量，v为过点P_s且方向沿关节运动旋量S₁正向的向量，也是机械臂的安装轴线，e为由点P_s指向点P_e的向量，向量w与v构成参考平面，点P_e绕向量w转动过程中，当其位于参考平面上时定义其为P_e0，过点P_s、P_w和P_e的平面为臂平面，与参考平面之间的夹角定义为臂角ψ，向量d为向量e在向量w上的投影，向量k为过点P_e0且与w垂直的向量，为臂平面的法向量，向量p为过点P_e且与w垂直的向量，为参考平面的法向量，所以，臂角ψ也是向量k与向量p之间的夹角；

2.2、臂角解析表达式

向量e在向量w上的投影d的表达式为：

式中，为向量w的单位向量，[·]^T为向量或矩阵[·]的转置，||·||表示向量的欧几里得范数。

位于臂平面内的向量p与向量w垂直的单位向量如下：

式中，I表示单位向量。

位于参考平面内的向量k与向量w垂直的单位向量如下：

k＝(w×v)×v (8)

根据向量之间的关系可以得到臂角ψ的三角函数表达式为：

式中，c_ψ＝cos(ψ)，s_ψ＝sin(ψ)。

因此，可以得到臂角ψ的解析表达式为：

步骤三：等效SRS机械臂的关节角θ₄解析表达式

设SSRMS构型机械臂的臂杆a₁和a₇为0时，即肩部和腕部偏置退化为SRS机械臂，结合图3所示，其余臂杆保持不变。

令等效SRS机械臂关节1、2和3的三轴交点为P_s0，关节5、6和7的三轴交点为P_w0，P_s0和P_w0两点在基坐标系的齐次坐标表示为：

P_s0＝[-a₀ 0 0 1]^T (11)

P_w0＝[-a₀ -a₂ -a₄ -a₆ 0 -a₃ -a₅ 1]^T (12)

根据位置不变原则，关节1、2和3的旋转不影响点P_s0的位置，即关节5、6和7的旋转不影响点P_w0的位置，即所以各关节旋转任意角度后P_s和P_w的齐次坐标分别为：

P_s＝P_s0＝[-a₀ 0 0 1]^T (13)

各关节转过一定角度后，截取以P_s为起点且以P_w为终点的机械臂部分结构为参考，则臂杆a₂、a₃、a₄、a₅和a₆之间存在图4所示的关系。

根据余弦定理可知图4中θ₄的余弦值计算公式为：

所以可以得到关节角θ₄存在的两组解为：

式中，d_sw为点P_s与P_w之间的距离；

步骤四：臂角处于零位状态的等效SRS机械臂运动学解析

臂角ψ处于零位时表明点P_e位于参考平面内，结合图5所示，此时P_e与P_e0重合，并存在以下关系式：

v^Tp＞0 (19)

将v和p的表达式代入公式(18)，为简化三角函数式表达，s_i＝sinθ_i，c_i＝cosθ_i，s_im＝sin(θ_i+θ_m)，c_im＝cos(θ_i+θ_m)，m＝1,…,7且i≠m，可以得到：

(a₂a₅c₃₄+a₄a₅c₃₄-a₃a₆c₃)s₂+a₃a₅s₄c₂＝0 (20)

因此，臂角ψ处于零位时关节2的正切值为：

联立公式(19)和公式(21)得到其它关节角不变的情况下，臂角ψ处于零位时关节角2所对应的角度值⁰θ₂。

根据余弦定理可以得到关系式：

式中，为点P_s与P_e0之间的距离，为点P_w与P_e0之间的距离，进而能够得到：

根据机械臂的臂杆参数定义，可以得到：

因此公式(23)中的cosα>0，即P_e0P_w不是三角形P_e0P_wP_s的最长边，故α<90°，可以得到：

根据几何关系，存在下列关系式：

式中，e₀为由点P_s指向点P_e0的向量，R(l,α)表示绕向量l旋转角度α的旋转矩阵，通过下式计算：

l＝w×v (27)

R(l,α)＝I+[u_l×]sinα+[u_l×]²(1-cosα) (28)

式中，[u_l×]表示向量u_l的反对称矩阵，u_l表示向量l的单位向量。

为方便后续分析，建立姿态与机械臂基坐标系保持一致的连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}，结合图6所示，其中，方向轴Z₃与臂杆a₃方向平行，方向轴X₃与关节4的向量w₄平行，连杆坐标系{X₃Y₃Z₃}表示与关节1、2和3相关的参考坐标系，方向轴Z₄与臂杆a₅方向平行，方向轴X₄与关节5的向量w₅平行，连杆坐标系{X₄Y₄Z₄}表示与关节1、2、3和4相关的参考坐标系。

从而得到连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄如下：

式中，³T₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的位姿矩阵，M₃为初始构型下连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵，M₄为初始构型下连杆坐标系相对基坐标系{X_bY_bZ_b}的位姿矩阵。

将各关节旋转一定角度后，连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄分别简写为：

式中，^bxi、^byi和^bzi为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}的各方向轴在基坐标系各方向轴的投影，^bti为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}原点在基坐标系中的位置，i取3,4。

联立公式(31)-(33)，得到如下关系式：

根据臂角ψ的定义方式，可以得到向量e和w-e在连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}中的坐标表示分别为：

e＝-a₃ ^bz₃-(a₂+a₄)^bx₃ (35)

w-e＝-a₅ ^bz₄-a₆ ^bx₄＝-a₅(^by₃s₄+^bz₃c₄)-a₆ ^bx₃ (36)

联立公式(35)-(36)，得到如下关系式：

可以得到^bx₃、^by₃和^by₃的表达式为：

因此，臂角ψ处于零位状态时，可以求解连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵表达式为：

步骤五：给定臂角时的等效SRS机械臂运动学分析

5.1、关节角θ₁、θ₂和θ₃的解析表达式

当臂角ψ不为0时，可以得到连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态为：

^bR_ψ＝I+[u_w×]sinψ+[u_w×]²(1-cosψ) (41)

式中，为臂角ψ处于零位状态时连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态矩阵，[u_w×]表示向量u_w的反对称矩阵，u_w表示向量w的单位向量。

联立公式(40)-(41)，得到如下关系式：

^bR₃＝A_esinψ+B_ecosψ+C_e (42)

式中，

根据公式(29)计算得到待求关节角θ₁、θ₂和θ₃的关系式为：

当s₂≠0时关节角θ₁、θ₂和θ₃的三角函数值分别为：

c₂＝A_e(1,1)sinψ+B_e(1,1)cosψ+C_e(1,1) (45)

当s₂＝0时，c₂＝±1，根据公式(43)和c₂的取值可以得到：

为方便求解通常取θ₁＝θ₂＝0，则公式(43)可以简化为：

进一步得到关节角θ₃的约束关系式为：

5.2、关节角θ₅、θ₆和θ₇的解析表达式

同理，连杆坐标系{X₄Y₄Z₄}相对末端坐标系的姿态关系如下：

^bR_ψ＝I+[u_w×]sinψ+[u_w×]²(1-cosψ) (52)

因此，可以得到：

^eR₄＝A_wsinψ+B_wcosψ+C_w (53)

式中， ³R₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的姿态。

联立公式(1)和(30)，得到包含待求关节角θ₅、θ₆和θ₇的关系式：

因此，s₆≠0时可以得到关节角θ₅、θ₆和θ₇的三角函数值表达式分别为：

c₆＝-[A_w(1,1)sinψ+B_w(1,1)cosψ+C_w(1,1)] (56)

s₆＝0时，c₆＝±1，根据公式(54)和c₆取值可以得到：

为方便求解通常令θ₆＝θ₇＝0，则公式(54)可以简化为：

可以得到关节角θ₅的约束关系式为：

通过上述分析可以发现，对于给定的机械臂末端位姿与臂角取值，可以求解得到各关节角度值，其中θ₄、θ₂和θ₆存在两组解，因此在[-π，π]范围内机械臂的位置级逆解共有八组解；

步骤六：SSRMS构型机械臂与等效SRS机械臂原理分析

通过已有文献“偏置式冗余空间机械臂逆运动学求解的参数化方法”可知，SSRMS构型机械臂求解位置级逆解时，关节角θ₁、θ₂、θ₆和θ₇的解析解表达式与臂杆a₁和a₇无关。结合上述分析可以得到，在a₁与a₇为0时对于SSRMS构型机械臂得到的等效SRS机械臂，同一期望位姿所对应的位置级逆解，关节角θ₁、θ₂、θ₆和θ₇的角度值不发生变化，同时关节3、4和5的角度值之和(θ₃+θ₄+θ₅)相等，因此，对于同一期望位姿，SSRMS构型机械臂与等效SRS机械臂的逆运动学求解结果对比结合表2所示：

表2 SSRMS构型机械臂与等效SRS机械臂逆运动学求解等效关系

由于SSRMS构型机械臂肘关节存在的偏置特性，使得θ₄与θ₄′存在微调，但二者符号具有一致性，以避免臂平面的翻折，关节偏置特性也导致计算结果得到的实际臂角与给定期望臂角存在差别。

因此可以得到，利用臂角ψ和期望末端位姿求解SSRMS构型机械臂位置级逆运动学过程如下：

1)利用得到的等效SRS机械臂逆解模型，求解对应期望位姿和臂角ψ的位置级逆解，共八组；

2)对于每一组逆解，利用SSRMS构型机械臂的逆运动学求解模型，参考文献“偏置式冗余空间机械臂逆运动学求解的参数化方法”中的公式(27)至公式(36)，代入关节角θ₁、θ₂、θ₆和θ₇的角度值，得到关节角θ₃、θ₄和θ₅的约束关系方程(θ₃+θ₄+θ₅)，逐步求解θ₃、θ₄和θ₅的实际角度值；

步骤七：臂角ψ与关节角θ_i的映射关系

7.1、臂角ψ与关节角θ₁、θ₃、θ₅、θ₇的映射关系

利用臂角ψ求解等效SRS机械臂的逆运动学时，根据公式(17)可以得到关节4的角度值θ₄与臂角ψ无关，根据公式(44)和公式(55)可以得到关节1、3、5和7的角度正切值tanθ_j与臂角ψ存在以下关系：

利用正切函数和复合函数求导公式，对公式(62)求微分得到公式：

式中，a_t＝b_dc_n-b_nc_d，b_t＝a_nc_d-a_dc_n，c_t＝a_nb_d-a_db_n。

公式(63)右侧的分母项始终大于0，对分子项进行分析。当时，公式(62)存在驻点，臂角ψ的取值存在两种情况，分别为：

当时，公式(62)单调，当时，可以得到关系式：

将公式(65)代入公式(62)可以发现等式的分子和分母项均为0，表明臂角取值为ψ₀时机械臂奇异并导致该处的关节角不确定，为此，对公式(62)的分子项进一步开展极限分析并将公式(65)代入得到：

式中，δ为机械臂奇异时的臂角阈值。

分析臂角取值为ψ₀时公式(62)的极限，得到关系式：

对应左右极限分别为：

公式(68)说明从机械臂奇异的臂角ψ₀左右两侧靠近时关节角的正切值tanθ_j相等，但存在180°的跳变，也对应机械臂的肩或肘关节奇异；

7.2、臂角ψ与关节角θ₂和θ₆的映射关系

根据公式(44)和公式(55)可以发现关节2和6的余弦值cosθ_r与臂角ψ存在以下关系：

cosθ_r＝F(ψ)＝asinψ+bcosψ+c,(r＝2,6) (69)

同样利用余弦函数和复合函数的求导公式对公式(69)求微分可以得到：

当sinθ_r≠0时，公式(69)存在驻点的条件如下：

当sinθ_r＝0时，cosθ_r＝±1，公式(69)可以改写为：

对应臂角ψ的解如下：

机械臂的奇异构型即分别对应公式(73)的临界状态，此时臂角ψ取值如下：

分析公式(74)的左右极限可以发现，奇异构型处导数左右不连续，但函数值连续，同样对应机械臂的肩或肘关节奇异；

步骤八：臂角法逆运动学求解结果数值校正

通过上述分析可知，在关节角θ_g(g＝1,2,3,5,6,7)邻域内，臂角ψ通常保持单调递增或递减，为减小求解误差，通过数值算法进一步校正利用臂角法得到的逆运动学求解结果。

根据机械臂构型分析可知，当SSRMS构型机械臂的关节1、2、6和7中的一个关节角固定时，其余关节可以构成有三个连续关节平行的机械臂，对于给定的末端期望位姿，存在八组解析解。为减小通过等效SRS机械臂得到的逆运动学求解结果的误差，通过调整关节1的角度值，计算在末端位姿不变情况下的对应构型，并分析该构型的臂角变化趋势，直至实际臂角ψ(θ₁)无限趋近于给定臂角ψ_d。

定义误差函数如下：

f(θ₁)＝ψ(θ₁)-ψ_d (75)

式中，ψ(θ₁)为关节1角度为θ₁时的实际臂角，ψ_d为期望臂角。

为此，公式(75)的数值校正问题可以转换为以下方程的求解问题：

f(θ₁)＝ψ(θ₁)-ψ_d＝ε (76)

式中，ε为满足条件的精度阈值。

为得到满足期望臂角的角度值θ₁，对公式(76)采用牛顿下山法进行数值校正，即：

式中，θ_1(q)为当前关节1的角度值，θ_1(q+1)为迭代得到的角度值，μ(τ)为下山因子，f′(θ_1(q))为函数f(θ_1(q))在θ_1(q)处的一阶导数。

迭代过程中，为避免下山因子过大导致的振荡现象以及退出失败问题，设置动态下山因子μ(τ)，表达式如下所示：

μ(τ)＝e^-0.5τ (78)

其变化曲线结合图7所示，同时为避免迭代过程的计算时间过长，对公式(77)设置迭代次数上限。因此，当通过公式(77)迭代得到的θ_1(q+1)满足公式(76)中给定的精度阈值或达到迭代次数上限时，求解算法结束。

本发明通过给定期望末端位姿和臂角参数求解位置级逆运动学，发挥了冗余机械臂的灵活性使其能够适应空间工作环境中周围设备状态多变的特性；在保证末端位姿不变的情况下通过机械臂的自运动调整臂角取值，充分利用了七自由度机械臂的冗余特性避免机械臂与周围运动物体发生碰撞，使其能够在受限的活动区域和工作空间中完成预期操作任务；通过建立臂角与关节角之间的映射关系，分析机械臂处于奇异构型时关节角与臂角之间的联系，并使用臂角阈值避免因臂角取值不当造成机械臂奇异，为机械臂自身和周围设备带来损伤，影响空间任务的正常开展；将臂角法求解得到机械臂关节角作为初始条件进行数值迭代，采用牛顿下山法提高了使用传统臂角法进行逆运动学的求解精度，其中，动态下山因子的设置避免了数值迭代过程中出现振荡和退出失败问题，使其能够适应工作载荷多且分布密集的空间环境。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的装体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同条件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

Claims (1)

1.一种基于改进臂角法的SSRMS机械臂位置级逆解算法，其特征在于：包括以下步骤：

步骤一：建立SSRMS构型机械臂的关节运动旋量坐标

以{X_bY_bZ_b}和{X_eY_eZ_e}分别表示机械臂的基坐标系和末端坐标系，以S_i{i＝1,…,7}表示在基坐标系中的各关节运动旋量，其中S₇与末端坐标系的X_e轴重合且方向一致，S_i＝(w_i,v_i)，w_i为运动旋量的单位方向向量，v_i＝-w_i×q_i为与基坐标系原点重合的点相对运动旋量的速度，以q_i表示位于各关节运动旋量上的参考点，以a_k(k＝0,…,8)表示臂杆长度，从而建立七自由度的SSRMS构型机械臂的关节运动旋量坐标，根据各关节运动旋量S_i转过一定角度θ_i后的矩阵指数得到SSRMS构型机械臂的正运动学指数积公式如下：

式中，^bT_e为各关节转过一定角度后末端坐标系相对基坐标系的位姿矩阵，M为机械臂处于零位构型时末端坐标系相对基坐标系的位姿矩阵；

步骤二：SSRMS构型机械臂的臂平面和臂角定义

2.1、臂平面、参考平面和臂角定义

点P_s为关节运动旋量S₂与S₃轴线的交点，点P_w为关节运动旋量S₅与S₆轴线的交点，点P_e为关节运动旋量S₄与臂杆a₅轴线的交点，w为由点P_s指向点P_w的向量，v为过点P_s且方向沿关节运动旋量S₁正向的向量，e为由点P_s指向点P_e的向量，向量w与v构成参考平面，点P_e绕向量w转动过程中，当其位于参考平面上时定义其为P_e0，过点P_s、P_w和P_e的平面为臂平面，与参考平面之间的夹角定义为臂角ψ，向量d为向量e在向量w上的投影，向量k为过点P_e0且与w垂直的向量，为臂平面的法向量，向量p为过点P_e且与w垂直的向量，为参考平面的法向量，臂角ψ也是向量k与向量p之间的夹角；

2.2、臂角解析表达式

得到臂角ψ的解析表达式为：

式中，为向量w的单位向量，为向量k与向量w垂直的单位向量，为向量p与向量w垂直的单位向量；

步骤三：等效SRS机械臂的关节角θ₄解析表达式

设SSRMS构型机械臂的臂杆a₁和a₇为0时，即肩部和腕部偏置退化为SRS机械臂，其余臂杆保持不变，令等效SRS机械臂关节1、2和3的三轴交点为P_s0，关节5、6和7的三轴交点为P_w0，关节1、2和3的旋转不影响点P_s0的位置，即关节5、6和7的旋转不影响点P_w0的位置，即各关节旋转任意角度后变为P_s和P_w，截取以P_s为起点且以P_w为终点的机械臂部分结构为参考，根据余弦定理得到关节角θ₄存在的两组解为：

式中，d_sw为点P_s与P_w之间的距离；

步骤四：臂角处于零位状态的等效SRS机械臂运动学解析

臂角ψ处于零位时表明点P_e位于参考平面内，此时P_e与P_e0重合，臂角ψ处于零位时关节2的正切值为：

式中，s_i＝sinθ_i，c_i＝cosθ_i，c_im＝cos(θ_i+θ_m)，m＝1,…,7且i≠m，从而能够计算得到其它关节角不变的情况下，臂角ψ处于零位时关节角2所对应的角度值⁰θ₂；

根据机械臂的臂杆参数定义，能够得到点P_s与P_e0之间的距离

根据几何关系，存在下列关系式：

式中，e₀为由点P_s指向点P_e0的向量，R(l,α)表示绕向量l旋转角度α的旋转矩阵，通过下式计算：

l＝w×v (27)

R(l,α)＝I+[u_l×]sinα+[u_l×]²(1-cosα) (28)

式中，I表示单位向量，[u_l×]表示向量u_l的反对称矩阵，u_l表示向量l的单位向量；

建立姿态与机械臂基坐标系保持一致的连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}，其中，方向轴Z₃与臂杆a₃方向平行，方向轴X₃与关节4的向量w₄平行，连杆坐标系{X₃Y₃Z₃}表示与关节1、2和3相关的参考坐标系，方向轴Z₄与臂杆a₅方向平行，方向轴X₄与关节5的向量w₅平行，连杆坐标系{X₄Y₄Z₄}表示与关节1、2、3和4相关的参考坐标系，从而能够得到连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄，将各关节旋转一定角度后，连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}相对基坐标系的位姿矩阵^bT₃和^bT₄分别简写为：

式中，³T₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的位姿矩阵，M₃为初始构型下连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵，M₄为初始构型下连杆坐标系相对基坐标系{X_bY_bZ_b}的位姿矩阵，^bx_i、^by_i和^bz_i为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}的各方向轴在基坐标系各方向轴的投影，^bt_i为连杆坐标系{X₃Y₃Z₃}和{X₄Y₄Z₄}原点在基坐标系中的位置，i取3,4；

联立公式(31)-(33)，得到如下关系式：

进而得到^bx₃、^by₃和^by₃的表达式为：

因此，臂角ψ处于零位状态时，可以求解连杆坐标系{X₃Y₃Z₃}相对基坐标系的位姿矩阵表达式为：

步骤五：给定臂角时的等效SRS机械臂运动学分析

5.1、关节角θ₁、θ₂和θ₃的解析表达式

根据臂角ψ不为0时，根据连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态，得到如下关系式：

^bR₃＝A_esinψ+B_ecosψ+C_e (42)

式中，[u_w×]表示向量u_w的反对称矩阵，u_w表示向量w的单位向量，为臂角ψ处于零位状态时连杆坐标系{X₃Y₃Z₃}相对基坐标系的姿态矩阵；

当s₂≠0时关节角θ₁、θ₂和θ₃的三角函数值分别为：

c₂＝A_e(1,1)sinψ+B_e(1,1)cosψ+C_e(1,1) (45)

取θ₁＝θ₂＝0，关节角θ₃的约束关系式为：

5.2、关节角θ₅、θ₆和θ₇的解析表达式

同理，根据连杆坐标系{X₄Y₄Z₄}相对末端坐标系的姿态，得到如下关系式：

^eR₄＝A_wsinψ+B_wcosψ+C_w (53)

式中， ³R₄为连杆坐标系{X₄Y₄Z₄}相对连杆坐标系{X₃Y₃Z₃}的姿态；

进一步得到包含待求关节角θ₅、θ₆和θ₇的关系式：

当s₆≠0时关节角θ₅、θ₆和θ₇的三角函数值表达式分别为：

c₆＝-[A_w(1,1)sinψ+B_w(1,1)cosψ+C_w(1,1)] (56)

令θ₆＝θ₇＝0，关节角θ₅的约束关系式为：

步骤六：SSRMS构型机械臂与等效SRS机械臂原理分析

利用臂角ψ和期望末端位姿求解SSRMS构型机械臂位置级逆运动学过程如下：

1)利用得到的等效SRS机械臂逆解模型，求解对应期望位姿和臂角ψ的位置级逆解；

2)对于每一组逆解，利用SSRMS构型机械臂的逆运动学求解模型，参考文献“偏置式冗余空间机械臂逆运动学求解的参数化方法”中的公式(27)至公式(36)，代入关节角θ₁、θ₂、θ₆和θ₇的角度值，得到关节角θ₃、θ₄和θ₅的约束关系方程，逐步求解θ₃、θ₄和θ₅的实际角度值；

步骤七：臂角ψ与关节角θ_i的映射关系

7.1、臂角ψ与关节角θ₁、θ₃、θ₅、θ₇的映射关系

关节1、3、5和7的角度正切值tanθ_j与臂角ψ存在以下关系：

利用正切函数和复合函数求导公式，对公式(62)求微分得到公式：

式中，a_t＝b_dc_n-b_nc_d，b_t＝a_nc_d-a_dc_n，c_t＝a_nb_d-a_db_n；

对公式(62)的分子项进一步开展极限分析得到：

式中，δ为机械臂奇异时的臂角阈值；

分析臂角取值为ψ₀时公式(62)的极限，得到关系式：

对应左右极限分别为：

说明从机械臂奇异的臂角ψ₀左右两侧靠近时关节角的正切值tanθ_j相等，但存在180°的跳变，对应机械臂的肩或肘关节奇异；

7.2、臂角ψ与关节角θ₂和θ₆的映射关系

关节2和6的余弦值cosθ_r与臂角ψ存在以下关系：

cosθ_r＝F(ψ)＝asinψ+bcosψ+c,(r＝2,6) (69)

同样利用余弦函数和复合函数的求导公式对公式(69)求微分可以得到：

当sinθ_r≠0时，公式(69)存在驻点的条件如下：

当sinθ_r＝0时，cosθ_r＝±1，对应臂角ψ的解如下：

机械臂的奇异构型即分别对应公式(73)的临界状态，此时臂角ψ取值如下：

奇异构型处导数左右不连续，但函数值连续，对应机械臂的肩或肘关节奇异；

步骤八：臂角法逆运动学求解结果数值校正

定义误差函数如下：

f(θ₁)＝ψ(θ₁)-ψ_d (75)

式中，ψ(θ₁)为关节1角度为θ₁时的实际臂角，ψ_d为期望臂角；

为此，公式(75)的数值校正问题转换为以下方程的求解问题：

f(θ₁)＝ψ(θ₁)-ψ_d＝ε (76)

式中，ε为满足条件的精度阈值；

为得到满足期望臂角的角度值θ₁，对公式(76)采用牛顿下山法进行数值校正，即：

式中，θ_1(q)为当前关节1的角度值，θ_1(q+1)为迭代得到的角度值，μ(τ)为下山因子，f′(θ_1(q))为函数f(θ_1(q))在θ_1(q)处的一阶导数；

迭代过程中，设置动态下山因子μ(τ)，表达式如下所示：

μ(τ)＝e^-0.5τ (78)

同时对公式(77)设置迭代次数上限，当通过公式(77)迭代得到的θ_1(q+1)满足公式(76)中给定的精度阈值或达到迭代次数上限时，求解算法结束。