CN103481288A - 一种5关节机器人末端工具位姿控制方法 - Google Patents

Abstract

5旋转关节关节机器人末端工具位置和姿态控制方法，将5R机器人看做是6R（6旋转关节）机器人的特例，对于任意给定的末端执行器姿态和工作点位置，在工作点位置不变的前提下，借助本发明中的方法，找到某个相应的新的末端工具姿态，使这一6R机器人能满足某个腕关节的转动角度始终为0，即相当于使6R机器人退化为5R机器人。以这一新的末端工具姿态和原工作点位置不变为条件，借助6R机器人成熟的逆运动学计算方法，计算出各关节角度所需要的值，并将这些值用于5R机器人各关节的控制。这样得到的5R机器人末端工具位置和姿态，与作为条件给出的具有任意性的末端工具位置和姿态相比，工作点位置没有发生变化，而三个姿态角中只有一个发生了变化，从而解决了在任意给定机器人末端工具姿态和其工作点位置的情况下，5R机器人往往不存在逆运动学解而无法控制的缺点。

Description

一种5关节机器人末端工具位姿控制方法

（一）技术领域

本发明涉及的是5R（5旋转关节）机器人末端工具在直角坐标工作空间中的位姿控制方法，适用于快速、精确地实现5R机器人的轨迹规划及控制。

（二）背景技术

在工业应用中，5R（5旋转关节）机器人具有结构简单、成本低的优点，而且具有5个独立自由度，因此能满足很多场合的应用，如焊接、喷涂、下料等。与5R机器人相比，6R（6旋转关节）机器人因为具有六个旋转关节，因而在其直角工作空间内能使其末端工具达到任意的六自由度方位，为其直角工作空间内的轨迹规划带来了很大方便。根据已有的研究成果，只要6R机器人能满足Piper准则，即有三个相邻的关节其轴线相交于一点，包括三个相邻关节的轴线平行（相当于在无穷远处相交于一点），对任意给定的末端工具的位置和姿态都有对应的逆运动学解。相比之下，5R机器人由于少一个自由度而不能使其末端工具达到其直角坐标工作空间内的任意方位，给其直角坐标空间内的逆运动学计算和轨迹规划带来了困难。曾经有国内外学者用螺旋理论推导了5R机器人逆运动学解析解的框架，也有针对早期的5自由度MOTOMAN机器人给出了用几何方法推导了其逆运动学解的思路，也有的用代数方法对5R机器人逆运动学解析解进行了研究。以上这些方法都从理论上推导了求解一般5R机器人逆解的存在，但并没有进一步给出能用于其轨迹规划的更详细、实用的求解方法。5R机器人逆运动学计算的难点在于，由于5R机器人末端工具不能实现任意姿态和位置，当任意给定5R机器人末端工具目标工作点位置和目标姿态时，5R机器人的逆运动学往往无解。这就严重限制了5R机器人在实际生产中的应用，尤其是在其直角坐标工作空间的轨迹规划中。由于6R机器人不存在这样的问题，因此目前国内外成熟的轨迹规划方法都是针对6R机器人的，所规划出的轨迹往往都是以离散的位置和姿态的形式出现，这些位置和姿态在直角坐标空间中具有一般性和任意性，因此轨迹上很多中间点的位置和姿态对于只有5个自由度的5R机器人来讲难以达到。

要解决这一问题，5R机器人的逆运动学计算及轨迹规划都应在其可达的直角坐标空间内进行，使其末端工具的位置和姿态都能由5R机器人实现。考虑到5R机器人末端工具在笛卡尔空间内的可达位姿空间是6R机器人可达位姿空间的一个子集，因此可以将5R机器人末端工具位姿看做是6R机器人位姿的特例，只要找到将6R机器人位姿映射到5R机器人位姿的方法即可。因此5R机器人的逆运动学问题可以描述为：在其工作的直角坐标空间中任意给定末端工具的一个工作位置和工作姿态，找到一种控制方法，在保证末端工具工作位置不变的前提下，将这一给定的工作姿态映射为5R机器人能达到的工作姿态，根据这一能达到的姿态和给定的工作位置再进一步求出5R机器人各关节角的值。

