CN109033688B - Inverse solution modeling and resolving method for universal 7R mechanical arm based on axis invariant - Google Patents

Abstract

The invention discloses a universal 7R mechanical arm inverse solution modeling and calculating method based on shaft invariants, wherein 7 rotating shafts are set, a picking point is positioned on the axis of a 7 th shaft, and a mechanical arm with a 5 th shaft and a 6 th shaft which are not coaxial is a universal 7R mechanical arm; the general 7R mechanical arm controls the 7 th shaft to align with a desired position and a desired posture through the front 6 shafts, so that the 7 th shaft can rotate infinitely or the 7 th shaft is controlled to meet radial alignment; the 7R mechanical arm kinematics equation is expressed by adopting a Jub-Gibbs quaternion expression, a point with a certain distance from a 6 th axis to a pickup point is taken as a nominal pickup point, the inverse solution of the universal 6R mechanical arm is calculated firstly, then a numerical iteration method is applied, the motion planning and the inverse solution calculation of the universal 7R mechanical arm are completed, and the problem that the inverse solution of the 7R mechanical arm cannot be calculated in the prior art is solved.

Description

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

Technical Field

The invention relates to an inverse solution modeling and resolving method for a 7R 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.

Because the universal 6R mechanical arm does not have the concurrent constraint, the inverse solution calculation in the prior art is very difficult, and the decoupling constraint has to be complied with in engineering, so that the constraint not only increases the processing and assembling difficulty of the mechanical arm, but also reduces the absolute positioning precision of the mechanical arm. The inverse solution of the universal 7R mechanical arm is extremely high in calculation complexity and cannot be realized at all under the condition of the prior art.

Disclosure of Invention

The invention aims to solve the technical problem of providing a universal 7R mechanical arm inverse solution modeling and solving method based on axis invariants, and solving the problem that the 7R mechanical arm inverse solution cannot be calculated in the prior art.

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

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

7 rotating shafts are set, the picking point is positioned on the axis of the 7 th shaft, and the mechanical arm with the 5 th shaft and the 6 th shaft which are not coaxial is a universal 7R mechanical arm; the general 7R mechanical arm controls the 7 th shaft to align with a desired position and a desired posture through the front 6 shafts, so that the 7 th shaft can rotate infinitely or the 7 th shaft is controlled to meet radial alignment;

will give the desired position of the 7 th axis

And 7 th axis desired attitude

And given the desired position of the 6 th axis

And the 6 th axis desired attitude

The inverse solution problem of (2) is equivalent, and a 7R mechanical arm kinematic equation is expressed by adopting an Jubes-Gibbs quaternion expression;

and establishing a general 7R mechanical arm increment pose equation based on the axis invariant motion planning of the general mechanical arm, taking a point with a certain distance from a pickup point on the 6 th axis as a nominal pickup point, calculating the inverse solution of the general 6R mechanical arm, and then applying a numerical iteration method to complete the motion planning and inverse solution calculation of the general 7R mechanical arm.

If 7R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃The expected position vector and the Ju-Gibbs quaternion are respectively recorded as

And

the 7R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

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

In the formula, the character is a continuation character;

zero vectors and radial vectors of the shafts 5 to 6 and 6 to 7 are respectively represented;

is axis invariant

A cross-product matrix of; 0₃＝[0 0 0]^T；

A 4 x 4 matrix representing system configuration parameters;

express get

The first row of elements of (a), and so on,

express get

Row 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 4 to bar 5, which is an axis invariant;

is axis invariant⁴n₅The other bars are the same.

Elimination of tau₅And τ₆The subsequent pose equation (200) is a 4-element polynomial equation of order 2.

The general mechanical arm motion planning based on the axis invariant specifically comprises the following steps:

【1】 Establishing an incremental pose equation of a universal 7R axis chain;

【2】 And performing motion planning on the universal 7R mechanical arm based on the partial velocity iteration.

Define the Ju-Gibbs incremental quaternion:

the Ju-Gibbs norm quaternion is

Wherein: axial invariant

Is a joint variable; if it is used

Representing attribute placeholders, the form of expression in the formula

Representing a member access character; expression form power symbol in formula

To represent

To the x-th power of; the right upper corner is marked with ^ or

Representing a separator.

Defining Jubs incremental quaternions

Wherein:

in the step [1 ], if a 6R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃Position vector and Ju-Gibbs incremental quaternionAre respectively marked as

And

then the general 6R mechanical arm increment pose equation represented by the Ju-Gibbs increment quaternion is expressed as:

wherein:

³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; \\ is a continuation symbol;

zero vectors and radial vectors of the shafts 5 to 6 and 6 to 7 are respectively represented;

is axis invariant

A cross-product matrix of; 0₃＝[00 0]^T；

The system structure parameter matrix composed of row quaternions is expressed as

Wherein the content of the first and second substances,

a 4 x 4 matrix representing system configuration parameters; 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 i and all columns.

