CN103901898A - Inverse-kinematics universal solving method of robot with multi-degree of freedom - Google Patents

Abstract

The invention provides an inverse-kinematics universal solving method of a robot with multi-degree of freedom. The common space theory is used for setting a universal kinematical equation of an nR robot, the weighted space vector projection method is used for analyzing the relationship between space vector projection values and revolute joint angles of a space robot, projection weighted values of joint vectors on robot tail end vectors are used as the basis for regulating the tail end pose of the robot, and projection weighted values of joint vectors are determined to achieve half-analytic solution of the inverse kinematics. The method achieves kinematical salvation of the nR robot and also finishes the space obstacle avoidance task. The method can be widely applied to series type space nR robots, has the advantages of being high in calculation speed and high in solving precision, provides control input parameters for series type robots, and meets the requirement for robot kinematics solving operation on industrial sites.

Description

A kind of general method for solving of inverse kinematics of multi-freedom robot

Technical field

The present invention is to provide a kind of half Analytical Solution method for nR robot kinematics, particularly combine the modeling method of joint of robot, the space vector sciagraphy of weighting.

Background technology

Along with the application widely of Industrial Robot Technology, the application of space nR serial mechanism has great importance.Serial mechanism Inverse Kinematics Solution is the condition precedent that serial machine people controls calculating, it is directly connected to the work such as robot off-line programming, trajectory planning, control in real time, in robotics, occupy critical role, only have by Inverse Kinematics stem-butts cutting off spatial pose and be converted to joint variable, could realize end effector of robot is carried out to programming Control (as straight path and arc track etc.) by spatial pose.

In serial mechanism kinematics, the Inverse Kinematics Solution of space 6R serial mechanism is the most difficult, and the monocycle 7R kinematics of mechanism in this problem and space mechanism belongs to same problem against solution, is once called the Mountain Everest in Kinematic Analysis for Spatial Mechanism.Various countries scholar carries out a lot of useful exploration and research to this.The Inverse Kinematics Solution method for solving of space 6R serial mechanism is divided into analytical form and numerical value form.Structural parameters are many because relating to for general 6R serial operation arm Inverse Kinematics Solution, the non-linear and coupling of solution and need the problems such as Solving Algebraic Equation to become to be difficult to obtain analytic solution.Analytic solution is applicable to have the 6R serial operation arm of particular geometries parameter, can apply vector, spiral or Lie algebraic methods and obtain Theory Solution, this method have result of calculation accurately, can obtain all the advantages such as solutions, but need to carry out a large amount of algebraic sum matrix operations, derivation more complicated, and to have the condition of solution be that the number of times that the position of motion arm and attitude have decoupling zero feature or its proper polynomial is less than or equal to 4.Liao Qizheng introduces Double quaternions in the middle of tandem-in-space robot kinematics studies, and has solved the Inverse Kinematics Problem of a classical 6R robot.2006, have scholar to propose series connection kinematic chain to split into the combination of several simple parts, but the method is only suitable for the special circumstances of some decoupling zero.Mainly contain D-H matrix method, spherical trigonometry, real matrix method, dual numbers method etc. for the method for serial mechanism inverse position solution mathematical modeling in the past, obtained different inverse arithmetic, do not there is versatility.Raghavan and Roth construct 14 basic equations by vector calculus by 6 inverse kinematics equatioies, disappear after n ary operation and obtain monobasic 24 equation of n th order n, obtain maximum 16 groups of inverse kinetics solutions, but exist 8 extraneous root .Manocha to adopt 24 rank matrix character decomposition methods to improve the algorithm of Raghavan, improved stability and precision that inverse kinematics is resolved.For solving a Displacement Analysis difficult problem for spatial 7R mechanism, adopt respectively plural method and 10 basic equations of matrix operation structure, and then obtain monobasic 16 equation of n th order n, eliminate extraneous root.Use for reference scholar's research achievement in early stage, 6R tandem type solution of Inverse Kinematics problem is divided into two classes: sealing solution solves the Inverse Kinematics Problem of the 6R robot that meets Pieper criterion; The objective matrix that vector calculates and symbolic operation obtains Manocha is reduced to 16 rank from 24 rank, and improve efficiency and the stability of General 6 R Robot inverse kinematics with matrix character decomposition method, and combine the gloomy iterative algorithm solution of the newton-pressgang Inverse Kinematics Problem of the 6R robot of Pieper criterion by no means.

