CN102609002A - 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 inverse position of six degree of freedom series connection mechanical arm is separated control method

Technical field

The inverse position that the present invention relates to a kind of six degree of freedom series connection mechanical arm is separated control method.

Background technology

The six degree of freedom mechanical arm control system comprises the host computer, slave computer, motor driver, motor and the six degree of freedom mechanical arm that connect successively.To separate problem most important for the inverse position of six degree of freedom mechanical arm in total system.The inverse position problem of separating be manipulator mechanism learn so that mechanics in basis also be most important one of study a question, it is directly connected to robot movement analysis, off-line programing, trajectory planning and work such as real-time.The inverse position algorithm of mechanical arm; Be exactly hand certain any and a certain attitude in fixing rectangular coordinate space of mechanical arm; Find the solution the corresponding joint angle of mechanical arm six-freedom degree (or claiming joint coordinates), so algorithm for inversion is the element of robot control.

The structure of traditional mechanical arm is general more special, and like axes intersect or parallel, axial length is zero or the like, just not coupling between its attitude and the position like this, and it is contrary separates way realization that is easy to variables separation.Yet for the more general complicated machinery hand of a class formation size; Because attitude and position height coupling generally can't be carried out variable and separate, at this moment must be by means of numerical algorithm; Can be divided three classes: (1) numerical value-analytical method, Newton-Raphson method etc.; But these algorithm requirement of real time are more difficult to get all against separating, and must provide suitable initial value.(2) optimized Algorithm, interval iteration method, genetic algorithm etc., this type algorithm convergence scope is big, can obtain all against separating, but general real-time is poor.(3) position and attitude difference process of iteration, this type algorithm can more promptly be tried to achieve all and separated, but when position of manipulator and the coupling of attitude height, iterative process can be dispersed.

Summary of the invention

Separate the deficiency that real-time is relatively poor, accuracy is not high of control method in order to overcome existing inverse position, the present invention provides the inverse position of the six degree of freedom series connection mechanical arm that a kind of real-time is good, accuracy is high to separate control method.

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

A kind of inverse position of six degree of freedom series connection mechanical arm is separated control method; It is characterized in that: utilize dual quaterion can represent the character of three-dimensional body rotation and translation; Each cradle head of six degree of freedom mechanical arm is showed by dual quaterion through conversion; In three dimensions, around unit vector u=(u _x, u _y, u _z) rotation at rotation θ angle can be expressed as with unit quaternion:

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

Promptly

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

Unit quaternion is suc as formula the rotation that can describe out rigid body shown in (2), and it is synthetic that a displacement in the three dimensions can add translation by rotation, representes rotation, p=(p with unit quaternion q _x, p _y, p _z) the expression translation vector, then 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>) (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)

(R wherein _w, T _w) represent each cradle head of mechanical arm with respect 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 with respect to the conversion formula on the 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,1≤j≤5 and L respectively ₁=[R _w, T _w],

For the total variable that can extract the mechanical arm cradle head as about s, v, p _x, p _y, p _zFunction, be fixedly coupled parameter, assert S _iAnd L _J+1Correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂S ₆=L ₆

Technical conceive of the present invention is: the character that the displacement that can in three dimensions, describe out rigid body to dual quaterion can be described its rotation again is applied on the six degree of freedom mechanical arm; Confirm the cradle head of six degree of freedom mechanical arm and the transformational relation of dual quaterion, establish equation with the contact between each cradle head again and obtain contrary separating.The technical matters that solves is how dual quaterion to be applied on the six degree of freedom mechanical arm, and establishes contact between the cradle head and establish equation and invert and separate.

Beneficial effect of the present invention mainly shows: 1) invention applies to dual quaterion and finds the solution that the six degree of freedom mechanical arm is contrary and separate, and improves and innovates finding the solution the contrary method of separating of mechanical arm, and expanded new research field; 2) proposed by the inventionly find the solution real-time and the accuracy that the contrary new method of separating of mechanical arm is well positioned to meet mechanical arm inverse kinematics algorithm; 3) method that the present invention adopted is very practical, and calculated amount is little, is easy to realize, has embodied the through engineering approaches and the practicability of new theory well.

Description of drawings

Fig. 1 is a RS type mechanical arm simple diagram.

Embodiment

With reference to the accompanying drawings the present invention is further described.