Analyzing the inverse solution of the incremental pose of the general 7R mechanical arm, wherein the formula (239) is related to the { epsilon_lI ∈ [1:4 }. Re-expression of the formula (239) as

A·[ε₁ε₂ε₃ε₄]^T＝b； (247)

If A^-1Exist, solution (245) to

[ε₁ε₂ε₃ε₄]^T＝A^-1·b； (248)

Thus, a complete inverse solution is obtained.

By incremental position vectors

And Gibbs incremental quaternion

And (5) applying an iterative approximation algorithm to align the pose of the universal 7R mechanical arm to the expected pose.

In the step [ 2 ], the step of iterative optimization based on the bias speed is as follows:

the kinematic chain is recorded as

l∈(i,1:6]Is provided with

Note the book

Noting the expected poses as

And

and is provided with

Co-writing formula (251) as

Obtained by applying a gradient descent method of the formula (252)

Wherein: step >0, Step → 0; is provided with

Selecting Step size from initial state

Starting iteration until the final state

(1) Determining an objective function

Goal representation

And

the variance of (a);

(2) selecting a step length;

(3) and carrying out iterative calculation to obtain a steady state solution, namely the pose inverse solution of the universal mechanical arm.

Determining step size using construction method

Only when

Then, Step → 0, Goal → 0;

wherein: epsilon_(i,6]＝[ε₁ε₂… ε₆](ii) a Step size is determined by equations 257 and 253

Wherein:

iterative process

Taking Step of equation 258, the iterative calculation is performed by equation 259:

wherein: when in use

When so, the iteration process is ended;

for the iterative process of equation (259), then

δGoal≤0， (260)

That is, the iterative process of equation (259) must converge;

when in use

Then obtain a steady state solution phi_(i,6]Namely, the pose inverse solution of the universal mechanical arm is obtained.

The invention achieves the following beneficial effects:

the invention establishes a general 7R mechanical arm increment pose equation and carries out real-time inverse solution calculation. Is characterized in that:

the method has the advantages of simple and elegant kinematic chain symbolic system, pseudo code function, iterative structure and guarantee of reliability and mechanization calculation of system implementation.

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 position

Time, natural coordinate system

Vector around axis

Angle of rotation

Will be provided with

Go to F^[l]；

In that

Coordinate vector of

At F^[l]Coordinate vector of

Is constant, i.e. has

According to the formula, the method has the advantages that,

or

Independent of adjacent coordinate systems

And F^[l](ii) a So it is called

Or

Is axis invariant. When invariance is not emphasized, the method can be called a coordinate axis vector (axis vector for short).

Or

Characterized by being a body

Coordinate vector of reference unit common to body l, and reference point

And O_lIs irrelevant. Body

The 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 literature

And 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 array

Since it is a real matrix whose modes are unity, it has a real eigenvalue λ₁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 vector

Is 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 invariant

Is 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 vector

And joint variables

Can directly describe the rotating coordinate array

It 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 vectors

The 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 consolidation

Is and

any vector of consolidation, in turn

Is F^[l]And

a common unit vector, therefore

Is F^[l]And

a common basis vector. Thus, the axis is invariant

Is F^[l]And

common reference base. The axis invariants are parameterized natural coordinate bases, and are primitives of the multi-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 measurement

In the kinematic pair

Axial vector during motion

Is not provided withA variable; axial vector

And joint variables

Uniquely identifying kinematic pair

The 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 axis

Rotated to an angular position

Coordinate vector of

Is composed of

Define 5 translation coordinate vectors: vector along coordinate axis

Translation to linear position

Coordinate vector of

Is 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 pair

At an initial time t₀Zero position of time, joint absolute encoder

Not necessarily zero, which is called mechanical zero;

hence the joint

Control amount of

Is composed of

Defining 8 natural motion vectors: will be represented by natural coordinate axis vectors

And natural coordinate q_lDetermined vector

Referred 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 vector

Is 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 variables

Indicating that there must be at system zero

Is called the natural joint space.

As shown in FIG. 2, a given link

Origin O_lPosition-dependent vector

Constrained axis vector

Is a fixed axis vector, is denoted as

Wherein:

axial vector

Is the natural reference axis for the natural coordinates of the joint. Due to the fact that

Is an axis invariant, so it is called

For the invariants of fixed axes, it characterizes kinematic pairs

The natural coordinate axis is determined. Fixed shaft invariant

Is a chain link

Natural 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,

and

without rod omega_lIs a constant structural reference.

Determines the axis l relative to the axis

Five structural parameters of (a); and joint variable q_lTogether, the rod omega is expressed completely_lThe 6D bit shape. Given a

The natural coordinate system of the fixed rod can be determined by the structural parameters

And joint variables

And (4) uniquely determining. Balance shaft invariant

Fixed shaft invariant

Variation of joint

And

is naturally invariant. Obviously, invariant by a fixed axis

And joint variables

Natural invariance of constituent joints

And from a coordinate system

To F^[l]Determined spatial configuration

Having 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 sequence

Is 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; shaft

The 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 sequence

A 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 as

And is

Cardinal number is recorded as $^ll_k|。^ll_kThe execution process comprises the following steps: execute

If it is

Then execute

Otherwise, ending.^ll_kThe computational complexity is O (# |)^ll_k|)。

【6】^ll is a child of axis l. The operation is represented in

Finding the address k of the member l; thus, a sub-A of the axis l is obtained^[k]. Due to the fact that

Has 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, if

Then remember

Namely, it is

Representing 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 axes

And the amount of attribute between the rods

Has the property of order bias.

Appointing:

representing attribute placeholders; if the attribute P or P is location-related, then

Is understood to be a coordinate system

To F^[l]The origin of (a); if the property P or P is directional, then

Is understood to be a coordinate system

To F^[l]。

And

are to be understood as a function of time t, respectively

And

and is

And

is t₀A constant or array of constants at a time. But in the body

And

should 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; r is_l ^kRepresents 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"; attribute characterRepresents a "kinematic chain"; attribute character u represents a "unit vector"; the attribute symbol ω 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 member

And a kinematic pair of the rod piece l;

kinematic pair

An axis vector of (a);

and

are respectively at

And F^[l]A lower coordinate vector;

is an axis invariant, being a structural constant;

as rotation vector, rotation vector/angle vector

Is a free vector, i.e. the vector mayFree translation;

(6)

along the axis

The linear position (translational position) of (c),

-about an axis

The 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 shaft

The mechanical zero position of the magnetic field sensor,

-a rotating shaft

Mechanical 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 symbol

To represent

To the x-th power of; the right upper corner is marked with ^ or

A representation separator; such as:

or

Is composed of

To the x power of.

To represent

The 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 vector

In a coordinate system F^[k]The projection vector in (1) is recorded as

Is a cross multiplier; such as:

is axis invariant

A cross-product matrix of; given any vector

Is cross-multiplication matrix of

The cross-multiplication matrix is a second order tensor.

Cross-multiplier operations have a higher priority than projecters

The priority of (2). Projecting sign

Is higher priority than the member access character

Or

Member access sign

Priority higher than power symbol

(10) Projection vector of unit vector in geodetic coordinate system

Unit zero vector

(11)

Zero position by origin

To the origin O_lIs translated by the vector of

Representing 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 joint

Natural 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 [ omega ]]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 chain

If n represents a Cartesian rectangular system, it is called

Is a Cartesian axis chain; if n represents a natural reference axis, then call

Is a natural axis chain.

(16) Rodrigues quaternion expression form:

euler quaternion expression:

quaternion (also called axis quaternion) representation of invariants

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 noted

Is 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 (1) 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 memorize

Is composed of

The 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 polynomial

Wherein:

and

is 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 (4) is established.

The expressions (3) and (4) can be generalized to n-dimensional space. Equation (3) 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 (4) indicates that: determinants of vector polynomials are prone to "combinatorial explosion".

Example 1

Given 2-dimensional row vector polynomials

On the one hand, is obtained by the formula (4)

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

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

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.n

Is 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.m

Is 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 a cm ∪ cn ═ 1: m + n](ii) a Then the square matrix M determinant and the block matrix