And under the actual motion operation of 6R tandem type robot, only need one to find real-time and meet certain job requirement (as keeping away barrier and dynamics requirement) and end working point pose requirement inverse kinetics solution.Produced the serial machine people inverse kinematics method of a variety of numerical value forms for this reason.A conventional numerical method is that the radially parameters ai in the D-H parameter in the each joint of 6R serial operation arm, α i and axial parameter s i, θ i are separated, use biquaternion method or Lie algebraic methods that 6R serial operation arm forward kinematics solution matrix construction is become to two independently homogeneous transformation systems of linear equations, by two system of equations simultaneous successive iterations or the unit that disappears are obtained to 16 groups of Inverse Kinematics Solutions about each joint rotation angle.The numerical solution of 6R serial operation arm Inverse Kinematics Solution as obtained in the utilization biquaternion theories such as QIAO; The methods such as utilization Lie group, Lie algebra such as ROCCO have also obtained the numerical solution of this problem.Another numerical method of relatively commonly using is that the instrument such as genetic algorithm and neural network is introduced in the Inverse Kinematics Solution problem of 6R motion arm, by setting the constraint conditions such as joint rotation angle feed value, be minimised as objective function with the difference between forward kinematics solution and desired value, adopt the joint rotation angle feed value of above-mentioned Algorithm for Solving best-fit.As CHIDDARWAR etc. has compared forecasting type and the impact of conventional type neural network algorithm on solution efficiency;

deng having proposed the contrary neural network algorithm of separating of a kind of 3DOF robot kinematics who considers joint velocity and acceleration; KALRA etc. have proposed a kind of 6DOF industrial robot kinematics inverse arithmetic based on genetic algorithm; The continuous propagation algorithm of the employings such as HAMMOUR has been planned the movement locus of 6R motion arm; ZHA[20] curved surface features of utilizing end effector position and orientation vector to form, search this curved surface minimal eigenvalue by genetic algorithm and obtain optimal trajectory planning etc.

Summary of the invention

The object of the present invention is to provide a kind of limitation that solves robot configuration and selectivity that can overcome traditional analytic method, also can overcome the general method for solving of inverse kinematics of the multi-freedom robot of the non real-time property of general alternative manner and precision problem.

The object of the present invention is achieved like this:

Step 1: the joint parameter to computer input nR robot: joint of robot type, joint dimensional parameters, range of movement, connection angle information, multi-freedom joint to input carries out resolution process, be broken down into multiple single-DOF-joints, and set up kinematics model, impact point position and the attitude matrix of the input space simultaneously;

Step 2: the joint of robot kinematics matrix of setting up according to previous step, set up the general motion of nR robot according to general character geometric space theory and learn equation, set up the angle expression formula between the vector of joint simultaneously, and transformational relation between angle and the joint motions amount at vector link position place between vector;

Step 3: the kinematical equation of the nR robot setting up according to previous step, determine nR robot space vector relation according to robot topological structure relation, and then the projection amount of the space vector in definite each joint on robot end's vector;

Step 4: keep the topological relation between nR joint of robot, projection amount maximal value taking the space vector in joint on robot end's vector is as target, by rotary machine person joint, the projection amount peaked robot operating configuration of the space vector of determining joint on robot end's vector;

Step 5: taking the peaked robot of projection amount operating configuration as initial configuration, by the mode of decomposing, finding projection amount is 0 robot operating configuration, and by the each joint motions amount of analytical method solving robot;

Step 6: the robot operating configuration taking projection amount as 0 is basis, adjust robot configuration, require as prerequisite, away from the joint space limit to meet robotic joint space, make robot end put the status requirement that reaches target point, and then the operating configuration of definite robot location requirement;

Step 7: revise and adjust the attitude of robot configuration, realize the error correction of attitude by adjusting the deviation angle of current location vector and target location vector, realize robot end and put position and the Gesture of meeting spatial simultaneously;

Step 8: calculate the articulation amount of the robot configuration finally obtaining, and this group joint of robot general motion amount is input to robot controller, realize robot motion's control.

