CN103802114A - 工业机器人奇异点处理方法及装置 - Google Patents

Abstract

本发明公开了一种工业机器人奇异点处理方法及装置，该方法包括：建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解；根据所述工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值；根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。本发明采用上述的技术方案，可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

Description

工业机器人奇异点处理方法及装置

技术领域

本发明涉及机器人控制领域，尤其涉及一种工业机器人奇异点处理方法及装置。

背景技术

工业机器人是工作机器，其可以装备用于对对象进行自动处理和/或加工的工具，并可以对多个运动轴，例如就方向、位置和工作流程进行编程。工业机器人通常包括具有多个轴的机器人臂以及可编程控制器（控制装置），控制器在运行中控制或调整工业机器人的运动过程。

对于球形腕型六自由度工业机器人，机器人腕部的三个自由度的轴线(四五六轴)相交于一点。五轴为零度时，四、六轴运动方向重合，即为腕部奇异点。机器人工作的时候很容易走到奇异点，这时候如果可以设计一种简易的算法使机器人能够走过奇异点，将对工业机器人的性能有很大提高，不但可以增加工业机器人的操作空间，还可以使机器人在实际应用中减少了很多的不便。例如，如何利用运动学解决机器人快速安全的通过奇异位形是一个十分有意义的问题。

发明内容

本发明提供一种工业机器人奇异点处理方法及装置，其可克服上述缺陷。为了解决工业机器人在运动中遇到腕部奇异点不能通过的问题，本发明提出了基于检测模块和过渡模块的腕部奇异点处理方法。

为实现上述目的，本发明采用如下技术方案：

建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解；

根据所述工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值；

根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。

其中，所述建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解的公式为：

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

其中，所述建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解包括：

正向求解，由θ1--θ6推导T关节角求位置；反向求解，由T推导θ1--θ6反向运动学，在末端直线轨迹时，进入奇异点处理程序；

显示并下达驱动命名。

其中，所述根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点，包括：

判断所述工业机器人的关节5的值是否大于阈值；

若所述关节5的值不大于阈值，则判定所述工业机器人处于奇异点，并进一步判断末端是否在机器人6轴的X方向运动；

若判定在X轴方向运动，则缓冲过渡关节4运动90度；若判定未在X轴方向运动，则关节5为零（θ₅=0）求解公式。

其中，所述工业机器人为球形腕型六自由度工业机器人。

对应的，本发明还提供一种工业机器人奇异点处理装置，包括：

模型处理模块，用于建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解；

奇异点检测模块，用于根据所述模型处理模块所获取的工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值；

奇异点过渡模块，用于根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。

其中，所述模型处理模块包括：

模型建立单元，用于通过以下传递矩阵公式建立所述工业机器人的运动学模型；

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

模型求解单元，用于根据以下公式求解并获取所述工业机器人运动学的正反解：

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵

得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

其中，所述模型求解单元包括：

正向求解子单元，用于正向求解，由θ1--θ6推导T关节角求位置；

反向求解子单元，用于由T推导θ1--θ6反向运动学，在末端直线轨迹时，进入奇异点处理程序；

显示处理子单元，用于显示工业机器人的位置与姿态信息，将工业机器人关节位置信息发送于关节的驱动器。

其中，所述奇异点过渡模块包括：

第一判断单元，用于判断所述工业机器人的关节5的值是否大于阈值；若所述关节5的值不大于阈值，则判定所述工业机器人处于奇异点；

第二判断单元，用于在所述第一判断单元判定所述工业机器人处于奇异点时，进一步判断其末端是否在机器人6轴的X方向运动；

奇异点处理单元，用于在所述第二判断单元判定所述工业机器人在X轴方向运动时，缓冲过渡——关节4运动90度；在所述第二判断单元判定所述工业机器人未在X轴方向运动时，则关节5为零（θ₅=0）的反解公式求解。

其中，所述工业机器人为球形腕型六自由度工业机器人。

采用上述的技术方案，本发明可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

附图说明

图1为本发明提供的工业机器人奇异点处理方法第一实施例流程示意图；

图2为本发明实施例提供的工业机器人运动学坐标系；

图3为本发明提供的工业机器人奇异点处理方法中，获取所述工业机器人运动学的正反解的流程示意图；

图4为本发明提供的工业机器人奇异点处理方法中，获得机器人运动学的正反解后的处理方法流程示意图；

图5为本发明提供的工业机器人奇异点处理装置第一实施例结构示意图；

图6为本发明提供的工业机器人奇异点处理装置第二实施例结构示意图；

图7为本发明提供的工业机器人奇异点处理装置模型求解单元结构示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参考图1所示，为本发明提供的工业机器人奇异点处理方法第一实施例流程示意图，如图所述，该方法包括：

步骤S101，建立所述工业机器人的运动学模型。

步骤S102，获取所述工业机器人运动学的正反解。

步骤S103，根据所述工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值。

步骤S104，根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，执行步骤S105；若所述判断为否，则执行步骤S106。

步骤S105，控制所述工业机器人通过所述奇异点。

步骤S106，控制所述工业机器人正常运行。

采用上述的技术方案，本发明可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

参考图2、3所示，图2为本发明实施例提供的工业机器人运动学坐标系，在该坐标系下，工业机器人的传动矩阵如以下公式所示：

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

在上述坐标系及传动矩阵下，求得运动学正反解的公式为：

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵

得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

图3为本发明提供的工业机器人奇异点处理方法中，获取所述工业机器人运动学的正反解的流程示意图，如图3所示，该流程包括：

步骤S201，判断可否正向求解述工业机器人运动学。若判断为是，则执行步骤S202；若判断为否，则执行步骤S203。

步骤S202，正向求解，由θ1--θ6推导T关节角求位置。

步骤S203，反向求解，由T推导θ1--θ6反向运动学。

步骤S204，在末端直线轨迹时，进入奇异点处理程序。

步骤S205，显示并下达驱动器命名。

参见图4，为本发明提供的工业机器人奇异点处理方法中，获得机器人运动学的正反解后的处理方法及流程，如图所示，该流程包括：

步骤S301，根据位置参数求解工业机器人的关节1、2、3的值。机器人6轴位置只与关节1、2、3的角度相关，所以首先求出关节1、2、3的值。

步骤S302，判断工业机器人的关节5的值是否大于阈值。关节5值的求解与给定的姿态相关，且姿态部分含有关节1、2、3的值，关节1、2、3的值由S301给出。若判断为是，则执行步骤S303，否则，执行步骤S304。所述阈值是综合机器人控制器截断误差给出的。

步骤S303，判定工业机器人未处于奇异点，正常求解。

步骤S304，判定工业机器人处于奇异点。

步骤S305，进一步判断末端是否在机器人6轴的X方向运动；若判定在X轴方向运动，则执行步骤S306，若判定未在X轴方向运动，则执行步骤S307。

步骤S306，缓冲过渡关节4运动90度。

步骤S307，按关节5为零（θ₅=0）的反解公式进行求解。

步骤S308，显示并下达驱动器命名。

采用上述的技术方案，可以解决工业机器人在运动中遇到腕部奇异点不能通过的问题，本发明可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

参见图5，为本发明提供的工业机器人奇异点处理装置第一实施例结构示意图，如图所示，该装置包括：

模型处理模块1，用于建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解。

奇异点检测模块2，用于根据所述模型处理模块1所获取的工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值。

奇异点过渡模块3，用于根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。

本实施例提供的工业机器人奇异点处理装置，可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

参见图6，为本发明提供的工业机器人奇异点处理装置第二实施例结构示意图，在本实施例中，将进一步描述该装置的结构和各模块的功能。如图所示，该装置包括：模型处理模块1、奇异点检测模块2和奇异点过渡模块3。

模型处理模块1，用于建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解。更为具体的，所述模型处理模块1包括：模型建立单元11和模型求解单元12。

模型建立单元11，用于通过以下传递矩阵公式建立所述工业机器人的运动学模型；

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

模型求解单元，用于根据所述模型建立单元所建立的所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解。

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵

得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

进一步的，所述模型求解单元12如图7所示，包括：

正向求解子单元121，用于正向求解，由θ1--θ6推导T关节角求位置。

反向求解子单元122，用于由T推导θ1--θ6反向运动学，在末端直线轨迹时，进入奇异点处理程序。

显示处理子单元123，用于显示工业机器人的位置与姿态信息，将工业机器人关节位置信息发送于关节的驱动器。

奇异点检测模块2，用于根据所述模型处理模块1所获取的工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值。

奇异点过渡模块3，用于根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。更为具体的，所述奇异点过渡模块3包括：

第一判断单元31，用于判断所述工业机器人的关节5的值是否大于阈值；若所述关节5的值不大于阈值，则判定所述工业机器人处于奇异点。所述阈值是综合机器人控制器截断误差给出的。

第二判断单元32，用于在所述第一判断单元判定所述工业机器人处于奇异点时，进一步判断末端是否在机器人6轴的X方向运动。

奇异点处理单元33，用于在所述第二判断单元判定所述工业机器人在X轴方向运动时，缓冲过渡关节4运动90度；在所述第二判断单元判定所述工业机器人未在X轴方向运动时，则按关节5为零（θ₅=0）的反解公式进行求解。

采用上述的技术方案，可以解决工业机器人在运动中遇到腕部奇异点不能通过的问题，本发明提出了基于检测模块和过渡模块的腕部奇异点处理方法，可以使得工业机器人顺利通过腕部的奇异值，可以使得工业机器人的工作空间更大，在工业机器人接近腕部奇异点时，机器人不会停止工作，提高了工业机器人的灵活性。

需要说明的是，本发明提供的各个工业机器人奇异点处理方法及装置，特别适用于球形腕型六自由度工业机器人。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (10)

1.一种工业机器人奇异点处理方法，其特征在于，包括：

建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解；

根据所述工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值；

根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。

2.如权利要求1所述的工业机器人奇异点处理方法，其特征在于，所述建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解的公式为：

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解。

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

3.如权利要求2所述的工业机器人奇异点处理方法，其特征在于，所述建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解包括：

