CN109015641A

CN109015641A - The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method

Info

Publication number: CN109015641A
Application number: CN201810933665.6A
Authority: CN
Inventors: 居鹤华
Original assignee: Individual
Current assignee: Individual
Priority date: 2018-08-16
Filing date: 2018-08-16
Publication date: 2018-12-18
Anticipated expiration: 2038-08-16
Also published as: CN109015641B; WO2020034415A1

Abstract

The inverse solution modeling of the invention discloses a kind of general 6R mechanical arm based on axis invariant and calculation method, are set with 6 rotation axis, and pickup point is located on the 6th axis axis, and the 4th axis and the not coaxial mechanical arm of the 5th axis are general 6R mechanical arm；The pose equation of 6R mechanical arm specification is expressed using residence-gibbs quaternary number expression formula, completes to be aligned by preceding 5 axis, to eliminate the joint variable of the 4th axis and the 5th axis；The 6th axis is controlled by preceding 5 axis to be aligned with desired position and direction, and the 6th axis infinite rotational or the 6th axis of control is enable to meet radially aligned.Method of the invention breaches the inverse solution calculation method of modeling of general 6R mechanical arm, can satisfy the demand of mechanical arm precise operation.

Description

Inverse solution modeling and resolving method for universal 6R mechanical arm based on axis invariant

Technical Field

The invention relates to an inverse solution modeling and resolving method for a 6R mechanical arm of a multi-axis robot, and belongs to the technical field of robots.

Background

An important aspect of autonomous robot research is the need to solve the problem of kinematic modeling of variable topology robots. In the MAS, a Dynamic Graph Structure (Dynamic Graph Structure) is provided, a directional Span tree based on a motion axis can be dynamically established, and a foundation is laid for researching robot modeling and control of a Variable Topology Structure (Variable Topology Structure). Therefore, an inverse solution principle of the universal mechanical arm based on an axis invariant needs to be provided, a completely parameterized positive kinematics model containing a coordinate system, polarity, structural parameters and joint variables needs to be established, and a pose equation needs to be calculated in real time; on one hand, the autonomy of the robot can be improved, and on the other hand, the absolute accuracy of the robot posture control can be improved.

The 6R decoupling mechanical arm structurally has a concurrent constraint: either the 4 to 6 axes are concurrent or the 4 axis is concurrent with the 5 axis and the 5 axis is concurrent with the 6 axis. For a high precision robot arm, this assumption is not valid due to machining and assembly errors. Because the universal 6R mechanical arm does not have the concurrent constraint, the inverse solution calculation is very difficult, the decoupling constraint has to be complied with in engineering, and the constraint not only increases the machining and assembling difficulty of the mechanical arm, but also reduces the absolute positioning precision of the mechanical arm. The requirement of the mechanical arm for precise operation can be met only by breaking through the inverse solution method of the universal 6R mechanical arm, and the autonomous robot theory can be improved.

Disclosure of Invention

The invention aims to solve the technical problem of providing an inverse solution modeling method of a universal 6R mechanical arm based on an axis invariant, breaking through the inverse solution method of the universal 6R mechanical arm and meeting the requirement of precise operation of the mechanical arm.

In order to solve the technical problems, the invention adopts the following technical scheme:

a universal 6R mechanical arm inverse solution modeling and resolving method based on an axis invariant is characterized in that,

6 rotating shafts are set, the picking point is positioned on the axis of the 6 th shaft, and the mechanical arm with the 4 th shaft and the 5 th shaft which are not coaxial is a universal 6R mechanical arm;

expressing a pose equation normalized by the 6R mechanical arm by adopting a Jub-Gibbs quaternion expression, and finishing alignment through the front 5 axes to eliminate joint variables of the 4 th axis and the 5 th axis; the 6 th shaft is controlled to be aligned with the expected position and direction through the front 5 shafts, the 6 th shaft can be infinitely rotated or the 6 th shaft is controlled to meet the radial alignment, and the 6 th shaft expected position vector is givenAnd desired attitudeAnd given the 6 th axis desired position vectorAnd 5 th axis desired attitudeThe inverse solution problem of (2) is equivalent.

For any rod memberDefine the Ju-Gibbs or Ju-Gibbs canonical quaternion isomorphic with the Euler quaternion:

wherein:is a Gibbs vector;

the Gibbs conjugate quaternion is:

wherein:

in the formula,norm quaternion for curie-gibbsSquare of the mold; expression form power symbolTo representTo the x-th power of; the right upper corner is marked with ^ orA representation separator; axial invariant Is a joint variable; axial vectorAnd joint variablesUniquely determining the rotation relation of the kinematic pair;is axis invariantA cross-product matrix of;is the Gibbs vectorA cross-product matrix of; if it is usedRepresenting attribute placeholders, the form of expression in the formulaRepresenting a member access character; expression form power symbol in formulaTo representTo the x-th power of; the right upper corner is marked with ^ orRepresenting a separator.

If a 6R axis chain is givenⁱl₆＝(i，1:6]，ⁱl₁＝0₃The 6 th axis desired position vector isAnd 5 th axis desired attitudeThe 3 rd axis joint has the following standard quaternionOther axis expression ways are the same; the 6R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

the matrix formed based on the structural parameters of the 6R mechanical arm system and the expected attitude Jubs quaternion is expressed as

In the formula, the character is a continuation character;respectively represent the zero vector and the radial vector of the shafts 4 to 5 and 5 to 6;is axis invariantA cross-product matrix of; 0₃＝[0 0 0]^T； A 4 x 4 matrix representing system configuration parameters;express getFirst row of³n₄YuanElement, and the like in turn,express getRow k +1 elements of (1); the upper right corner expression form [ 2 ]]Representing the form of expression [. cndot. ] taken by rows or columns]Representing taking all columns;express get³E₅Row 3, all columns;³n₄is the coordinate vector of bar 3 to bar 4, which is an axis invariant; the cross-multiplication matrix of the axis invariant is obtained, and the other rods are treated in the same way.

Elimination of tau₄And τ₅The subsequent position equation (169) is a 3-element 2-order polynomial equation which is equivalent to a 3R mechanical arm problem and is solved by adopting an axis invariant-based 3R mechanical arm position inverse solution method.

If a 6R axis chain is givenⁱl₆＝(i,1:6]，ⁱl₁＝0₃(ii) a The expected position vector and the Ju-Gibbs quaternion are respectively recorded asAndthen equation (170) forms a polynomial system F₃(Y₂|T₂) The Dixon matrix has the following structure:

wherein:

in the formula,is a Dixon matrix of size SxS, the [ i ] th][j]Member is univariate τ₁Polynomial of order N.

The general 3R mechanical arm inverse solution modeling and resolving method based on the axis invariant is used for calculating pose inverse solution by applying Dixon elimination and solving principles of N 'N-element N-order' polynomials, and mainly comprises the following steps of:

【1】 Obtaining n 'n-element 2-order' polynomial equations according to the n-element 3D vector pose equation of the mechanical arm;

【2】 Carrying out diagonalization calculation on the determinant or a Dixon determinant calculation formula based on an axis invariant, a determinant calculation formula of a block matrix or a diagonalization calculation formula simplified determinant;

【3】 Using Dixon elimination elements and solving principles of N 'N-element N-order' polynomials to finish pose inverse solution calculation, wherein: and obtaining a unary high-order polynomial equation according to the determinant of the Dixon matrix as 0, and solving the solution of the unary high-order polynomial equation by applying the unary high-order polynomial equation based on the friend matrix.

In the step (1),

for axle chainIs provided with

Establishing a standard attitude equation as follows:

establishing a standard positioning equation:

in the formula, k represents a group,in any rod, expression formTo representTo the x-th power of; the right upper corner is marked with ^ orA representation separator;is axis invariantThe cross-multiplication matrix of (a), the bar l,is a rod member k, and is a rod member,replacing simultaneously; 1 is a three-dimensional identity matrix;ⁱQ_nrepresenting a gesture;is along a vector axisThe line position of (a);from the origin at zero positionTo the origin O_lA translation vector of (a);in order to be a projected symbol,is composed ofProjection vectors in a geodetic coordinate system.

In the step [ 2 ], the Dixon determinant calculation formula based on the axis invariants is as follows:

according to the Dixon determinant property of the kinematic chain, the following properties are:

and memorize:

in the formula,is a rotation transformation matrix;auxiliary variable y for representation_lFirst l of the sequence of substitution of the original variable τ_lTaking "|" as a replacement operator for l variables in the list;

formula (128) isAndis converted intoMultiple linear types of (2); at the same time, the user can select the desired position,for y_lAnd τ_lHas symmetry;

equation of 3R kinematics from equation (48)

Is obtained by formula (143)

Is obtained by the formula (144)

Note the book

Then the general formula (62) and the general formula (146)

Is obtained from formula (145) and formula (146)

The Dixon determinant structural model and the characteristics of the 3R mechanical arm kinematics equation are as follows:

obtaining a 3R kinematic polynomial equation from equation (148)

Polynomial system F₃(Y₂|T₂) According to the formula of bilinear determinant

Then there is

Wherein:

the medium combined variable coefficient is independent column vector, so it is selectedTo form a square matrixThe remaining column vectors are given a sumAre related to each column of;

is obtained by the formula (128) and the formula (146)

In the formula,respectively representing axes 2 to 3, and axes 3 to 3_SZero vector, radial vector and axial vector; wherein

The simplified Dixon determinant of 3-element N order is