The present invention is with the artificial research object of many cradle heads machine, and the method for using the space vector sciagraphy of conformal geometry Space Theory and weighting to combine, solves the general rapid solving problem of robot inverse kinematics, for joint of robot control provides kinematic parameter.

Principal feature of the present invention is:

1, set up the general motion equation of nR robot according to general character geometric space theory;

Robot end's position vector is:

$p_n=a_0+\underset{k=1}{\overset{n}{\mathrm{\Σ}}}{a}_k\left({\mathrm{\β}}_k\right)---\left(1\right)$

In formula, a _i(i=0,1,2 ... n) be the vector that i+1 cradle head center pointed at i cradle head center; Vector a _i-1with vector a _ibetween angle β _iwith i cradle head angle of rotation θ _ifixing numerical relation; Pn is robot end's position vector.

According to general character geometric space theory, the nR robot inverse kinematical equation that can set up common version is as follows:

$\cos {\mathrm{\β}}_i=\frac{a_{i-1}\·a_i}{\left|a_{i-1}\right|\left|a_i\right|}(i=\mathrm{1,2},...n)---\left(2\right)$

β _iwith θ _ibetween relation can be represented by space geometry relation, can obtain formula (3).

$\cos {\mathrm{\θ}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\β}}_i}{{2h}_i{h}_{i-1}}---\left(3\right)$

H in formula _i, k _i(i=1,2 ... n) be the fixing structural parameters of robot, can obtain by actual measurement.

2, use the space vector sciagraphy of weighting to determine nR joint of robot angle, each joint broad sense joint angle, and then realize solution of Inverse Kinematics;

Can set up following formula by space vector projection relation:

$\left|\right|p_n\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+{\mathrm{\κ}}_n\left|\right|a_n\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\κ}}_{n-1}\left|\right|a_k\left|\right|---\left(4\right)$

κ in formula _i(i=0,1 ... n) be vector a _i(i=0,1 ... n) at space p _nprojection on vector and vector a _i(i=0,1 ... n) ratio of length; When after robot architecture and work pose determination, κ _i(i=1,2 ... n) be and β _i(i=1 ... and κ n) _i-1relevant, therefore can be expressed from the next:

κ _i=f(β _i,κ _i-1)(i=1,2…n) (5)

The weighted value deterministic process of the space vector sciagraphy of utilization weighting is as follows:

Step 1: the space vector projection value of maximum value is determined

Determine that each joint vector is at vector P _nmax (κ in direction _i) (i=1,2 ... n-1), can obtain so the new projection value on pose vector, be shown below:

$\left|\right|P^{\′}\left|\right|={\mathrm{\κ}}_0\left|\right|a_0\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\Σ}}}\max \left({\mathrm{\κ}}_{n-1}\right)\left|\right|a_k\left|\right|---\left(6\right)$

Vector P' and vector P _nidentical in the direction of vector, close the following relation of having fastened:

||P'||≥||P _n||-κ _n||a _n|| (7)

Definition δ is the total allowance of joint vector projection on pose vector, has:

δ=||P'||-||P _n||+κ _n||a _n|| (8)

Step 2: δ → 0 operating configuration is determined

For rod member vector a _iat the pose vector P of robot _nupper maximum value projection.The method of distributing total allowance is divided equally in employing, must be by a _iat the pose vector P of robot _nupper projection is by original b _ibecome b _i-δ/n, if the joint angle of joint i due to range of motion, cannot be by it at the pose vector P of robot _nupper projection becomes b _ithis value of-δ/n(can be negative), by a _iat the pose vector P of robot _nupper minimum projection is designated as b' _i, have:

Δ _i=b' _i-b _i+δ/n (9)

Step 3: attitude compensation is revised

P' _n-1the space vector of n articulation center in the operating configuration of δ → 0, P " ' _n-1the space vector of n articulation center of target configuration, because two vectors are at P _nin direction, projection value is identical, therefore vector P spatially " ' _n-1for vector P' _n-1around space vector axle P _nrotation φ obtains.According to this relation, taking δ → 0 operating configuration as initial configuration, find the operating configuration of φ → 0.In the time of the operating configuration of definite δ → 0, adopt the method for dividing equally δ, therefore there is motion surplus in most of joint of robot now.