如果能找到这种5R机器人的控制方法，就能充分利用6R机器人成熟的轨迹规划方法，将6R机器人轨迹规划中得到的具有任意性的末端工具位置和姿态快速准确地转变为5R机器人能实现的轨迹。由于5R机器人比6R机器人少一个关节，成本可以大幅度下降，同时又能完成绝大多数6R机器人能完成的功能，因此这种末端工具的姿态控制方法具有很好的实用价值。

（三）发明内容

当任意给定机器人末端工具目标姿态和目标工作点位置时，5R机器人往往不存在逆运动学解从而无法实现这一给定的目标姿态和工作点位置。为了克服这一缺点，本发明提供这样的关节角控制方法，能将任意给定的机器人末端工具姿态映射为5R机器人能达到的姿态，同时保证末端工具工作点位置不变。

本发明解决其技术问题所采用的技术方案是：

一种5R（5旋转关节）机器人末端工具位置和姿态控制方法，针对的是图1所示的5R机器人。这种形式的机器人可以看做是图2所示的6关节机器人的特例。在图2中，将机器人关节6固定，就构成了图1的构形。在这种控制方法中，由图1所示5R机器人转换来的6R机器人（图2所示）被处理成具有表1所示的D-H参数，本控制方法即是以这种6自由度机器人为基础加以实现。

表1：6关节机器人的D-H参数表

根据机器人学，表1中的D-H参数含义如下：

θ_i——关节i的转角，i=1,2,3,4,5,6，见图1、图2；

α_i-1——关节i-1的轴线和关节i的轴线之间的夹角，i=1,2,3,4,5,6，通常定义α₀为0；

a_i-1——关节i-1的轴线和关节i的轴线之间的公垂线长度，i=1,2,3,4,5,6，通常定义a₀为0；

d_i——定义关节i-1的轴线和关节i的轴线之间的公垂线为A_i-1，则两个相邻的公垂线A_i-1与A_i之间的距离定义为d_i，i=1,2,3,4,5,6，通常定义d₀为0；

本发明的思路是，对于由图1所示5R机器人转换来的具有图2所示结构并具有表1所示D-H参数的6R机器人，在其工作空间内，任意给定末端工具的姿态和工作点位置，在工作点位置不变的前提下，如果能找到某个对应的新姿态，使这一6R机器人关节6的角度始终保持为0，也就相当于将关节6固定而使6R机器人退化为5R机器人，则这一新的姿态可以作为图1所示的5R机器人能实现的姿态，同时保持工作点位置不变。在此基础上，进一步求出5R机器人所对应的各关节角度并加以控制，从而实现对末端工具工作点位置和姿态的控制。

因此，5R机器人末端工具在笛卡尔空间中的可达位置和姿态可用6R机器人定义如下：在6R机器人可达姿态空间中，使某一腕关节转角始终保持不变的那些末端工具的位置姿态。

在描述5R机器人末端工具的位姿控制方法之前，首先约定机器人末端工具姿态描述坐标为X-Y-Z固定直角坐标系，末端工具绕坐标轴X、Y、Z旋转的方位角分别用γ、β、α表示。

对于图1所示的5R机器人，任意给定其末端工具的工作点位置和姿态，按照以下步骤来控制其5个关节角度，就能保证其末端工具工作点位置不变，而姿态却是图1所示5R机器人完全能实现的。

（1）首先将图1所示的5R机器人处理成具有表2所示D-H参数的6R机器人（图2所示）。任意给定5R机器人的末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），根据已有知识，末端工具的位姿矩阵T可按下式算出：