And

has 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).

The step-stepping method comprises the steps of firstly sorting the highest order of the first row of the determinant from high to low, and then carrying out primary row transformation elimination for at most (S-1) multiplied by n times to obtain the determinant with the first element of the first row not being 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.

Example 2

Through the primary line transformation of the matrix, the method obtains

A 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 products

The 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 (8), 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 which the upper right hand symbols α, α represent powers;

from formula (6) and formula (12)

Wherein:

defining separable compositional variables

And

the following were used:

the following equations (14) and (15) show that: replaceable type

Is about

And

double linear type of (1). Accordingly, the polynomial system replaced by the auxiliary variable is denoted by

Given N 'N-element N-order' polynomial systems

Defining the Dixon polynomial as

Is obtained by formula (17)

The isolated variables in formula (15) differ from the literature: original variablesX_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 (13) and (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; there is a constancy of volume in different cartesian spaces. Wherein:

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; presence and elimination of variable x₂,…,x_nIndependent Dixon matrix_SΘ_S(x₁) Of Dixon polynomials thereof

Expressed as a separate variable

And

double linear type of (2):

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

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

wherein:

if it is

Then there is

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

Consider formula (22) if

So that

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

The 'n-elements' in the formula (28) 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 matrix

Namely at_SΘ_SS' independent column vectors are selected.

Any one of N 'N-element N-order' polynomial systems

Examples of (A), (B) are denoted by

Wherein:

and is provided with

According to

Determining Dixon matrix, separation variables

And

selecting

And

satisfy the requirement of

Determining bilinear forms

Wherein:

neutralization of

The corresponding column lines are linearly independent. Due to the fact that

Is obtained from formula (22) and formula (25)

It is referred to as a knot or elimination. Formula (32) is a univariate 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 formula (34) having the formula (32)

At will, therefore, get

Namely have

If necessary under the conditions

Solve equation (35) to obtain the eliminated variable

The solution of (1); otherwise, the complete solution needs to be obtained by combining equation (16). Considering equation (25), x on both sides of equation (22)₁The order is equal, so it must be

If at the same time satisfy

Then can be solved by formula (35)

N-1 mutually different combination variables; thus, solutions for all independent variables are obtained.

Given N 'N-ary N' polynomials

The Dixon matrix calculation steps are as follows:

① determining system structure, the equation number and independent variable number are n, and the independent variable is X_n(ii) a The polynomial complex variable is expressed as

The replacement variable is noted

The number of the replacing variables is n-1; dixon matrix of size S.S is denoted

The member coefficient is represented by the formula (24), wherein: s is determined by equation (32); the variable to be eliminated is x₁。

② from formula (8) x α and

correspondence relationship, in expression (11)

There are at most S terms.

③ Dixon (F) is calculated according to equation (19) and the Sarrus rule_n(Y_n-1|X_n-1) ); according to

And corresponding N carry word operation results are obtained, and polynomial combination is completed.

④ Dixon matrix members are shown as equation (32), and Dixon matrices are calculated from equation (32)_SΘ_S(n + 1). S²A coefficient.

⑤ when the direct solution criterion of equations (37) and (38) is satisfied, all numerical solutions are obtained from equations (34) and (35).

Example 3

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

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

Wherein:

5 solutions were obtained from equations (34) and (40):

wherein:

is not a solution to the system of equations. Other solutions are substituted into formula (35), respectively. When in use

When obtained from formula (35)

Obtaining by solution: tau is₃＝1，τ₄-2. Will be provided with