A kind of general nR solution of Inverse Kinematics method of carrying out fast of the present invention, the method overcomes the limitation that solves robot configuration and the selectivity of traditional analytic method, also overcome the non real-time property of general alternative manner and precision problem, can solve nR solution of Inverse Kinematics problem.

Brief description of the drawings

Fig. 1 is solution procedure schematic diagram.

Fig. 2 is nR type serial machine people's kinematics model.

Fig. 3 β _iwith θ _ibetween relation.

The projection relation of Fig. 4 space vector.

The projection of Fig. 5 vector and joint angle relation.

Fig. 6 adjacent segment projection compensation.

The compensation of Fig. 7 attitude angle.

Fig. 8 φ → 0 operating configuration and target operating configuration are related to schematic diagram.

Fig. 9 General 6 R Robot model.

Embodiment

Below in conjunction with accompanying drawing, the present invention is described in more detail.

Main solution procedure as shown in Figure 1.

Shown in the kinematics universal model accompanying drawing 2 of the nR robot adopting, formed by cradle head and the rigid body connecting rod of n degree of freedom.In accompanying drawing 2, J _i(i=1,2 ... n) be i cradle head center; L _i(i=0,1,2 ... n) be i rigid body; Σ _i(i=1,2 ... n) be L _ithe coordinate system that is connected, Σ ₀for base coordinate system; a _i(i=0,1,2 ... n) be the vector that i+1 cradle head center pointed at i cradle head center, because joint parameter is fixed, vector a _i-1with vector a _ibetween angle β _iwith i cradle head angle of rotation θ _ifixing numerical relation, vector a _inorm by rigid body L _iit is a fixing number that shape determines, its variation is relevant with cradle head angle, therefore can be expressed as a _i(β _i).

β _iwith θ _ibetween relation can be represented by space geometry relation shown in accompanying drawing 3, can obtain formula (3).

$\cos {\mathrm{\θ}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\β}}_i}{{2h}_i{h}_{i-1}}---\left(3\right)$

H in formula _i, k _i(i=1,2 ... n) be the fixing structural parameters of robot, can obtain by actual measurement, the Inverse Kinematics Problem of space nR robot is exactly a in how to confirm space like this _i(i=1,2 ... n-1) azimuth, the norm in its space is known.

κ _i(i=1,2 ... n) at β _i(i=0,1 ... n) under constraint, there is a kind of iterative relation, as shown in Figure 4.Work as a _i-1after space vector is determined, it is at P _nprojection κ _i-1|| a _i-1|| also just determine, the rotation axis vector in i joint too can cicada, κ thereupon _i|| a _i|| variation range be also cicada, so just according to κ _i|| a _i|| scope, meeting in constraint condition situation, determine a _i, and then obtain β _iand θ _i, realize solving of inverse kinematics.

Vector projection and joint angle relation are as shown in Figure 5.Figure midplane τ is joint vector a _iaround joint shaft J _iafter rotating to an angle and initial joint vector

plane (the joint vector a forming _iaround joint shaft J _iwhat rotate to form is circular conical surface), due to the not necessarily drag articulation axle J of rigid body rod member of robot _i, therefore establish B point, F point at joint shaft J _ion common subpoint A, the angle theta of AB and AF is exactly the joint angle that will solve; O _ib, O _if and joint shaft J _iangle is fixed, and therefore AB and AF value are known;

for joint vector a _ibe the vector of 0 o'clock at joint angle, i.e. O _ib; B point, F point are at pose vector P _nsubpoint be respectively D point and G point, cross D point and do the parallel lines of GF, obtain intersection point C, easily know that DG is a _iat vector P' _non projection value with

at vector P' _non the difference DELTA of projection value; Pose vector P _nhand over BC and E point in plane τ projection; Due to O _ib, P _n, plane τ is known, the O that can be asked by conformal geometry Space Theory _ib and P _nangle α, P _nwith plane τ angle γ.

When Δ is known, while solving angle theta, can set up following equation by diagram relation:

$O_{i}D=a_{\mathrm{<figure-callout\; id="0"\; label="i"\; filenames="HDA0000483642710000031.png"\; state="\{\{state\}\}">i</figure-callout>}}^0\cos \mathrm{\&alpha;}---\left(10\right)$