$T=\left|\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right|=\left[\begin{array}{cccc}{c}_{\mathrm{\α}}{c}_{\mathrm{\β}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}-s_{\mathrm{\α}}{c}_{\mathrm{\γ}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}+s_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_x\\ s_{\mathrm{\α}}{c}_{\mathrm{\β}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}+c_{\mathrm{\α}}{c}_{\mathrm{\γ}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}-c_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_y\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{\mathrm{\γ}}& c_{\mathrm{\β}}{c}_{\mathrm{\γ}}& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

式中：c_α、c_β、c_γ——分别代表α、β、γ的余弦；

s_α、s_β、s_γ——分别代表α、β、γ的正弦。

在表1所示的D-H参数下，工作点位置（p_x、p_y、p_z）位于图2所示6R关节机器人的关节4、关节5和关节6轴线相交的地方，其所处的直角坐标系原点位于6R关节机器人的关节1、关节2轴线相交的地方，见图2所示。

（2）末端工具的位姿矩阵T得到后，根据已知的6R机器人逆运动学公式，机器人关节角度θ₁、θ₂、θ₃可按下式计算，而θ₄、θ₅、θ₆则另式计算：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=a\tan (p_y/p_x)\\ {\mathrm{\θ}}_3=a\tan (a_3/d_4)-a\tan \left(\frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\right)\\ {\mathrm{\θ}}_2=a\tan (t_1/t_2)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_4=a\tan \left(\frac{r_{23}{c}_1-r_{13}{s}_1}{r_{33}{s}_{23}-r_{13}{c}_1{c}_{23}-r_{23}{s}_1{c}_{23}}\right)\\ {\mathrm{\θ}}_5=a\tan (s_5/c_5)\\ {\mathrm{\θ}}_6=a\tan (s_6/c_6)\end{array}\right.---\left(2\right)$

式中s_i、c_i——分别代表sin(θ_i)、cos(θ_i)，i=1,2,3,4,5,6；

s_ij、c_ij——分别代表sin(θ_i+θ_j)、cos(θ_i+θ_j)，i,j=1,2,3,4,5,6；

r_ij——机器人末端工具位姿矩阵元素，i,j=1,2,3，见式（1）；

p_x,p_y,p_z——末端工具工作点位置坐标，见式（1）；

atan——反正切函数。

式（2）中的其他参数见下式：

$\left\{\begin{array}{c}K=\frac{{p_x}^2+{p_y}^2+{p_z}^2-a_2^2-a_3^2-d_4^2}{2a_2}\\ t_1=(c_1{p}_x+s_1{p}_y)(a_2{s}_3-d_4)-(a_3+a_2{c}_3)p_z\\ t_2=(a_2{s}_3-d_4)p_z+(a_3+a_2{c}_3)(c_1{p}_x+s_1{p}_y)\\ s_5=r_{33}{s}_{23}{c}_4-r_{13}(c_1{c}_{23}{c}_4+s_1{s}_4)-r_{23}(s_1{c}_{23}{c}_4-c_1{s}_4)\\ c_5=-r_{13}{c}_1{s}_{23}-r_{23}{s}_1{s}_{23}-r_{33}{c}_{23}\\ s_6=r_{31}{s}_{23}{s}_4-r_{11}(c_1{c}_{23}{s}_4-s_1{c}_4)-r_{21}(s_1{c}_{23}{s}_4+c_1{c}_4)\\ c_6=r_{11}((c_1{c}_{23}{c}_4+s_1{s}_4)c_5-c_1{s}_{23}{s}_5)+r_{21}((s_1{c}_{23}{c}_4-c_1{s}_4)c_5-s_1{s}_{23}{s}_5)\\ -r_{31}(s_{23}{c}_4{c}_5+c_{23}{s}_5)\end{array}\right.---\left(3\right)$

（3）增加约束条件，令6R机器人的θ₆等于0，使图2所示的6R机器人退化为图1所示的5R机器人。根据式（2）和式（3）则有下式成立：

s₆=r₃₁s₂₃s₄-r₁₁(c₁c₂₃s₄-s₁c₄)-r₂₁(s₁c₂₃s₄+c₁c₄)=0   （4）

（4）根据增加的约束条件θ₆等于0，将图2所示的6R机器人末端工具的一般位姿（α、β、γ、p_x、p_y、p_z）修改为符合图1所示5R机器人能达到的位姿。具体做法是，保持末端工具工作点位置（p_x、p_y、p_z）和姿态方位角中的α、β不变（或按某种规则重新确定α、β），对姿态方位角γ进行修正，假设修正为γ′，目的是保证机器人末端工具的位姿（α、β、γ′、p_x、p_y、p_z）是图1所示的5R机器人所能达到的。求解γ′的步骤如下：

从式（1）中可以看出，r₁₁、r₂₁、r₃₁只和α、β有关，如果α、β已知则r₁₁、r₂₁、r₃₁可以直接由式（1）给出，从而由式（4）求出关节角θ₄表达式如下：

${\mathrm{\θ}}_4=a\tan \left(\frac{r_{11}{s}_1-r_{21}{c}_1}{r_{11}{c}_1{c}_{23}+r_{21}{s}_1{c}_{23}-r_{31}{s}_{23}}\right)---\left(5\right)$

将式（5）中计算出的θ₄代入到式（2）中θ₄的表达式，可得：

$\tan {\mathrm{\θ}}_4=\frac{r_{23}{c}_1-r_{13}{s}_1}{r_{33}{s}_{23}-r_{13}{c}_1{c}_{23}-r_{23}{s}_1{c}_{23}}---\left(6\right)$

由式（1）可知，T中的姿态元素r₁₃、r₂₃、r₃₃和机器人末端工具的姿态角有如下关系：

r₁₃=c_αs_βc_γ+s_αs_γ;r₂₃=s_αs_βc_γ-c_αs_γ;r₃₃=c_βc_γ   （7）

式中：c_α、c_β、c_γ——分别代表α、β、γ的余弦；

s_α、s_β、s_γ——分别代表α、β、γ的正弦。

将式（7）中的r₁₃、r₂₃、r₃₃代入到式（6）中，可以得到：

$\tan {\mathrm{\θ}}_4=\frac{(s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}-c_{\mathrm{\α}}{s}_{\mathrm{\γ}})c_1-(c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}+s_{\mathrm{\α}}{s}_{\mathrm{\γ}})s_1}{c_{\mathrm{\β}}{c}_{\mathrm{\γ}}{s}_{23}-(c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}+s_{\mathrm{\α}}{s}_{\mathrm{\γ}})c_1{c}_{23}-(s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}-c_{\mathrm{\α}}{s}_{\mathrm{\γ}})s_1{c}_{23}}---\left(8\right)$