τ₃And τ₄T is obtained by substituting formula (39)₂1. Likewise, three other sets of solutions are available. Obviously, the dependent variable does not satisfy equation (26), and the Dixon matrix shown in equation (40) 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 given

And unit vector consolidated therewith

For unit vectors before rotation

To system zero axis

Is projected vector of

To the radial axis of the system

Has a moment vector of

Radial vector is

Axial vector

Relative to the rod member

And omega_lOr natural coordinate system

And F^[l]Is fixed and not changed, so the rotation is called fixed axis rotation. Unit vector

Around shaft

Rotation of

Rear, rotated null vector

To system zero axis

Is projected vector of

Zero vector after rotation

To the radial axis of the system

Has a moment vector of

An axial component of

So as to obtain the Rodrigues vector equation with chain indexes

Factor unit vector

Is arbitrary and

obtaining the Rodrigues equation of rotation with chain index

If it is

From formula (42) to

If it is

I.e. coordinate system

And F^[l]The directions of (A) and (B) are identical, and the formula (42) shows that: antisymmetric part

Must have

Thus, the system zero is a natural coordinate system

And 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 (43) relates to

And

is an axis invariant

A second order polynomial of (a). Given natural null vector

As

Zero reference of

And

representing the null vector and the radial vector, respectively. Formula (43) is

Symmetrical part

Representing zero-axis tensor, antisymmetric part

Representing the radial-axis tensor, respectively the axial-outer product tensor

Orthogonal, thereby determining a three-dimensional natural axis space; the formula (43) 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 change

And joint variables

The parameterization of the coordinate system and the polarity is realized.

For axle chain

Is provided with

Is obtained from formula (44) and formula (43)

Then

Is that

And

multiple linear type of (a), wherein: l is an element ofⁱl_k. Formula (43) can be represented as

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

The normalized position equation is

Determination of "jubes" quaternion:

for any rod member

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 of

The square of the mode. Since the Cure-Gibbs quaternion is a quaternion, it satisfies the fourElement multiplication operation

Wherein:

is obtained by the formula (52)

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 (53): only given axes l and

the norm Ju-Gibbs quaternion, and the two axes are orthogonal,

is a canonical quaternion.

Is obtained by formula (53)

By four-dimensional complex nature

Note the book

Is obtained by the formula (52)

Therefore, it is

Is a unit Ju-Gibbs quaternion.

From formula (48) to formula (50) and formula (55)

Is obtained from formula (50), formula (54) and formula (57)

DCM-like and properties:

for axle chain

The canonical attitude equation is:

is obtained by formula (59)

In the formula (I), the compound is shown in the specification,

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;

wherein:

from the formula (61):ⁱQ_nand

is about tau_kN is multiplied by a polynomial of order 2. From the formula (60): due to the fact that

And

similarly, it is called DCM-like (DCM, directional cosine matrix). Is obtained by the formula (62)

Obviously, DCM-like can be expressed by Ju-Gibbs quaternion. Therefore, the equation of the attitude of equation (59) and the equation of the position of equation (47) 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_lWherein

If make the axis vector

Vector of desired axis

Aligned, there is at least one Ju-Gibbs direction quaternion

Wherein:

and is provided with

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

cayley positive transformation of fixed axis rotation

Is obtained by the formula (69)

Is obtained by the formula (70)

And

due to the fact that

And

is a unit vector, if

And is

Then there is

Formula (73) shows

And

are orthogonal to each other. The optimal axial vector is obtained from the equations (72) and (73)

And

the formula (66) is obtained from the formulae (74) and (75). If it is

Or

Is obtained by the formula (70)

Is obtained by formula (76)

Due to the fact that

Thus obtained from formula (77)

And

the formulae (67) and (68) are obtained from the formulae (78) and (79), respectively. After the syndrome is confirmed.

The pointing alignment principle based on the Ju-Gibbs quaternion shows that: there is at least one desired Ju-Gibbs quaternion

Make unit vector

And the expected unit vector

And (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.

If a 6R rotating chain is givenⁱl₆＝(i,1:6]]In the direction of the 5 th axis joint Ju-Gibbs, the quaternion is desirably expressed as

And the 3 rd shaft joint Ju-Gibbs standard quaternion is

There is an inverse solution when pointing to the alignment

Wherein:

quaternion in Ju-Gibbs orientation

Satisfy the requirement of

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

first, consider Euler quaternion-based pose alignment. By

To obtain

Is obtained by the formula (86)

Wherein:

is obtained by the formula (64) and the formula (88)

Is obtained by formula (87)

Wherein:

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

Is obtained by the formula (92)

If it is

The two sides of the formula (92) and the formula (93) are correspondingly divided

Second, consider the directional alignment of the Ju-Gibbs quaternion. Due to the fact that

Thus, (82) was obtained. Is obtained by the formula (58)

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

Is obtained by formula (96)

Due to the fact that³n₄And⁴n₅independently, as can be seen from the formula (83),³E₅must be present. It is clear that,

by³n₄And⁴n₅and (4) uniquely determining. By substituting formula (96), formula (97) and formula (91) for formula (95)

If it is

Obtained by the 1 st line of the formula (98)

By substituting formula (99) for formula (98)

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

From the equation (101), the equation (80) holds. Factoring in the memory (100) into 4 equations, 2 independent variables, and constraint equations derived from the 4 th element in equations (101) and (100)

If it is

From formula (93) to C₄C₅0; is obtained by formula (87)

Is obtained by formula (103)

It is clear that,

when in use

When, if

Is obtained by the formula (102)

If it is

Is obtained by the formula (102)

From equation (106): or either

Or either