In the formula, is a Dixon matrix of size SxS, the [ i ] th][j]Member is univariate τ₁Polynomial of order N.

In the step (2), the determinant calculation formula of the block matrix is as follows:

if the matrix with the size of (n + M) · (n + M) is M, the matrix with the size of n.nIs a sub-matrix formed by the first n rows and any n columns of elements of the square matrix M and has the size of m.mIs a sub-matrix formed by M rows and the rest M columns of elements behind the square matrix M; the sequences cn and cm, which are composed of the matrix sequence numbers arranged in ascending order, are the sequences [1: m + n ]]A subset of [ cn, cm ]]∈<1:n+m>and has cmU cn ═ 1: m + n](ii) a Then the square matrix M determinant and the block matrixAndhas a determinant relationship of

In the step (2), a step calculation principle is carried out on the determinant:

for an S × S matrix, each entry is for τ₁Polynomial of order n. When the determinant of the matrix is calculated, the original determinant can be changed into an upper triangular determinant through primary row transformation, and then nonzero diagonal elements are multiplied to obtain a polynomial expression of the determinant. The formula is 0 to give τ₁All solutions of (a).

The specific method of the line ladder is that the highest order of the first column of the determinant is firstly sequenced from high to low, and then the maximum (S-1) multiplied by n times of primary equal line transformation elimination is carried out, so as to obtain the determinant of which the first element of the first column is not 0. And performing primary row transformation elimination on the residue sub-formulas of the 1 st row and the 1 st column of the determinant, and sequentially performing iterative solution.

In the step [ 3 ], the Dixon polynomial construction steps of N 'N-element N-order' polynomial systems are as follows:

introducing an auxiliary variable [ y₂,y₃,…,y_n]And is provided with

For multivariate polynomial polynomialsBy auxiliary variables Y_mThe first m sequentially replace the original variable X_nM variables in the polynomial are marked with "|" as replacing operational characters to obtain an augmented polynomial

To obtain

Wherein:

defining separable compositional variablesAndthe following were used:

the following equations (15) and (16) show that: replaceable typeIs aboutAndthe dual linear type of (3); accordingly, the polynomial system replaced by the auxiliary variable is denoted by

Given N 'N-element N-order' polynomial systemsDefining the Dixon polynomial as

Is obtained by the formula (18)

Dixon determinant of this polynomial taking into account equations (14) and (19)

In cartesian space, a determinant consisting of a position vector or a rotation vector represents a Volume (Volume) in which the vector opens up into space; there is a constancy of volume in different cartesian spaces. Wherein:

given N "N-ary N-th order" polynomial systems F_n(Y_n-1|X_n-1) N is not less than 2; presence and elimination of variable x₂,…,x_nIndependent Dixon matrix_SΘ_S(x₁) Of Dixon polynomials thereofExpressed as a separate variableAnddouble linear type of (2):

α[l]∈[0,N·(n-l+1)-1],l∈[2:n]； (24)

is a Dixon matrix of size SxS, the [ i ] th][j]Member is univariate x₁Polynomial of order N:

wherein:

consider formula (23) ifSo that

Det(_SΘ_S(x₁))＝0； (29)

The 'n-elements' in the formula (29) are called as the necessary conditions of Dixon elimination elements, so that a feasible solution is obtained.

Is obtained from formula (29), formula (152) and formula (153)

The formula (169) relates to₁Of order 16, quadratic blocking using equation (5)Determinant calculation or diagonalization calculation of a determinant.

The general 6R mechanical arm 4 and 5 axis solving method comprises the following steps: 2R direction inverse solution based on 'Ju-Gibbs' quaternion or 2R direction inverse solution based on DCM-like

Based on a quaternion 2R inverse solution of 'Ju-Gibbs':

firstly, the pointing alignment principle based on the Ju-Gibbs quaternion is introduced, and the axial chain is consideredⁱl_l，Vector of unit axisAnd desired unit axis vectorWhen aligned, at least one multi-axis rotation Ju-Gibbs quaternion exists

Wherein

Then, on the basis of pointing alignment of the Ju-Gibbs quaternion, explaining the pointing inverse solution theorem of the 2R mechanical arm;

if a 6R rotating chain is givenⁱl₆＝(i,1:6]Note that the 5 th axis joint Ju-Gibbs quaternion is expected to beAnd the 3 rd shaft joint Ju-Gibbs standard quaternion isThere is an inverse solution when pointing to the alignment

Wherein:

quaternion in Ju-Gibbs orientationSatisfy the requirement of

In the formula,express get³E₅Row 3, all columns; is the coordinate vector of bar 3 to bar 4, which is an axis invariant;is axis invariant³n₄The other rods are in the same way;

compared with the Euler quaternion and the dual quaternion, the pose alignment represented by the Ju-Gibbs quaternion has no redundant equation; by directional alignment, the joints of the 4 th axis and the 5 th axis can be solved³n₄And the variable lays a foundation for 6R and 7R mechanical arm inverse solution.

DCM-like based 2R inverse solution:

giving 6R axial chainⁱl₆＝(i,1:6]Axial vector of³n₄And⁴n₅the desired 5 th axis DCM isDCM of the desired 3 rd axis isDirection vector⁵l₆To the desired directionThe inverse solution of the alignment needs to satisfy the following equation:

in the formula, the character is a continuation character;respectively representing the zero vector and the radial vector of the shafts 5 to 6;is axis invariantA cross-product matrix of; 0₃＝[0 0 0]^T； ³n₄Is the coordinate vector of bar 3 to bar 4, which is an axis invariant;is axis invariant³n₄The other bars are the same.

The invention achieves the following beneficial effects:

the method solves the problems of reversible solution kinematics modeling and inverse solution calculation of the 6R mechanical arm, has a simple and elegant kinematic chain symbolic system, a pseudo code function and an iterative structure, and ensures the reliability and the mechanized calculation of the system realization;

the method has an iterative formula based on an axis invariant, and the real-time performance of calculation is ensured; the complete parameterization of a coordinate system, polarity and system structure parameters is realized, the reversible solution kinematics based on the axis invariant has uniform expression and a simple structured hierarchical model, and the universality of the pose analysis inverse solution is ensured.

Structural parameters based on the fixed shaft invariant obtained by precision measurement of the laser tracker are directly applied, and the accuracy of pose inverse solution is ensured; therefore, the absolute positioning and attitude determination precision of the system is close to the repetition precision.

Drawings

FIG. 1 a natural coordinate system and axis chain;

FIG. 2 is a fixed axis invariant;

FIG. 3 is a schematic view of the fixed axis rotation;

FIG. 4 is a derived invariant of an axis invariant.

Detailed Description

The invention is further described below. The following examples are only for illustrating the technical solutions of the present invention more clearly, and the protection scope of the present invention is not limited thereby.

Define 1 natural coordinate axes: a unit reference axis having a fixed origin, referred to as being coaxial with the axis of motion or measurement, is a natural coordinate axis, also referred to as the natural reference axis.

Defining 2 a natural coordinate system: as shown in fig. 1, if the multi-axis system D is located at the zero position, the directions of all cartesian body coordinate systems are the same, and the origin of the body coordinate system is located on the axis of the moving shaft, the coordinate system is a natural coordinate system, which is simply referred to as a natural coordinate system.

The natural coordinate system has the advantages that: (1) the coordinate system is easy to determine; (2) the joint variable at zero is zero; (3) the system postures at the zero position are consistent; (4) and accumulated errors of measurement are not easily introduced.

From definition 2, it can be seen that the natural coordinate system of all the rods coincides with the orientation of the base or world system when the system is in the zero position. With the system in zero positionTime, natural coordinate systemVector around axisAngle of rotationWill be provided withGo to F^[l]；In thatCoordinate vector ofAt F^[l]Coordinate vector ofIs constant, i.e. has

According to the formula, the method has the advantages that,orIndependent of adjacent coordinate systemsAnd F^[l](ii) a So it is calledOrIs axis invariant. When invariance is not emphasized, the method can be called a coordinate axis vector (axis vector for short).OrCharacterized by being a bodyCoordinate vector of reference unit common to body l, and reference pointAnd O_lIs irrelevant. BodyThe body l is a rod or a shaft.

The axis invariants are essentially different from coordinate axes:

(1) the coordinate axis is a reference direction with a zero position and unit scales, and can describe the position of translation along the direction, but cannot completely describe the rotation angle around the direction, because the coordinate axis does not have a radial reference direction, namely, the zero position representing rotation does not exist. In practical applications, the radial reference of the shaft needs to be supplemented. For example: in the Cartesian system F^[l]In the rotation around lx, ly or lz is used as a reference zero position. The coordinate axes themselves are 1D, with 3 orthogonal 1D reference axes constituting a 3D cartesian frame.

(2) The axis invariant is a 3D spatial unit reference axis, which is itself a frame. It itself has a radial reference axis, i.e. a reference null. The spatial coordinate axes and their own radial reference axes may define cartesian frames. The spatial coordinate axis may reflect three basic reference properties of the motion axis and the measurement axis.

The axis vector of the chainless index is recorded in the literatureAnd is called the Euler Axis (Euler Axis), and the corresponding joint Angle is called the Euler Angle. The present application is no longer followed by the euler axis, but rather is referred to as the axis invariant because the axis invariant has the following properties:

【1】 Given rotation transformation arraySince it is a real matrix, whose modes are unitary, soWhich has a real eigenvalue lambda₁And two complex eigenvalues λ conjugated to each other₂＝e^iφAnd lambda₃＝e^-iφ(ii) a Wherein: i is a pure imaginary number. Therefore, | λ₁|·||λ₂||·||λ₃1, to obtain lambda₁1. Axial vectorIs a real eigenvalue λ₁1, is an invariant;

