CN103529856B - 5旋转关节机器人末端工具位姿控制方法 - Google Patents

Abstract

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

Description

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

(一)技术领域

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

(二)背景技术

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

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

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

(三)发明内容

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

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

5R(5旋转关节)机器人末端工具位置和姿态控制方法，针对的是图1所示的5旋转关节机器人。这种形式的机器人可以看做是图2所示的6旋转关节机器人的特例。在图2中，将机器人关节4固定则构成了图1的构形。在这种控制方法中，由图1所示5旋转关节机器人转换来的6旋转关节机器人(图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所示5旋转关节机器人转换来的具有图2所示结构并具有表1所示D-H参数的6旋转关节机器人，在其工作空间内，任意给定末端工具的姿态和工作点位置，在工作点位置不变的前提下，如果能找到某个对应的新姿态，使这一6旋转关节机器人关节4的角度始终保持为0，也就相当于将关节4固定而使6旋转关节机器人退化为5旋转关节机器人，则这一新的姿态可以作为图1所示的5旋转关节机器人能实现的姿态，同时保持工作点位置不变。在此基础上，进一步求出5旋转关节机器人所对应的各关节角度并加以控制，从而实现对末端工具位置和这一新姿态的控制。

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

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

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

(1)首先将图1所示的5旋转关节机器人处理成具有表2所示D-H参数的6旋转关节机器人(图2所示)。任意给定末端工具工作点位置(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所示6旋转关节关节机器人的关节4、关节5和关节6轴线相交的地方，其所处的直角坐标系原点位于6旋转关节关节机器人的关节1、关节2轴线相交的地方，如图2所示。

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

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=a\tan \left(p_y/p_x\right)\\ {\mathrm{\θ}}_3=a\tan \left(a_3/d_4\right)-a\tan \left(\frac{K}{\sqrt{{a_3}^2+{d_4}^2-K^2}}\right)\\ {\mathrm{\θ}}_2=a\tan \left(t_1/t_2\right)-{\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 \left(s_5/c_5\right)\\ {\mathrm{\θ}}_6=a\tan \left(s_6/c_6\right)\end{array}\right.---\left(2\right)$

式中

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=\left(c_1{p}_x+s_1{p}_y\right)\left(a_2{s}_3-d_4\right)-\left(a_3+a_2{c}_3\right)p_z\\ t_2=\left(a_2{s}_3-d_4\right)p_z+\left(a_3+a_2{c}_3\right)\left(c_1{p}_x+s_1{p}_y\right)\\ s_5=r_{33}{s}_{23}{c}_4-r_{13}\left(c_1{c}_{23}{c}_4+s_1{s}_4\right)-r_{23}\left(s_1{c}_{23}{c}_4-c_1{s}_4\right)\\ c_5=-r_{13}{c}_1{s}_{23}-r_{23}{s}_1{s}_{23}-r_{33}{c}_{33}\\ s_6=r_{31}{s}_{23}{s}_4-r_{11}\left(c_1{c}_{23}{s}_4-s_1{c}_4\right)-r_{21}\left(s_1{c}_{23}{s}_4+c_1{c}_4\right)\\ c_6=r_{11}\left(\left(c_1{c}_{23}{c}_4+s_1{s}_4\right)c_5-c_1{s}_{23}{s}_5\right)+r_{21}\left(\left(s_1{c}_{23}{c}_4-c_1{s}_4\right)c_5-s_1{s}_{23}{s}_5\right)\\ -r_{31}\left(s_{23}{c}_4{c}_5+c_{23}{s}_5\right)\end{array}\right.---\left(3\right)$

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

(3)增加约束条件，令图2所示6旋转关节机器人的θ₄等于0，使6旋转关节机器人退化为图1所示的5旋转关节机器人，根据式(2)则有如下关系成立：

${\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)=0---\left(4\right)$

由此可得：

r₂₃c₁-r₁₃s₁＝0(5)

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

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

r₁₃＝c_αs_βc_γ+s_αs_γ；r₂₃＝s_αs_βc_γ-c_αs_γ；r₃₃＝c_βc_γ(6)

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

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

在姿态角α、β确定的前提下，将式(6)中的r₁₃、r₂₃代入到式(5)中，可以得到：

(s_αs_βc_γ-c_αs_γ)c₁-(c_αs_βc_γ+s_αs_γ)s₁＝0(7)

从中可以解出修正后的姿态角γ′为：

${\mathrm{\γ}}^{\′}=atan\left(\frac{s_{\mathrm{\α}}{s}_{\mathrm{\β}}-c_{\mathrm{\α}}{s}_{\mathrm{\β}}{\mathrm{tan\θ}}_1}{c_{\mathrm{\α}}+s_{\mathrm{\α}}{\mathrm{tan\θ}}_1}\right)---\left(8\right)$

式中的θ₁由式(2)计算出，α、β由步骤1中已知的姿态角给出。

(5)求出γ′后，构造新的末端工具位姿参数(α，β，γ′，p_x，p_y，p_z)，按照下式重新构造5旋转关节机器人可达位姿矩阵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(9\right)$

式中，由于r₁₁、r₂₁、r₃₁只和α、β有关，如果α、β已知则可算出r₁₁、r₂₁、r₃₁。根据上面得出的T′，利用式(3)，可进一步求解相应的关节角θ₅、θ₆的正弦和余弦表达式如下：

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

(6)根据式(9)得出的T′、步骤(2)求出的θ₁、θ₂、θ₃和式(10)求解出的θ₅、θ₆的正弦值和余弦值，按照式(2)进一步求出θ₄、θ₅、θ₆如下：

$\left\{\begin{array}{c}{\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 \left(s_5/c_5\right)\\ {\mathrm{\θ}}_5=a\tan \left(s_6/c_6\right)\end{array}\right.---\left(11\right)$

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

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

(四)附图说明

图1是适用于本发明的5旋转关节机器人结构原理图。

图2是6旋转关节机器人结构原理图。

(五)具体实施方式

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

针对图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(12\right)$

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

$\begin{array}{c}{T}_{tool}=T\·T_7=\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}1& 0& 0& d_x\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x+d_x{r}_{11}\\ r_{21}& r_{22}& r_{23}& p_y+d_x{r}_{21}\\ r_{31}& r_{32}& r_{33}& p_z+d_x{r}_{31}\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_{toolx}\\ r_{21}& r_{22}& r_{23}& p_{tooly}\\ r_{31}& r_{32}& r_{33}& p_{toolz}\\ 0& 0& 0& 1\end{array}\right]\end{array}---\left(13\right)$

式中，T为式(1)所示位姿矩阵。

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

$T_{toll}=\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_{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_{tooly}\\ -s_{\mathrm{\β}}& c_{\mathrm{\β}}{s}_{\mathrm{\γ}}& c_{\mathrm{\β}}{c}_{\mathrm{\γ}}& p_{toolz}\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_{toolx}\\ r_{21}& r_{22}& r_{23}& p_{tooly}\\ r_{31}& r_{32}& r_{33}& p_{toolz}\\ 0& 0& 0& 1\end{array}\right]---\left(14\right)$

根据上式给出的位姿矩阵T_tool和式(13)给出的关系，可以得到式(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_{toolx}-d_x{r}_{11}\\ r_{21}& r_{22}& r_{23}& p_{tooly}-d_x{r}_{21}\\ r_{31}& r_{32}& r_{33}& p_{toolz}-d_x{r}_{31}\\ 0& 0& 0& 1\end{array}\right]---\left(15\right)$

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

实施例

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

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

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

(3)根据步骤(2)计算出的θ₁，以及步骤(1)给出的工具姿态角α、β，用式(8)计算修正后的γ′；

(4)用求出的γ′与α、β一起组成新的姿态角(α，β，γ′)，这是图1所示的5旋转关节机器人末端工具能实现的姿态角；

(5)用新的工具姿态角(α，β，γ′)和步骤(1)中算出的(p_x，p_y，p_z)，按照式(9)重新构造位姿矩阵T′；

(6)根据步骤(5)得出的位姿矩阵T′和步骤(2)计算出的θ₁、θ₂、θ₃，利用式(11)计算关节角θ₄、θ₅、θ₆，计算结果中的θ₄将始终为0。

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

Claims (1)

1.5旋转关节机器人末端工具位姿控制方法，其特征在于：将5旋转关节机器人看做是具有表1所示D-H参数的6旋转关节机器人中令关节6角度θ₆固定为0的特例，在其直角坐标工作空间内任意给定末端工具工作点位置(p_x，p_y，p_z)和姿态角(α，β，γ)；

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

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

θ_i——关节i的转角，i＝1,2,3,4,5,6；

α_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；这种5旋转关节机器人的各关节角度的控制值计算过程如下：

步骤(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{\β}}& s_{\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后，根据已知的6旋转关节机器人逆运动学公式计算6旋转关节机器人关节角度θ₁、θ₂、θ₃如下：

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

式中

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)$

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

步骤(3)根据上一步骤计算出的θ₁和步骤(1)给出的α、β，按下式求解修正后的姿态角γ′为：

${\mathrm{\γ}}^{\′}=atan\left(\frac{s_{\mathrm{\α}}{s}_{\mathrm{\β}}-c_{\mathrm{\α}}{s}_{\mathrm{\β}}{\mathrm{tan\θ}}_1}{c_{\mathrm{\α}}+s_{\mathrm{\α}}{\mathrm{tan\θ}}_1}\right)---\left(4\right)$

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

步骤(4)用求出的γ′，与给定的(p_x，p_y，p_z)和姿态角α、β组成新的位姿(α，β，γ，p_x，p_y，p_z)，这是使6旋转关节机器人中关节角θ₄等于0的位姿；

步骤(5)根据新的位姿参数(α，β，γ，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{\β}}& s_{\mathrm{\β}}{s}_{{\mathrm{\γ}}^{\′}}& c_{\mathrm{\β}}{c}_{{\mathrm{\γ}}^{\′}}& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(5\right)$

式中，由于位姿矩阵元素r₁₁、r₂₁、r₃₁只和姿态角α、β相关，因此在α、β确定的前提下可计算r₁₁、r₂₁、r₃₁，见式(1)；

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

$\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{s}_4)\\ c_6=r_{11}\left(\right(c_1{c}_{23}{c}_4+s_1{s}_4)c_5-c_1{s}_{23}{s}_5)+r_{21}\left(\right(s_1{c}_{23}{s}_4-c_1{s}_4)c_5-s_1{s}_{23}{s}_5)\\ -r_{31}\left(s_{23}{c}_4{c}_5+c_{23}{s}_5\right)\end{array}\right.---\left(6\right)$

步骤(7)在由步骤(2)求出θ₁的条件下，进而用下式求出关节角θ₄、θ₅、θ₆：

$\left\{\begin{array}{c}{\mathrm{\θ}}_4=atan\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=atan(s_5/c_5)\\ {\mathrm{\θ}}_6=atan(s_6/c_6)\end{array}\right.---\left(7\right)$

其中θ₄的计算结果将始终为0；

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