From the equations (106) and (101), the equation (81) is also established. When in use

When, if

Equation (80) also holds. After the syndrome is confirmed.

Quaternion in Ju-Gibbs orientation

From the formulae (80), (81) and (99)

And is

Handle

Substitution into the above two equations results in equation (84), which shows a particular Ju-Gibbs quaternion, referred to as the Ju-Gibbs orientation 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 (83)³ E ₅1. Is obtained from formula (80) and formula (81)

Obtained by the formula (81):

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 (63) shows that DCM-like represented by Ju-Gibbs quaternion is isomorphic with DCM. 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 quaternion

And the expected position vector

Consider equations (47) and (95); 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.

The following equations (59) and (47) show that: 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 (47) 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 (4): 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 application

The 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 parameters

And

are 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 link

The D-H parameter is

It is clear that,

is a shaft l and

the common perpendicular or common radial vector of (a),

is the axial vector of the axis l. From equation (112): any one of the structure parameter vectors

Decomposable into zero invariants independent of coordinate system

And axial invariance

Their radial vectors are noted

Vector of structural parameters

And axis invariant

The radial coordinate system is uniquely defined with 2 independent dimensions. If two axial invariants

And

collinear, then it is marked

If two zero-position invariants

And

and any two radial invariants

And

coplanar is then recorded

Therefore, the axial invariants and the null invariants shown in equation (110) are the decomposition of the structural parameter vector to the natural axis.

From the equations (113) and (114): 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 (115) is called the inversion equation of the zero vector; the expression (116) is called as a interchange expression of a zero vector and a radial vector; equation (117) is referred to as the radial vector invariance equation. From formula (109), formula (115) to formula (117)

Is obtained by the formula (118)

Due to the fact that

Is that

Is a structural constant of the symmetric part, so that the expression (118) is a vector

Is a symmetric decomposition. Due to the fact that

Is that

So that equation (119) is a vector

Is disclosed in (1). Equation (120) is referred to as a return-to-zero equation.

【3】 Kinematic chain Dixon determinant Properties

Definition of

Is obtained by the formula (52)

Wherein:

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

And is provided with

Is obtained from the formula (62) and the formula (110)

Is proved by formula (126)

Is represented by formula (129)

Formula (127) can be

And

can be converted into

Multiple linear types of (a). At the same time, the user can select the desired position,

for y_lAnd τ_lWith symmetry (rotation). Is obtained from formula (111), formula (118) and formula (119)

Equation (129) is derived from three independent structural parameters

And a motion variable τ_lAnd (4) forming. Is represented by formula (129)

Is obtained from formula (127) and formula (131)

From formula (127) and formula (132)

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 is

DCM of the desired 3 rd axis is

Direction vector⁵l₆To the desired direction

The 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 direction

Alignment needs to be satisfied

Is obtained by the formula (60)

Namely, it is

Equation (137) is re-expressed as equation (134).

Cayley transform based on axis invariants

When a given angle

Then, the sine and cosine and the sine and cosine of the half angle are constants; for convenient expression, record

Is obtained by formula (138)

Definition of

Therefore it has the advantages of

And radial vector

And tangential vector

Is a linear relation, scale

Is "Rodrigues Linear invariant". Is generally called as

Namely, it is

For Rodrigues or Gibbs vectors, and

referred to as Modified Rodrigues Parameters (MRPs).

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

Given 3R rotating chain

And desired attitude

Axial invariant sequence

Finding joint variable sequence

This 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 (47)

Is obtained by the formula (142)

Is obtained by formula (143)

If remember

Then the general formula (61) and the formula (145)

From formula (144) and formula (145)

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 (147)

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

Then there is

Wherein:

is obtained from the formula (18), the formula (147) and the formula (148)

Expression (151) is established from expression (22) and expression (153). Is obtained from formula (127) and formula (145)

Is obtained from formula (145), formula (154) and formula (155)

Wherein: applying a formula (133) calculation

Obviously, y in the formula (156)₂Order β 2 ∈ 0:3]And y₃Order β 3 ∈ [0:1 ] for]. Consider the last three terms of equation (153):

y in (1)₂Order β 2 ∈ 0:3]And y₃Order β 3 ∈ [0:1 ] for]；

Y in (1)₂Order β 2 ∈ 0:2]And y₃Order β 3 ∈ [0:1 ] for]；

Y in (1)₂Order of β 2 e [0:3 ]]And y₃Order of β 3 e [0:1 ]]. From the above, it can be seen that: y in formula (153)₂Order β 2 ∈ 0:3]And y₃Order of β 3 e [0:1 ]]. So, S is 8.

From formula (145), formula (153) to formula (156):

the medium combined variable coefficient is independent column vector, so it is selected

To form a square matrix

Remainder ofColumn vector is given by

Are related. Therefore, the formula (147) is established.

【2】 The determinant calculation is simplified by applying a Dixon determinant calculation method based on axis invariants and a high-dimensional determinant calculation method of block matrixes.

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

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

Obtained by formula (157)

Is obtained by formula (158)

Is obtained by formula (159)

Is obtained by formula (153)

Substituting formulae (160) to (162) for formula (163) to obtain

【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.

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 the eigenvalue, v, of the matrix_lIs the corresponding feature vector. If the characteristic equation of the matrix A is

The Matrix is called a friend Matrix 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 (167)。

From the formulae (28), (151) and (152)

Since S is 8, the calculation of equation (1) is applied

The complexity of (c) is 8.8! 322560; and performing determinant calculation of secondary partitioning by applying an equation (5), wherein: 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, equation (168) 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 vector