从中可以解出修正后的末端工具姿态角γ′：

${\mathrm{\γ}}^{\′}=a\tan \left(\frac{c_1{s}_{\mathrm{\α}}{s}_{\mathrm{\β}}-s_1{c}_{\mathrm{\α}}{s}_{\mathrm{\β}}-(c_{\mathrm{\β}}{s}_{23}-c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_1{c}_{23}-s_1{c}_{23}{s}_{\mathrm{\α}}{s}_{\mathrm{\β}})\tan {\mathrm{\θ}}_4}{s_{\mathrm{\α}}{s}_1+c_{\mathrm{\α}}{c}_1+(c_{\mathrm{\α}}{s}_1{c}_{23}-s_{\mathrm{\α}}{c}_1{c}_{23})\tan {\mathrm{\θ}}_4}\right)---\left(9\right)$

式中，θ₄的正切值由式（6）求出，α、β根据步骤1中已知的姿态角给出，θ₁、θ₂、θ₃则由式（2）求出。

（5）求出γ′后，构造新的末端工具位姿参数（α、β、γ′、p_x、p_y、p_z），并按照下式重新构造5R机器人可达位姿矩阵T′：

$T^{\′}=\left|\begin{array}{cccc}{r}_{11}& {r_{12}}^{\′}& {r_{13}}^{\′}& p_x\\ r_{21}& {r_{22}}^{\′}& {r_{23}}^{\′}& p_y\\ r_{31}& {r_{32}}^{\′}& {r_{33}}^{\′}& p_z\\ 0& 0& 0& 1\end{array}\right|=\left[\begin{array}{cccc}{c}_{\mathrm{\α}}{c}_{\mathrm{\β}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}-s_{\mathrm{\α}}{c}_{{\mathrm{\γ}}^{\′}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}+s_{\mathrm{\α}}{s}_{{\mathrm{\γ}}^{\′}}& p_x\\ s_{\mathrm{\α}}{c}_{\mathrm{\β}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}+c_{\mathrm{\α}}{c}_{{\mathrm{\γ}}^{\′}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}-c_{\mathrm{\α}}{s}_{{\mathrm{\γ}}^{\′}}& p_y\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}& c_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(10\right)$

式中，由于位姿矩阵元素r₁₁、r₂₁、r₃₁只和α、β有关，因此如果α、β确定则r₁₁、r₂₁、r₃₁也确定，见式（1）。根据上面得出的T′，利用6R机器人逆运动学公式（3），可进一步求解相应的关节角θ₅、θ₆的正弦和余弦表达式如下：