【2】 Is a 3D reference shaft, not only having an axial reference direction, but also having a radial reference null, as will be described in section 3.3.1.

【3】 Under a natural coordinate system:i.e. axial invariantIs a very special vector, has invariance to the derivative of time and has very good mathematical operation performance;

for an axis invariant, its absolute derivative is its relative derivative. Since the axis invariant is a natural reference axis with invariance, its absolute derivative is always a zero vector. Thus, the axis invariants have invariance to the time differential. Comprises the following steps:

【4】 In a natural coordinate system, passing an axial vectorAnd joint variablesCan directly describe the rotating coordinate arrayIt is not necessary to establish a separate system for the rods other than the roots. Meanwhile, the measurement precision of the system structure parameters can be improved by taking the only root coordinate system to be defined as reference;

【5】 Using axial vectorsThe method has the advantages that a completely parameterized unified multi-axis system kinematics and dynamics model comprising a topological structure, a coordinate system, a polarity, a structure parameter and a mechanics parameter is established.

Factor vector e_lIs a reaction of with F^[l]Any vector, base vector, of consolidationIs andany vector of consolidation, in turnIs F^[l]Anda common unit vector, thereforeIs F^[l]Anda common basis vector. Thus, the axis is invariantIs F^[l]Andcommon reference base. The axis invariants are parameterized natural coordinate bases and are multiplesPrimitives of the axis system. The translation and rotation of the fixed shaft invariant are equivalent to the translation and rotation of a coordinate system fixedly connected with the fixed shaft invariant.

When the system is in a zero position, the natural coordinate system is used as a reference, and coordinate axis vectors are obtained through measurementIn the kinematic pairAxial vector during motionIs an invariant; axial vectorAnd joint variablesUniquely identifying kinematic pairThe rotational relationship of (1).

Thus, with the natural coordinate system, only a common reference frame need be determined when the system is in the null position, rather than having to determine individual body coordinate systems for each rod in the system, as they are uniquely determined by the axis invariants and the natural coordinates. When performing system analysis, the other natural coordinate systems, apart from the base system, to which the bars are fixed, only occur conceptually, and are not relevant to the actual measurement. The theoretical analysis and engineering functions of a natural coordinate system on a multi-axis system (MAS) are as follows:

(1) the measurement of the structural parameters of the system needs to be measured by a uniform reference system; otherwise, not only is the engineering measurement process cumbersome, but the introduction of different systems introduces greater measurement errors.

(2) A natural coordinate system is applied, and except for the root rod piece, the natural coordinate systems of other rod pieces are naturally determined by the structural parameters and the joint variables, so that the kinematics and dynamics analysis of the MAS system is facilitated.

(3) In engineering, the method can be applied to optical measurement equipment such as a laser tracker and the like to realize the accurate measurement of the invariable of the fixed shaft.

(4) As the kinematic pairs R and P, the spiral pair H and the contact pair O are special cases of the cylindrical pair C, the MAS kinematics and dynamics analysis can be simplified by applying the cylindrical pair.

Definition 3 invariant: the quantities that are not measured in dependence on a set of coordinate systems are called invariant.

Define 4 rotational coordinate vectors: vector around coordinate axisRotated to an angular positionCoordinate vector ofIs composed of

Define 5 translation coordinate vectors: vector along coordinate axisTranslation to linear positionCoordinate vector ofIs composed of

Define 6 natural coordinates: taking the vector of the natural coordinate axis as a reference direction, and marking the angular position or linear position of the zero position of the system as q_lCalled natural coordinates; weighing the quantity mapped one by one with the natural coordinate as a joint variable; wherein:

define 7 mechanical zero: for kinematic pairAt an initial time t₀Zero position of time, joint absolute encoderNot necessarily zero, which is called mechanical zero;

hence the jointControl amount ofIs composed of

Defining 8 natural motion vectors: will be represented by natural coordinate axis vectorsAnd natural coordinate q_lDetermined vectorReferred to as natural motion vectors. Wherein:

the natural motion vector realizes the unified expression of the translation and rotation of the shaft. Vectors to be determined from natural coordinate axis vectors and joints, e.g.Called free motion vector, also called free helix. Obviously, axial vectorIs a specific free helix.

Define 9 the joint space: by joint natural coordinates q_lThe space represented is called joint space.

Define a 10-bit shape space: a cartesian space expressing a position and a posture (pose for short) is called a configuration space, and is a dual vector space or a 6D space.

Defining 11 a natural joint space: with reference to natural coordinate system and by joint variablesIndicating that there must be at system zeroIs called the natural joint space.

As shown in FIG. 2, a given linkOrigin O_lPosition-dependent vectorConstrained axis vectorIs a fixed axis vector, is denoted asWherein:

axial vectorIs the natural reference axis for the natural coordinates of the joint. Due to the fact thatIs an axis invariant, so it is calledFor the invariants of fixed axes, it characterizes kinematic pairsThe natural coordinate axis is determined. Fixed shaft invariantIs a chain linkNatural description of structural parameters.

Defining 12 a natural coordinate axis space: the fixed axis invariant is used as a natural reference axis, and a space represented by corresponding natural coordinates is called a natural coordinate axis space, which is called a natural axis space for short. It is a 3D space with 1 degree of freedom.

As shown in figure 2 of the drawings, in which,andwithout rod omega_lIs a constant structural reference.Determines the axis l relative to the axisFive structural parameters of (a); and joint variable q_lTogether, the rod omega is expressed completely_lThe 6D bit shape. Given aThe natural coordinate system of the fixed rod can be determined by the structural parametersAnd joint variablesAnd (4) uniquely determining. Balance shaft invariantFixed shaft invariantVariation of jointAndis naturally invariant. Obviously, invariant by a fixed axisAnd joint variablesNatural invariance of constituent jointsAnd from a coordinate systemTo F^[l]Determined spatial configurationHaving a one-to-one mapping relationship, i.e.

Given a multi-axis system D ═ T, a, B, K, F, NT }, in the system null position, only the base or inertial frame is established, as well as the reference points O on the axes_lOther rod coordinate systems are naturally determined. Essentially, only the base or inertial frame need be determined.

Given a structural diagram with a closed chain connected by kinematic pairs, any kinematic pair in a loop can be selected, and a stator and a mover which form the kinematic pair are divided; thus, a loop-free tree structure, called Span tree, is obtained. T represents a span tree with direction to describe the topological relation of tree chain motion.

I is a structural parameter; a is an axis sequence, F is a rod reference system sequence, B is a rod body sequence, K is a kinematic pair type sequence, and NT is a sequence of constraint axes, i.e., a non-tree.For taking an axis sequenceIs a member of (1). The revolute pair R, the prismatic pair P, the helical pair H and the contact pair O are special cases of the cylindrical pair C.

The basic topological symbol and operation for describing the kinematic chain are the basis for forming a kinematic chain topological symbol system, and are defined as follows:

【1】 The kinematic chain is identified by a partially ordered set (].

【2】A^[l]Is a member of the axis-taking sequence A; since the axis name l has a unique number corresponding to A^[l]Number of (2), therefore A^[l]The computational complexity is O (1).

【3】Is a father axis of the taking axis l; shaftThe computational complexity of (2) is O (1). The computation complexity O () represents the number of operations of the computation process, typically referred to as the number of floating point multiplies and adds. The calculation complexity is very complicated by the expression of the times of floating point multiplication and addition, so the main operation times in the algorithm circulation process are often adopted; such as: joint pose, velocity, acceleration, etc.

【4】For taking an axis sequenceA member of (a);the computational complexity is O (1).