And desired attitude

And given the 6 th axis desired position vector

And 5 th axis desired attitude

The 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₃6 th axis desired position vectorIs composed of

And 5 th axis desired attitude

The 3 rd axis joint Ju-Gibbs standard quaternion is

Other 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 invariant

A 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 the content of the first and second substances,

a 4 x 4 matrix representing system configuration parameters;

express get

The first row of elements of (a), and so on,

express get

Row 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 of

Desired attitude to 5 th axis

Alignment, if given a kinematic chainⁱl_n，k∈ⁱl_nExpectation criterion Ju-Gibbs quaternion

And the expected position vector

The position and orientation alignment relationship is expressed as

Is obtained by formula (175)

Further, it is obtained by the formula (58)

Formula (170) is obtained from formula (52), and formula (176) is obtained

Is obtained by formula (53)

Wherein:

from formula (177) and formula (178)

Is obtained by the formula (64)

Substituting formula (181) for formula (177)

Wherein:

formula (172) is obtained from formula (183). From formula (172) and formula (182)

The formula (184) relates to

Desired attitude

And 4-axis and 5-axis structural parameters. Is obtained by the formula (62)

In one aspect, the formula (184), formula (185), and formula (186) are used to obtain

On the other hand, the compounds represented by the formulas (176), (184) and (187)

So that

From formula (185) to (174)

Is obtained by the formula (184) and the formula (174)

Equations (184) through (190) 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 the equation (182)

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

Is obtained from formula (47) and formula (193)

Further, obtain

Namely have

Obviously, there are

Left-hand of formula (194) from formula (110), formula (176) and formula (195)

Structural parameters

And

are 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 by the formula (110), the formula (185), the formula (190) and the formula (188)

Is obtained from formula (190) and formula (197)

From the formulae (184), (190) and (198)

Substituting (194) with formula (196) and formula (199), and eliminating both sides

To obtain the formula (169).

Elimination of tau₄And τ₅The subsequent position equation (169) is 3 '3-element 2-order' polynomial equations which can be equivalent to the 3R mechanical arm problem, the inverse solution method of the 3R mechanical arm position based on the axis invariant is adopted for solving, and the general 6R mechanical arm position is calculated in real timeThe basis is established for the inverse solution of the R-axis mechanical arm. 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.

Third, general 7R mechanical arm pose inverse solution based on axis invariant

The robot arm is called to have 7 rotating shafts, the picking point is positioned on the 7 th shaft axis, and the 5 th shaft and the 6 th shaft are not coaxial, and is a universal 7R robot arm. The general 7R mechanical arm controls the 7 th shaft to be aligned with a desired position and posture through the front 6 shafts, and the 7 th shaft rotates infinitely or the 7 th shaft is controlled to meet the requirement of radial alignment. Thus, a 7R universal robot arm has greater flexibility of space operation than a 6R robot arm.

Given a desired position

And desired attitude

Inverse solution problem of (1) to given desired position

And desired attitude

The 7R mechanical arm is essentially a 6R axis chain system, and can be solved according to the above-mentioned axis invariant-based 6R mechanical arm position inverse solution method. Next, the inverse solution process of the general 7R robot will be described.

If 7R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃The expected position vector and the Ju-Gibbs quaternion are respectively recorded as

And

the 7R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

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

The establishment steps of the above formula are as follows:

expectation of

And posture

Alignment, obtained by the formula (175)

Further, it is obtained by the formula (58)

Formula (201) is obtained from formula (52), and formula (206) is obtained

Is obtained by formula (53)

Wherein:

is obtained by the formula (207) and the formula (208)

Is obtained by the formula (64)

By substituting formula (211) for formula (210)

Wherein:

expression (203) is obtained from expression (213). Is obtained from the formula (203) and the formula (212)

Formula (214) relates to

Posture

And constraint equations of 5-axis and 6-axis structural parameters. From formula (203) to (205) and

is obtained from formula (214) and formula (205)

Equations (214) to (217) are used for subsequent equation simplification, and C is a structural constant matrix. Consider the two-sided 2-norm of equation (216)

Is obtained by the formula (62)

On the one hand, the compounds represented by the formulae (216), (215) and (219)

On the other hand, the expression (206), (214) and (220) are used to obtain

So that

The attitude equation in equation (200) is derived from equation (222).

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

Is obtained from formula (47) and formula (223)

Further, obtain

Namely have

Obviously, there are

A left-hand version of formula (224) from formula (206) and formula (225)

Is obtained from the formula (110), the formula (215), the formula (216) and the formula (221)

Is obtained from formula (216) and formula (227)

From formula (214), formula (217) and formula (228)

Will be the formula (226) and formula(229) Substituted (224) and erased on both sides

And (5) obtaining a position equation in (200).

Elimination of tau₅And τ₆The subsequent pose equation (200) is a 4-element polynomial equation of order 2.

Ju-Gibbs incremental quaternion and Properties

The calculation complexity of the pose inverse solution of the universal 7R mechanical arm is high, and the technical problem of real-time calculation needs to be solved. In engineering, the calculation accuracy is a relative concept, and the engineering requirements can be met only by ensuring that the numerical calculation accuracy is 4 to 6 times higher than the accuracy of the system structure parameters. The attitude equation shown in equation (59) and the position equation shown in equation (47) are essentially expressions of the Ju-Gibbs quaternion. The engineering precision requirement can be met as long as the equations (59) and (47) have enough calculation precision. Firstly, a 'Jub-Gibbs' increment quaternion (Delta-quaternion) is provided, then a general 7R mechanical arm increment pose equation is established, and real-time inverse solution calculation is carried out.