With reference to Fig. 1; A kind of inverse position of six degree of freedom series connection mechanical arm is separated control method; It is characterized in that: utilize dual quaterion can represent the character of three-dimensional body rotation and translation; Each cradle head of six degree of freedom mechanical arm is showed by dual quaterion through conversion, in three dimensions, around unit vector u=(u _x, u _y, u _z) rotation at rotation θ angle can be expressed as with unit quaternion:

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

Promptly

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

Unit quaternion is suc as formula the rotation that can describe out rigid body shown in (2), and it is synthetic that a displacement in the three dimensions can add translation by rotation, representes rotation, p=(p with unit quaternion q _x, p _y, p _z) the expression translation vector, then 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>) (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)

(R wherein _w, T _w) represent each cradle head of mechanical arm with respect 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 with respect to the conversion formula on the 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,1≤j≤5 and L respectively ₁=[R _w, T _w],

For the total variable that can extract the mechanical arm cradle head as about s, v, p _x, p _y, p _zFunction, be fixedly coupled parameter, assert S _iAnd L _I+1Correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂S ₆=L ₆

In the present embodiment, the definition of hypercomplex number is following: if establish q=a+bi+cj+dk (a, b, c, d ∈ R), i, j, k satisfy

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="2"\; label="i"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">i</figure-callout>}}^2={\mathrm{<figure-callout\; id="2"\; label="j"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">j</figure-callout>}}^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)$

Claim that then q is a hypercomplex number, i wherein, j, k are the imaginary number unit, the relation between each imaginary number unit is suc as formula mould a shown in (3) ²+ b ²+ c ²+ d ²=1 hypercomplex number is called unit quaternion.

is that the conjugate quaternion of q is:

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

Unit quaternion satisfies formula:

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

This paper with Quaternion Representation is for convenience's sake:

q＝(s，v) (6)

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

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

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

Promptly

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) rotation at rotation θ angle can be expressed as with unit quaternion

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

Dual quaterion: unit quaternion can be described out the rotation of rigid body suc as formula shown in (9), but does not describe out the rigid body displacement relation in three dimensions.It is synthetic that a displacement in the three dimensions can add translation by rotation, representes rotation, p=(p with unit quaternion q _x, p _y, p _z) the expression 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 ₂) multiply each other and can be expressed as:

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

Q wherein ₁* q ₂Can get by formula (7), and

${\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">q</figure-callout>}}_1*{\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_2*q_1^{-1}+{\mathrm{<figure-callout\; id="1"\; label="p"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_1={\mathrm{<figure-callout\; id="2"\; label="p"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">p</figure-callout>}}_2+2s_1({\mathrm{<figure-callout\; id="1"\; label="v"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">v</figure-callout>}}_1\&times;p_2)+2{\mathrm{<figure-callout\; id="1"\; label="v"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">v</figure-callout>}}_1\&times;({\mathrm{<figure-callout\; id="1"\; label="v"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">v</figure-callout>}}_1\&times;p_2)---\left(12\right)$

Ask the contrary of unit quaternion 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)

Finding the solution the contrary of six degree of freedom mechanical arm separates: manipulator motion is learned contrary separating and in robot, is occupied critical role, is directly connected to motion analysis, off-line programing and trajectory planning etc.Separate problem in order to solve the contrary of six degree of freedom mechanical arm, 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)

(R wherein _w, T _w) represent each cradle head of mechanical arm with respect 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 with respect to the conversion formula on the 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,1≤j≤5 and L respectively ₁=[R _w, T _w].

For the total variable that can extract the mechanical arm cradle head as about s, v, p _x, p _y, p _zFunction, be fixedly coupled parameter, assert S _iAnd L _J+1Correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂S ₆=L ₆

Instance: the RS mechanical arm simple diagram with as shown in Figure 1 is an example, finds the solution mechanical arm against separating.θ among the figure ₁, θ ₂, θ ₃, θ ₄, θ ₅, θ ₆Be desired each joint rotation angle, d ₃Be the displacement in removable joint, l ₁And l ₂Be length of connecting rod.

Each mechanical arm cradle head with respect to the space conversion relation of mechanical arm base is:

${\mathrm{<figure-callout\; id="1"\; label="Q"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">Q</figure-callout>}}_1=\left(\right[\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_1},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="1"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_1}k],<l_1{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_1,l_1{\mathrm{<figure-callout\; id="1"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_1,{\mathrm{<figure-callout\; id="1"\; label="h"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">h</figure-callout>}}_1>)---\left(20\right)$

${\mathrm{<figure-callout\; id="2"\; label="Q"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">Q</figure-callout>}}_2=\left(\right[\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2}k],<l_2{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2,l_2{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2,0>)---\left(21\right)$

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

${\mathrm{<figure-callout\; id="4"\; label="Q"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">Q</figure-callout>}}_4=\left(\right[\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_4},-\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="4"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_4}k],<\mathrm{0,0,0}>)---\left(23\right)$

${\mathrm{<figure-callout\; id="5"\; label="Q"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">Q</figure-callout>}}_5=\left(\right[\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="5"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_5},-\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="5"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_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).

Can try to achieve the contrary of them according to formula (14):

$Q_1^{-1}=([\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_1},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="1"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_1}k],<{\mathrm{<figure-callout\; id="1"\; label="l"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">l</figure-callout>}}_1,0,h>)---\left(26\right)$

$Q_2^{-1}=([\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_2},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="2"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_2}k],<{\mathrm{<figure-callout\; id="2"\; label="l"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">l</figure-callout>}}_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;}{{\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_4},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="4"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_4}k],<0,0,0>)---\left(29\right)$

$Q_5^{-1}=([\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_4},\stackrel{\&OverBar;}{{\mathrm{<figure-callout\; id="4"\; label="s"\; filenames="HDA0000140710260000011.png"\; state="\{\{state\}\}">s</figure-callout>}}_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)$

The transformational relation that passes through formula (18) (19) again can be obtained each contrary separating of six free mechanical arms.

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 contrary 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 (2)

1. the inverse position of six degree of freedom series connection mechanical arm is separated control method, it is characterized in that: dual quaterion is applied to find the solution that mechanical arm is contrary and separate, dual quaterion can be represented three-dimensional body rotation and translation, in three dimensions, around unit vector u=(u _x, u _y, u _z) rotation at rotation θ angle can be expressed as with unit quaternion:

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

Promptly

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

Unit quaternion is suc as formula the rotation that can describe out rigid body shown in (2), and it is synthetic that a displacement in the three dimensions can add translation by rotation, representes rotation, p=(p with unit quaternion q _x, p _y, p _z) the expression translation vector, then 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>) (2)

Then dual quaterion is inverted 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，<a，b，c>，<p _x，p _y，p _z>]) (16)

(R wherein _w, T _w) represent each cradle head of mechanical arm with respect 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 with respect to the conversion formula on the 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,1≤j≤5 and L respectively ₁=[R _w, T _w],

For the total variable that can extract the mechanical arm cradle head as about s, v, p _x, p _y, p _zFunction, be fixedly coupled parameter, assert S _iAnd L _J+1Correspondent equal, i.e. S ₁=L ₁, S ₂=L ₂S ₆=L ₆

2. the inverse position of a kind of six degree of freedom series connection mechanical arm as claimed in claim 1 is separated control method, and it is characterized in that: the definition of hypercomplex number is following: if establish q=a+bi+cj+dk (a, b, c, d ∈ R), i, j, k satisfy

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

Claim that then q is a hypercomplex number, i wherein, j, k are the imaginary number unit, the relation between each imaginary number unit is suc as formula mould a shown in (3) ²+ b ²+ c ²+ d ²=1 hypercomplex number is called unit quaternion;

is that the conjugate quaternion of q is:

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

Unit quaternion satisfies formula:

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

With Quaternion Representation be:

q＝(s，v) (6)

Wherein s representes scale a, and v representes vectorial bi+cj+dk, and two unit quaternion q ₁And q ₂Multiply each other and be expressed as:

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

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

Promptly

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) rotation at rotation θ angle is expressed as with unit quaternion

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

Unit quaternion can be described out the rotation of mechanical arm suc as formula shown in (9), but does not describe out the mechanical arm displacement relation in three dimensions; It is synthetic that a displacement of the mechanical arm in the three dimensions adds translation by rotation, representes rotation, p=(p with unit quaternion q _x, p _y, p _z) the expression translation vector, then 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 ₂) multiply each other and can be expressed as:

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

Q wherein ₁* q ₂Can get 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)$

Ask the reciprocal representation of unit quaternion to become:

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

Obtain the dual quaterion expression formula (14) of inverting by said formula (13).

Images