【5】^ll_kTo take the kinematic chain from axis l to axis k, the output is represented asAnd isCardinal number is recorded as $^ll_k|。^ll_kThe execution process comprises the following steps: executeIf it isThen executeOtherwise, ending.^ll_kThe computational complexity is O (# |)^ll_k|)。

【6】^ll is a child of axis l. The operation is represented inFinding the address k of the member l; thus, a sub-A of the axis l is obtained^[k]. Due to the fact thatHas no off-order structure, therefore^lThe computational complexity of l is

【7】^lL denotes obtaining a closed sub-tree consisting of the axis L and its sub-tree, ^l l is a subtree containing no L; recursive execution^ll, the computational complexity is

【8】 Adding and deleting operations of branches, subtrees and non-tree arcs are also necessary components; thus, the variable topology is described by a dynamic Span tree and a dynamic graph. In the branch^ll_kIn, ifThen remember Namely, it isRepresenting the child of member m taken in the branch.

The following expression or expression form is defined:

the shafts and the rod pieces have one-to-one correspondence; quantity of property between axesAnd the amount of attribute between the rodsHas the property of order bias.

Appointing:representing attribute placeholders; if the attribute P or P is location-related, thenIs understood to be a coordinate systemTo F^[l]The origin of (a); if the property P or P is directional, thenIs understood to be a coordinate systemTo F^[l]。

Andare to be understood as a function of time t, respectivelyAndand isAndis t₀A constant or array of constants at a time. But in the bodyAndshould be considered a constant or an array of constants.

In the present application, the convention: in a kinematic chain symbolic operation system, attribute variables or constants with partial order include indexes representing partial order in name; or the upper left corner and the lower right corner, or the upper right corner and the lower right corner; the direction of the parameters is always from the upper left corner index to the lower right corner index of the partial order index or from the upper right corner index to the lower right corner index. For example:can be briefly described as (representing k to l) translation vectors;represents the line position (from k to l);^kr_lrepresents a translation vector (from k to l); wherein: r represents the "translation" attribute, and the remaining attributes correspond to: the attribute token phi represents "rotate"; the attribute symbol Q represents a "rotation transformation matrix"; the attribute symbol l represents "kinematic chain"; attribute character u represents a "unit vector"; attribute symbol w represents "angular velocity"; the angle index i represents an inertial coordinate system or a geodetic coordinate system; other corner marks can be other letters and can also be numbers.

The symbolic specification and convention of the application are determined according to the principle that the sequence bias of the kinematic chain and the chain link are the basic unit of the kinematic chain, and reflect the essential characteristics of the kinematic chain. The chain index represents the connection relation, and the upper right index represents the reference system. The expression of the symbol is simple and accurate, and is convenient for communication and written expression. Meanwhile, the data are structured symbolic systems, which contain elements and relations for forming each attribute quantity, thereby facilitating computer processing and laying a foundation for automatic modeling of a computer. The meaning of the index needs to be understood through the context of the attribute symbol; such as: if the attribute symbol is of a translation type, the index at the upper left corner represents the origin and the direction of a coordinate system; if the attribute is of the pivot type, the top left indicator represents the direction of the coordinate system.

(1)l_S-a point S in the bar l; and S denotes a point S in space.

(2)Origin O of bar k_kTo the origin O of the rod l_lA translation vector of (a);

in a natural coordinate system F^[k]The coordinate vector from k to l;

(3)-origin O_kTo point l_SA translation vector of (a);

at F^[k]A lower coordinate vector;

(4)-origin O_kA translation vector to point S;

at F^[k]A lower coordinate vector;

(5)-a connecting rod memberAnd a kinematic pair of the rod piece l;

kinematic pairAn axis vector of (a);

andare respectively atAnd F^[l]A lower coordinate vector;is an axis invariant, being a structural constant;

as rotation vector, rotation vector/angle vectorIs a free vector, i.e., the vector is free to translate;

(6)along the axisThe linear position (translational position) of (c),

-about an axisThe angular position of (a), i.e. joint angle, joint variable, is a scalar;

(7) when the index of the lower left corner is 0, the index represents a mechanical zero position; such as:

-a translation shaftThe mechanical zero position of the magnetic field sensor,

-a rotating shaftMechanical zero position of (a);

(8) 0-three-dimensional zero matrix; 1-a three-dimensional identity matrix;

(9) appointing: "\\" represents a continuation symbol;representing attribute placeholders; then

Power symbolTo representTo the x-th power of; the right upper corner is marked with ^ orA representation separator; such as:orIs composed ofTo the x power of.To representThe transpose of (1) indicates transposing the set, and no transpose is performed on the members; such as:

the projection symbol is a projection vector or a projection sequence of a vector or a second-order tensor to a reference base, namely a coordinate vector or a coordinate array, and the projection is dot product operation "·"; such as: position vectorIn a coordinate system F^[k]The projection vector in (1) is recorded as

Is a cross multiplier; such as:is axis invariantA cross-product matrix of; given any vectorIs cross-multiplication matrix ofThe cross-multiplication matrix is a second order tensor.

Cross-multiplier operations have a higher priority than projectersThe priority of (2). Projecting signIs higher priority than the member access characterOrMember access signPriority higher than power symbol

(10) Projection vector of unit vector in geodetic coordinate systemUnit zero vector

(11)Zero position by originTo the origin O_lIs translated by the vector ofRepresenting the location structure parameter.

(12)ⁱQ_lA rotation transformation matrix in relative absolute space;

(13) taking the vector of the natural coordinate axis as a reference direction, and marking the angular position or linear position of the zero position of the system as q_lCalled natural coordinates; variation of jointNatural joint coordinate phi_l；

(14) For a given ordered set r ═ 1,4,3,2]^TRemember r^[x]The representation takes the x-th row element of the set r. Frequently remembered [ x ]]、[y]、[z]And [ w]This is shown with the elements in columns 1, 2, 3 and 4.

(15)ⁱl_jRepresents a kinematic chain from i to j;^ll_ktaking a kinematic chain from an axis l to an axis k;

given kinematic chainIf n represents a Cartesian rectangular system, it is calledIs a Cartesian axis chain; if n represents a natural reference axis, then callIs a natural axis chain.

(16) Rodrigues quaternion expression form:

euler quaternion expression:

quaternion (also called axis quaternion) representation of invariants

For a 6R universal manipulator, the operability problem of analyzing the inverse solution needs to be solved: on one hand, the engineering structure parameters are represented by fixed shaft invariant, and the absolute positioning precision of the multi-axis system is ensured; on the other hand, the problem of dimension reduction of the motion equation and the problem of computability of inverse solution by applying a variable elimination method need to be solved.

The number of the translational shafts and the number of the rotating shafts in the natural space are respectively 3, wherein the translational shafts can be replaced by the rotating shafts. The translational axis and the rotational axis in the 6R kinematic chain are respectivelyPrism pair P, revolute pair R and kinematic pairMoving axis l, moving chainⁱl_n. It is clear that, the kinematic chains can be divided into three major categories: pure translation (3 types), pure rotation (6 types) and composite rotation and translation (12 types), 21 types in total. Of these, 3 pure translational chains are a trivial kinematic problem, without the need forDiscussion is made. Thus, the existence of the non-trivial inverse solution of axial chain kinematics is:

when the oxygen deficiency is reachedⁱl_nWhen | ═ 6, then the requirement isNamely, at least 3 revolute pairs are needed to meet the requirement of pose alignment.

The manual derivation of the kinematic equation of the 6R kinematic chain is very complicated and easy to make mistakes, and the reliability of modeling is difficult to ensure; on one hand, an iterative equation needs to be established to meet the requirement of automatically establishing a multi-axis system symbolic model by a computer; on the other hand, a smaller number of axes of the kinematic chain needs to be applied for equivalence. The kinematic equation has a plurality of equivalent forms, and only the kinematic equation with a specific structure has feasibility of inverse solution, namely the positive kinematic equation is required to have the minimum order, the minimum number of equations and the minimum number of independent variables; and the inverse solution process is required to have no singularity caused by numerical calculation.

Because the pose of the natural space has 6 dimensions, a 6-pose equation only containing 6 joint variables needs to be established. Obviously, the pose equation based on the euler quaternion or the dual quaternion does not meet the requirement of minimum equation number. The motion vectors including translation and rotation are essentially natural spirals, and the final axis of the mechanical arm always needs to be aligned with the desired direction to perform the required operation; after the first 5 axes control the alignment of the 6 th axis with the desired position and orientation, the 6 th axis is controlled to meet the radial alignment; therefore, for a general-purpose 6R robot arm, only pose equations containing the first 5 joint variables need to be established.

Therefore, the invention provides a 'Jubs' attitude quaternion, which aims to: the alignment is completed through the front 5 axes to eliminate the joint variables of the 4 th axis and the 5 th axis, and a foundation is laid for the subsequent inverse solution.

High-dimensional determinant calculation of the block matrix:

note the book<1:n>Represents a natural number [1: n ]]Has a total of n! An example. Given a matrix M of size n × n belonging to a number domain, the elements of j rows and i columns are notedIs defined according to determinant

Wherein: i [ I1, … in]Showing the arrangement<i1，…in>The number of the reverse orders of (1). The computational complexity of equation (2) is: n! N product times and n! The secondary addition has exponential calculation complexity and can only be applied to determinants with smaller dimensions. For the determinant with larger dimension, Laplace formula is usually applied to carry out recursion operation and memorizeIs composed ofThe adjoint Matrix (adjoint Matrix) of (2) then has

The simpler algorithm usually applies gaussian elimination or LU decomposition, and first converts the matrix into a triangular matrix or a product of triangular matrices by elementary transformation, and then calculates the determinant. The determinant calculation method for the number domain is not suitable for a high-dimensional polynomial matrix, and a determinant calculation method for a block matrix needs to be introduced. The determinant for computing Vector Polynomial (Vector multinomial) is a specific block matrix determinant computing problem, which expresses the intrinsic relationship of vectors and determinants at the Vector level. And the block matrix determinant calculation expresses the intrinsic rules of the block matrix and the determinant from the matrix level.

If given a vector polynomialWherein:andis a 3D coordinate vector of the image,is a polynomial variable sequence; if contract

Then there is

The derivation steps of the above formula are: due to the fact that

Therefore, the formula (5) is established.

The expressions (4) and (5) can be generalized to n-dimensional space. Equation (4) helps analyze the intrinsic regularity of the determinant from the vector level; for example, when any two vectors are parallel or three vectors are coplanar, the corresponding determinant is zero. Formula (5) indicates that: determinants of vector polynomials are prone to "combinatorial explosion".

Example 1

Given 2-dimensional row vector polynomialsAnd on the one hand, is obtained by the formula (5)

On the other hand, in the case of a liquid,

the above results verify the correctness of equation (5).

Giving a determinant calculation theorem of a block matrix:

if the matrix with the size of (n + M) · (n + M) is M, the matrix with the size of n.nIs a sub-matrix formed by the first n rows and any n columns of elements of the square matrix M and has the size of m.mIs a sub-matrix formed by M rows and the rest M columns of elements behind the square matrix M; the sequences cn and cm, which are composed of the matrix sequence numbers arranged in ascending order, are the sequences [1: m + n ]]A subset of [ cn, cm ]]∈<1:n+m>and has cmU cn ═ 1: m + n](ii) a Then square matrixAndhas a determinant relationship of