【1】 "Jubs" incremental quaternion definition

Definitions of "Jubs" incremental quaternions

Wherein:

it is obvious that the "Jubs" incremental quaternion is a four-dimensional complex number, and has

【2】 "Jubs" incremental quaternion property

Is obtained by the formula (48) and the formula (230)

Is obtained by the formula (51)

Is obtained by the formula (234)

Is obtained by the formula (50)

Is obtained from formula (236) and formula (54)

Is obtained by the formula (62)

2. Universal 7R mechanical arm motion planning based on axis invariants

The inverse solution of the universal 7R mechanical arm cannot be realized under the prior art because of extremely high calculation complexity. However, the pick-up point located on the 7 th axis is usually located at a small distance from the 6 th axis. Therefore, a point closer to the picking point on the 6 th axis is taken as a nominal picking point, and the inverse solution of the universal 6R mechanical arm is calculated firstly; based on the motion planning and the inverse solution calculation, a numerical iteration method is applied to complete the motion planning and the inverse solution calculation of the universal 7R mechanical arm. The problem of establishing and solving the incremental (Delta) pose equation of the general 7R mechanical arm is discussed below.

【1】 General 7R axis chain increment pose equation

The incremental pose equation of the general 7R axis chain represented by the Gibbs incremental quaternion is stated first, and finally the inverse solution is solved.

If a 6R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃The position vector and the Ju-Gibbs increment quaternion are respectively recorded as

And

then the general 6R mechanical arm increment pose equation represented by the Ju-Gibbs increment quaternion is expressed as:

wherein:

³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; \\ is a continuation symbol;

zero vectors and radial vectors of the shafts 5 to 6 and 6 to 7 are respectively represented;

is axis invariant

A cross-product matrix of; 0₃＝[0 0 0]^T；

The system structure parameter matrix composed of row quaternions is expressed as

Next, the incremental pose inverse solution of the generic 7R manipulator is analyzed. It is apparent that equation (239) relates to [. epsilon. ]_lI ∈ [1:4 }. Re-expression of the formula (239) as

A·[ε₁ε₂ε₃ε₄]^T＝b (247)

If A^-1Exist, solution (245) to

[ε₁ε₂ε₃ε₄]^T＝A^-1·b。 (248)

From formula (235) and formula (248)

From equation (215) to ε₅And epsilon₆. ToThis gives the full inverse solution.

The inverse solution of the pose of the universal 7R mechanical arm is as follows: may be indexed by incremental position vectors

And Gibbs incremental quaternion

An Iterative Approximation (Iterative Approximation) algorithm is applied to align the pose of the universal 7R mechanical arm with the expected pose.

【2】 Universal 7R mechanical arm motion planning based on partial velocity iteration

When k ∈ⁱl_nFrom the formulas (3.304) to (3.306)

The kinematic chain is recorded as

Is obtained by formula (249) and formula (250)

Note the book

Noting the expected poses as

And

and is provided with

Write formula (251) togetherIs composed of

Obtained by the Method of formula (252) by using Gradient (Gradient Descent Method/GDM) reduction Method

Wherein: step >0, Step → 0. Is obviously provided with

Selecting Step size from initial state

Starting iteration until the final state

The iterative optimization steps based on the yaw rate are as follows:

(1) determining an objective function

Obviously, the Goal stands for

And

the variance of (c).

(2) Selecting a step size

In one aspect, the step size is determined using a construction method

Then, as shown in equations (253) and (256): only when

When, Step → 0, Goal → 0.

On the other hand, it is obtained from the formulas (231) and (252)

Wherein: epsilon_(i,6]＝[ε₁ε₂… ε₆]^T. Step size is determined by equations 257 and 253

(3) Iterative process

On the one hand, if Step size Step of equation (258) is taken, the iterative calculation is performed by equation (259):

wherein: when in use

When so, the iterative process ends. For the iterative process of equation (259), then

δGoal≤0， (260)

I.e., the iterative process of equation (259) must converge.

The derivation process of the iterative calculation formula is as follows: is obtained from the formula (252) and the formula (255)

From formula (253), formula (256) and formula (261)

Since equation (256) is theoretically equivalent to equation (258), equation (258) may be used instead of equation (256). But formulae (256) andthe calculation process of equation (258) is different: because the word length of the computer is limited, the computer can be used for a long time

The former precision is increasingly poor, while the latter is increasingly high; at the same time, the latter is relatively computationally inexpensive. Therefore, in engineering applications, it is better to apply the step size in equation (258).

When in use

Then obtain a steady state solution phi_(i,6]Namely, the pose inverse solution of the universal mechanical arm is obtained.

The general 7R mechanical arm motion planning based on the deflection speed is characterized in that: through iteration, the expected pose is gradually approached, and a path from the initial pose to the expected pose can be obtained. Because this method is a goal-oriented optimization process, real-time performance is poor. If the joint increment is controlled in the iteration process, the constraint of joint speed can be met; therefore, a set of inverse solutions corresponding to the expected pose is also obtained while the motion planning is completed.

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 (10)

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

defining a natural coordinate system: if the multi-axis system D is in a zero position, all the Cartesian body coordinate systems have the same direction, 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;

when the system is in a zero position, the natural coordinate systems of all the rod pieces are consistent with the direction of the base or the world system; with the system in zero position

Time, natural coordinate system

Vector around axis

Angle of rotation

Will be provided with

Go to F^[l]；

In that

Coordinate vector of

At F^[l]Coordinate vector of

