CN102609002B - Position reversal solution control method of six-freedom cascade mechanical arm - Google Patents

Abstract

The invention discloses a position reversal solution control method of a six-freedom cascade mechanical arm. Characteristics of rotation and translation of a three-dimensional object can be expressed by using a dual quaternion, all rotational joints of the six-freedom cascade mechanical arm are expressed by using the dual quaternion through conversion, a conversion relationship of the rotational joints and the dual quaternion of the six-freedom cascade mechanical arm is determined, and an equation is determined by using links among rotational joints for solving a reversal solution. The position reversal solution control method of the six-freedom cascade mechanical arm has the advantages of better instantaneity and high accuracy.

Description

A kind of position-based routing control method of six degree of freedom cascade machine mechanical arm

Technical field

The present invention relates to a kind of position-based routing control method of six degree of freedom cascade machine mechanical arm.

Background technology

Sixdegree-of-freedom simulation control system, comprises the host computer, slave computer, motor driver, motor and the sixdegree-of-freedom simulation that connect successively.The position-based routing problem of sixdegree-of-freedom simulation is most important in the entire system.To be the most basic in manipulator mechanism and even mechanics be also position-based routing problem most importantly one of to study a question, and it is directly connected to robot movement analysis, off-line programing, trajectory planning and the work such as real-time.The Inverse Kinematics of mechanical arm, be exactly that certain is a bit and a certain attitude in fixing rectangular coordinate space for the hand of mechanical arm, solve the corresponding joint angle of mechanical arm six-freedom degree (or claiming joint coordinates), therefore algorithm for inversion is the element of robot controlling.

The structure of traditional mechanical arm is general more special, and as axes intersect or parallel, axial length is zero etc., and be not just coupled between its attitude and position like this, it is easy to realize by the way of variables separation against solution.But for the more general complicated machinery hand of a class formation size, due to attitude and position height coupling, generally separating variables cannot be carried out, at this moment must by means of numerical algorithm, can be divided three classes: (1) numerical value-analytical method, Newton-Raphson method etc., these algorithms can requirement of real time, is more difficult to get all against solution, and must provides suitable initial value.(2) optimized algorithm, Interval Iterative Methods, genetic algorithm etc., this kind of algorithm convergence scope is large, can obtain all against solution, but general poor real.(3) position and attitude process of iteration respectively, this kind of algorithm more promptly can try to achieve all solutions, but when position of manipulator and the coupling of attitude height, iterative process can be dispersed.

Summary of the invention

In order to overcome the deficiency that real-time is poor, accuracy is not high of existing position-based routing control method, the invention provides the position-based routing control method of the six degree of freedom cascade machine mechanical arm that a kind of real-time is good, accuracy is high.

The technical solution adopted for the present invention to solve the technical problems is:

A kind of position-based routing control method of six degree of freedom cascade machine mechanical arm, it is characterized in that: utilize dual quaterion can represent the character of three-dimensional body rotation and translation, each cradle head of sixdegree-of-freedom simulation is showed by dual quaterion through conversion, in three dimensions, around unit vector u=(u _x, u _y, u _z) rotate the rotation at θ angle and can be expressed as by unit quaternion:

cos(θ/2)+sin(θ/2)(u _xi+u _yj+u _zk)

Namely

q＝[cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>] (1)

Unit quaternion is such as formula the rotation that can describe out rigid body (2) Suo Shi, and a displacement in three dimensions can be synthesized by rotation plus translation, represents rotation, p=(p with unit quaternion q _x, p _y, p _z) represent translation vector, be then expressed as with dual quaterion:

Q(q，p)＝([cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>]，<p _x，p _y，p _z>) (2)

Then dual quaterion is inverted and is expressed as:

Q ^-1＝(q ^-1，-q ^-1*p*q) (14)

Wherein

-q ^-1*p*q＝-p+[-2s(v×(-p))+2v×(v×(-p))] (15)

The cradle head of mechanical arm and the transformational relation of dual quaterion are defined as:

[R _w，T _w]＝([w，<a，b，c>，<p _x，p _y，p _z>]) (16)

Wherein (R _w, T _w) represent each cradle head of mechanical arm relative to the rotation direction of base and the motion vector in three dimensions;

If Q _i, 1≤i≤6 be each mechanical arm cradle head relative to the conversion formula on mechanical arm base space, utilize this conversion formula each cradle head link together as:

Q ₁(q ₁，p ₁)，Q ₂(q ₁，p ₂)…Q ₆(q ₆，p ₆) (17)

And establish

S _i＝Q _iQ _i+1…Q ₆(18)

$L_{j+1}=Q_j^{-1}{L}_j---\left(19\right)$

Wherein get 1≤i≤6 respectively, 1≤j≤5 and L ₁=[R _w, T _w],

In order to can the total variable of extractor mechanical arm cradle head as about s, v, p _x, p _y, p _zfunction, fixing Connecting quantity, assert S _iand L _j+1correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂s ₆=L ₆.

Technical conceive of the present invention is: the displacement that dual quaterion can describe out rigid body in three dimensions can be described again its character rotated and be applied on sixdegree-of-freedom simulation, determine the cradle head of six degree of freedom mechanical arm and the transformational relation of dual quaterion, then obtain inverse solution with the contact establishment equation between each cradle head.The technical matters solved how to be applied on sixdegree-of-freedom simulation by dual quaterion, and the contact established between cradle head is established, and Solving Equations is inverse to be separated.

Beneficial effect of the present invention is mainly manifested in: 1) dual quaterion applies to and solves sixdegree-of-freedom simulation against in solution by invention, has carried out mend gene, and extend new research field to solving mechanical arm against solution method; 2) the proposed by the invention new method solving the inverse solution of mechanical arm is well positioned to meet real-time and the accuracy of mechanical arm Arithmetic of inverse kinematics; 3) the method applied in the present invention is very practical, and calculated amount is little, is easy to realize, and embodies the through engineering approaches of new theory and practical well.

Accompanying drawing explanation

Fig. 1 is RS type mechanical arm simple diagram.

Embodiment

The invention will be further described with reference to the accompanying drawings.

With reference to Fig. 1, a kind of position-based routing control method of six degree of freedom cascade machine mechanical arm, it is characterized in that: utilize dual quaterion can represent the character of three-dimensional body rotation and translation, each cradle head of sixdegree-of-freedom simulation is showed by dual quaterion through conversion, in three dimensions, around unit vector u=(u _x, u _y, u _z) rotate the rotation at θ angle and can be expressed as by unit quaternion:

cos(θ/2)+sin(θ/2)(u _xi+u _yj+u _zk)

Namely

q＝[cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>] (1)

Unit quaternion is such as formula the rotation that can describe out rigid body (2) Suo Shi, and a displacement in three dimensions can be synthesized by rotation plus translation, represents rotation, p=(p with unit quaternion q _x, p _y, p _z) represent translation vector, be then expressed as with dual quaterion:

Q(q，p)＝([cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>]，<p _x，p _y，p _z>) (2)

Then dual quaterion is inverted and is expressed as:

Q ^-1＝(q ^-1，-q ^-1*p*q) (14)

Wherein

-q ^-1*p*q＝-p+[-2s(v×(-p))+2v×(v×(-p))] (15)

The cradle head of mechanical arm and the transformational relation of dual quaterion are defined as:

[R _w，T _w]＝([w，<a，b，c>，<p _x，p _y，p _z>]) (16)

Wherein (R _w, T _w) represent each cradle head of mechanical arm relative to the rotation direction of base and the motion vector in three dimensions;

If Q _i, 1≤i≤6 be each mechanical arm cradle head relative to the conversion formula on mechanical arm base space, utilize this conversion formula each cradle head link together as:

Q ₁(q ₁，p ₁)，Q ₂(q ₁，p ₂)…Q ₆(q ₆，p ₆) (17)

And establish

S _i＝Q _iQ _i+1…Q ₆(18)

$L_{j+1}=Q_j^{-1}{L}_j---\left(19\right)$

Wherein get 1≤i≤6 respectively, 1≤j≤5 and L ₁=[R _w, T _w],

In order to can the total variable of extractor mechanical arm cradle head as about s, v, p _x, p _y, p _zfunction, fixing Connecting quantity, assert S _iand L _i+1correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂s ₆=L ₆.

In the present embodiment, hypercomplex number is defined as follows: if establish q=a+bi+cj+dk (a, b, c, d ∈ R), and i, j, k meet

$\left\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ \mathrm{ij}=-\mathrm{ji}=k\\ \mathrm{jk}=-\mathrm{kj}=i\\ \mathrm{ki}=-\mathrm{ik}=j\end{array}\right.---\left(3\right)$