Carrying out a stepped calculation principle on the determinant:

for an S × S matrix, each entry is for τ₁Polynomial of order n. When the determinant of the matrix is calculated, the original determinant can be changed into an upper triangular determinant through primary row transformation, and then nonzero diagonal elements are multiplied to obtain a polynomial expression of the determinant. Since the formula is 0, τ is obtained₁All solutions of (a).

Example 2

Through the primary line transformation of the matrix, the method obtainsA matrix of row ladders.

The method comprises the following steps: rk represents the k-th row. To obtain

Then obtain

An N-order polynomial system based on 'N carry word':

if n 'n-element 1-order' polynomial power productsThe medium independent variable appears repeatedly for N times to obtain N 'N-element N-order' polynomial systems"N-element N-order polynomial system" and "N-bit N-carry word"And (4) isomorphism.

Dixon polynomials of N "nth order N" polynomial systems:

introducing an auxiliary variable [ y₂,y₃,…,y_n]And is provided with

In the multivariate polynomial (9), with the auxiliary variable Y_mThe first m sequentially replacing original variables (OriginalVariables) X_nM variables in the (b) are marked with "|" as a replacing operator to obtain an Extended polynomial

in the formula, the upper right symbols alpha and alpha represent powers;

from formula (7) and formula (13)

Wherein:

defining separable compositional variablesAndthe following were used:

the following equations (15) and (16) show that: replaceable typeIs aboutAnddouble linear type of (1). Accordingly, the polynomial system replaced by the auxiliary variable is denoted by

Given N 'N-element N-order' polynomial systemsDefining the Dixon polynomial as

Is obtained by the formula (18)

The isolated variables in formula (16) differ from literature: original variable X_n-1Assisted variable Y_n-1The ordering of the substitutions is different, as are the Dixon polynomials. Dixon determinant of this polynomial taking into account equations (14) and (19)

the orders and the number of the replacement variable terms of the Dixon determinant of N 'N-element N-order' polynomials are respectively as follows:

n "N-ary N-th order" Dixon matrices:

given N "N-ary N-th order" polynomial systems F_n(Y_n-1|X_n-1) N is not less than 2; exist ofAnd elimination of variable x₂,…,x_nIndependent Dixon matrix_SΘ_S(x₁) Of Dixon polynomials thereofExpressed as a separate variableAnddouble linear type of (2):

α[l]∈[0,N·(n-l+1)-1],l∈[2:n](24)

wherein:

if it is

Then there is

Dixon elimination and solution of N 'N-element N-order' polynomials

Consider formula (23) ifSo that

Det(_SΘ_S(x₁))＝0。 (29)

The 'n-elements' in the formula (29) are called as the necessary conditions of Dixon elimination elements, so that a feasible solution is obtained. If it is_SΘ_SIf there is a zero row or zero column vector, x cannot be established₁A polynomial equation of (a); at this time, through elementary transformation except scalar product, will_SΘ_SBecomes a Row ladder (Row Echelon) matrix Ech (_SΘ_S) (ii) a Obtaining the square matrix after calculating the product of the input axes (Pivot) of the matrixNamely at_SΘ_SS' independent column vectors are selected.

Any one of N 'N-element N-order' polynomial systemsExamples of (A), (B) are denoted byWherein: and is provided withAccording toDetermining Dixon matrix, separation variablesAndselectingAndsatisfy the requirement of

Determining bilinear forms

Wherein:neutralization ofThe corresponding column lines are linearly independent. Due to the fact thatFrom the formulae (23) and (26)

It is referred to as a knot or elimination. Formula (33) is a univariateQuantity x₁A polynomial equation of (a); n-1 unknowns are eliminated; thus, a univariate x can be obtained₁Is possible. If x₁At the same time satisfy

X is then₁Is a correct solution. Will have solved x₁Substituted for (35) and found in (33)At will, therefore, get

Namely have

If necessary under the conditions

Solve equation (36) to obtain the eliminated variableThe solution of (1); otherwise, the complete solution needs to be obtained by combining formula (17). Considering equation (26), x on both sides of equation (23)₁The order is equal, so it must be

If at the same time satisfy

Then the formula (36) can be solvedN-1 mutually different combination variables; thus, solutions for all independent variables are obtained.

Given N 'N-ary N' polynomialsThe Dixon matrix calculation steps are as follows:

determining system structure, recording equation number and independent variable number as n, recording independent variable as X_n(ii) a The polynomial complex variable is expressed asThe replacement variable is notedThe number of the replacing variables is n-1; dixon matrix of size S.S is denotedThe member coefficient is represented by the formula (25), wherein: s is determined by formula (33); the variable to be eliminated is x₁。

② x is obtained from the formula (9)^αAndcorrespondence relationship, in expression (12)There are at most S terms.

③ Dixon (F) according to formula (20) and Sarrus rule_n(Y_n-1|X_n-1) ); according toAnd corresponding N carry word operation results are obtained, and polynomial combination is completed.

the Dixon matrix member is shown as formula (33), and the Dixon matrix is calculated from formula (33)_SΘ_S(n + 1). S²A coefficient.

when the direct solution criterion of the formula (38) and the formula (39) is satisfied, obtaining all numerical solutions from the formula (35) and the formula (36).

Example 3

Dixon elimination is performed on the polynomial system (40).

The method comprises the following steps: the formula is 4 '4-element 1-order' polynomial systems, and satisfies the Dixon elimination condition. From the formulae (20) and (23) to give

Wherein:

5 solutions were obtained from equations (35) and (41):

wherein:is not a solution to the system of equations. Other solutions are substituted into equations (36), respectively. When in useThen, it is obtained from the formula (36)

Obtaining by solution: tau is₃＝1，τ₄-2. Will be provided withτ₃And τ₄T is obtained by substituting formula (40)₂1. Likewise, three other sets of solutions are available. Obviously, the dependent variable does not satisfy equation (27), and the Dixon matrix shown in equation (41) is asymmetric. This example shows that a Dixon determinant of zero is sufficient for a multiple linear polynomial system.

Fixed axis rotation based on axis invariants

As shown in FIG. 3, axis vectors are givenAnd unit vector consolidated therewithFor unit vectors before rotation To system zero axisIs projected vector ofTo the radial axis of the systemHas a moment vector ofRadial vector is

Axial vectorRelative to the rod memberAnd omega_lOr natural coordinate systemAnd F^[l]Is fixed and not changed, so the rotation is called fixed axis rotation. Unit vectorAround shaftRotation ofRear, rotated null vectorTo system zero axisIs projected vector ofZero vector after rotationTo the radial axis of the systemHas a moment vector ofAn axial component ofSo as to obtain the Rodrigues vector equation with chain indexes

Factor unit vectorIs arbitrary andobtaining the Rodrigues equation of rotation with chain index

If it isFrom formula (43) toIf it isI.e. coordinate systemAnd F^[l]The directions of (A) and (B) are identical, and the following formula (43) shows that: antisymmetric partMust haveThus, the system zero is a natural coordinate systemAnd F^[l]The sufficient requirement for coincidence, i.e. the direction of the natural coordinate system at the initial moment is consistent, is a precondition for the zero definition of the system. The kinematics and dynamics of the multi-axis system can be conveniently analyzed by utilizing a natural coordinate system.

Formula (44) relates toAndis an axis invariantA second order polynomial of (a). Given natural null vectorAsZero reference ofAndrespectively representNull vectors and radial vectors. Formula (44) isSymmetrical partRepresenting zero-axis tensor, antisymmetric partRepresenting the radial-axis tensor, respectively the axial-outer product tensorOrthogonal, thereby determining a three-dimensional natural axis space; the formula (44) only comprises a sine and cosine operation, 6 product operations and 6 sum operations, and the calculation complexity is low; at the same time, the passing shaft does not changeAnd joint variablesThe parameterization of the coordinate system and the polarity is realized.

For axle chainIs provided with

Is obtained by formula (45) and formula (44)ThenIs thatAndmultiple linear type of (a), wherein: l is an element ofⁱl_k. Formula (44) can be represented as

The modified Cayley transform is designated (46). Namely have

The normalized position equation is

Determination of "jubes" quaternion:

as for any of the bar members l,define the "Ju-Gibbs" (Ju-Gibbs) canonical quaternion isomorphic with Euler quaternion:

wherein:is a Gibbs vector. The Gibbs conjugate quaternion is:

wherein:

it is clear that,is composed ofThe square of the mode. Since the Cure-Gibbs quaternion is a quaternion, quaternion multiplication is satisfied

Wherein:

is obtained by formula (53)

Conventionally, the expected postures of the single joints and the kinematic chains are expressed by a standard Ju-Gibbs quaternion (the standard Ju-Gibbs quaternion is abbreviated as the quaternion with 1); however, their product operation is usually irregular, i.e. its scale is not 1. From the formula (54): only given axes l andthe norm Ju-Gibbs quaternion, and the two axes are orthogonal,is a canonical quaternion.

Is obtained by the formula (54)

By four-dimensional complex nature

Note the bookIs obtained by formula (53)

Therefore, it isIs a unit Ju-Gibbs quaternion.

From formula (49) to formula (51) and formula (56)

From formula (51), formula (55) and formula (58)

DCM-like and properties:

for axle chainThe canonical attitude equation is:

is obtained by the formula (60)

wherein:

from the formula (62):ⁱQ_nandis about tau_kN is multiplied by a polynomial of order 2. From the formula (61): due to the fact thatAndsimilarly, it is called DCM-like (DCM, directional cosine matrix). Is obtained by formula (63)

Obviously, DCM-like can be expressed by Ju-Gibbs quaternion. Therefore, the equation of the attitude of equation (60) and the equation of the position of equation (48) are expressions with respect to the quaternion of Ju-Gibbs.

Inverse of block matrix:

if reversible square matrixes K, B and C are given, wherein B and C are square matrixes of l × l and C × C respectively; A. d are matrices of l × c, c × l, respectively, an

Then there is

Pointing alignment principle based on Ju-Gibbs quaternion

Considering axle chainsⁱl_l，Vector of unit axisAnd desired unit axis vectorWhen aligned, at least one multi-axis rotation Ju-Gibbs quaternion exists

Wherein

The specific establishment steps of the above formula are as follows:

cayley positive transformation of fixed axis rotation

From formula (70) to

Is obtained by formula (71)

And is

Axial vector of causeAnd the expected vectorIs a unit vector, let us assumeAnd isTo obtain

Formula (74) showsAndare orthogonal to each other. The optimal axis vector is obtained from equations (73) and (74)

And is

From formulae (75) and (76) to (67), ifOrIs obtained by formula (71)

Is obtained by formula (77)

Due to the fact thatIs obtained by the formula (78)

And is

Formula (68) derived from formula (79), formula (69) derived from formula (80)

The pointing alignment principle based on the Ju-Gibbs quaternion shows that: there is at least one desired Ju-Gibbs quaternionMake unit vectorAnd the expected unit vectorAnd (4) aligning.

Example 4

Considering axle chainsⁱl₆Derived from the principle of pointing alignment based on Ju-Gibbs quaternion

2R direction inverse solution based on Ju-Gibbs quaternion

Based on the alignment of the Ju-Gibbs quaternion orientation, the 2R direction inverse solution is explained.

Wherein:

quaternion in Ju-Gibbs orientationSatisfy the requirement of

The specific establishment steps of the above formula are as follows:

first consider the Euler quaternion-based pose alignment, and

is obtained by the formula (86)

Is obtained by formula (87)

Wherein:

is obtained by formula (65) and formula (89)

Is obtained by the formula (88)

Wherein:

is obtained by the formula (90) and the formula (91)

Is obtained by formula (93)

If it isFormula (93) and formula (94) are divided by

Second, consider the directional alignment of the Ju-Gibbs quaternion. Due to the fact thatThus, the product (83) was obtained. Is obtained by formula (59)

The joint variable is represented by a canonical Ju-Gibbs quaternion which is obtained by the formula (54)

Is represented by formula (97)

Due to the fact that³n₄And⁴n₅independently, as can be seen from the formula (84),³E₅must be present. It is clear that,by³n₄And⁴n₅and (4) uniquely determining. By substituting formula (97), formula (98) and formula (90) for formula (96)

If it isObtained by the formula (99) line 1

By substituting formula (100) for formula (99)

The two principles are equivalent to each other as shown in the formulas (95) and (101). Obtained by 2 nd and 3 rd of formula (101)

From the equation (102), it is understood that the equation (81) holds. There are 4 equations, 2 independent variables, for equation (101), and the constraint equation from line 4 in equation (102) and equation (101)

If it isFrom formula (94) to C₄C₅0; is obtained by the formula (88)

Is obtained by the formula (104)

It is clear that,when in useWhen, ifIs obtained by formula (103)

If it isIs obtained by formula (103)

From the formula (107): or eitherOr eitherFrom the equations (107) and (102), the equation (82) is also established. When in useWhen, ifEquation (81) also holds. After the syndrome is confirmed.

Quaternion in Ju-Gibbs orientation

Obtained by the formulae (81), (82) and (100)

And

handleSubstituting into the above two equations to obtain equation (85), which indicates a specific Ju-Gibbs quaternion, called Ju-Gibbs direction quaternion.

Example 5

Continuing with example 4, consider a chain of axlesⁱl₆And is provided with³n₄＝1^[x]，⁴n₅＝1^[y]Is obtained by the formula (84)³E₅1. Is obtained from formula (81) and formula (82)

The 2R direction inverse solution principle based on the Ju-Gibbs quaternion shows that the Ju-Gibbs quaternion and the Euler quaternion are isomorphic; meanwhile, equation (64) shows that DCM-like isomorphism with DCM is expressed by Ju-Gibbs quaternion. Therefore, the pose relation can be completely expressed by applying the Ju-Gibbs quaternion.

If given a kinematic chainⁱl_n，k∈ⁱl_nExpectation criterion Ju-Gibbs quaternionAnd the expected position vectorConsider equations (48) and (96); the position and orientation alignment relationship is expressed as

And has a modulus invariance

Compared with the Euler quaternion and the dual quaternion, the pose alignment represented by the Ju-Gibbs quaternion has no redundant equation; through pointing alignment, the joint variables of the 4 th axis and the 5 th axis can be solved, and a foundation is laid for 6R and 7R mechanical arm inverse solution.

Dixon determinant principle of calculation based on axis invariants:

and the Dixon determinant basic properties of the radial invariant and the kinematic chain are provided based on the axis invariant, so that a foundation is laid for the inverse kinematics analysis of the robot based on the axis invariant.

【1】 Axial invariant

First, axis invariants have a substantial difference from coordinate axes: the coordinate axis is a reference direction with a zero position and unit scales, can describe the linear position of the axial translation, but cannot completely describe the angular position around the axial direction, because the coordinate axis does not have a radial reference direction per se, namely, a zero position representing rotation does not exist. In practical applications, a supplementary radial reference of the coordinate axes is required. The coordinate axes are 1D, and 3 orthogonal coordinate axes form a 3D Cartesian frame; the axis invariant is a 3D spatial unit reference axis (3D reference axis for short) with a radial reference null. The "3D reference axis" and its radial reference null may determine the corresponding cartesian system. The three basic properties of coaxiality, polarity and zero position of the motion axis and the measurement axis can be accurately reflected by the axis invariants based on a natural coordinate system.

Second, the axis invariants are fundamentally different from the euler axis: the Direction Cosine Matrix (DCM) is a real matrix, and the axis vector is a feature vector corresponding to the feature value 1 of the DCM and is an invariant; the fixed shaft invariant is a 3D reference shaft, and has an original point, an axial direction and a radial reference zero position; under a natural coordinate system, the axis invariance does not depend on the adjacent fixed natural coordinate system, namely, the axis invariance has an unchangeable natural coordinate under the adjacent fixed natural coordinate system; the shaft invariants have excellent mathematical operation functions such as power zero characteristics and the like; in a natural coordinate system, the DCM and the reference polarity can be uniquely determined through the axis invariants and the joint coordinates; it is not necessary to establish a separate system for each rod, and the workload of modeling can be greatly simplified.

Meanwhile, the only Cartesian rectangular coordinate system to be defined is used as a reference, and the measuring axis is invariant, so that the measuring precision of the structural parameters can be improved. Based on the excellent operation and attributes of the axis invariants, iterative kinematics and kinetics equations including topological structure, coordinate system, polarity, structure parameters and kinetics parameters can be established.

From the equations (60) and (48): the attitude and position equations of multiaxial systems are essentially multivariate second-order polynomial equations, the inverse solution of which is essentially attributable to the elimination problem of multivariate second-order polynomials, including two subproblems of Dixon matrix and Dixon determinant calculations. The expression 3R mechanical arm position equation of the formula (48) is 3 '3-element 2-order' polynomials, the inverse solution is calculated by applying a Dixon elimination method, two alternative variables are provided, and the maximum possible order is 16 when an 8 x 8 Dixon determinant is calculated. As can be seen from the formula (5): determinant calculation is an arrangement process and faces the problem of 'combinatorial explosion'.

All questions that are not solvable within a certain polynomial time are called NP questions. Non-deterministic algorithms decompose the problem into two phases, "guess" and "verify": the "guess" phase of the algorithm is non-deterministic, and the "verify" phase of the algorithm is deterministic, with verification to determine if the guessed solution is correct. If it can be calculated within the polynomial time, it is called the polynomial non-deterministic problem. The elimination of multivariate polynomials is generally considered an NP problem. General applicationThe basis is to perform elimination of the multivariate polynomial and has to resort to heuristic "guessing" and "verification" to solve the problem.

【2】 Radial invariance

Structural parametersAndare structural variables of the chain links l, which can be obtained by external measurement in the zero position of the system. As shown in fig. 4, the null vector, the radial vector, and the axial vector are invariant independent of the rotation angle. Wherein the null vector is a particular radial vector.

Any vector can be decomposed into a null vector and an axial vector, so

Wherein:

consider a chain linkThe D-H parameter is

It is clear that,is a shaft l andthe common perpendicular or common radial vector of (a),is the axial vector of the axis l. Is composed ofTherefore, the following steps are carried out: any one of the structure parameter vectorsDecomposable into zero invariants independent of coordinate systemAnd axial invarianceTheir radial vectors are notedVector of structural parametersAnd axis invariantThe radial coordinate system is uniquely defined with 2 independent dimensions. If two axial invariantsAndcollinear, then it is marked

If two zero-position invariantsAndand any two radial invariantsAndcoplanar is then recorded

Therefore, the axial invariants and null invariants expressed by expression (111) are the decomposition of the structural parameter vector on the natural axis.

The following equations (114) and (115) show that: the determinant of three radial vectors of the same axis is zero; the determinant of any two axial vectors of the same axis is zero. Dixon determinant computations can be simplified with axis invariants and their derived invariants.