根据特征2中运动学反解，给出腕部奇异位置的判别方法，θ₅=0进入奇异点处理程序。

4.如权利要求1所述的工业机器人奇异点处理方法，其特征在于，所述根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点，包括：

判断所述工业机器人的关节5的值是否大于阈值；

若所述关节5的值不大于阈值，则判定所述工业机器人处于奇异点，并进一步判断末端是否在机器人6轴的X方向运动；

若判定在6轴X方向运动，则缓冲过渡关节4运动90度；若判定未在6轴X方向运动，则按关节5为零的反解公式进行求解。

5.如权利要求1至4中任一项所述的工业机器人奇异点处理方法，其特征在于，所述工业机器人为球形腕型六自由度工业机器人。

6.一种工业机器人奇异点处理装置，其特征在于，包括：

模型处理模块，用于建立所述工业机器人的运动学模型，获取所述工业机器人运动学的正反解；

奇异点检测模块，用于根据所述模型处理模块所获取的工业机器人的运动学的正反解，获取所述工业机器人处于奇异点的条件值；

奇异点过渡模块，用于根据所述条件值判断所述工业机器人是否处于奇异点；若所述判断为是，则控制所述工业机器人通过所述奇异点。

7.如权利要求6所述的工业机器人奇异点处理装置，其特征在于，所述模型处理模块包括：

模型建立单元，用于通过以下传递矩阵公式建立所述工业机器人的运动学模型；

$T_1^0=\left[\begin{array}{cccc}c1& -s1& 0& 0\\ s1& c1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& c2& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -s3& 0\\ 0& s3& c3& d_2\\ 0& 0& 0& 1\end{array}\right]$ $T_4^3=\left[\begin{array}{cccc}c4& 0& s4& 0\\ 0& 1& 0& d_4\\ -s4& 0& c4& d_3\\ 0& 0& 0& 1\end{array}\right]$

$T_5^4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c5& -s5& 0\\ 0& s5& c5& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_6^5=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& 0\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$ $T_7^6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& d_5\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

模型求解单元，用于根据以下公式求解并获取所述工业机器人运动学的正反解：

正解 $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ 给定工具端位姿矩阵

得到 $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ 令 $T_6^0=M=\left[\begin{array}{cccc}{m}_{11}& m_{12}& m_{13}& p_x\\ m_{21}& m_{22}& m_{23}& p_y\\ m_{31}& m_{32}& m_{33}& p_z\\ 0& 0& 0& 1\end{array}\right],$ 得到运动学反解：

$\left\{\begin{array}{c}{\mathrm{\θ}}_1=-a\tan 2(p_x,p_y)\mathrm{or\π}-a\tan 2(p_x,p_y)\\ {\mathrm{\θ}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)(d_3+c_3{d}_2),(c_1{p}_z-s_1{p}_x-d_1)(d_4+s_3{d}_2)+d_3{p}_z+c_3{d}_2{p}_z)-{\mathrm{\θ}}_3\\ {\mathrm{\θ}}_3=a\tan 2(k,\±\sqrt{1-k^2})-a\tan 2(d_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{d_3}^2-{d_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\θ}}_4=a\tan 2(c_1{m}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\θ}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23})m_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{m}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\θ}}_6=a\tan 2(c_1{c}_{23}{m}_{21}+s_{23}{m}_{31}-s_1{c}_{23}{m}_{11},s_1{c}_{23}{m}_{13}-c_1{c}_{23}{m}_{23}-s_{23}{m}_{32})\end{array}\right.$

${\mathrm{\θ}}_4+{\mathrm{\θ}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\±\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ₅=0

其中，c_i...j=cos(θ_i+...+θ_j)，s_i...j=sin(θ_i+...+θ_j)，θ_i为关节i的转角，d_i，i＝1,2,3,4为机器人连杆参数，m_ij为机器人腕部姿态矩阵中的值。

8.如权利要求7所述的工业机器人奇异点处理装置，其特征在于，所述模型求解单元包括：

正向求解子单元，用于正向求解，由θ1--θ6推导T关节角求位置；

反向求解子单元，用于由T推导θ1--θ6反向运动学，在末端直线轨迹时，进入奇异点处理程序；

显示处理子单元，用于显示工业机器人的位置与姿态信息，将工业机器人关节位置信息发送于关节的驱动器。

9.如权利要求2所述的工业机器人奇异点过渡模块，其特征在于，所述奇异点过渡模块包括：

第一判断单元，用于判断所述工业机器人的关节5的值是否大于阈值；若所述关节5的值不大于阈值，则判定所述工业机器人处于奇异点；

第二判断单元，用于在所述第一判断单元判定所述工业机器人处于奇异点时，进一步判断末端是否在机器人6轴的X方向运动；

奇异点处理单元，用于在所述第二判断单元判定所述工业机器人在X轴方向运动时，缓冲过渡关节4运动90度；在所述第二判断单元判定所述工业机器人未在X轴方向运动时，则关节5为零（θ₅=0）求解公式。

10.如权利要求6至9中任一项所述的工业机器人奇异点处理装置，其特征在于，所述工业机器人为球形腕型六自由度工业机器人。

Images