$O_{i}F=a_i^0---\left(11\right)$

$O_{i}C=\frac{O_{i}D*O_{i}F}{O_{i}D+\mathrm{\&Delta;}}---\left(12\right)$

$O_{i}E=\frac{O_{i}D}{\cos \mathrm{\&gamma;}}---\left(13\right)$

$\mathrm{BC}=\sqrt{{\left(O_{i}B\right)}^2-{\left(O_{i}E\right)}^2}+\sqrt{{\left(O_{i}C\right)}^2-{\left(O_{i}E\right)}^2}---\left(14\right)$

If ∠ is FO _ib is θ ' _i, have:

${\mathrm{\&theta;}}_i^{\&prime;}=\arccos \left(\frac{O_{i}E}{O_{i}B}\right)+\arccos \left(\frac{O_{i}E}{O_{i}C}\right)---\left(15\right)$

${\left|\right|a_{\mathrm{<figure-callout\; id="0"\; label="i"\; filenames="HDA0000483642710000031.png"\; state="\{\{state\}\}">i</figure-callout>}}^0\left|\right|}^2+{\left|\right|a_i\left|\right|}^2-2\left|\right|a_{\mathrm{<figure-callout\; id="0"\; label="i"\; filenames="HDA0000483642710000031.png"\; state="\{\{state\}\}">i</figure-callout>}}^0\left|\right|*\left|\right|a_i\left|\right|*\cos {\mathrm{\&theta;}}_i^{\&prime;}={\left(\mathrm{BF}\right)}^2---\left(16\right)$

(AB) ²+(AF) ²-2(AB)*(AF)*cosθ=(BF) ² (17)

Through type (10)-(17), can set up Δ and θ relation, otherwise it is known to work as θ, solves Δ, also can relation as shown in Figure 5 solve.

Be illustrated in figure 6 rod member vector a _iat the pose vector P of robot _nupper maximum value projection.According to the method for distributing total allowance of dividing equally of introducing above, must be by a _iat the pose vector P of robot _nupper projection is by original b _ibecome b _i-δ/n, if the joint angle of joint i due to range of motion, cannot be by it at the pose vector P of robot _nupper projection becomes b _ithis value of-δ/n(can be negative), by a _iat the pose vector P of robot _nupper minimum projection is designated as b' _i, have:

Δ _i=b' _i-b _i+δ/n (18)

The adjacent connecting rod a of such i connecting rod _i-1and a _i+1at the pose vector P of robot _nupper projection value reduces respectively δ/n+ Δ _i/ 2, try to achieve successively the new projection value on pose vector according to which, make δ → 0, thereby obtain new robot operating configuration by initial configuration.

As shown in Figure 7, P' _n-1the space vector of n articulation center in the operating configuration of δ → 0, P " ' _n-1the space vector of n articulation center of target configuration, because two vectors are at P _nin direction, projection value is identical, therefore vector P spatially " ' _n-1for vector P' _n-1around space vector axle P _nrotation φ obtains.According to this relation, taking δ → 0 operating configuration as initial configuration, find the operating configuration of φ → 0.In the time of the operating configuration of definite δ → 0, adopt the method for dividing equally δ, therefore there is motion surplus in most of joint of robot now.

With β _i(i=1,2 ... n-1) be input quantity, adjust robot rod member space vector, make φ → 0.

Due to the space vector a of n rod member _ndetermine, n joint rotating vector J _nalso determine, determined like this space vector a _n-1place plane vector is determined.And rod member vector a in the operating configuration of φ → 0 " _n-1may be not at rotating vector J _nin the definite plane of plane vector, now vector a " _n-1only have an intersection point with this plane, at n articulation center, therefore φ → 0 operating configuration is not also actual needed target configuration, and also discontented biped robot attitude needs, and needs further to carry out attitude correction for this reason.

As shown in Figure 8, φ → 0 operating configuration and target operating configuration are related to schematic diagram.In the drawings, φ → 0 operating configuration has ensured that its n articulation center overlaps with n articulation center of real work configuration, but the J in the operating configuration of φ → 0 " _nwith the J in target operating configuration _ndo not overlap, thereby make robot end's attitude not meet job requirement.