The null, radial and axial vectors derived from the axis invariants have the following relationships:

equation (116) is called the inversion equation of the zero vector; equation (117) is called the interchange equation of the null vector and the radial vector; equation (118) is referred to as the radial vector invariance equation. From formula (110), formula (116) to formula (118)

Is obtained by formula (119)

Due to the fact thatIs thatIs a vector because of the structural constant of the symmetric part of (1), so that the formula (119) isIs a symmetric decomposition. Due to the fact thatIs thatSo that equation (120) is a vectorIs disclosed in (1). Equation (121) is referred to as a return-to-zero equation.

【3】 Kinematic chain Dixon determinant Properties

Definition of

Is obtained by formula (53)

Wherein:

considering equation (123), if M is a matrix of 4.4, then there is

And is provided with

Is obtained from formula (63) and formula (111)

Is proved by formula (127)

Is obtained by the formula (130)

Formula (128) can beAndcan be converted intoMultiple linear types of (a). At the same time, the user can select the desired position,for y_lAnd τ_lWith symmetry (rotation). From formula (112), formula (119) and formula (120)

Equation (130) is derived from three independent structural parametersAnd a motion variable τ_lAnd (4) forming. Is obtained by the formula (130)

From formula (128) and formula (132)

Is obtained from the formula (128) and the formula (133)

DCM-like based 2R direction inverse solution

Giving 6R axial chainⁱl₆＝(i，1:6]Axial vector of³n₄And⁴n₅the desired 5 th axis DCM isDCM of the desired 3 rd axis isDirection vector⁵l₆To the desired directionThe inverse solution of the alignment needs to satisfy the following equation:

the establishment steps of the above equation are:

direction vector⁵l₆To the desired directionAlignment needs to be satisfied

Is obtained by formula (61)

Namely, it is

Equation (138) is re-expressed as equation (135).

Cayley transform based on axis invariants

When a given angleThen, the sine and cosine and the sine and cosine of the half angle are constants; for convenient expression, record

Is obtained by formula (139)

Definition of

Therefore it has the advantages of

And radial vectorAnd tangential vectorIs a linear relation, scaleIs "Rodrigues Linear invariant". Is generally called asNamely, it isFor Rodrigues or Gibbs vectors, andreferred to as Modified Rodrigues Parameters (MRPs).

3R mechanical arm position inverse solution method based on axis invariant

Given 3R rotating chainAnd desired attitudeAxial invariant sequenceFinding joint variable sequenceThis is the 3R pose inverse solution problem.

【1】 And obtaining n 'n-element 2-order' polynomial equations according to the n-element 3D vector pose equation of the mechanical arm.

Equation of 3R kinematics from equation (48)

Is obtained by formula (143)

Is obtained by the formula (144)

If remember

Then the general formula (62) and the general formula (146)

Is obtained from formula (145) and formula (146)

Next, a Dixon determinant structural model and characteristics of the kinematic equation of the 3R manipulator are explained. Obtaining a 3R kinematic polynomial equation from equation (148)

Then there is

Wherein:

from formula (19), formula (148) and formula (149)

The expression (152) is satisfied from the expressions (23) and (154). Is obtained by the formula (128) and the formula (146)

Is obtained from formula (146), formula (155) and formula (156)

Wherein: using formula (134) calculations

Obviously, y in the formula (157)₂order β 2 ∈ [0:3 ]]And y₃order β 3 ∈ [0:1 ]]. Consider the last three terms of equation (154):y in (1)₂order β 2 ∈ [0:3 ]]And y₃order β 3 ∈ [0:1 ]]；Y in (1)₂order β 2 ∈ [0:2 ]]And y₃order β 3 ∈ [0:1 ]]；Y in (1)₂order of [ beta ] 2 ∈ [0:3 ]]And y₃order of [ beta ] 3 ∈ [0:1 ]]. From the above, it can be seen that: y in formula (154)₂order β 2 ∈ [0:3 ]]And y₃order of [ beta ] 3 ∈ [0:1 ]]. So, S is 8.

The following equations (146), (154) to (157) show:the medium combined variable coefficient is independent column vector, so it is selectedTo form a square matrixThe remaining column vectors are given a sumAre related. Therefore, the formula (153) is established.

【2】 The determinant calculation is simplified by applying a Dixon determinant calculation method based on axis invariants, a high-dimensional determinant calculation method of a block matrix or a step calculation method of the determinant.

According to the Dixon determinant property of kinematic chain, obtained from the formula (128) and the formula (146)

Respectively representing axes 2 to 3, and axes 3 to 3_SNull, radial and axial vectors.

Is obtained by formula (158)

Is obtained by formula (159)

Is obtained by the formula (160)

Is obtained by the formula (154)

Substituting formulae (161) to (163) for formula (164)

Unary nth order polynomial p (x) ═ a₀+a₁x+…a_n-1x^n-1+xⁿWith n solutions. If a matrix A can be found, satisfy | A- λ_l·1_n|·v_l0, wherein: is an element of [1: n ]]，λ_lIs a characteristic value of the matrix, upsilon_lIs the corresponding feature vector. If the characteristic equation of the matrix A isThe Matrix is called a friend Matrix (Companion Matrix for short) of the polynomial p (x), and thus, the polynomial equation p (λ:)_l) Solution to 0 as characteristic equation | a- λ of its friend matrix a_l·1_nSolution of 0.

If the polynomial p (x) has a lattice of

The matrix formed by the eigenvectors of matrix a is a van der monde (Vandermonde) matrix

And is provided with

p(λ_l)＝|A-λ_l·1_n|＝0 (168)。

Is obtained from formula (29), formula (152) and formula (153)

Since S is 8, the calculation of equation (2) is appliedThe complexity of (c) is 8.8! 322560; and performing a quadratic partition determinant calculation using equation (6) in which: the 2 · 2 block matrix has a computational complexity of 4! (2 · 2! +2 · 2! + 1)/(2! 2!) ═ 30, and the 4 · 4 partition matrices have a computational complexity of 8! (30+30+1)/(4 |) 4270. In general, formula (169) relates to₁16 order polynomial equation.

Second, axis invariant based universal 6R mechanical arm pose inverse solution method

6 rotating shafts are set, the picking point is located on the axis of the 6 th shaft, and the mechanical arm with the 4 th shaft and the 5 th shaft which are not coaxial is a universal 6R mechanical arm. The 6 th shaft is controlled to be aligned with a desired position and direction through the front 5 shafts, the 6 th shaft can be infinitely rotated or the 6 th shaft is controlled to meet the radial alignment.

The pose inverse solution of the universal 6R mechanical arm based on the axis invariant is as follows:

given the 6 th axis desired position vectorAnd desired attitudeAnd given the 6 th axis desired position vectorAnd 5 th axis desired attitudeThe inverse solution problem of (2) is equivalent; a generic 6R robot arm is essentially a 5R axis chain system.

If a 6R axis chain is givenⁱl₆＝(i,1:6]，ⁱl₁＝0₃The 6 th axis desired position vector isAnd 5 th axis desired attitudeThe 3 rd axis joint Ju-Gibbs standard quaternion isOther axis expression ways are the same; the 6R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

in the formula, the character is a continuation character;respectively represent the zero vector and the radial vector of the shafts 4 to 5 and 5 to 6;is axis invariantA cross-product matrix of; 0₃＝[0 0 0]^T；

The matrix formed by the system structure parameters and the quaternion of the expected Ju-Gibbs attitude is expressed as

Wherein,a 4 x 4 matrix representing system configuration parameters;express getThe first row of elements of (a), and so on,express getRow k +1 elements of (1); the upper right corner expression form [ 2 ]]Representing the form of expression [. cndot. ] taken by rows or columns]Representing taking all columns;express get³E₅Row 3, all columns.

And defined as follows:

the establishment procedure for the above equation is given below:

expectation ofDesired attitude to 5 th axisAlignment, if given a kinematic chainⁱl_n，k∈ⁱl_nExpectation criterion Ju-Gibbs quaternionAnd the expected position vectorThe position and orientation alignment relationship is expressed as

Is represented by formula (176)Further, it is obtained by the formula (59)

Formula (171) is obtained from formula (53), and formula (177)

Is obtained by the formula (54)

Wherein:

from formula (178) and formula (179)

Is obtained by formula (65)

By substituting formula (182) for formula (178)

Wherein:

formula (173) is obtained from formula (184). Is obtained from formula (173) and formula (183)

Formula (185) relates toDesired attitudeAnd 4-axis and 5-axis structural parameters. Is obtained by formula (63)

In one aspect, the formula (185), formula (186) and formula (187) give

On the other hand, it is obtained from the formulae (177), (185) and (188)

So that

Formula (175) and (iv) from formula (186)

Is obtained from formula (185) and formula (175)

Equations (185) through (191) are used for subsequent equation simplification, and C with superscripts and subscripts is a structural constant matrix. Considering the 2 norm on both sides of formula (183)

Consider whenⁱl₁＝0₃The position vector of time is aligned

Is obtained from formula (48) and formula (194)

Further, obtain

Namely have

Obviously, there are

The left-hand type of formula (195) is obtained from formula (111), formula (177) and formula (196)

Structural parametersAndare structural variables of the chain links l, which can be obtained by external measurement in the zero position of the system. The null, radial and axial vectors are invariant independent of the angle of rotation. Wherein the null vector is a particular radial vector.

Is obtained from formula (111), formula (186), formula (191) and formula (189)