$\left\{\begin{array}{c}{s}_5={r_{33}}^{\′}{s}_{23}{c}_4-{r_{13}}^{\′}(c_1{c}_{23}{c}_4+s_1{s}_4)-{r_{23}}^{\′}(s_1{c}_{23}{c}_4-c_1{s}_4)\\ c_5=-{r_{13}}^{\′}{c}_1{s}_{23}-{r_{23}}^{\′}{s}_1{s}_{23}-{r_{33}}^{\′}{c}_{23}\\ s_6=r_{31}{s}_{23}{s}_4-r_{11}(c_1{c}_{23}{s}_4-s_1{c}_4)-r_{21}(s_1{c}_{23}{s}_4+c_1{c}_4)\\ c_6=r_{11}((c_1{c}_{23}{c}_4+s_1{s}_4)c_5-c_1{s}_{23}{s}_5)+r_{21}((s_1{c}_{23}{c}_4-c_1{s}_4)c_5-s_1{s}_{23}{s}_5)\\ -r_{31}(s_{23}{c}_4{c}_5+c_{23}{s}_5)\end{array}\right.---\left(11\right)$

从而可根据式（2）进一步求出θ₅、θ₆如下：

$\left\{\begin{array}{c}{\mathrm{\θ}}_5=a\tan (s_5/c_5)\\ {\mathrm{\θ}}_6=a\tan (s_6/c_6)\end{array}\right.---\left(12\right)$

式中，所求出的θ₆始终为0。

（6）将步骤（2）中求出的θ₁、θ₂、θ₃，步骤（4）中求出的θ₄，步骤（5）中求出的θ₅分别作为5R机器人关节1～关节5的控制目标，也就是将关节1控制为θ₁，将关节2控制为θ₂，将关节3控制为θ₃，将关节4控制为θ₄，将关节5控制为θ₅，此时末端工具的姿态和工作点位置则一定为（α、β、γ′、p_x、p_y、p_z），是图1所示的5R机器人能实现的姿态和工作点位置，与任意给定的姿态和工作点位置（α、β、γ、p_x、p_y、p_z）相比，工作点位置不变，仍然为（p_x、p_y、p_z），而姿态角中α、β不变，只有γ有所变化。

（四）附图说明

图1是适用于本发明的5R机器人结构原理图。

图2是6R机器人结构原理图。

（五）具体实施方式

下面结合附图对本发明作进一步描述。

针对图1、图2所示的带有工具的机器人，其工具工作点（p_toolx、p_tooly、p_toolz）往往不会位于腕中心点位置（例如图1所示关节4、关节5轴线相交处，或图2所示关节4、关节5、关节6轴线相交处），而是位于腕中心点之外。本发明中实施例中，这个工作点在机器人最后一个关节坐标系中的位置转换矩阵必须按下式设置：

$T_7=\left[\begin{array}{cccc}1& 0& 0& d_x\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]---\left(13\right)$

式中的d_x为工具工作点与腕中心点之间的距离，而且工具工作点与关节4轴线形成的平面必须与关节5的轴线垂直，这样可获得式（13）所表示的工作点转换矩阵。根据机器人运动学中坐标变换的原理，工具位姿矩阵T_tool就可以表示为如下矩阵的乘积：

式中，T为式（1）所示姿态矩阵。

通常在机器人应用中，往往给出的是工具工作点的位置（p_toolx、p_tooly、p_toolz）和姿态角（α、β、γ），根据机器人运动学，组成的位姿矩阵T_tool可表示如下：

$T_{\mathrm{tool}}=\left[\begin{array}{cccc}{c}_{\mathrm{\α}}{c}_{\mathrm{\β}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}-s_{\mathrm{\α}}{c}_{\mathrm{\γ}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}+s_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_{\mathrm{toolx}}\\ s_{\mathrm{\α}}{c}_{\mathrm{\β}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}+c_{\mathrm{\α}}{c}_{\mathrm{\γ}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}-c_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_{\mathrm{tooly}}\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{\mathrm{\γ}}& c_{\mathrm{\β}}{c}_{\mathrm{\γ}}& p_{\mathrm{toolz}}\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_{\mathrm{toolx}}\\ r_{21}& r_{22}& r_{23}& p_{\mathrm{tooly}}\\ r_{31}& r_{32}& r_{33}& p_{\mathrm{toolz}}\\ 0& 0& 0& 1\end{array}\right]---\left(15\right)$

根据上式给出的位姿矩阵T_tool和式（14）给出的关系，可以得到式（1）中的位姿矩阵T与T_tool的关系如下：

$T=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_{\mathrm{toolx}}-d_x{r}_{11}\\ r_{21}& r_{22}& r_{23}& p_{\mathrm{tooly}}-d_x{r}_{21}\\ r_{31}& r_{32}& r_{33}& p_{\mathrm{toolz}}-d_x{r}_{31}\\ 0& 0& 0& 1\end{array}\right]---\left(16\right)$

用式（16）得出T后，就可以用式（1）～式（12）给出的过程求解5R机器人各关节角的控制值。从式（16）可以看出，给定工作点坐标（p_toolx、p_tooly、p_toolz）后，根据式（1）、式（10）或式（15），只要α、β确定则可计算出r₁₁、r₂₁、r₃₁，从而求出坐标（p_x、p_y、p_z），这也是工作点在机器人最后一个关节坐标系中的位置转换矩阵需要设置为式（13）形式的原因。

实施例

针对图1所示的带有末端工具的5R机器人，任意给定末端工具工作点位置（p_toolx、p_tooly、p_toolz）和姿态角（α、β、γ），按如下步骤求出其关节1～关节5的目标角度值或控制值：

（1）首先将图1所示5R机器人结构处理成具有表2所示D-H参数的6R机器人（图2所示）结构，工具工作点转换矩阵需要按式（13）进行设置。任意给定末端工具所期望的工作点位置（p_toolx、p_tooly、p_toolz）和姿态角（α、β、γ），用式（15）和式（16）得到式（1）所表示的T，并计算出T中的工作点位置（p_x、p_y、p_z）；

（2）根据步骤（1）计算出的工作点位置（p_x、p_y、p_z），用式（2）、式（3）计算出θ₁、θ₂、θ₃；

（3）根据上一步骤计算出的θ₁、θ₂、θ₃，以及步骤（1）计算出的T，用式（5）计算出关节角θ₄；

（4）根据步骤（3）计算出的θ₄、步骤（2）计算出的θ₁、θ₂、θ₃以及步骤（1）给出的α、β值，按照式（9）求出修正后的γ′；

（5）用求出的γ′与α、β一起组成新的姿态角（α、β、γ′），这是图1所示的5R机器人末端工具能实现的姿态角；

（6）用新的姿态角（α、β、γ′）和步骤（1）中算出的（p_x、p_y、p_z），按照式（10）重新构造位姿矩阵T′；

（7）根据步骤（6）得出的位姿矩阵T′，利用式（12）计算关节角θ₅、θ₆，计算结果中的θ₆始终为0。

（8）将步骤（2）、步骤（3）和步骤（7）得出的θ₁、θ₂、θ₃、θ₄、θ₅，分别作为图1所示5R机器人关节1～关节5的目标值加以控制，也就是将关节1控制为θ₁，将关节2控制为θ₂，将关节3控制为θ₃，将关节4控制为θ₄，将关节5控制为θ₅，所得到的末端工具的姿态和工作点位置即为（α、β、γ′、p_toolx、p_tooly、p_toolz），这是图1所示5R机器人能实现的姿态角和工作位置，与最初给出的任意工作点位置和姿态相比，工作点位置（p_toolx、p_tooly、p_toolz）不变，方位角α、β不变，只有方位角γ有所变化。

Claims (4)

1.5旋转关节机器人末端工具位姿控制方法，其特征在于：将图1所示的5R（5旋转关节）机器人看做是图2所示的并且具有表1所示D-H参数的6R（6旋转关节）机器人中令关节6角度θ₆固定为0的特例，在其直角坐标工作空间内任意给定末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），这种5R机器人的各关节角度的控制值计算过程如下：

表1：6关节机器人的D-H参数表

（1）根据任意给定的机器人末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），按照已知的公式其位姿矩阵T可按下式算出：

$T=\left|\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right|=\left[\begin{array}{cccc}{c}_{\mathrm{\α}}{c}_{\mathrm{\β}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}-s_{\mathrm{\α}}{c}_{\mathrm{\γ}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}+s_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_x\\ s_{\mathrm{\α}}{c}_{\mathrm{\β}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{\mathrm{\γ}}+c_{\mathrm{\α}}{c}_{\mathrm{\γ}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{\mathrm{\γ}}-c_{\mathrm{\α}}{s}_{\mathrm{\γ}}& p_y\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{\mathrm{\γ}}& c_{\mathrm{\β}}{c}_{\mathrm{\γ}}& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

式中：γ、β、α——机器人末端工具分别为绕固连于机器人底座的直角固定坐标轴X、Y、Z旋转的末端工具方位角；

p_x、p_y、p_z——机器人末端工具工作点在X、Y、Z轴上的坐标值。

c_α、c_β、c_γ——分别代表α、β、γ的余弦；

s_α、s_β、s_γ——分别代表α、β、γ的正弦；

（2）求出T后，根据已知公式计算图2所示6R机器人关节角度θ₁、θ₂、θ₃如下：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=a\tan (p_y/p_x)\\ {\mathrm{\θ}}_3=a\tan (a_3/d_4)-a\tan \left(\frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\right)\\ {\mathrm{\θ}}_2=a\tan (t_1/t_2)-{\mathrm{\θ}}_3\end{array}\right.---\left(2\right)$

式中s_i、c_i——分别代表sin(θ_i)、cos(θ_i)，i=1～6；

atan——反正切函数。

式（2）中的其他参数见下式：

$\left\{\begin{array}{c}K=\frac{{p_x}^2+{p_y}^2+{p_z}^2-a_2^2-a_3^2-d_4^2}{2a_2}\\ t_1=(c_1{p}_x+s_1{p}_y)(a_2{s}_3-d_4)-(a_3+a_2{c}_3)p_z\\ t_2=(a_2{s}_3-d_4)p_z+(a_3+a_2{c}_3)(c_1{p}_x+s_1{p}_y)\\ \end{array}\right.---\left(3\right)$

（3）求出T、θ₁、θ₂、θ₃后，按下式计算θ₄：

${\mathrm{\θ}}_4=a\tan \left(\frac{r_{11}{s}_1-r_{21}{c}_1}{r_{11}{c}_1{c}_{23}+r_{21}{s}_1{c}_{23}-r_{31}{s}_{23}}\right)---\left(4\right)$

式中：s_ij、c_ij——分别代表sin(θ_i+θ_j)、cos(θ_i+θ_j)，i,j=1～6；

r_ij——位姿矩阵元素，i,j=1～3，见式（1）；

（4）求出θ₄后，再按下式计算γ′：

${\mathrm{\γ}}^{\′}=a\tan \left(\frac{c_1{s}_{\mathrm{\α}}{s}_{\mathrm{\β}}-s_1{c}_{\mathrm{\α}}{s}_{\mathrm{\β}}-(c_{\mathrm{\β}}{s}_{23}-c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_1{c}_{23}-s_1{c}_{23}{s}_{\mathrm{\α}}{s}_{\mathrm{\β}})\tan {\mathrm{\θ}}_4}{s_{\mathrm{\α}}{s}_1+c_{\mathrm{\α}}{c}_1+(c_{\mathrm{\α}}{s}_1{c}_{23}-s_{\mathrm{\α}}{c}_1{c}_{23})\tan {\mathrm{\θ}}_4}\right)---\left(5\right)$

（5）用求出的γ′，与给定的（p_x、p_y、p_z）和姿态角α、β组成新的位姿（α、β、γ′、p_x、p_y、p_z），这是使图2所示的6R机器人中关节角θ₆等于0的位姿；

（6）根据新的位姿参数（α、β、γ′、p_x、p_y、p_z），按照下式重新计算机器人位姿矩阵T′：

$T^{\′}=\left|\begin{array}{cccc}{r}_{11}& {r_{12}}^{\′}& {r_{13}}^{\′}& p_x\\ r_{21}& {r_{22}}^{\′}& {r_{23}}^{\′}& p_y\\ r_{31}& {r_{32}}^{\′}& {r_{33}}^{\′}& p_z\\ 0& 0& 0& 1\end{array}\right|=\left[\begin{array}{cccc}{c}_{\mathrm{\α}}{c}_{\mathrm{\β}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}-s_{\mathrm{\α}}{c}_{{\mathrm{\γ}}^{\′}}& c_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}+s_{\mathrm{\α}}{s}_{{\mathrm{\γ}}^{\′}}& p_x\\ s_{\mathrm{\α}}{c}_{\mathrm{\β}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}+c_{\mathrm{\α}}{c}_{{\mathrm{\γ}}^{\′}}& s_{\mathrm{\α}}{s}_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}-c_{\mathrm{\α}}{s}_{{\mathrm{\γ}}^{\′}}& p_y\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}& c_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(6\right)$

式中，由于位姿矩阵元素r₁₁、r₂₁、r₃₁只和姿态角α、β相关，因此在α、β不变的前提下，r₁₁、r₂₁、r₃₁也不变，见式（1）。

（7）根据上面得出的T′，利用6R机器人逆运动学公式，按下式计算相应的关节角θ₅、θ₆的正弦和余弦值：

$\left\{\begin{array}{c}{s}_5={r_{33}}^{\′}{s}_{23}{c}_4-{r_{13}}^{\′}(c_1{c}_{23}{c}_4+s_1{s}_4)-{r_{23}}^{\′}(s_1{c}_{23}{c}_4-c_1{s}_4)\\ c_5=-{r_{13}}^{\′}{c}_1{s}_{23}-{r_{23}}^{\′}{s}_1{s}_{23}-{r_{33}}^{\′}{c}_{23}\\ s_6=r_{31}{s}_{23}{s}_4-r_{11}(c_1{c}_{23}{s}_4-s_1{c}_4)-r_{21}(s_1{c}_{23}{s}_4+c_1{c}_4)\\ c_6=r_{11}((c_1{c}_{23}{c}_4+s_1{s}_4)c_5-c_1{s}_{23}{s}_5)+r_{21}((s_1{c}_{23}{c}_4-c_1{s}_4)c_5-s_1{s}_{23}{s}_5)\\ -r_{31}(s_{23}{c}_4{c}_5+c_{23}{s}_5)\end{array}\right.---\left(7\right)$

进而用下式求出节角θ₅、θ₆：

$\left\{\begin{array}{c}{\mathrm{\θ}}_5=a\tan (s_5/c_5)\\ {\mathrm{\θ}}_6=a\tan (s_6/c_6)\end{array}\right.---\left(8\right)$

其中θ₆的计算结果将始终为0。

（8）将上面步骤（2）、步骤（3）、步骤（7）计算出的θ₁、θ₂、θ₃、θ₄、θ₅作为目标值，分别应用于图1所示5R机器人关节1～关节5的角度控制，得到的末端工具姿态和位置即为（α、β、γ′、p_x、p_y、p_z），与初始给定的5R机器人末端工具姿态和位置（α、β、γ、p_x、p_y、p_z）相比，工作点位置（p_x、p_y、p_z）没有变化，工作姿态中只有γ角发生了变化。

2.如权利要求1所述的方法，其特征在于：在其直角坐标工作空间内任意给定机器人末端工具工作点位置（p_x、p_y、p_z）和姿态角（α、β、γ），将图1所示5R机器人结构变换为图2所示6R机器人的特例，然后用式（4）、式（5）计算出θ₄、γ′。

3.如权利要求1所述的方法，其特征在于：针对图1所示的结构，利用重新计算的姿态角γ′，以及给出的α、β、p_x、p_y、p_z，按照式（6）重新构造T′。

4.如权利要求1所述的方法，其特征在于：根据重新构造的T′按照式（7）、式（8）求解θ₅，与式（4）中求出的θ₄以及式（2）中求出的θ₁、θ₂、θ₃一起组成对应着图1所示5R机器人关节1～关节5的角度控制值θ₁、θ₂、θ₃、θ₄、θ₅，其中将关节1角度控制为θ₁，将关节2角度控制为θ₂，将关节3角度控制为θ₃，将关节4角度控制为θ₄，将关节5角度控制为θ₅。

Images