J _nknown, can determine a according to robot topological relation _n-1the plane η at place _n-1, the problem of attitude correction is exactly on operating configuration basis, φ → 0 like this, by adjusting β _i(i=1,2 ... n-1) value, according to the space constraint relation of robot rod member, at η _n-1plane finds suitable a _n-1.

As shown in Figure 9, set up space 6R robot general motion model, calculate by MATLAB, determine robot architecture's parameter model according to table 1.

Table 1 robot architecture parameter list

The terminal angle matrix of given robot target point:

$T=\left[\begin{array}{cccc}-0.8673& 0.4894& -0.0908& 50.0142\\ 0.2884& 0.3456& -0.8929& -1003\\ -0.4056& -0.8007& -0.4409& -471.074\\ 0& 0& 0& 1\end{array}\right]$

First according to robot motion model, determine the space vector of projection maximum functional configuration according to the method for the space vector sciagraphy step 1 of weighting, this vector is 1822mm, and the operating configuration joint angle of this configuration is as shown in table 2.

The joint angle of table 2 projection maximum functional configuration

Method according to the space vector sciagraphy step 2 of weighting is determined δ → 0 operating configuration, and the joint angle of this operating configuration is as shown in table 3.

The joint angle of table 3 δ → 0 operating configuration

Under the constraint condition not changing between joint of robot, adjust the weighted value of space vector projection, robot end is put and meet robot end's status requirement, the joint of robot angle obtaining is as shown in table 4.

Table 4 meets the joint angle of status requirement operating configuration

Robot operating configuration not only will meet the status requirement of the distal point of work, also needs to meet its Gesture.Also need to carry out the correction of operating configuration attitude for this reason.According to the method for the space vector sciagraphy step 3 of weighting, obtain last robot operating configuration, its joint of robot angle is as shown in table 5.

The last joint of robot of table 5 angle

Each joint angle numerical value in table 5 is input in robot controller, and controller just can control joint motions, realize robot end and meet the requirement of impact point position and attitude.

Claims (3)

1. the general method for solving of the inverse kinematics of multi-freedom robot, is characterized in that:

Step 1: the joint parameter to computer input nR robot: joint of robot type, joint dimensional parameters, range of movement, connection angle information, multi-freedom joint to input carries out resolution process, be broken down into multiple single-DOF-joints, and set up kinematics model, impact point position and the attitude matrix of the input space simultaneously;

Step 2: the joint of robot kinematics matrix of setting up according to previous step, set up the general motion of nR robot according to general character geometric space theory and learn equation, set up the angle expression formula between the vector of joint simultaneously, and transformational relation between angle and the joint motions amount at vector link position place between vector;

Step 3: the kinematical equation of the nR robot setting up according to previous step, determine nR robot space vector relation according to robot topological structure relation, and then the projection amount of the space vector in definite each joint on robot end's vector;

Step 4: keep the topological relation between nR joint of robot, projection amount maximal value taking the space vector in joint on robot end's vector is as target, by rotary machine person joint, the projection amount peaked robot operating configuration of the space vector of determining joint on robot end's vector;

Step 5: taking the peaked robot of projection amount operating configuration as initial configuration, by the mode of decomposing, finding projection amount is 0 robot operating configuration, and by the each joint motions amount of analytical method solving robot;

Step 6: the robot operating configuration taking projection amount as 0 is basis, adjust robot configuration, require as prerequisite, away from the joint space limit to meet robotic joint space, make robot end put the status requirement that reaches target point, and then the operating configuration of definite robot location requirement;

Step 7: revise and adjust the attitude of robot configuration, realize the error correction of attitude by adjusting the deviation angle of current location vector and target location vector, realize robot end and put position and the Gesture of meeting spatial simultaneously;

Step 8: calculate the articulation amount of the robot configuration finally obtaining, and this group joint of robot general motion amount is input to robot controller, realize robot motion's control.

2. the general method for solving of the inverse kinematics of multi-freedom robot according to claim 1, is characterized in that the described general motion of setting up nR robot according to general character geometric space theory learns equation and comprise:

Robot end's position vector is:

$p_n=a_0+\underset{k=1}{\overset{n}{\mathrm{\&Sigma;}}}{a}_k\left({\mathrm{\&beta;}}_k\right)$

In formula, a _i, i=0,1,2 ... n is the vector that i+1 cradle head center pointed at i cradle head center; Vector a _i-1with vector a _ibetween angle β _iwith i cradle head angle of rotation θ _ifixing numerical relation; Pn is robot end's position vector;

The nR robot inverse kinematical equation of setting up common version is as follows:

$\cos {\mathrm{\&beta;}}_i=\frac{a_{i-1}\&CenterDot;a_i}{\left|a_{i-1}\right|\left|a_i\right|}(i=\mathrm{1,2},...n)$

β _iwith θ _ibetween relation represented by space geometry relation, obtain formula

$\cos {\mathrm{\&theta;}}_i=\frac{h_i^2+h_{i-1}^2+k_i^2-a_i^2-a_{i-1}^2+{2a}_i{a}_{i-1}\cos {\mathrm{\&beta;}}_i}{{2h}_i{h}_{i-1}}$

H in formula _i, k _i, i=1,2 ... n is the fixing structural parameters of robot, obtains by actual measurement.

3. the general method for solving of the inverse kinematics of multi-freedom robot according to claim 2, is characterized in that describedly comprising with analytical method solving robot each joint motions measurer body:

Use the space vector sciagraphy of weighting to determine nR joint of robot angle, each joint broad sense joint angle, and then realize solution of Inverse Kinematics;

Set up following formula by space vector projection relation:

$\left|\right|p_n\left|\right|={\mathrm{\&kappa;}}_0\left|\right|a_0\left|\right|+{\mathrm{\&kappa;}}_n\left|\right|a_n\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\&Sigma;}}}{\mathrm{\&kappa;}}_{n-1}\left|\right|a_k\left|\right|$

κ in formula _i, i=0,1 ... n is vector a _i, i=0,1 ... n is at space p _nprojection on vector and vector a _i, i=0,1 ... the ratio of n length; When after robot architecture and work pose determination, κ _i, i=1,2 ... n is and β _i, i=1 ... n and κ _i-1relevant, therefore be expressed from the next:

κ _i=f(β _i,κ _i-1)(i=1,2…n)

The weighted value deterministic process of the space vector sciagraphy of utilization weighting is as follows:

Step 1: the space vector projection value of maximum value is determined

Determine that each joint vector is at vector P _nmax (κ in direction _i), i=1,2 ... n-1, can obtain the new projection value on pose vector like this, is shown below:

$\left|\right|P^{\&prime;}\left|\right|={\mathrm{\&kappa;}}_0\left|\right|a_0\left|\right|+\underset{k=1}{\overset{n-1}{\mathrm{\&Sigma;}}}\max \left({\mathrm{\&kappa;}}_{n-1}\right)\left|\right|a_k\left|\right|$

Vector P' and vector P _nidentical in the direction of vector, close the following relation of having fastened:

||P'||≥||P _n||-κ _n||a _n||

Definition δ is the total allowance of joint vector projection on pose vector, has:

δ=||P'||-||P _n||+κ _n||a _n||

Step 2: δ → 0 operating configuration is determined

For rod member vector a _iat the pose vector P of robot _nupper maximum value projection, adopts and divides equally the method for distributing total allowance, by a _iat the pose vector P of robot _nupper projection is by original b _ibecome b _i-δ/n, if the joint angle of joint i due to range of motion, cannot be by it at the pose vector P of robot _nupper projection becomes b _i-δ/n, by a _iat the pose vector P of robot _nupper minimum projection is designated as b' _i, have:

Δ _i=b' _i-b _i+δ/n

Step 3: attitude compensation is revised

P' _n-1the space vector of n articulation center in the operating configuration of δ → 0, P ' " _n-1be the space vector of n articulation center of target configuration, two vectors are at P _nin direction, projection value is identical, spatially vector P ' " _n-1for vector P' _n-1around space vector axle P _nrotation φ obtains, and according to this relation, taking δ → 0 operating configuration as initial configuration, finds the operating configuration of φ → 0, in the time of the operating configuration of definite δ → 0, adopts the method for dividing equally δ.

Images