From formula (191) and formula (198)

From formula (185), formula (191) and formula (199)

Substituting formula (197) and formula (200) into (195), and eliminating both sidesTo obtain the formula (170).

Elimination of tau₄And τ₅The subsequent position equation (170) is a 3-element 2-order polynomial equation which can be equivalent to the problem of the 3R mechanical arm, and the inverse solution method of the 3R mechanical arm position based on the axis invariant is adopted for solving, so that a foundation is laid for calculating the inverse solution of the universal 6R axis mechanical arm in real time. On one hand, the absolute positioning precision of the 6R mechanical arm is improved; on the other hand, the 4 th shaft and the 5 th shaft of the traditional decoupling mechanical arm can move in the root direction structurally, so that the structure of the mechanical arm can be optimized, and the flexibility of the 6R mechanical arm in avoiding obstacles is improved.

Example 6

The structural parameters of the 6R mechanical arm are as follows:ⁱn₁＝1^[z]，¹n₂＝1^[y]，²n₃＝1^[y]，³n₄＝1^[x]，⁴n₅＝1^[y]，⁵n₆＝1^[x]；ⁱl₁＝0₃m， if given the desired positionAnd the desired direction, there are 8 sets of inverse solutions as follows:

φ_[1][*]＝[-76.69657,170.546093,-20,33.69583,-16.915188]Deg，

φ_[2][*]＝[-76.69657,170.546093,-20,-146.30417,16.915188]Deg，

φ_[3][*]＝[-76.69657,150,20,-16.44416,-34.76538]Deg，

φ_[4][*]＝[-76.69657,150,20,-163.55584,34.76538]Deg，

φ_[5][*]＝[-90,30,-20,30,40]Deg，φ_[6][*]＝[-90,30,-20,-150,-40]Deg，

φ_[7][*]＝[90,9.4539,-20,-130.008225,-24.80936]Deg，

φ_[8][*]＝[90,9.4539,-20,49.99178,24.80936]Deg。

example 7

The structural parameters of the 6R mechanical arm are as follows:ⁱn₁＝1^[z]，¹n₂＝1^[y]，²n₃＝1^[y]，³n₄＝1^[x]，⁴n₅＝1^[y]，⁵n₆＝1^[x]；ⁱl₁＝0₃m， ⁵l₆＝0₃and m is selected. (a) If given the desired positionAnd desired directionThen there is only one set of solutions phi_[1][*]＝[90，30,-20,30,40]Deg are provided. (b) If given the desired positionAnd desired directionThen there are two sets of inverse solutions: phi is a_[1][*]＝[90,30,-20,0,0]Degand φ_[2][*]＝[90,9.45391,20,0,-19.45391]Deg。

The real-time inverse solution of the universal 6R mechanical arm is as follows: the absolute positioning precision of the mechanical arm is improved, the structure of the mechanical arm can be further optimized, and the weight of the system is reduced.

Universal 6R mechanical arm Dixon matrix structure based on axis invariant

In the following, the Dix of the general 6R mechanical arm kinematics equation is explained based on the equation_oAnd (5) structural characteristics of the n matrix.

wherein:

the establishment steps of the above equation are: note the book

From formula (170), formula (204) to formula (205)

Is obtained by the formula (206)

Wherein: is obtained by the formula (125) and the formula (204)

Is obtained from the formula (126) and the formula (204)

From formula (125), formula (126) and formula (205)

From the equations (208) to (211):is about y₂1 and y₃A 0 th order polynomial;about y₂2 and y₃Polynomial of order 1. Meanwhile, since the formula (206) is a vector polynomial, when the structure parameter vectors of any two columns are the same, the corresponding determinant is zero, and thus the formula (201) relates to y₂3 and y₃And (3) is satisfied, since the polynomial of order 1 is obtained. And factor (206) is related to₁And (2) a polynomial, so that the equation (202) holds.

The above description is only a preferred embodiment of the present invention, and it should be noted that, for those skilled in the art, several modifications and variations can be made without departing from the technical principle of the present invention, and these modifications and variations should also be regarded as the protection scope of the present invention.

Claims

1. A universal 6R mechanical arm inverse solution modeling and resolving method based on an axis invariant is characterized in that,

expressing a pose equation normalized by the 6R mechanical arm by adopting a Jub-Gibbs quaternion expression, and finishing alignment through the front 5 axes to eliminate joint variables of the 4 th axis and the 5 th axis; the 6 th shaft is controlled to be aligned with the expected position and direction through the front 5 shafts, so that the 6 th shaft can be infinitely rotated or the 6 th shaft can be controlledThe axes meet the radial alignment, which will give the 6 th axis desired position vectorAnd desired attitudeAnd given the 6 th axis desired position vectorAnd 5 th axis desired attitudeThe inverse solution problem of (2) is equivalent.

2. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 1,

wherein:is a Gibbs vector;

the Gibbs conjugate quaternion is:

wherein:

3. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 2,

if a 6R axis chain is givenⁱl₆＝(i,1:6]，ⁱl₁＝0₃The 6 th axis desired position vector isAnd 5 th axis desired attitudeThe 3 rd axis joint has the following standard quaternionOther axis expression ways are the same; the 6R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

In the formula, the character is a continuation character;respectively represent the zero vector and the radial vector of the shafts 4 to 5 and 5 to 6;is axis invariantA cross-product matrix of; 0₃＝[0 0 0]^T； A 4 x 4 matrix representing system configuration parameters;express getThe first row of elements of (a), and so on,express getRow k +1 elements of (1); the upper right corner expression form [ 2 ]]Representing the form of expression [. cndot. ] taken by rows or columns]Representing taking all columns;express get³E₅Row 3, all columns;³n₄is the coordinate vector of bar 3 to bar 4, which is an axis invariant;is axis invariant³n₄The other rods are in the same way;

4. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 3,

wherein:

5. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 4,

6. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 5,

in the step (1),

for axle chainIs provided with

Establishing a standard attitude equation as follows:

establishing a standard positioning equation:

in the formula,in any rod, expression formTo representTo the x-th power of; the right upper corner is marked with ^ orA representation separator;is axis invariantCross-multiplication matrix, rodsIs a rod pieceReplacing simultaneously; 1 is a three-dimensional identity matrix;ⁱQ_nrepresenting a gesture;is along a vector axisThe line position of (a);from the origin at zero positionTo the origin O_lA translation vector of (a);in order to be a projected symbol,is composed ofProjection vectors in a geodetic coordinate system.

7. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 6,

and memorize:

equation of 3R kinematics from equation (48)

Is obtained by formula (143)

Is obtained by the formula (144)

Note the book

Then the general formula (62) and the general formula (146)

Is obtained from formula (145) and formula (146)

obtaining a 3R kinematic polynomial equation from equation (148)

Then there is

Wherein:

is obtained by the formula (128) and the formula (146)

The simplified Dixon determinant of 3-element N order is

8. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 5,

9. The inverse solution modeling and solving method for a universal 3R robot arm based on axis invariants according to claim 5,

for an S × S matrix, each entry is for τ₁A polynomial of order n; when the determinant of the matrix is calculated, the original determinant can be changed into an upper triangular determinant through primary row transformation, and then nonzero diagonal elements are multiplied to obtain a polynomial expression of the determinant, wherein the expression is 0, and tau is obtained₁All solutions of (a);

the specific method of the line ladder is that the highest order of the first row of the determinant is firstly sequenced from high to low, and then primary equal line transformation elimination is carried out for at most (S-1) multiplied by n times to obtain the determinant of which the first element of the first row is not 0; and performing primary row transformation elimination on the residue sub-formulas of the 1 st row and the 1 st column of the determinant, and sequentially performing iterative solution.

10. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 5,

introducing an auxiliary variable [ y₂,y₃,…,y_n]And is provided with

To obtain

Wherein:

defining separable compositional variablesAndthe following were used:

Given N 'N-element N-order' polynomial systemsDefining the Dixon polynomial as

Is obtained by the formula (18)

In cartesian space, a determinant consisting of a position vector or a rotation vector represents a Volume (Volume) in which the vector opens up into space; the invariance of volume under different Cartesian spaces; wherein:

α[l]∈[0,N·(n-l+1)-1],l∈[2:n]； (24)

wherein:

consider formula (23) ifSo that

Det(_SΘ_S(x₁))＝0； (29)

11. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 7,

is obtained from formula (29), formula (152) and formula (153)

The formula (169) relates to₁The quadratic partition determinant or diagonalization of the determinant is performed by applying equation (5) to the 16 th order monomial equation of (1).

12. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 3,

the general 6R mechanical arm 4 and 5 axis solving method comprises the following steps: 2R direction inverse solution based on a quaternion of 'Ju-Gibbs' or 2R direction inverse solution based on DCM-like.

13. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 12,

based on a quaternion 2R inverse solution of 'Ju-Gibbs':

firstly, based on the pointing alignment principle of Ju-Gibbs quaternion, considering the axis chainⁱl_l，Vector of unit axisAnd desired unit axis vectorWhen aligned, at least one multi-axis rotation Ju-Gibbs quaternion exists

Wherein

Wherein:

quaternion in Ju-Gibbs orientationSatisfy the requirement of

In the formula,express get³E₅Row 3, all columns;³n₄is the coordinate vector of bar 3 to bar 4, which is an axis invariant;is axis invariant³n₄The other rods are in the same way;

14. The inverse solution modeling and solving method for a generic 6R robot arm based on axis invariants according to claim 12,

DCM-like based 2R inverse solution: