CN103802114A - Industrial robot singular point processing method and device - Google Patents

Abstract

The invention discloses an industrial robot singular point processing method and an industrial robot singular point processing device. The method comprises the following steps of establishing a kinematics model of an industrial robot, and acquiring forward and reverse kinematics of the industrial robot; acquiring a condition value when the industrial robot stays on a singular point according to the forward and reverse kinematics of the industrial robot; judging whether the industrial robot stays on the singular point or not according to the condition value; controlling the industrial robot to pass through the singular point if the industrial robot stays on the singular point. By adopting the technical scheme, the industrial robot can smoothly pass through a singular value of a wrist, so that the working space of the industrial robot is larger, when the industrial robot approaches the singular point of the wrist, the robot cannot stop work, and the flexibility of the industrial robot can be improved.

Description

Industrial robot singular point processing method and device

Technical field

The present invention relates to robot control field, relate in particular to a kind of industrial robot singular point processing method and device.

Background technology

Industrial robot is Work machine, and it can equip the instrument for object is automatically processed and/or processed, and can to multiple kinematic axis, for example, programme with regard to direction, position and workflow.Industrial robot generally includes robots arm and the Programmable Logic Controller (control device) with multiple axles, and controller is in operation and controls or adjust the motion process of industrial robot.

For spherical wrist type Six-DOF industrial robot, the axis (4 56 axle) of the three degree of freedom of robot wrist intersects at a point.When five axles are zero degree, four, the six axle directions of motion overlap, and are wrist singular point.When robot work, be easy to go to singular point, make the robot singular point of can passing by if at this time can design a kind of easy algorithm, performance to industrial robot is improved a lot, not only can increase the operating space of industrial robot, can also make robot reduce in actual applications a lot of inconvenience.For example, how to utilize kinematics solve robot fast and safely be a very significant problem by Singularity.

Summary of the invention

The invention provides a kind of industrial robot singular point processing method and device, it can overcome above-mentioned defect.Run at the volley the intransitable problem of wrist singular point in order to solve industrial robot, the present invention proposes the wrist singular point processing method based on detection module and transition module.

For achieving the above object, the present invention adopts following technical scheme:

Set up the kinematics model of described industrial robot, obtain the positive and negative solution of described industrial robot kinematics;

According to the kinematic positive and negative solution of described industrial robot, obtain the condition value of described industrial robot in singular point;

Judge that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.

Wherein, the described kinematics model of setting up described industrial robot, the formula that obtains the positive and negative solution of described industrial robot kinematics is:

$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]$ $\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& \mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">c</figure-callout>}2& 0\\ 0& 0& 0& 1\end{array}\right]$ $\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">s</figure-callout>}3& 0\\ 0& s3& c3& {\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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& {\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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]$

Normal solution $T_7^0=\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1{}_1{}^0\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)({\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\sqrt{1-k^2})-a\tan 2({\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{{\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_3}^2-{{\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\&theta;}}_4=a\tan 2(c_1{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\&theta;}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23}){\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

Wherein, the described kinematics model of setting up described industrial robot, the positive and negative solution of obtaining described industrial robot kinematics comprises:

Forward solves, and asks position by θ 1--θ 6 derivation T joint angles; Oppositely solve, by T derivation θ 1--θ 6 inverse kinematics, when straight path, enter singular point handling procedure endways;

Show and assign and drive name.

Wherein, describedly judge that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point, comprising:

Whether the value that judges the joint 5 of described industrial robot is greater than threshold value;

If the value in described joint 5 is not more than threshold value, judges that described industrial robot is in singular point, and further judge whether end moves at the directions X of robot 6 axles;

If judge in X-direction motion, cushion transition joint 4 90 degree that move; If judge, not in X-direction motion, joint 5 is zero (θ ₅=0) solution formula.

Wherein, the artificial spherical wrist type Six-DOF industrial robot of described industrial machine.

Corresponding, the present invention also provides a kind of industrial robot singular point treating apparatus, comprising:

Model processing modules, for setting up the kinematics model of described industrial robot, obtains the positive and negative solution of described industrial robot kinematics;

Singular point detection module, for the kinematic positive and negative solution of the industrial robot that obtains according to described model processing modules, obtains the condition value of described industrial robot in singular point;

Singular point transition module, for judging that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.

Wherein, described model processing modules comprises:

Model is set up unit, for by the kinematics model of industrial robot described in following transfer matrix Formula;

$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]$ $\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& \mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">c</figure-callout>}2& 0\\ 0& 0& 0& 1\end{array}\right]$ $\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">s</figure-callout>}3& 0\\ 0& s3& c3& {\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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& {\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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]$

Model solution unit, for solve and obtain the positive and negative solution of described industrial robot kinematics according to following formula:

Normal solution $T_7^0=\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1{}_1{}^0\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix

obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_2=a\tan 2(p_z{d}_4+p_z{s}_3{d}_2-(c_1{p}_z-s_1{p}_x-d_1)({\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\sqrt{1-k^2})-a\tan 2({\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_3,d_4),k=({({\cos }_1{p}_y-s_1{p}_x-d_1)}^2+{p_z}^2-{d_4}^2-{{\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_3}^2-{{\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_2}^2)/\sqrt{{\left(d_2{d}_3\right)}^2+{\left(d_2{d}_4\right)}^2}\\ {\mathrm{\&theta;}}_4=a\tan 2(c_1{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+s_1{m}_{22},s_1{s}_{23}{m}_{12}-c_1{s}_{23}{m}_{22}+c_{23}{m}_{32})\\ {\mathrm{\&theta;}}_5=a\tan 2((s_4{c}_1+c_4{s}_1{s}_{23}){\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+(s_4{s}_1-c_1{c}_4{s}_{23})m_{22}+c_4{c}_{23}{m}_{32},{-s}_1{c}_{23}{\mathrm{<figure-callout\; id="12"\; label="m"\; filenames="HDA00002372742800061.png"\; state="\{\{state\}\}">m</figure-callout>}}_{12}+c_1{c}_{23}{m}_{22}+s_{23}{m}_{32})\\ {\mathrm{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

Wherein, described model solution unit comprises:

Forward solves subelement, solves for forward, asks position by θ 1--θ 6 derivation T joint angles;

Oppositely solve subelement, for by T derivation θ 1--θ 6 inverse kinematics, when straight path, enter singular point handling procedure endways;

Graphics Processing subelement, for showing position and the attitude information of industrial robot, is sent in industrial robot joint positional information the driver in joint.

Wherein, described singular point transition module comprises:

Whether the first judging unit, be greater than threshold value for the value in the joint 5 that judges described industrial robot; If the value in described joint 5 is not more than threshold value, judge that described industrial robot is in singular point;

The second judging unit, in the time that described the first judging unit is judged described industrial robot in singular point, further judges whether its end moves at the directions X of robot 6 axles;

Singular point processing unit, for judging that at described the second judging unit described industrial robot is in the time that X-direction is moved, transition---buffering is moved 90 degree in joint 4; Judge that at described the second judging unit described industrial robot is not in the time that X-direction is moved, joint 5 is zero (θ ₅=0) anti-solution formula solves.

Wherein, the artificial spherical wrist type Six-DOF industrial robot of described industrial machine.

Adopt above-mentioned technical scheme, the present invention can make industrial robot smoothly by the singular value of wrist, can make the working space of industrial robot larger, in the time that industrial robot approaches wrist singular point, robot can not quit work, and has improved the flexibility of industrial robot.

Accompanying drawing explanation

Fig. 1 is industrial robot singular point processing method the first embodiment schematic flow sheet provided by the invention;

The industrial robot kinematics coordinate system that Fig. 2 provides for the embodiment of the present invention;

Fig. 3 is in industrial robot singular point processing method provided by the invention, obtains the schematic flow sheet of the positive and negative solution of described industrial robot kinematics;

Fig. 4 is in industrial robot singular point processing method provided by the invention, the process flow schematic diagram after acquisition robot kinematics's positive and negative solution;

Fig. 5 is industrial robot singular point treating apparatus the first example structure schematic diagram provided by the invention;

Fig. 6 is industrial robot singular point treating apparatus the second example structure schematic diagram provided by the invention;

Fig. 7 is industrial robot singular point treating apparatus model solution cellular construction schematic diagram provided by the invention.

The specific embodiment

In order to make object of the present invention, technical scheme and advantage clearer, below in conjunction with drawings and Examples, the present invention is further elaborated.Should be appreciated that specific embodiment described herein, only in order to explain the present invention, is not intended to limit the present invention.

Shown in figure 1, be industrial robot singular point processing method the first embodiment schematic flow sheet provided by the invention, as described in Figure, the method comprises:

Step S101, sets up the kinematics model of described industrial robot.

Step S102, obtains the positive and negative solution of described industrial robot kinematics.

Step S103, according to the kinematic positive and negative solution of described industrial robot, obtains the condition value of described industrial robot in singular point.

Step S104, judges that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, execution step S105; If described in be judged as NO, perform step S106.

Step S105, controls described industrial robot by described singular point.

Step S106, controls described industrial robot and normally moves.

Adopt above-mentioned technical scheme, the present invention can make industrial robot smoothly by the singular value of wrist, can make the working space of industrial robot larger, in the time that industrial robot approaches wrist singular point, robot can not quit work, and has improved the flexibility of industrial robot.

Shown in figure 2,3, the industrial robot kinematics coordinate system that Fig. 2 provides for the embodiment of the present invention, under this coordinate system, the transmission matrix of industrial robot is as shown in following formula:

$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]$ $\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& \mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">c</figure-callout>}2& 0\\ 0& 0& 0& 1\end{array}\right]$ $\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">s</figure-callout>}3& 0\\ 0& s3& c3& {\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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& {\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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]$

Under above-mentioned coordinate system and transmission matrix, the formula of trying to achieve the positive and negative solution of kinematics is:

Normal solution $T_7^0=\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1{}_1{}^0\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix

obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

Fig. 3 is in industrial robot singular point processing method provided by the invention, obtains the schematic flow sheet of the positive and negative solution of described industrial robot kinematics, and as shown in Figure 3, this flow process comprises:

Step S201, judgement could forward solves states industrial robot kinematics.If be judged as YES, perform step S202; If be judged as NO, perform step S203.

Step S202, forward solves, and asks position by θ 1--θ 6 derivation T joint angles.

Step S203, oppositely solves, by T derivation θ 1--θ 6 inverse kinematics.

Step S204, when straight path, enters singular point handling procedure endways.

Step S205, shows and assigns driver and name.

Referring to Fig. 4, in industrial robot singular point processing method provided by the invention, processing method and flow process after acquisition robot kinematics's positive and negative solution, as shown in the figure, this flow process comprises:

Step S301, solves the value in the joint 1,2,3 of industrial robot according to location parameter.Robot 6 shaft positions only with the Angular correlation in joint 1,2,3, so first obtain the value in joint 1,2,3.

Step S302, judges whether the value in the joint 5 of industrial robot is greater than threshold value.What joint 5 was worth solves relevant to given attitude, and the attitude part value that contains joint 1,2,3, and the value in joint 1,2,3 is provided by S301.If be judged as YES, perform step S303, otherwise, execution step S304.Described threshold value is that comprehensive robot controller truncated error provides.

Step S303, judges that industrial robot, not in singular point, normally solves.

Step S304, judges that industrial robot is in singular point.

Step S305, further judges whether end moves at the directions X of robot 6 axles; If judge in X-direction motion, perform step S306, if judge not in X-direction motion, perform step S307.

Step S306,90 degree move in buffering transition joint 4.

Step S307 is zero (θ by joint 5 ₅=0) anti-solution formula solves.

Step S308, shows and assigns driver and name.

Adopt above-mentioned technical scheme, can solve industrial robot and run at the volley the intransitable problem of wrist singular point, the present invention can make industrial robot smoothly by the singular value of wrist, can make the working space of industrial robot larger, in the time that industrial robot approaches wrist singular point, robot can not quit work, and has improved the flexibility of industrial robot.

Referring to Fig. 5, be industrial robot singular point treating apparatus the first example structure schematic diagram provided by the invention, as shown in the figure, this device comprises:

Model processing modules 1, for setting up the kinematics model of described industrial robot, obtains the positive and negative solution of described industrial robot kinematics.

Singular point detection module 2, for the kinematic positive and negative solution of the industrial robot that obtains according to described model processing modules 1, obtains the condition value of described industrial robot in singular point.

Singular point transition module 3, for judging that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.

The industrial robot singular point treating apparatus that the present embodiment provides, can make industrial robot smoothly by the singular value of wrist, can make the working space of industrial robot larger, in the time that industrial robot approaches wrist singular point, robot can not quit work, and has improved the flexibility of industrial robot.

Referring to Fig. 6, be industrial robot singular point treating apparatus the second example structure schematic diagram provided by the invention, in the present embodiment, will further describe the structure of this device and the function of each module.As shown in the figure, this device comprises: model processing modules 1, singular point detection module 2 and singular point transition module 3.

Model processing modules 1, for setting up the kinematics model of described industrial robot, obtains the positive and negative solution of described industrial robot kinematics.More specifically, described model processing modules 1 comprises: model is set up unit 11 and model solution unit 12.

Model is set up unit 11, for by the kinematics model of industrial robot described in following transfer matrix Formula;

$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]$ $\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c2& -s2& d_1\\ 0& s2& \mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">c</figure-callout>}2& 0\\ 0& 0& 0& 1\end{array}\right]$ $\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& c3& -\mathrm{<figure-callout\; id="3"\; label="s"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">s</figure-callout>}3& 0\\ 0& s3& c3& {\mathrm{<figure-callout\; id="2"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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& {\mathrm{<figure-callout\; id="3"\; label="d"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">d</figure-callout>}}_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]$

Model solution unit, for set up the kinematics model of the described industrial robot of setting up unit according to described model, obtains the positive and negative solution of described industrial robot kinematics.

Normal solution $T_7^0=\mathrm{<figure-callout\; id="2"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_2^1{}_1{}^0\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="HDA00002372742800051.png"\; state="\{\{state\}\}">T</figure-callout>}_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix

obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

Further, described model solution unit 12 as shown in Figure 7, comprising:

Forward solves subelement 121, solves for forward, asks position by θ 1--θ 6 derivation T joint angles.

Oppositely solve subelement 122, for by T derivation θ 1--θ 6 inverse kinematics, when straight path, enter singular point handling procedure endways.

Graphics Processing subelement 123, for showing position and the attitude information of industrial robot, is sent in industrial robot joint positional information the driver in joint.

Singular point detection module 2, for the kinematic positive and negative solution of the industrial robot that obtains according to described model processing modules 1, obtains the condition value of described industrial robot in singular point.

Singular point transition module 3, for judging that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.More specifically, described singular point transition module 3 comprises:

Whether the first judging unit 31, be greater than threshold value for the value in the joint 5 that judges described industrial robot; If the value in described joint 5 is not more than threshold value, judge that described industrial robot is in singular point.Described threshold value is that comprehensive robot controller truncated error provides.

The second judging unit 32, in the time that described the first judging unit is judged described industrial robot in singular point, further judges whether end moves at the directions X of robot 6 axles.

Singular point processing unit 33, for judging that at described the second judging unit described industrial robot is in the time that X-direction is moved, 90 degree move in buffering transition joint 4; Judging that at described the second judging unit described industrial robot is not in the time that X-direction is moved, is zero (θ by joint 5 ₅=0) anti-solution formula solves.

Adopt above-mentioned technical scheme, can solve industrial robot and run at the volley the intransitable problem of wrist singular point, the present invention proposes the wrist singular point processing method based on detection module and transition module, can make industrial robot smoothly by the singular value of wrist, can make the working space of industrial robot larger, in the time that industrial robot approaches wrist singular point, robot can not quit work, and has improved the flexibility of industrial robot.

It should be noted that each industrial robot singular point processing method and device provided by the invention are specially adapted to spherical wrist type Six-DOF industrial robot.

The above; only for the preferably specific embodiment of the present invention, but protection scope of the present invention is not limited to this, is anyly familiar with in technical scope that those skilled in the art disclose in the present invention; the variation that can expect easily or replacement, within all should being encompassed in protection scope of the present invention.Therefore, protection scope of the present invention should be as the criterion with the protection domain of claim.

Claims (10)

1. an industrial robot singular point processing method, is characterized in that, comprising:

Set up the kinematics model of described industrial robot, obtain the positive and negative solution of described industrial robot kinematics;

According to the kinematic positive and negative solution of described industrial robot, obtain the condition value of described industrial robot in singular point;

Judge that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.

2. industrial robot singular point processing method as claimed in claim 1, is characterized in that, the described kinematics model of setting up described industrial robot, and the formula that obtains the positive and negative solution of described industrial robot kinematics is:

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

Normal solution $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic.

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

3. industrial robot singular point processing method as claimed in claim 2, is characterized in that, the described kinematics model of setting up described industrial robot, and the positive and negative solution of obtaining described industrial robot kinematics comprises:

According to inverse kinematic in feature 2, provide the method for discrimination of wrist singular position, θ ₅=0 enters singular point handling procedure.

4. industrial robot singular point processing method as claimed in claim 1, is characterized in that, describedly judges that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point, comprising:

Whether the value that judges the joint 5 of described industrial robot is greater than threshold value;

If the value in described joint 5 is not more than threshold value, judges that described industrial robot is in singular point, and further judge whether end moves at the directions X of robot 6 axles;

If judge in 6 axle directions Xs motions, cushion transition joint 4 90 degree that move; If judge, not in 6 axle directions X motions, the anti-solution formula that is zero by joint 5 solves.

5. the industrial robot singular point processing method as described in any one in claim 1 to 4, is characterized in that, the artificial spherical wrist type Six-DOF industrial robot of described industrial machine.

6. an industrial robot singular point treating apparatus, is characterized in that, comprising:

Model processing modules, for setting up the kinematics model of described industrial robot, obtains the positive and negative solution of described industrial robot kinematics;

Singular point detection module, for the kinematic positive and negative solution of the industrial robot that obtains according to described model processing modules, obtains the condition value of described industrial robot in singular point;

Singular point transition module, for judging that according to described condition value whether described industrial robot is in singular point; If described in be judged as YES, control described industrial robot by described singular point.

7. industrial robot singular point treating apparatus as claimed in claim 6, is characterized in that, described model processing modules comprises:

Model is set up unit, for by the kinematics model of industrial robot described in following transfer matrix Formula;

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

Model solution unit, for solve and obtain the positive and negative solution of described industrial robot kinematics according to following formula:

Normal solution $T_7^0=T_2^1{}_1{}^{0}T_3^{2}T_4^{3}TT_6^5{}_5{}^{4}TT_7^6,$ Given tool ends end pose matrix

obtain $T_6^0=T_7^0*{\left(T_7^6\right)}^{-1},$ Order $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],$ Obtain inverse kinematic:

$\left\{\begin{array}{c}{\mathrm{\&theta;}}_1=-a\tan 2(p_x,p_y)\mathrm{or\&pi;}-a\tan 2(p_x,p_y)\\ {\mathrm{\&theta;}}_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{\&theta;}}_3\\ {\mathrm{\&theta;}}_3=a\tan 2(k,\&PlusMinus;\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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_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{\&theta;}}_4+{\mathrm{\&theta;}}_6=a\tan 2(c_1{m}_{13}+s_1{m}_{23},\&PlusMinus;\sqrt{1-{(c_1{m}_{13}+s_1{m}_{23})}^2}),$ whenθ ₅=0

Wherein, c _i...j=cos (θ _i+ ...+θ _j), s _i...j=sin (θ _i+ ...+θ _j), θ _ifor the corner of joint i, d _i, i=1,2,3,4 is robot links parameter, m _ijfor the value in robot wrist attitude matrix.

8. industrial robot singular point treating apparatus as claimed in claim 7, is characterized in that, described model solution unit comprises:

Forward solves subelement, solves for forward, asks position by θ 1--θ 6 derivation T joint angles;

Oppositely solve subelement, for by T derivation θ 1--θ 6 inverse kinematics, when straight path, enter singular point handling procedure endways;

Graphics Processing subelement, for showing position and the attitude information of industrial robot, is sent in industrial robot joint positional information the driver in joint.

9. industrial robot singular point transition module as claimed in claim 2, is characterized in that, described singular point transition module comprises:

Whether the first judging unit, be greater than threshold value for the value in the joint 5 that judges described industrial robot; If the value in described joint 5 is not more than threshold value, judge that described industrial robot is in singular point;

The second judging unit, in the time that described the first judging unit is judged described industrial robot in singular point, further judges whether end moves at the directions X of robot 6 axles;

Singular point processing unit, for judging that at described the second judging unit described industrial robot is in the time that X-direction is moved, 90 degree move in buffering transition joint 4; Judge that at described the second judging unit described industrial robot is not in the time that X-direction is moved, joint 5 is zero (θ ₅=0) solution formula.

10. the industrial robot singular point treating apparatus as described in any one in claim 6 to 9, is characterized in that, the artificial spherical wrist type Six-DOF industrial robot of described industrial machine.

Images