Then claim q to be hypercomplex number, wherein i, j, k are imaginary number unit, and the relation between each imaginary number unit is such as formula mould a (3) Suo Shi ²+ b ²+ c ²+ d ²the hypercomplex number of=1 is called unit quaternion.

being the conjugate quaternion of q is:

$\stackrel{\&OverBar;}{q}=a-\mathrm{bi}-\mathrm{cj}-\mathrm{dk}---\left(4\right)$

Unit quaternion meets formula:

$q\stackrel{\&OverBar;}{q}=1---\left(5\right)$

Herein hypercomplex number is expressed as convenience's sake:

q＝(s，v) (6)

Wherein s represents scale a, and v represents vectorial bi+cj+dk.And two unit quaternion q ₁and q ₂be multiplied and be expressed as:

q ₁*q ₂＝(s ₁+v ₁)*(s ₂+v ₂₂)＝ (7)

s ₁*s ₂-v ₁·v ₂+v ₁×v ₂+s ₁*v ₂+s ₂*v ₁

Namely

q ₁*q ₂＝[s ₁s ₂-v ₁·v ₂，s ₁v ₂+s ₂v ₁+v ₂×v ₁] (8)

In three dimensions, around unit vector u=(u _x, u _y, u _z) rotate the rotation at θ angle and can be expressed as by unit quaternion

q＝[cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>] (9)

Dual quaterion: unit quaternion such as formula the rotation that can describe out rigid body (9) Suo Shi, but does not describe out rigid body displacement relation in three dimensions.A displacement in three dimensions can be synthesized by rotation plus translation, represents rotation, p=(p with unit quaternion q _x, p _y, p _z) representing translation vector, then can be expressed as with dual quaterion:

Q(q，p)＝([cos(θ/2)，sin(θ/2)<u _x，u _y，u _z>]，<p _x，p _y，p _z>) (10)

Two dual quaterion Q ₁(q ₁, p ₁) and Q ₂(q ₂, p ₂) be multiplied and can be expressed as:

Q ₁Q ₂＝(q ₁，p ₁)*(q ₂，p ₂)＝(q ₁*q ₂，q ₁*p ₂*q ₁ ^-1+p ₁) (11)

Wherein q ₁* q ₂can be able to be obtained by formula (7), and

$q_1*p_2*q_1^{-1}+p_1=p_2+2s_1(v_1\&times;p_2)+2v_1\&times;(v_1\&times;p_2)---\left(12\right)$

The inverse of unit quaternion is asked to be expressed as:

q ^-1＝[s，-v] (13)

Then dual quaterion is inverted and can be expressed as:

Q ^-1＝(q ^-1，-q ^-1*p*q) (14)

Wherein

-q ^-1*p*q＝-p+[-2s(v×(-p))+2v×(v×(-p))] (15)

Solve the inverse of sixdegree-of-freedom simulation to separate: Mechanical transmission test occupies critical role against solution in robot, is directly connected to motion analysis, off-line programing and trajectory planning etc.In order to solve the inverse solution problem of sixdegree-of-freedom simulation, the cradle head of mechanical arm and the transformational relation of dual quaterion may be defined to:

[R _w，T _w]＝([w，<a，b，c>，<p _x，p _y，p _z>]) (16)

Wherein (R _w, T _w) represent each cradle head of mechanical arm relative to the rotation direction of base and the motion vector in three dimensions.

If Q _i(1≤i≤6) for each mechanical arm cradle head is relative to the conversion formula on mechanical arm base space, utilize this conversion formula each cradle head link together as

Q ₁(q ₁，p ₁)，Q ₂(q ₁，p ₂)…Q ₆(q ₆，p ₆) (17)

And establish

S _i＝Q _iQ _i+1…Q ₆(18)

$L_{j+1}=Q_j^{-1}{L}_j---\left(19\right)$

Wherein get 1≤i≤6 respectively, 1≤j≤5 and L ₁=[R _w, T _w].

In order to can the total variable of extractor mechanical arm cradle head as about s, v, p _x, p _y, p _zfunction, fixing Connecting quantity, assert S _iand L _j+1correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂s ₆=L ₆.

Example: for RS mechanical arm simple diagram as shown in Figure 1, solves the inverse solution of mechanical arm.θ in figure ₁, θ ₂, θ ₃, θ ₄, θ ₅, θ ₆for required each joint rotation angle, d ₃for the displacement in removable joint, l ₁and l ₂for length of connecting rod.

Each mechanical arm cradle head is closed relative to the space transforming of mechanical arm base:

$Q_1=\left(\right[\stackrel{\&OverBar;}{c_1},\stackrel{\&OverBar;}{s_1}k],<l_1{c}_1,l_1{s}_1,h_1>)---\left(20\right)$

$Q_2=\left(\right[\stackrel{\&OverBar;}{c_2},\stackrel{\&OverBar;}{s_2}k],<l_2{c}_2,l_2{s}_2,0>)---\left(21\right)$

$Q_3=\left(\right[\mathrm{1,0}],<\mathrm{0,0},-d_3>)---\left(22\right)$

$Q_4=\left(\right[\stackrel{\&OverBar;}{c_4},-\stackrel{\&OverBar;}{s_4}k],<\mathrm{0,0,0}>)---\left(23\right)$

$Q_5=\left(\right[\stackrel{\&OverBar;}{c_5},-\stackrel{\&OverBar;}{s_5}j],<\mathrm{0,0,0}>)---\left(24\right)$

$Q_6=\left(\right[\stackrel{\&OverBar;}{c_6},-\stackrel{\&OverBar;}{s_6}k],<\mathrm{0,0,0}>)---\left(25\right)$

Wherein, s _i, c _irepresent θ respectively _i/ 2, sin (θ _i/ 2), cos (θ _i/ 2), sin (θ _i) and cos (θ _i).

The inverse of them can be tried to achieve according to formula (14):

$Q_1^{-1}=([\stackrel{\&OverBar;}{c_1},\stackrel{\&OverBar;}{s_1}k],<l_1,0,h>)---\left(26\right)$

$Q_2^{-1}=([\stackrel{\&OverBar;}{c_2},\stackrel{\&OverBar;}{s_2}k],<l_2,0,0>)---\left(27\right)$

$Q_3^{-1}=\left(\right[\mathrm{1,0}],<\mathrm{0,0},d_3>)---\left(28\right)$

$Q_4^{-1}=([\stackrel{\&OverBar;}{c_4},\stackrel{\&OverBar;}{s_4}k],<0,0,0>)---\left(29\right)$

$Q_5^{-1}=([\stackrel{\&OverBar;}{c_4},\stackrel{\&OverBar;}{s_4}k],<0,0,0>)---\left(30\right)$

$Q_6^{-1}=([\stackrel{\&OverBar;}{c_6},\stackrel{\&OverBar;}{s_6}k],<0,0,0>)---\left(31\right)$

Transformational relation again through formula (18) (19) can obtain each inverse solution of six free mechanical arms.

As shown in Table 1 and Table 2, known L ₁=([5, <10,25,30>] <10,15,25>), l ₁=100, l ₂=300, h ₁=400 can try to achieve the inverse of mechanical arm separates

θ 1/rad	θ 2/rad	d 3/mm
			0.9828-3.7881i	3.1416-1.0945i	375
-2.1588+3.7881i	3.1416-1.0945i	375
			0.9828-3.7881i	-3.1416+1.0945i	375
-2.1588+3.7881i	-3.1416+1.9045i	375
			0.9828-3.7881i	3.1416-1.0945i	375

0.9828-3.7881i	-3.1416+1.0945i	375
			-2.1588+3.7881i	3.1416-1.0945i	375
-2.1588+3.7881i	-3.1416+1.0945i	375

Table 1

θ 4/rad	θ 5r/ad	θ 6/rad
			-0.4255-3.0915i	0.2119-0.1407i	0.9723+1.7911i
1.1191+0.7214i	0.1724-0.1962i	-0.6246-1.9733i
			-0.4255-0.9025i	0.2119-0.1407i	0.9723+1.7911i
1.1192+2.9109i	0.1726-0.1964i	-0.6246-1.9733i
			-0.4255-3.0915i	-0.2119+0.1407i	0.9723+1.7911i
-0.4255-0.9025i	-0.2119+0.1407i	0.9723+1.7911i
			1.1191+0.7214i	-0.1724+0.1962i	-0.6246-1.9733i
1.1192+2.9109i	-0.1726+0.1964i	-0.6246-1.9733i

Table 2.

Claims (1)

1. a position-based routing control method for six degree of freedom cascade machine mechanical arm, is characterized in that: applied to by dual quaterion and solve mechanical arm against in solution, and dual quaterion can represent that three-dimensional body rotates and translation, in three dimensions, around unit vector u=(u _x, u _y, u _z) rotate the rotation unit quaternion at θ angle and be expressed as:

cos(θ/2)+sin(θ/2)(u _xi+u _yj+u _zk)

Namely

q＝[cos(θ/2),sin(θ/2)<u _x,u _y,u _z>] (1)

Unit quaternion is such as formula the rotation that can describe out rigid body (1) Suo Shi, and a displacement in three dimensions is synthesized by rotation plus translation, represents rotation, p=(p with unit quaternion q _x, p _y, p _z) represent translation vector, be then expressed as with dual quaterion:

Q(q,p)＝([cos(θ/2),sin(θ/2)<u _x,u _y,u _z>],<p _x,p _y,p _z>) (2)

Then dual quaterion inverts expression for (14):

Q ^-1＝(q ^-1,-q ^-1*p*q) (14)

Wherein

-q ^-1*p*q＝-p+[-2s(v×(-p))+2v×(v×(-p))] (15)

The cradle head of mechanical arm and the transformational relation of dual quaterion are defined as:

[R _w,T _w]＝([w,<α,β,γ>,<p _x,p _y,p _z>]) (16)

Wherein (R _w, T _w) represent each cradle head of mechanical arm relative to the rotation direction of base and the motion vector in three dimensions.

If Q _i, 1≤i≤6, for each mechanical arm cradle head is relative to the conversion formula on mechanical arm base space, utilize this conversion formula that namely each cradle head is linked together:

Q ₁(q ₁,p ₁),Q ₂(q ₁,p ₂)…Q ₆(q ₆,p ₆) (17)

And establish

S _i＝Q _iQ _i+1…Q ₆(18)

$L_{j+1}=Q_j^{-1}{L}_j---\left(19\right)$

Wherein get 1≤i≤6 respectively, 1≤j≤5 and L ₁=[R _w, T _w],

In order to the total variable of energy extractor mechanical arm cradle head is namely about s, v, p _x, p _y, p _zfunction, fixing Connecting quantity, S ₁=L ₁, S ₂=L ₂s ₆=L ₆;

Hypercomplex number is defined as follows: if establish q=a+bi+cj+dk, a, b, c, d ∈ R, and i, j, k meet

$\left\{\begin{array}{c}{i}^2=j^2=k^2=-1\\ \mathrm{ij}=-\mathrm{ji}=k\\ \mathrm{jk}=-\mathrm{kj}=i\\ \mathrm{ki}=-\mathrm{ik}=j\end{array}\right.---\left(3\right)$

Then claim q to be hypercomplex number, wherein i, j, k are imaginary number unit, and the relation between each imaginary number unit is such as formula mould a (3) Suo Shi ²+ b ²+ c ²+ d ²the hypercomplex number of=1 is called unit quaternion;

being the conjugate quaternion of q is:

$\stackrel{\&OverBar;}{q}=a-\mathrm{bi}-\mathrm{cj}-\mathrm{dk}---\left(4\right)$

Unit quaternion meets formula:

$q\stackrel{\&OverBar;}{q}=1---\left(5\right)$

Hypercomplex number is expressed as:

q＝(s,v) (6)

Wherein s represents scalar a, and v represents vectorial bi+cj+dk, and two unit quaternion q ₁and q ₂be multiplied and be expressed as:

$\begin{array}{c}{q}_1*q_2=(s_1+v_1)*(s_2+v_2)=\\ s_1*s_2-v_1\&CenterDot;v_2+v_1\&times;v_2+s_1*v_2+s_2*v_1\end{array}---\left(7\right)$

Namely

q ₁*q ₂＝[s ₁s ₂-v ₁·v ₂,s ₁v ₂+s ₂v ₁+v ₂×v ₁] (8)

According to formula (2), two dual quaterion Q ₁(q ₁, p ₁) and Q ₂(q ₂, p ₂) be multiplied and can be expressed as:

Q ₁Q ₂＝(q ₁,p ₁)*(q ₂,p ₂)＝(q ₁*q ₂,q ₁*p ₂*q ₁ ^-1+p ₁) (11)

Wherein q ₁* q ₂can be obtained by formula (7), and

$q_1*p_2*q_1^{-1}+p_1=p_2+2s_1(v_1\&times;p_2)+2v_1\&times;(v_1\&times;p_2)---\left(12\right)$

The reciprocal representation of unit quaternion is asked to become:

q ^-1＝[s,-v] (13)

Obtain dual quaterion by described formula (13) to invert expression formula (14).