Is constant, i.e. has

Or

Independent of adjacent coordinate systems

And F^[l](ii) a Balance

Or

Is an axis invariant;

appointing: 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; for a certain attribute symbol, the direction of the attribute symbol 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;

determination of "jubes" quaternion:

as for any of the bar members l,

define the Ju-Gibbs norm quaternion isomorphic with Euler quaternion:

wherein:

is a Gibbs vector;

7 rotating shafts are set, the picking point is positioned on the axis of the 7 th shaft, and the mechanical arm with the 5 th shaft and the 6 th shaft which are not coaxial is a universal 7R mechanical arm; the general 7R mechanical arm controls the 7 th shaft to align with a desired position and a desired posture through the front 6 shafts, so that the 7 th shaft can rotate infinitely or the 7 th shaft is controlled to meet radial alignment;

will give the desired position of the 7 th axis

And 7 th axis desired attitude

Inverse solution ofSubject to given 6 th axis desired position

And the 6 th axis desired attitude

The inverse solution problem of (2) is equivalent, and a 7R mechanical arm kinematic equation is expressed by adopting an Jubes-Gibbs quaternion expression;

and establishing a general 7R mechanical arm increment pose equation based on the axis invariant motion planning of the general mechanical arm, taking a point with a certain distance from a pickup point on the 6 th axis as a nominal pickup point, calculating the inverse solution of the general 6R mechanical arm, and then applying a numerical iteration method to complete the motion planning and inverse solution calculation of the general 7R mechanical arm.

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

if 7R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃The expected position vector and the Ju-Gibbs quaternion are respectively recorded as

And

the 7R arm kinematics polynomial equation characterized by the axis invariant is then:

wherein:

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

In the formula, the character is a continuation character;

zero vectors and radial vectors of the shafts 5 to 6 and 6 to 7 are respectively represented;

is axis invariant

A cross-product matrix of; 0₃＝[0 0 0]^T；

A 4 x 4 matrix representing system configuration parameters;

express get

The first row of elements of (a), and so on,

express get

Row 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 4 to bar 5, which is an axis invariant;

is axis invariant⁴n₅The other rods are in the same way; expression form power symbol in formula

To represent

To the x-th power of; the right upper corner is marked with ^ or

A representation separator;

elimination of tau₅And τ₆The subsequent pose equation (200) is a 4-element polynomial equation of order 2.

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

the general mechanical arm motion planning based on the axis invariant specifically comprises the following steps:

【1】 Establishing an incremental pose equation of a universal 7R axis chain;

【2】 And performing motion planning on the universal 7R mechanical arm based on the partial velocity iteration.

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

define the Ju-Gibbs incremental quaternion:

the Ju-Gibbs norm quaternion is

Wherein: axial invariant

Is a joint variable; if it is used

Representing attribute placeholders, the form of expression in the formula

Representing a member access character; defining Jubs incremental quaternions

Wherein:

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

in the step [1 ], if a 6R axis chain is givenⁱl₇＝(i,1:7]，ⁱl₁＝0₃The position vector and the Ju-Gibbs increment quaternion are respectively recorded as

And

then the general 6R mechanical arm increment pose equation represented by the Ju-Gibbs increment quaternion is expressed as:

wherein:

³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; character of continuation;

zero vectors and radial vectors of the shafts 5 to 6 and 6 to 7 are respectively represented;

is axis invariant

A cross-product matrix of;0₃＝[0 00]^T；

the system structure parameter matrix composed of row quaternions is expressed as

Wherein the content of the first and second substances,

a 4 x 4 matrix representing system configuration parameters; 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 i and all columns.

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

analyzing the inverse solution of the incremental pose of the general 7R mechanical arm, wherein the formula (239) is related to the { epsilon_lL belongs to the linear equation of [1:4 ]; re-expression of the formula (239) as

A·[ε₁ε₂ε₃ε₄]^T＝b； (247)

If A^-1Exist, solution (245) to

[ε₁ε₂ε₃ε₄]^T＝A^-1·b； (248)

Thus, a complete inverse solution is obtained.

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

by incremental position vectors

And Gibbs incremental quaternion

And (5) applying an iterative approximation algorithm to align the pose of the universal 7R mechanical arm to the expected pose.

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

in the step [ 2 ], the step of iterative optimization based on the bias speed is as follows:

the kinematic chain is recorded as

l∈(i,1:6]Is provided with

Note the book

Noting the expected poses as

And

and is provided with

Co-writing formula (251) as

Obtained by applying a gradient descent method of the formula (252)

Wherein: step >0, Step → 0; is provided with

Selecting Step size from initial state

Starting iteration until the final state

(1) Determining an objective function

Goal representation

And

the variance of (a);

(2) selecting a step length;

(3) and carrying out iterative calculation to obtain a steady state solution, namely the pose inverse solution of the universal mechanical arm.

9. The inverse solution modeling and solving method for a universal 7R mechanical arm based on axis invariants as claimed in claim 8, wherein the step size is determined by using a construction method

Only when

Then, Step → 0, Goal → 0;

wherein: epsilon_(i,6]＝[ε₁ε₂…ε₆](ii) a Step size is determined by equations 257 and 253

Wherein:

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

iterative process

Taking Step of equation 258, the iterative calculation is performed by equation 259:

wherein: when in use

When so, the iteration process is ended;

for the iterative process of equation (259), then

δGoal≤0， (260)

That is, the iterative process of equation (259) must converge;

when in use

Then obtain a steady state solution phi_(i,6]I.e. a universal robot armThe pose of (1) is solved reversely.

Images