CN117325143A

CN117325143A - Redundant mechanical arm singular position type lower kinematics optimization method

Info

Publication number: CN117325143A
Application number: CN202210765140.2A
Authority: CN
Inventors: 李艳杰; 王天赐; 张玉璞
Original assignee: Shenyang Ligong University
Current assignee: Shenyang Ligong University
Priority date: 2022-06-24
Filing date: 2022-06-24
Publication date: 2024-01-02

Abstract

A kinematic optimization method under a redundant mechanical arm singular position type is mainly used for carrying out kinematic analysis and optimization treatment on a limited space of a seven-axis robot with an S-R-S configuration; the method comprises the following steps: step one: establishing a positive kinematic model of the redundant mechanical arm; step two: establishing an inverse kinematics model of the redundant mechanical arm, and solving inverse kinematics closed solutions of all joints by using an arm angle parameterization method; step three: analyzing the specific odd-abnormal type of the redundant mechanical arm under the S-R-S configuration by adopting a geometric method, and resolving the inverse solution of each joint under the odd-abnormal type by utilizing the geometric method; step four: and (3) synthesizing an inverse solution of arm angle parameterization and an inverse solution under a specific odd-abnormal type, so as to realize the avoidance of the specific odd-abnormal type of the redundant mechanical arm. The method is a redundant mechanical arm singular position lower kinematics optimization method (mainly avoiding singular points) based on an equivalent degree of freedom method, has simple related operation, strong operability and good technical effect, and has great economic value and social value which can be expected.

Description

Redundant mechanical arm singular position type lower kinematics optimization method

Technical Field

The invention relates to the technical scheme and the application technical field of a method for avoiding singular points of a redundant mechanical arm, and particularly provides a method for optimizing kinematics under singular positions of the redundant mechanical arm.

Background

The environment of the nuclear industry place is complex, and the robot is applied to the maintenance and upgrade of the nuclear industry place and the alarming equipment, so that on one hand, the cost including the management cost and the labor cost can be reduced, and on the other hand, the danger and the labor amount can be reduced. Compared with the common mechanical arm, the redundant mechanical arm has better flexibility, fault tolerance and reliability.

Singular position type research on redundant mechanical arms is also indispensable, and typical technical problems are as follows:

(1) When the mechanical arm is in an odd-abnormal type, the mechanical arm can lose the degree of freedom in a certain direction, and the tail end of the mechanical arm also can lose the degree of freedom in a certain direction, so that the flexibility of the mechanical arm is reduced; (2) When the mechanical arm is in an odd-dislocation type, an infinite solution may occur to the inverse kinematics problem.

The general singular position type judging method is realized by solving singular values of a jacobian matrix of the mechanical arm, and can also judge through a geometric method, wherein the singular position type can appear: (1) Two or more axial joints co-linear occur within the working space (2) at the boundary of the working space (when the robotic arm is fully deployed or retracted).

The existing singular avoidance method for the mechanical arm comprises the following steps: (1) The least damping square method can actively avoid the obstacle for the dynamic pedestrians in the environment in real time and avoid the singular points of the mechanical arm, but certain non-smoothness can appear when resolving near the singular area; (2) Setting a singular region, judging whether an interpolated point falls in the singular region or not when a path is planned, if so, discarding the point, otherwise, leaving the point, but the method only ensures that the point does not pass through the singular region, and possibly generating larger tremble due to the motion inertia of the mechanical arm to cause pose errors.

The trajectory planning of the robotic arm generally includes two aspects, depending on different control requirements: (1) A curve track or a linear track to be taken by the tail end of the mechanical arm, namely, planning in a Cartesian space to obtain the advancing speed and the accelerating speed of the tail end; (2) The profile of the displacement, velocity, or acceleration of the joints of the robotic arm during motion.

China starts relatively late relative to other countries, but China is also a large nuclear industry country, so research and development of nuclear power robots should be enhanced. For maintenance and upgrading of equipment, the device can effectively avoid harm to human bodies. The complexity of nuclear industry and petrochemical sites requires that the robot has certain flexibility, and the carried mechanical arm has high flexibility, strong obstacle avoidance and high reliability. It is a requirement to select redundant robotic arms for maintenance/upgrade of equipment.

The flexibility of the redundant robot arm is mainly due to the fact that the number of joints is large, but the rigidity of the robot arm is reduced due to the fact that the joints are increased. The redundant mechanical arm has certain redundancy characteristics, such as higher flexibility [ see literature: li Na, cui Zhiqin, left country, et al, based on quantitative upper limb motor function assessment study of end-pull rehabilitation robot [ J ]. Beijing biomedical engineering, 2018, 37 (06): 35-42+71.], strong obstacle avoidance [ see literature: the plug is rasagile. Flexible motion control based on adaptive admittance control [ j ]. Robot and automation international conference, 2002.Seraji H.Adaptive admittance control: an approach to explicit force control in compliant motion [ C ]// IEEE International Conference on Robotics & automation.2002 ], high reliability [ see literature: kriging. Robotics and control [ J ]. The culture education limited company, 2005 edition, namely: craig j.introduction to Robotics: mechanics and Control the Third Edition [ M. Pearson Education, inc.,2005 ], good torque optimality, good rigidity performance, and wide application in various fields. At present, two optimal structures exist for the redundant mechanical arm, one of which is an S-R-S structure mechanical arm, which has self-movement without influencing the terminal pose, and the redundant mechanical arm has wide application in a plurality of fields. The KUKA LBR iiiwa mechanical arm research object is used for researching the kinematic characteristics of the mechanical arm to build a kinematic model, so as to lay a foundation for practical application [ see literature: yang Zhiwei seven degrees of freedom mechanical arm kinematics and motion planning study [ D ]. Yan Shanda, 2019 ].

The characteristic that the joint number of the iwa mechanical arm is larger than the degree of freedom of the tail end increases flexibility, but the difficulty of control and kinematic solution is also increased. The high flexibility is mainly derived from the characteristic that the joints can move in a zero-space self-movement mode, namely, the condition that the mechanical arm is fixed at the tail end. If a numerical method is adopted to solve, accumulated errors exist, and the solution of singular states is not considered [ see literature: ma Jila. Mechanical arm flexibility controls the progress of the study [ J ]. System control journal, 2007, 11 (6): 418-432, namely: mason m.company and Force Control for Computer Controlled Manipulators [ J ]. IEEE Transactions on Systems Man & Cybernetics,2007, 11 (6): 418-432.]. Aiming at the advantages of the redundant mechanical arm, the iiwa mechanical arm is used as a research object, the solution is carried out by utilizing an analytic method with high solution speed, the solution of the equivalent degree of freedom in the singular state is utilized to avoid singular, and a theoretical basis is provided for practical application [ see literature: once, hermami. Overview of robot control [ J ]. Robotica,1997, (5): 473-482. Namely: zeng, hemami a.an overview of robot force control [ J ]. Robotica,1997, 15 (5): 473-482.].

Due to the complex working environment, the mechanical arm inevitably collides with the environment during the task [ see literature: magical gamma, ai Nuomo torr. Hybrid control of robotic arm position, attitude, force and moment [ j ]. IEEE computer society, 2009: huanga Q, enomoto R.hybrid position, post, force and moment control of robot manipulators [ C ]// IEEE International Conference on Robotics & biochemicals.IEEE Computer Society,2009.], especially for maintenance and upgrade of the working contents of robotic arms, the robotic arms must come into contact with task targets. In the face of a complex environment, the common six-axis mechanical arm is difficult to avoid the obstacle, so that the phenomenon that the six-axis mechanical arm cannot complete the task for avoiding the obstacle can occur in practice. The redundant mechanical arm can finish obstacle avoidance through the self redundancy characteristic without influencing the position of the tail end, but the position control has certain limitation [ see literature: i wans, alawanda, ni Gemu, etc. A six-degree-of-freedom mechanical arm control method [ J ] based on PD+I. Industrial robot,2008, 35 (2): 125-132. Namely: evans T, alavandar S, nigam M J. Fuzzy pd+ I control of a six DOF Robot manipulator [ J ]. Industrial Robot,2008, 35 (2): 125-132 ], the system inevitably deviates, and once the position deviates, the rigidity of the environment is very high, and the robot is very high in rigidity, so that a very high force is generated between the robot and the environment, and the robot and the environment are damaged very greatly. Aiming at the phenomenon, a compliance algorithm of the mechanical arm is researched, a proper compliance control algorithm is selected according to environment and applicability, and the construction of a compliance controller is completed, so that the method has important significance for practical application of the redundant mechanical arm [ see literature: kuma, pan Wa, su Kawa south, etc. Mechanical arm force/position hybrid control based on neural network [ J ]. Precision engineering and manufacturing,2011, 12 (3): 419-426. Namely: kumar N, panwar V, sukavanam N, et al neurol network based hybrid force/position control for robot manipulators [ J ]. International Journal of Precision Engineering & Manufacturing,2011, 12 (3): 419-426.].

Therefore, a redundant mechanical arm singular point avoidance method based on an equivalent degree of freedom method with excellent technical effects is highly desired.

Disclosure of Invention

The invention aims to provide a kinematic optimization method under a singular position of a redundant mechanical arm; the method is mainly used for avoiding singular points so as to facilitate active compliance control with better technical effects.

The invention is focused on an S-R-S configuration redundant mechanical arm. As shown in fig. 11, the first three mutually perpendicular joints form a shoulder (S), the fourth joint serves as an elbow joint (R), and the last three mutually perpendicular joints form a wrist (S), such a robot arm also being referred to as an unbiased redundant robot arm. The ika corporation' S iiwa manipulator is a typical example of an S-R-S configuration, with two adjacent joints of the iiwa manipulator perpendicular to each other, unbiased, with the first three joints making up the shoulder, the fourth joint serving as the elbow joint, and the second three making up the wrist. The iiiwa robot arm was developed by KUKA corporation, DLRLBRI was designed in 1995, DLR LBR II was designed in 2000, and the first two generations of designs were designed for DLR LBR III and were designed in 2003. DLR LBR III was licensed to KUKA corporation in 2004, after which KUKA continued to develop KUKALBR 4 (2008) and KUKALBR 4+ (2010), and KUKALBR iwa (2013) in the final edition, two models, iwa r800 and iwa r820. The related research of the invention is focused on the model Iiwa r 800.

The invention provides a redundant mechanical arm singular position type lower kinematics optimization method, which is mainly used for performing kinematics analysis and optimization treatment on a limited space aiming at a seven-axis robot; the method is characterized in that:

which comprises the following steps in sequence:

step one: firstly, a positive kinematic model of a redundant mechanical arm is established, and one of the common methods for solving by utilizing robot kinematics is utilized: establishing a reference joint coordinate system by using a standard D-H method, and obtaining a coordinate transformation matrix between adjacent joints by using a standard D-H transformation sequence to obtain a positive kinematic model;

step two: establishing an inverse kinematics model of the redundant mechanical arm, and solving inverse kinematics closed solutions of all joints by using an arm angle parameterization method;

step three: analyzing the specific odd-abnormal type of the redundant mechanical arm under the S-R-S configuration by adopting a geometric method, and resolving the inverse solution of each joint under the odd-abnormal type by utilizing the geometric method;

step four: combining the inverse solution of arm angle parameterization and the inverse solution under a specific odd-type, realizing the kinematic optimization of the redundant mechanical arm under the specific odd-type, wherein the focus is to avoid the specific odd-type; the kinematic solution of the redundant mechanical arm under the specific odd-type can also be provided, so that the mechanical arm can still realize kinematic controllability even under the specific odd-type.

The redundant mechanical arm kinematics optimization method based on the equivalent degree of freedom method disclosed by the invention has the following technical contents: the specific requirements of the first step are as follows:

firstly, establishing a coordinate system of each joint according to a standard D-H method, and obtaining a coordinate transformation matrix between adjacent joints by utilizing standard D-H coordinate change as follows:

in the formula (1): θ _i Is the joint angle value; c. s is the simplifications of cos and sin respectivelyWriting; a, a _i Representing the length of each public line; d, d _i Representing z _i The distance between adjacent public lines on the shaft;

and then sequentially multiplying transformation matrixes between adjacent coordinate systems to obtain a redundant mechanical arm positive kinematics model:

in the formula (9): (p) _x ，p _y ，p _z ) ^T Indicating the position information of the end of the robot arm,the 3×3 matrix is composed to represent the attitude information of the arm end.

In step one, further preferred technical content claimed is:

first, the D-H parameter table of the iwa-r800 mechanical arm is obtained according to the standard D-H method as shown in Table 1:

TABLE 1D-H parameter Table for Iiwa-r800 mechanical arm

Connecting rod i	θ _i (°)	d _i (m)	a _i (m)	αi(°)	Range of joint angles (°)
						1	θ ₁	d1	0	-90	-170～170
2	θ ₂	0	0	90	-120～120
						3	θ ₃	d3	0	-90	-170～170
4	θ ₄	0	0	90	-120～120
						5	θ ₅	d5	0	-90	-170～170
6	θ ₆	0	0	90	-120～120
						7	θ ₇	d7	0	0	-170～170

Under the condition that D1, D3, D5 and D7 are known, the terminal pose parameters are added into a transformation matrix of the connecting rods according to standard D-H parameter bands, so that a relation function between the terminal pose parameters and each connecting rod, namely a redundant mechanical arm forward kinematics solution, is obtained; substituting the data in table 1 into a conversion relation formula and formula (1) between adjacent rods according to a standard D-H conversion sequence to obtain a conversion matrix of a coordinate system between each adjacent rod:

Then the transformation matrix of the coordinate system between the adjacent rods is obtainedThe forward kinematics solution of the redundant mechanical arm can be obtained by multiplying the two components in sequence>The positive kinematic model is as follows:

in positive kinematic model of formula (9)(θ ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) The method is a kinematic positive solution of the mechanical arm, is also a changed matrix, describes the position and the posture of the tail end of the mechanical arm relative to a base coordinate system, and is the basis of kinematic analysis; subdividing space into joint space, i.e. q= (θ) ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) And the operation space, i.e., x= (p) _x ，p _y ，p _z ，θ _x ，θ _y ，θ _z ) The method comprises the steps of carrying out a first treatment on the surface of the Wherein (p) _x ，p _y ，p _z ) ^T Information indicating the position of the arm end (θ) _x ，θ _y ，θ _z ) And the attitude information of the tail end of the mechanical arm is shown.

In the second step, the technical content preferably claimed is:

the arm angle parameterization method establishes an inverse kinematics model formula, namely the angle values of all joints, as follows:

r in the formulas (21) to (27) is a rotation matrix; atan2 is a four-quadrant arctangent function; at theta ₁ Examples can be expressed as:

angle value theta ₁ The angle value of each joint is determined by the end pose and the arm angle θ.

In the case where the end pose and the arm angle θ are determined, the angle value of each joint may be calculated by the inverse kinematics model so that each joint angle value corresponds to each joint, as shown in fig. 15.

In the second step, further preferred technical content is as follows:

firstly, calculating an analytical solution of a redundant mechanical arm by using an arm angle parameterization method; specifically, a method for increasing the operation space is used for achieving the purpose of eliminating redundancy;

the redundant mechanical arm is specifically a library card KUKA LBR iwa r800 mechanical arm, and self-motion analysis is carried out on the mechanical arm: the method specifically refers to joint positions of all joints which can move from an initial position to a desired position in a joint space on the premise that the pose of the tail end of the redundant mechanical arm is unchanged; for the universal structure S-R-S type seven-degree-of-freedom mechanical arm used in this embodiment, there is a zero-space self-motion of the redundant mechanical arm, the motion does not affect the end tool motion, and the parameter describing the motion is the arm angle θ; the structure is simplified as shown in fig. 9 and 10;

the redundant mechanical arm specifically comprises a spherical hinge taking the origin of a coordinate system 2 as the center of sphere, wherein the rotation of the first three joint axes of the KUKA LBR iwa r800 corresponds to the rotation of the first three joint axes of the KUKA LBR iwa r 800; similarly, the rotation of the last three joints is also equivalent to a spherical hinge; therefore, the robot structure is simplified to the arm of a person, the first three joints are equivalent to shoulder joints, the fourth joint is equivalent to elbow joints, and the last three joints are equivalent to wrist joints; as shown in fig. 11. When the tip position and posture are given, the wrist position is fixed when the tip position is fixed because the rotation of the last joint has no effect on the position of the tip; similarly, the rotation of the first joint does not affect the position of the shoulder joint;

When the end pose is determined, the elbow joint E rotates around the axis formed by the shoulder joint and the wrist joint, as shown in fig. 9 and 10, the rotation motion of the elbow is self-motion, the circumference of the point E of the elbow is called redundant circumference, the radius EF is called redundant radius, and as shown in fig. 10, the point theta and the point E have one-to-one correspondence, namely, when the end position and the pose of the redundant mechanical arm are determined, the self-motion quantity theta is 0,2 pi]Any value in the set has a unique E point corresponding to the E point; when a reference plane is determined, the planeThe surface is formed by S, W and the highest point E (maximum z-axis component) on the redundant circumference, when the arm angle θ is specified, θ is a parameter of the introduced constraint double arm (big and small arms) position, meaning that E rotates θ degrees around axis SW to reach the new elbow E 'point, each E' determines the positions of big arm SE and small arm EW, then joint 4 angle θ can be resolved ₄ And angles θ of joints 1, 2, 3 ₁ 、θ ₂ 、θ ₃ Finally, the angles theta of the joints 5, 6 and 7 are calculated ₅ 、θ ₆ 、θ ₇ ；

Secondly, performing inverse kinematics modeling:

first solving the angle theta of the joint 4 ₄ In combination with the motion analysis configuration, the effect of rotation around the axis of the joint 4 causes an included angle to be formed between the large arm and the small arm; as shown in fig. 2. Thus θ ₄ The solution is converted into solution triangle, and the solution triangle is obtained by using trigonometric function, theta ₄ Complementary to the triangle interior angle SEW;

in addition, the position of the elbow E is unchanged, the axis of the joint 4 at E is changed from original perpendicular to the paper surface to the outside, and as a result, a negative sign is added to discuss two cases, and under the condition that the position of the large arm and the small arm is unchanged, the axis of the joint can have two orientations; finally get theta ₄ The calculation formula of (2) is as follows:

then solve for θ ₁ ，θ ₂ ，θ ₃ Angle (v): in the case of determining the end pose and the introduced arm angle θ, i.e., determining the elbow point E ' on the redundant circumference, the plane in which S, E ', W lies is set to be the arm plane τ, and in this case, in conjunction with fig. 12, the origin of the link coordinate system 3 is at E ', i.e., the homogeneous transformation matrix of the coordinate system 3 can be selected to calculate θ ₁ ，θ ₂ ，θ ₃ Is a function of the angle of (2);

e' position is determined, and the coordinate system 3 is also determined; the y3 axis of the coordinate system 3 is pointed at S by E', the z3 axis is perpendicular to the arm plane τ, and its orientation is pointed at θ ₄ Of the formula of calculationSign determination; when the orientations of the two coordinate axes of the coordinate system 3 are determined, a rotation matrix of the coordinate system 3 is determined;

(III) calculating a rotation matrix of the coordinate system 3: determining the highest point of the redundant circumference, namely determining E= [ E ] when the arm angle theta=0 _x E _y E _z ]The point coordinates are solved as follows:

the arm plane tau is a reference plane, is determined by a plane formed by points S, W and E, and can be expressed as a highest point E reached by an elbow under the constraint of arm lengths of a large arm SE and a small arm EW;

the coordinates of the E point are determined by the arm plane τ: due to vectorsSo the highest point E is coplanar with B, S, W; as shown in fig. 13 and 14.

The joint axes 2, 4 at S, E are now parallel, obtained from the established D-H coordinate system; firstly, calculating the center coordinates of a redundant circumference on a SW straight line, then, making a directional line segment perpendicular to the straight line SW on the BSW plane and having the length of the line segment being the radius r of the redundant circumference, wherein the end point is the highest point E;

as shown in fig. 12, vectorsParallel to vector->And vertically upwards, on the basis of which a new coordinate system is established, and a redundant circumference center F is taken as an origin, and a new coordinate system Fx 'y' z ', y' axis is established in a vector +.>On the z' axis in vector +.>The x ' axis is perpendicular to the plane BSW and is also perpendicular to the z ' and y ' axes; the redundant circumference is now in plane Fx 'z', assuming armsWhen the angle is equal to θ, the elbow E ' has a coordinate [ m 0 n ] in the coordinate system Fx ' y ' z ] ]' then the coordinates of E in the base coordinate system:

x 'in the above' _x ，X′ _y ，X′ _z The projection of the x 'axis of the coordinate system Fx' y 'z' on the x, y and z axes of the base coordinate system respectively; x'. _z ，Y′ _z ，Z′ _z Respectively projecting the three axes of the coordinate system Fx ' y ' z ' on the z axis of the base coordinate system; because Fx 'is perpendicular to the plane BSW, the projection of Fx' axis on the z axis of the base coordinate system is 0, the projection of Fy 'axis on the z axis of the base coordinate system is sin beta, the projection of Fz' axis on the z axis of the base coordinate system is cos beta, and beta is the fixed parameter angle after the terminal pose is determined, namely the known quantity;

[X′ _z Y′ _z Z′ _z ]＝[0 sinβ cosβ] (12)

as shown in fig. 12, in the case of introducing the arm angle θ, there is a relationship as follows:

in combination with the three formulas above, there are the following relations:

E′ _z ＝r*cosβ*cosθ (14)

after the end pose determination, equation (14) illustrates that the elbow position z-axis coordinate component takes a maximum value when the arm angle θ is 0; this also ensures the correctness of the elbow highest point determination method shown in fig. 12. Similarly, when the maximum and minimum values of the x component of the elbow coordinate are taken, only the vector is needed to be takenIs replaced by vector->The starting point is the point S, and the point E of the elbow joint is a planeIntersection of SWN with redundant circumference; similarly, the vector +.>Is replaced by vector- >The same starting point is the point S, and the point E of the elbow joint is the intersection point of the plane SWN and the redundant circumference;

the determination of the reference plane and the arm angle θ determines the spatial position between the large and small arms, and also provides more possibilities for flexible obstacle avoidance.

(IV) determination of the highest point of the elbow joint, the following is a specific solution to the highest point of the elbow joint coordinate E:

in FIG. 12, the coordinates of the center point F of the redundant circle are represented by vectorsThe unit vector of the line segment SF, the length of the line segment SF and the coordinates of the point S are shown, the lengths of the line segment SE and the EW are respectively fixed values d3 and d5 and the coordinates of the point S and the point W are known, and then the coordinates of the point F can be solved in the two right triangles delta EFS and delta EFW;

solving to obtain the coordinates of the highest point E of the elbow joint: from vectorsSum vector->Vertical, vector->Vectors +.>In particular here the vector +.>Normal vector to planar BSW>Judging the direction vertically, and combining the coordinates of the point F to obtain the coordinates of the point E; the method comprises the steps of applying a special to general idea, firstly obtaining the coordinate of an E point when the redundancy highest point is 0, and then obtaining the coordinate of the E point when the arm angle theta is at any angle through the generalized of the Rodrigues transformation;

Is provided withThe unit vector of u= [ ux uy uz ]]′，/>The following equation is obtained:

(Vector)having both upward and downward components, k=1 is set, which ensures that the lowest point of the elbow joint can be eliminated and can be brought into equation (15) to obtain the vector +.>The solution of (2) is:

the coordinates of the redundancy highest point E are finally obtained as:

(V) redundancyThe coordinates of the highest point E point have been determined, and then the rotation matrix of the coordinate system 3 is obtained from the coordinates of the E pointThrough the rotation matrix->Solving for theta ₁ 、θ ₂ 、θ ₃ Angle of (2)

The specific solving method of the coordinate system 3 is as follows: as shown in FIG. 2, the y-axis of the coordinate system 3 is pointed at S by E, the z-axis is perpendicular to the arm plane τ, and its orientation depends primarily on equation (10), namelyThe sign selected in (3); when the directions of 2 coordinate axes of the coordinate system 3 are determined, the x-axis is obtained by cross multiplication of positive unit vectors of y-axis and z-axis, the positive directions of the three axes are determined, and the unit column vectors of the positive directions are sequentially arranged, so that a rotation matrix of the coordinate system 3 at the highest point of the elbow joint relative to the basic coordinate system is formed, and the rotation matrix is shown as a formula (18):

in formula (18)Is selected and->The signs are opposite;

when the elbow joint moves around the axis SW, namely when the arm angle theta is not equal to 0, the Rodrigos transformation is adopted; the rondrigas transform is that a vector is multiplied by a rotation matrix, which is equivalent to a vector being rotated in some way, and the rotation matrix can also be understood as being obtained by directly rotating a certain angle at a time around a certain vector in space. At this time, the rotation matrix calculation formula of the coordinate system 3 on the elbow joint is shown in formula (19):

In the formula (19) [ u× ]]Representative vectorAn antisymmetric matrix of unit vectors u, I ₃ Is a 3x3 identity matrix;

in addition, in the case of the optical fiber,representing a rotation matrix of the first three joints, < >>The other expression is shown in the formula (20):

the inverse solution of the first three joints is obtained by two-way simultaneous:

in the formulae (21) to (23),representative is->The first in the matrixi row j column element;

more than (six) already has theta ₁ 、θ ₂ 、θ ₃ 、θ ₄ Then, it can be obtained by algebraic method ₅ ，θ ₆ ，θ ₇ The values, solving method, are as follows:

at the time of obtaining theta ₁ 、θ ₂ 、θ ₃ 、θ ₄ After the solution of (a),there are two expression modes:

from theta ₁ 、θ ₂ 、θ ₃ 、θ ₄ The angle and the end gesture of (2) are substituted into the left end of (24) to obtainThe values of the elements of the matrix can be finally obtained according to the third column and the third row of the right matrix ₅ 、θ ₆ 、θ ₇ Is the value of (1):

in the formulae (24) to (27),representative is->The ith row and jth column element of (a); the inverse solutions of all joints are solved.

In step three, the preferred technical content claimed is:

the specific singular position type of the redundant mechanical arm with the S-R-S configuration is shown in the figure 3, and the specific singular position type meets the following requirements:

1) Equivalent three degree of freedom solution

(1) On the premise that three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, when the straight line SW formed by the whole big arm and the small arm is collinear with the BS again, the joints 1, 2, 3, 4 and 5 are equivalent to a rotation pair, and theta ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are all equal to 0; the mechanical arm is equivalent to a three-degree-of-freedom mechanical arm; the joints 5, 6 and 7 to be solved are the same as the calculation formulas in the step two;

(2) on the premise that the three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the three points are also collinear with the end effector T, and when in the state, the three points are theta ₃ 、θ ₄ 、θ ₅ 、θ ₆ Are all 0; the robot arm in this case can also be equivalent to a three-degree-of-freedom robot arm; the formula of the angle value of each joint to be solved is as follows:

2) Equivalent five degree of freedom solution

When the three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the redundant circumference radius r is 0, the large arm SE and the small arm EW of the mechanical arm have no self-movement property, and the angle theta of the joint 4 is the same ₄ Also equal to 0;the axis collineation of the joint 3 and the joint 5 is equivalent to a revolute pair, the joint space of the mechanical arm is at most 5 degrees of freedom, and the mechanical arm is equivalent to a five-degree-of-freedom mechanical arm;

the formula of the angle value of each joint to be solved is as follows:

in the third step, further preferred technical content is as follows:

starting from the inverse kinematics configuration, two singular states of the mechanical arm are analyzed and calculated; singular state analysis is as follows: as shown in fig. 3, when the three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the redundant circumferential radius r is 0; the large arm SE and the small arm EW of the mechanical arm have no self-movement property, and the angle theta of the joint 4 is at the moment ₄ Also equal to 0; the axis collineation of the joint 3 and the joint 5 is equivalent to a revolute pair, and the joint space of the mechanical arm is at most 5 degrees of freedom; as shown in FIG. 4, when the straight line SW formed by the whole big arm and the small arm is collinear with BS again under the precondition of ESW three-point collineation, then the joints 1, 2, 3, 4, 5 are equivalent to a revolute pair, and θ ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are all equal to 0, and the mechanical arm end effector at most hasThree degrees of freedom;

under the two conditions, the mechanical arm is in a singular state, and then the two states are resolved;

the configuration analysis of the iiwa-r800 results in a singular position type as shown in fig. 3, and the inverse kinematics model is failed at this time because the mechanical arm is in the singular position type; when the inverse kinematics model fails, resolving the singular bit pattern, and establishing a singular bit pattern inverse solution based on equivalent degrees of freedom, wherein the specific process is as follows:

in the case of B, S, E, W being all collinear, the joints 1, 2, 3, 4, 5 are equivalent to a revolute pair, as shown in FIG. 4, and θ ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are all equal to 0, the angles of the joints 5, 6 and 7 are joint variables to be solved, namely the mechanical arm equivalent to three degrees of freedom at the moment, and the mechanical arm is constantI ₃ Is a 3-order identity matrix; the angles of the joints 5, 6, 7 at this time are as shown in formulas (25) to (27):

When the angle theta of the joint 6 ₆ When=0, the joints 5,6,7 are equivalent to a pair of rotations such that the angle θ of the joint 5 ₅ =0, only the angle of the joint 7 is the amount to be solved, the others are all 0, and the redundant arm is equivalent to a free arm;

equivalent three-degree-of-freedom solution

The foregoing mentions that when B, S, E, W are all collinearIn the case of joints 1, 2, 3, 4, 5 are equivalent to a revolute pair, and θ ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are all equal to 0, the angles of joints 5,6,7 are the joint variables to be solved, i.e. the mechanical arm equivalent to the mechanical arm is equivalent to the mechanical arm with three degrees of freedom, and the formulas (24) - (27) are all established, except that in the formula (24), the constantI ₃ Is a 3-order identity matrix; when the angle theta of the joint 6 ₆ When=0, the joints 5,6,7 are equivalent to a pair of rotations, so that the angle θ of the joint 5 ₅ =0, only the angle of the joint 7 is the amount to be solved, the others are all 0, and the redundant arm is equivalent to a free arm;

another 3-degree-of-freedom mechanism: is also collinear with the end effector T on the premise of being collinear with S, E, W, and theta when in this state ₃ 、θ ₄ 、θ ₅ 、θ ₆ The mechanical arm in this case can be equivalent to a mechanical arm with three degrees of freedom, the angles of the joints 1, 2 and 7 are joint variables to be solved, and by observing the standard D-H coordinate system, it is found that the axes of the coordinate system 2 and the coordinate system 6 are completely identical under the zero joint angle, and the values of 1, 2 and 7 can be obtained as shown in formulas (29) to (31):

(two) solution of equivalent five degrees of freedom

Equivalent five degrees of freedom, namely, the case that three points of E, S and W are collinear with SW and SW is not collinear with BS and WT, is shown in figure 3; at this time theta ₃ ，θ ₄ =0, the angles of joints 1,2,5, 6, 7 are the quantities to be solved, which can be obtained by observing a standard D-H coordinate system, coordinate system 2 and coordinate system 4 are identical in their axes at zero joint angle, so joints 1,2,5 are equivalent to the solutions of joints 1,2, 3;

the five elements of the second column and the third row of (2) give θ ₁ ，θ ₂ ，θ ₅ The calculation of (a) is shown in formulas (33) to (35):

solving the angles of the joints 1,2 and 5 to obtain theta ₆ And theta ₇ See (37) (38):

when the mechanical arm is in the odd-abnormal position type, whether the equivalent degree of freedom can meet the operation requirement can be judged according to the actual situation, if the condition of a specific odd-abnormal position type can be met, the operation is completed by using the inverse solution of the equivalent degree of freedom, otherwise, the angle value of the singular state is omitted, the angle value at the last moment or a program error reporting target point is not reachable, and the danger of the mechanical arm in the singular state is avoided.

In step four, the preferred technical content claimed is:

when a desired Cartesian track is input, judging whether the track is a specific odd-abnormal type or not; if the equivalent degree of freedom algorithm is adopted for the specific odd-type, if the equivalent degree of freedom algorithm is not adopted, the track is finally output, and the effect of avoiding the specific odd-type is achieved.

Step four, further preferred technical content claimed is:

combining a forward kinematics model, an inverse kinematics model and a singular position type inverse solution, and under the condition of giving a section of track, optimizing the track by utilizing the first three steps to finally obtain an optimized track;

a flow chart of a kinematic optimization method under a singular position of a redundant mechanical arm is shown in the attached figure 1.

The specific requirements of the kinematics model verification are as follows:

(1) positive kinematics verification

The positive kinematics verification method is to compare the calculation result with the positive kinematics solution solved by the robot tool kit to see whether the result is correct; the actual size of the iiwa mechanical arm is obtained by: d1 =0.360 m, d3=0.420 m, d5=0.400 m, d7=0.126 m;

because positive kinematics is a homogeneous matrix of end-to-base coordinates, given joint angle values, several sets of joint angle values are randomly substituted, with substituted angle values q= [0, pi/5,0, pi/4,0], [ pi/2,0, pi/2,0, pi/4,0], [0, pi/2,0, pi/5,0, pi/4,0], [0, pi/5,0, pi/4, pi/2], in units, respectively; substituting the four sets of joint angle values into the results T1, T2, T3 and T4 obtained in the kinematic algorithm are as follows:

Solving a positive kinematic function xxx.fkine (q) by using the robot toolbox, wherein q represents the joint angle; substituting the above four sets of joint angle values into the function to obtain homogeneous matrices t1_, t2_, t3_, t4_, as follows:

(2) inverse kinematics validation

The same inverse kinematics verifies the calculation principle, can realize the complete calculation of inverse kinematics solution, and the joint limit is given by the official of KUKA company; randomly assigning a position and a pose at the end of the workspace domain; here a set of kinematic positive solutions (also self-computing robot kinematic positive solutions) are solved directly using MATLAB robot toolboxes;

given a set of joint angle values q= [ 60-20 10 45 0 90 0 ]. Pi/180, unit rad; then, a homogeneous matrix T_D of the relative base coordinates of the end points is obtained by using fkine positive kinematic function as follows:

then substituting the obtained homogeneous matrix T_D as input into a written inverse kinematics algorithm, wherein the redundant mechanical arm adopts an inverse solution algorithm which is arm angle parameterization, and the range of theta obtained by motion analysis is [0,2 pi ];

firstly, verifying all inverse solutions when the self-motion variable theta is a fixed value, selecting theta=0, and obtaining 8 groups of different inverse solutions by the method, wherein the table 2 is shown in the specification;

Table 2θ=0, 8 sets of inverse solutions

Selecting any group of inverse solutions to be substituted into the positive kinematic function to obtain a new homogeneous matrix T_D_as follows:

the comparison result is identical, the correctness of inverse kinematics solution when θ=0 is verified, when θ is given to different values, joint solutions are substituted into a positive kinematics formula to obtain a homogeneous matrix which is identical to T_D, and the correctness of inverse kinematics models when θ is different is verified.

(3) Odd-type inverse solution verification

When the joint angle is q= [0 45 0 0 0 45 0 ]. Pi/180, the mechanical arm is in a special singular position type, the inverse solution of q= [0 45 0 0 0 45 0 ]. Pi/180 is verified, the kinematic positive solution of q= [0 45 0 0 0 45 0 ]. Pi/180 is firstly obtained, and the fkine positive kinematic function is utilized to obtain a homogeneous matrix T_S of the end relative base coordinates as follows:

substituting T_S as input into a written inverse kinematics algorithm to obtain an inverse solution of the redundant mechanical arm, and substituting any group of inverse solutions into a positive kinematics model to obtain a homogeneous matrix T_S_as follows:

/>

the results are identical to verify the correctness of the inverse solution under the singular bit pattern.

Further preferably, step four also meets the following requirements:

through combining a kinematic model and inverse solution under a special odd-type, the redundant mechanical arm with the S-R-S general structure can normally move under the specific odd-type when the arm angle theta exists or when the arm angle theta does not exist, and at the moment, the mechanical arm has stable and controllable kinematic performance, so that the effect of avoiding the singular is realized. In the technical background of the invention, the background knowledge mainly related to the trajectory planning of the mechanical arm is supplemented as follows:

Trajectory planning of joint space: when the robot is subjected to track planning in the joint space, an inverse kinematics equation of the robot needs to be solved, each operation path point of the end effector is converted into a corresponding joint angle value, then the angle value of each joint is planned and fitted into smooth functions, and the joint functions represent the motion track of each joint of the mechanical arm. The planning method is simple in calculation and free from the phenomenon of mechanism singularity, and the shape of the track between two path points in rectangular space coordinates does not need to be considered. However, when continuous trajectory tracking control is required for the robot end effector, the method cannot complete the task of trajectory tracking, and the robot arm needs to be subjected to trajectory planning in a Cartesian space.

(II) Cartesian space trajectory planning: when the trajectory planning is performed on the mechanical arm in the Cartesian space, the pose of the end effector of the mechanical arm is known at all times, and the motion generated by the mechanical arm is firstly described in the Cartesian space and then is converted into the joint space description. When the motion planning of the mechanical arm is carried out, the pose of the end effector needs to be converted into a joint variable by solving an inverse kinematics equation in real time. The calculation amount is far greater than that of joint space description when adopting Cartesian space planning, but a controllable and predictable path can be obtained by using a Cartesian space track planning method.

The two planning methods are actually opposite processes, and the path operation points given by the joint space trajectory planning need inverse kinematics calculation, and other times are forward kinematics processes. The whole process of the Cartesian space trajectory planning is converted into joint space after the inverse kinematics are continuously solved.

The arm angle θ is mainly planned from the actual situation, and will not be described here too much.

Compared with the common mechanical arm, the redundant mechanical arm has better flexibility, better fault tolerance and reliability, and in order to keep nuclear power station workers away from high-radiation hazards and meet the operation requirements, the redundant mechanical arm is required to be used for maintenance/upgrading of nuclear industrial equipment. The main research work in the technical background of the invention is based on a redundant mechanical arm of a library card KUKA LBR iwa carried on a nuclear industrial robot, and the following research is conducted on the iwa mechanical arm:

(1) Due to the complexity of the working environment, a redundant mechanical arm with high flexibility is selected, the kinematic modeling of the redundant mechanical arm is completed on the premise of not using a numerical method, and the closed solution of the redundant mechanical arm is solved.

(2) And performing track planning aiming at the Cartesian space to obtain a controllable space path. And selecting a proper method to complete the linear interpolation of the Cartesian space and the multi-line segment interpolation.

(3) And selecting a proper compliant control method, avoiding unrecoverable damage when the mechanical arm is in contact with the environment, and completing the operation requirement of equipment maintenance and upgrading.

The invention mainly completes the following work:

(1) Forward kinematic study of redundant robotic arms: establishing a coordinate system of each joint of the robot by using a standard D-H method, and completing modeling of forward kinematics;

(2) Performing motion analysis on the redundant mechanical arm, completing inverse kinematics solution of the redundant mechanical arm by using an analytic method according to the geometric configuration, and performing simple analysis and pre-solution on the singular state of the mechanical arm;

(3) The accuracy of the kinematic model was verified using MATLAB robot toolbox.

The invention provides a redundant mechanical arm kinematics optimization (mainly avoiding singular points) method based on an equivalent degree of freedom method, which has the advantages of simple related operation, strong operability, good technical effect and great economic and social values.

Drawings

The invention will be described in further detail with reference to the accompanying drawings and embodiments:

FIG. 1 is a system flow chart of a method for optimizing kinematics under singular position of a redundant manipulator according to embodiment 1;

FIG. 2 is a schematic diagram of the joint 4 solution in example 1;

FIG. 3 is one of the analysis charts of the specific odd-ectopic type in example 1: equivalent five degrees of freedom;

FIG. 4 is a second diagram of a specific ectopic type analysis chart in example 1: equivalent three degrees of freedom 1;

FIG. 5 is a third analysis chart of the specific ectopic type of the present invention in example 1: equivalent three degrees of freedom 2;

fig. 6 is one of the best configuration diagrams of the mechanical arm: six degrees of freedom optimal configuration;

FIG. 7 is a second diagram of the best configuration of the robot: seven degree of freedom configuration 1;

FIG. 8 is a third view of the best configuration of the robotic arm: configuration 2 of seven degrees of freedom;

FIG. 9 is a simplified diagram of the architecture of an iwa robotic arm;

FIG. 10 is a simplified diagram of a redundant circumference of an iwa robotic arm;

FIG. 11 is an illustration of various joints of an iwa robotic arm;

FIG. 12 is a schematic view of a link coordinate system 3 after determining the tip pose and the induced arm angle θ;

FIG. 13 is an overall illustration of the redundancy under the constraint of the arm lengths of the large arm SE and the small arm EW;

FIG. 14 is a partial view of the redundancy highest points under the constraint of the arm lengths of the large arm SE and the small arm EW;

fig. 15 shows the joint angle corresponding to inverse kinematics θ.

Detailed Description

The invention is described in further detail below with reference to the attached drawings and examples:

example 1 (taking an example of an iiwa-R800 mechanical arm with S-R-S configuration as an illustration of the implementation of the present example)

The iiiwa mechanical arm is a seven-degree-of-freedom mechanical arm of a general structure S-R-S mechanism, and two configurations are generally adopted for carrying out positive kinematics calculation aiming at the general structure, as shown in fig. 6, 7 and 8, and the iiiwa mechanical arm is mainly formed by combining an optimal position configuration and an optimal posture configuration, and the optimal position configuration and the optimal posture configuration are directly connected, namely the conventional six-axis robot widely applied to industry at present, as shown in fig. 6. The inverse kinematics of the robotic arm of both configurations of fig. 7 and 8 have a closed form solution, where fig. 7 is a configuration that is actually employed by a large number of seven-axis co-robots.

Under the condition that the position and the posture of the end effector are fixed, the elbow of the seven-degree-of-freedom configuration 1 shown in fig. 7 can continuously move, the distribution of the robot connecting rod in space can be adjusted, the position of the end effector, namely the space positions of the big arm and the small arm, is not changed, the characteristic enables the robot arm to have high flexibility and obstacle avoidance performance, and the control safety is high. For subsequent inverse kinematics solution, we studied its forward kinematics here using the seven degree of freedom configuration 1 as the initial configuration of the iwa robotic arm; as shown in fig. 7.

Step one: firstly, carrying out positive kinematics modeling on a redundant mechanical arm (iwa-r 800 mechanical arm) by using a D-H method, establishing a reference joint coordinate system by using a standard D-H method, and obtaining a coordinate transformation matrix of an adjacent joint by using a standard D-H transformation sequence to obtain a positive kinematics model;

A D-H parameter table for the iwa-r800 robot arm was obtained according to the standard D-H method, see Table 1.

TABLE 1D-H parameter Table for Iiwa-r800 mechanical arm

Connecting rod i	θ _i (°)	d _i (m)	a _i (m)	α _i (°)	Range of joint angles (°)
						1	θ ₁	d1	0	-90	-170～170
2	θ ₂	0	0	90	-120～120
						3	θ ₃	d3	0	-90	-170～170
4	θ ₄	0	0	90	-120～120
						5	θ ₅	d5	0	-90	-170～170
6	θ ₆	0	0	90	-120～120
						7	θ ₇	d7	0	0	-170～170

Wherein: d1 The relation function between the terminal pose parameter and each connecting rod, namely the forward kinematics solution of the redundant mechanical arm, is obtained by adding the relation function between the terminal pose parameter and each connecting rod into a transformation matrix of the connecting rod according to a standard D-H parameter, namely the conversion relation between the terminal pose parameter and adjacent rods according to the standard D-H transformation sequence is as follows:

the parameters in table 1 are substituted into formula (1) to obtain a transformation matrix of the coordinate system between adjacent rods:

/>

transformation matrix of the coordinate system of the adjacent rodsNamely, the formulas (2) - (8) are multiplied in sequence to obtain the solution +.>The positive kinematic model is as shown in formula (9):

(θ ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) The method is a kinematic positive solution of the mechanical arm, is also a changing matrix, describes the position and the posture of the tail end of the mechanical arm relative to a basic coordinate system, and is the basis of kinematic analysis. Subdividing space into joint space, i.e. q= (θ) ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) And the operation space, i.e., x= (p) _x ，p _y ，p _z ，θ _x ，θ _y ，θ _z ). Wherein (p) _x ，p _y ，p _z ) ^T Information indicating the position of the arm end (θ) _x ，θ _y ，θ _z ) And the attitude information of the tail end of the mechanical arm is shown.

Step two, an iiwa-r800 mechanical arm inverse kinematics model is established by using an arm angle parameterization-based method

From the joint space, q= (θ) ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) There are seven total amounts and the end of the robot arm has only six degrees of freedom from the operating space. The inverse solution of the redundant manipulator is a solution without a closed form, i.e. without an analytical solution, without conditional restrictions. In order to calculate the analytic solution of the redundant mechanical arm, the present embodiment uses an arm angle parameterization method to process, specifically, uses a method of increasing the operation space to achieve the purpose of eliminating redundancy.

The self-motion analysis is carried out on the KUKA LBR iwa r800 mechanical arm, and particularly the joint position of each joint in the joint space can move from the initial position to the desired position on the premise that the pose of the tail end of the redundant mechanical arm is unchanged. For the general-purpose S-R-S seven-degree-of-freedom robot used in this embodiment, there is a zero-space self-motion of the redundant robot, which does not affect the motion of the end tool, and the parameters describing the motion are the arm angle θ, and the structure is simplified as shown in fig. 9 and 10.

The rotation of the first three joint axes of the mechanical arm is equivalent to a spherical hinge taking the origin of a coordinate system 2 as the spherical center. The same rotation of the last three joints also corresponds to a spherical hinge. This simplifies the robot construction to the human arm, with the first three joints corresponding to shoulder joints, the fourth joint corresponding to elbow joints, and the last three joints corresponding to wrist joints. As in fig. 11. When the tip position and posture are given, the wrist position is fixed when the tip position is fixed because the rotation of the last joint has no effect on the position of the tip. Similarly, the rotation of the first joint does not affect the position of the shoulder joint.

When the end pose is determined, the elbow joint E rotates around the axis formed by the shoulder joint and the wrist joint, as shown in fig. 9 and 10, the rotation motion of the elbow is self-motion, the circumference of the point E of the elbow is called redundant circumference, the radius EF is called redundant radius, and as shown in fig. 10, the point theta and the point E have one-to-one correspondence, namely, when the end position and the pose of the redundant mechanical arm are determined, the self-motion quantity theta is 0,2 pi]Any value within has a unique E point corresponding thereto. When a reference plane is defined, which plane is formed by S, W and the highest point E (maximum z-axis component) on the redundant circumference, when the arm angle θ is specified, θ is a parameter of the introduced constraint arm (forearm and forearm) position, meaning that E rotates θ degrees about axis SW to reach the new elbow E 'point, each E' determines the position of the forearm SE and forearm EW, and then the joint 4 angle θ can be resolved ₄ And angles θ of joints 1, 2, 3 ₁ 、θ ₂ 、θ ₃ Finally, the angles theta of the joints 5, 6 and 7 are calculated ₅ 、θ ₆ 、θ ₇ 。

Modeling of inverse kinematics: first, the angle θ of the joint 4 is solved ₄ In combination with the motion analysis configuration, the effect of rotation about the axis of the joint 4 enables an angle to be formed between the large and small arms. As shown in fig. 2. Thus θ ₄ The solution can be converted into a solution triangle, and the solution triangle is obtained by using a trigonometric function, and theta ₄ Complementary to the internal angle SEW of the triangle, but in the case of a constant position of the elbow E, the axis of the joint 4 at E is oriented from normal to the plane of the paperTurning out perpendicular to the page, the result is a negative sign that is divided into two cases, where the axis of the joint may be oriented in two directions with the position of the arms unchanged. Finally get theta ₄ The calculation formula of (a) is formula (10):

then solve for θ ₁ 、θ ₂ 、θ ₃ Is a function of the angle of (a). In the case where the end pose and the introduced arm angle θ are determined, that is, the elbow point E ' on the redundant circumference is determined, the plane in which S, E ', W is located is set as the arm plane τ, and in this case, in conjunction with fig. 12, the origin of the link coordinate system 3 is at E ', so that the homogeneous transformation matrix of the coordinate system 3 can be selected to calculate θ ₁ 、θ ₂ 、θ ₃ Is a function of the angle of (a).

The determination of the E 'position is determined by the coordinate system 3, the y3 axis of the coordinate system 3 is pointed by E' to S, the z3 axis is perpendicular to the arm plane τ, and its orientation is pointed by θ ₄ The sign of the calculation formula determines, when the orientations of the two coordinate axes of the coordinate system 3 are determined, the rotation matrix of the coordinate system 3, and then the rotation matrix of the coordinate system 3 is calculated.

Determining the highest point of the redundant circumference, namely determining E= [ E ] when the arm angle theta=0 _x E _y E _z ]The point coordinates are solved as follows:

the arm plane τ, i.e., the reference plane, is defined by the plane formed by points S, W and E, and can be expressed as the highest point E that the elbow can reach under the arm length constraint of the large arm SE and the small arm EW, and then the coordinates of point E are determined by the arm reference plane. Factor vectorThe highest point E is coplanar with B, S, W as shown in fig. 13 and 14.

The joint axes 2,4 at S, E are now parallel, which can be obtained from the established D-H coordinate system. Firstly, the center coordinates of the redundant circumference are obtained on the SW straight line, then, the BSW plane is perpendicular to the straight line SW and is at the center, the line length is the directed line segment of the radius r of the redundant circumference, and the end point is the highest point E.

As shown in fig. 12, vectorsParallel to vector->And vertically upwards, on the basis of which a new coordinate system is established, and a redundant circumference center F is taken as an origin, and a new coordinate system Fx 'y' z ', y' axis is established in a vector +.>On the z' axis in vector +.>The x ' axis is perpendicular to the plane BSW and is also perpendicular to the z ' and y ' axes. The redundant circumference is now in the plane Fx 'z', and the elbow E 'has a coordinate in the coordinate system Fx' y 'z' of [ m 0 n ] assuming that the arm angle is equal to θ ]' the coordinates of E in the base coordinate system are:

x 'in formula (11)' _x ，X′ _y ，X′ _z The x 'axis of the coordinate system Fx' y 'z' is projected on the x, y, z axes of the base coordinate system, respectively. X'. _z ，Y′ _z ，Z′ _z The projection of the three axes of the coordinate system Fx ' y ' z ' on the base coordinate system z axis is respectively, because Fx ' is perpendicular to the plane BSW, the projection of the Fx ' axis on the base coordinate system z axis is 0, the projection of the Fy ' axis on the base coordinate system z axis is sin beta, the projection of the Fz ' axis on the base coordinate system z axis is cos beta, and beta is a fixed parameter angle after the end pose is determined, namely the known quantity.

[X′ _z Y′ _z Z′ _z ]＝[0 sinβ cosβ] (12)

in combination with the three preceding formulas (11) - (13), there are the following relations:

E′ _z ＝r*cosβ*cosθ (14)

after the end pose is determined, the above equation illustrates that the z-axis coordinate component of the elbow position takes the maximum value when the arm angle θ is 0, which also ensures the accuracy of the elbow highest point determination method shown in fig. 12. Similarly, when the maximum and minimum values of the x component of the elbow coordinate are taken, only the vector is needed to be takenIs replaced by vector->The starting point is the point S, and the point E of the elbow joint is the intersection point of the plane SWN and the redundant circumference. Similarly, when the maximum and minimum values of the y component of the elbow joint coordinate are taken, the vector is only required to be + >Is replaced by vector->The same starting point is point S, where point E is the intersection of plane SWN and the redundant circle.

The method of determining the highest point of the elbow joint has been demonstrated, and then it is how to calculate the elbow joint coordinates E at the highest point. In FIG. 12, the coordinates of the center point F of the redundant circle can be represented by a vectorThe unit vector of the line segment SF, the length of the line segment SF and the coordinates of the S point are shown, the lengths of the line segment SE and EW are fixed values d3 and d5, and the coordinates of the S and W points are known, so that the coordinates of the F point can be easily solved in the two right triangles Δefs and Δefw, and are not described herein.

After the coordinates of the drop foot F point are obtained, the coordinates of the elbow joint highest point E point are obtained. From vectorsSum vector->Vertical, vector->Vectors +.>In particular here the vector +.>Normal vector to planar BSW>The direction is judged vertically, and then the coordinates of the point F are combined, so that the coordinates of the point E can be obtained, and the special to general thought is applied, the coordinates of the point E when the arm angle is 0, which is the highest point of redundancy, are obtained, and then the coordinates of the point E when the arm angle theta is at any angle are obtained through the generalized of the Rodrigues transformation.

Is provided withThe unit vector of u= [ ux uy uz ]]′，/>The following equation can be obtained:

the coordinates of the redundancy highest point E are finally obtained as:

to this end, the coordinates of the redundant highest point E point have been determined, and then the rotation matrix of the coordinate system 3 is obtained from the coordinates of E pointThrough the rotation matrix->Solving for theta ₁ ，θ ₂ ，θ ₃ The specific solving method of the coordinate system 3 is as follows:

as shown in FIG. 2, the y-axis of the coordinate system 3 is pointed at S by E, the z-axis is perpendicular to the arm plane τ, and its orientation depends primarily on equation (10), namelyWhen the directions of 2 coordinate axes of the coordinate system 3 are determined, the positive unit vector of the x axis is formed by the positive unit vector of the y axis and the positive unit vector of the z axisThe positive directions of the three axes are determined by cross multiplication, and unit column vectors of the positive directions are sequentially arranged to form a rotation matrix of the coordinate system 3 at the highest point of the elbow joint relative to the basic coordinate system, and the rotation matrix is shown in a formula (18):

in formula (18)Is selected and->The signs are opposite.

The rodgers transformation is used when the elbow joint self-moves about the axis SW, i.e., when the arm angle θ+.0. The rondrigas transform is that a vector is multiplied by a rotation matrix, which is equivalent to a vector being rotated in some way, and the rotation matrix can also be understood as being obtained by directly rotating a certain angle at a time around a certain vector in space. The rotation matrix calculation formula of the coordinate system 3 on the elbow joint at this time is shown in formula (19):

In the formula (19) [ u× ]]Representative vectorAn antisymmetric matrix of unit vectors u, I ₃ Is a 3x3 identity matrix.

In addition, in the case of the optical fiber,representing a rotation matrix of the first three joints, < >>Another expression is as in formula (20):

the inverse solution to the first three joints obtained by combining equation (19) and equation (20) is:

in the formulae (21) to (23),represents->The ith row and jth column elements in the matrix; atan2 is the four-quadrant arctangent function in θ ₁ Expressed as an example:

theta is already as above ₁ ，θ ₂ ，θ ₃ ，θ ₄ Then, it can be obtained by algebraic method ₅ ，θ ₆ ，θ ₇ The values, solving method, are as follows:

at the time of obtaining theta ₁ ，θ ₂ ，θ ₃ ，θ ₄ After the solution of (a),there are two expression modes:

from theta ₁ ，θ ₂ ，θ ₃ ，θ ₄ The angle and the tail end posture of the (B) are substituted into the upper left end to obtainThe values of the elements of the matrix can be focused on the third column and the third row of the right matrix to finally obtain theta ₅ ，θ ₆ ，θ ₇ The values of (2) are represented by the following formulas (25) to (27):

in the foregoing description of the several formulas of the present invention,representative is->The ith row and jth column element of (a); the inverse solutions of all joints are solved.

And step three, singularity analysis of the mechanical arm: establishing an inverse kinematics model of the redundant mechanical arm, and solving the inverse kinematics closed solution of each joint by using an arm angle parameterization method

The method has great significance for the singularity analysis of the mechanical arm, and the main reasons are as follows:

(1) When the mechanical arm is in an odd-abnormal type, the mechanical arm can lose the degree of freedom in a certain direction, and the tail end of the mechanical arm also can lose the degree of freedom in a certain direction, so that the flexibility of the mechanical arm is reduced.

(2) When the mechanical arm is in an odd-abnormal type, the problem of inverse kinematics can be solved infinitely;

(3) As the robotic arm approaches the odd-numbered positions, slight changes in cartesian space can result in the possible instantaneous large velocities of the individual joints.

When the mechanical arm is in or is close to an odd-shaped position, the kinematics and the dynamics of the mechanical arm can change slightly in an instant, and the stability of the mechanical arm can be reduced, so that danger is easy to occur. Analysis of the singular state of the robot arm is necessary.

In the embodiment, the two singular states of the mechanical arm are simply analyzed from the inverse kinematics configuration, and are resolved. Singular state analysis is as follows: as shown in FIG. 3, when the shoulder joint S, the wrist joint W and the elbow joint E are collinear at three points, the redundant circumference radius r is 0, the large arm SE and the small arm EW of the mechanical arm have no self-movement property, and the angle theta of the joint 4 is equal to ₄ And the joint is also equal to 0, the axis of the joint 3 and the axis of the joint 5 are collinear and equivalent to a revolute pair, and the joint space of the mechanical arm at the moment is at most 5 degrees of freedom. As shown in FIG. 4, when the straight line SW formed by the whole big arm and the small arm is collinear with BS again under the precondition of ESW three-point collineation, then the joints 1,2,3,4,5 are equivalent to a revolute pair, and θ ₁ ，θ ₂ ，θ ₃ ，θ ₄ The values of (2) are all equal to 0, and the mechanical arm end effector has three degrees of freedom at most. In both cases the robot arm is in a singular state, and then the two states are resolved.

Equivalent three-degree-of-freedom solution

It was mentioned that in the case where B, S, E, W are all collinear, the joints 1,2,3,4,5 are equivalent to a revolute pair, and θ ₁ ，θ ₂ ，θ ₃ ，θ ₄ The values of (2) are all equal to 0, the angles of joints 5,6,7 are the joint variables to be solved, i.e. the mechanical arm equivalent to the mechanical arm is equivalent to the mechanical arm with three degrees of freedom, and the formulas (24) - (27) are all established, except that in the formula (24), the constantI ₃ Is a 3-order identity matrix. When the angle theta of the joint 6 ₆ When=0, the joints 5,6,7 are equivalent to a pair of rotations, so that the angle θ of the joint 5 ₅ Only the angle of joint 7 is the amount to be solved, the others are all 0, and the redundant arm corresponds to a free arm.

Another 3-degree-of-freedom mechanism is also possible, as shown in FIG. 5, that is collinear with the end effector T if S, E, W are collinear, in this state θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ The mechanical arm in this case can be equivalent to a mechanical arm with three degrees of freedom, the angles of joints 1,2 and 7 are joint variables to be solved, and the directions of axes of the coordinate system 2 and the coordinate system 6 are completely the same under the zero joint angle by observing a standard D-H coordinate system, so that the values of 1,2 and 7 can be obtained as shown in formulas (29) to (31):

(two) solution of equivalent five degrees of freedom

The equivalent five degrees of freedom, i.e., the case where the three points E, S, W are collinear with SW and SW is not collinear with BS, WT, is shown in fig. 3. At this time theta ₃ ，θ ₄ The angle of the joint 1,2,5,6,7 is the amount to be solved by observing the standard D-H coordinate systemIt can be obtained that coordinate system 2 and coordinate system 4 are identical in their axes at zero joint angle, so that joints 1,2,5 are equivalent to solutions for joints 1,2, 3. After the angles of the joints 1,2 and 5 are solved, the joint 6 and the joint 7 satisfy the formula (32) to obtain theta ₆ And theta ₇ As shown in formulas (33) and (34).

When the mechanical arm is in an odd-abnormal position type, whether the equivalent degree of freedom can meet the operation requirement can be judged according to the actual situation, if the operation can be completed by utilizing the equivalent degree of freedom, otherwise, the angle value of the singular state is omitted, the angle value at the last moment or a program error reporting target point is not reachable, and the danger of the mechanical arm in the singular state is avoided.

Step four: and under the condition of combining a forward kinematics model, an inverse kinematics model and a singular position type inverse solution and giving a section of track, optimizing the track by utilizing the first three steps, and finally obtaining an optimized track.

And (3) verifying a kinematic model:

1. positive kinematics verification

The positive kinematics verification method mainly uses MATLAB to write out an algorithm, compares a calculation result with a positive kinematics solution solved by a robot tool kit, and judges whether the result is correct. The actual size of the iiwa mechanical arm is obtained by: d1 =0.360 m, d3=0.420 m, d5=0.400 m, d7=0.126 m.

Because positive kinematics is a homogeneous matrix of end-to-base coordinates, given joint angle values, several sets of joint angle values are randomly substituted, where I substitute angle values q= [0, pi/5,0, pi/4,0], [ pi/2,0, pi/2,0, pi/4,0], [0, pi/2,0, pi/5,0, pi/4,0], [0, pi/5,0, pi/4, pi/2], in rad, respectively; substituting the four groups of joint angle values into the own kinematics algorithm to obtain results T1, T2, T3 and T4 as follows:

where the matrix is the result of retaining four significant digits.

And solving a positive kinematic function xxx.fkine (q) by using the robot tool kit, wherein q represents the joint angle. Substituting the above four sets of joint angle values into the function to obtain homogeneous matrices t1_, t2_, t3_, t4_, as follows:

where the matrix is the result of retaining four significant digits.

2. Inverse kinematics validation

The same inverse kinematics verification is also based on the previous calculation principle, and a program is written in MATLAB, so that complete calculation of inverse kinematics solution can be realized, and joint limitation is given by the official of KUKA company. The position and pose at the end of the workspace domain is randomly given, here a set of kinematic solutions (also self-calculated robot kinematic solutions can be used) is solved directly with the MATLAB robot toolbox.

and substituting the obtained homogeneous matrix T_D as input into a written inverse kinematics algorithm, wherein the adopted inverse solution algorithm is arm angle parameterization due to redundant mechanical arms, and the range of theta obtained by motion analysis is [0,2 pi ].

First, all inverse solutions when the independent motion variable θ is a constant value are verified, θ=0 is selected, and 8 different sets of inverse solutions are obtained by the above method, see table 2.

Table 2θ=0, 8 sets of inverse solutions

the comparison results are identical, and the correctness of the inverse kinematics solution when θ=0 is verified. When different values of theta are given, joint solutions are substituted into a positive kinematic formula to obtain a homogeneous matrix which is the same as T_D, and the correctness of the inverse kinematic model with different theta is verified.

3. Odd-type inverse solution verification

Claims

1. A kinematic optimization method under a redundant mechanical arm singular position type is mainly used for carrying out kinematic analysis and optimization treatment on a limited space aiming at a seven-axis robot; the method is characterized in that:

which comprises the following steps in sequence:

step one: firstly, establishing a positive kinematic model of a redundant mechanical arm;

2. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 1, wherein the method comprises the following steps: the specific requirements of the first step are as follows:

in the above formula: θ _i Is the joint angle value; c. s is the shorthand of cos and sin respectively; a, a _i Representing the length of each public line; d, d _i Representing z _i The distance between adjacent public lines on the shaft;

then, the transformation matrices between the adjacent coordinate systems are multiplied in sequence to obtain a redundant mechanical arm positive kinematic model as follows:

3. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 2, wherein the method comprises the following steps: in step one:

TABLE 1D-H parameter Table for Iiwa-r800 mechanical arm

Connecting rod i θ _i (°) d _i (m) α _i (m) α _i (°) Range of joint angles (°) 1 θ ₁ d1 0 -90 -170～170 2 θ ₂ 0 0 90 -120～120 3 θ ₃ d3 0 -90 -170～170 4 θ ₄ 0 0 90 -120～120 5 θ ₅ d5 0 -90 -170～170 6 θ ₆ 0 0 90 -120～120 7 θ ₇ d7 0 0 -170～170

then the transformation matrix of the coordinate system between the adjacent rods is obtained The forward kinematics solution of the redundant mechanical arm can be obtained by multiplying the two components in sequence>

In positive kinematic model of formula (9)Is the kinematic positive solution of the mechanical arm; subdividing space into joint space, i.e. q= (θ) ₁ ，θ ₂ ，θ ₃ ，θ ₄ ，θ ₅ ，θ ₆ ，θ ₇ ) And the operation space, i.e., x= (p) _x ，p _y ，p _z ，θ _x ，θ _y ，θ _z ) The method comprises the steps of carrying out a first treatment on the surface of the Wherein (p) _x ，p _y ，p _z ) ^T Information indicating the position of the arm end (θ) _x ，θ _y ，θ _z ) And the attitude information of the tail end of the mechanical arm is shown.

4. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 1, wherein the method comprises the following steps: in step two: the inverse kinematics closed solution formulas of all joints are solved by using an arm angle parameterization method, wherein the closed solution formulas are shown in formulas (21) to (27):

R in the formulas (21) to (27) is a rotation matrix; atan2 is a four-quadrant arctangent function; wherein: at theta ₁ Expressed as an example:

angle value theta _i The angle value of each joint is determined by the end pose and the arm angle θ.

5. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 4, wherein the method comprises the following steps: in step two:

the redundant mechanical arm is specifically a library card KUKA LBR iwa r800 mechanical arm, and self-motion analysis is carried out on the mechanical arm: the redundant mechanical arm has a zero-space self-motion, the motion does not affect the motion of the end tool, and the parameter describing the motion is an arm angle theta;

the redundant mechanical arm specifically comprises a spherical hinge taking the origin of a coordinate system 2 as the center of sphere, wherein the rotation of the first three joint axes of the KUKA LBR iwa r800 corresponds to the rotation of the first three joint axes of the KUKA LBR iwa r 800; similarly, the rotation of the last three joints is also equivalent to a spherical hinge; the first three joints correspond to shoulder joints, the fourth joint corresponds to elbow joints, and the last three joints correspond to wrist joints; when the tip position and posture are given, the wrist position is fixed when the tip position is fixed because the rotation of the last joint has no effect on the position of the tip; similarly, the rotation of the first joint does not affect the position of the shoulder joint;

When the end pose is determined, the elbow joint E rotates around the axis formed by the shoulder joint and the wrist joint, and the rotation movement of the elbow is self-movement; the circumference where the elbow E point is located is called as a redundant circumference, the radius EF of the circumference is called as a redundant radius, and the arm angle theta and the E point have a one-to-one correspondence; namely, when the position and the gesture of the tail end of the redundant mechanical arm are determined, the radian value of the angle of the arm angle theta is [0,2 pi ]]Any value in the set has a unique E point corresponding to the E point; when a reference plane is defined which is formed by S, W and the maximum of the highest point E, i.e. the z-axis component, on the redundant circumference, when the arm angle θ is specified, θ is a parameter of the introduced constraint arm, i.e. the positions of the big and small arms, indicating that E rotates by θ degrees about the axis SW to reach a new point of elbow E ', each E' determining the position of the big arm SE and small arm EW, and then the joint 4 angle θ can be resolved ₄ And angles θ of joints 1, 2, 3 ₁ 、θ ₂ 、θ ₃ Finally solving the angles theta of the joints 5, 6 and 7 ₅ 、θ ₆ 、θ ₇ ；

Secondly, performing inverse kinematics modeling:

first solving the angle theta of the joint 4 ₄ In combination with the motion analysis configuration, the effect of rotation around the axis of the joint 4 causes an included angle to be formed between the large arm and the small arm; thus θ ₄ The solution is converted into solution triangle, and the solution triangle is obtained by using trigonometric function, theta ₄ Complementary to the triangle interior angle SEW;

in addition, the position of the elbow E is unchanged, the axis of the joint 4 at E is changed from original perpendicular to the paper surface to the outside, and under the condition that the position of the large arm and the small arm is unchanged, the axis of the joint can have two orientations; finally get theta ₄ The calculation formula of (2) is as follows:

then solve for θ ₁ 、θ ₂ 、θ ₃ Angle (v): in the case where the tip attitude and the introduced arm angle θ, that is, the elbow point E ' on the redundant circumference are determined, the plane in which S, E ', W lies is set to be the arm plane τ, the origin of the link coordinate system 3 is at E ', and the homogeneous transformation matrix of the coordinate system 3 is selected to calculate θ ₁ ，θ ₂ ，θ ₃ Is a function of the angle of (2);

e' position is determined, and the coordinate system 3 is also determined; the y3 axis of the coordinate system 3 is pointed at S by E', the z3 axis is perpendicular to the arm plane τ, and its orientation is pointed at θ ₄ Determining the sign of a calculation formula; when the orientations of the two coordinate axes of the coordinate system 3 are determined, a rotation matrix of the coordinate system 3 is determined;

The coordinates of the E point are determined by the arm plane τ: due to vectorsThe highest point E is coplanar with B, S, W;

the joint axes 2,4 at S, E are now parallel, obtained from the established D-H coordinate system; firstly, calculating the center coordinates of a redundant circumference on a SW straight line, then, making a directed line segment perpendicular to the straight line SW on a BSW plane and centered on the center, wherein the line segment length is the radius r of the redundant circumference, and the end point is the calculated highest point E;

(Vector)parallel to vector->And vertically upward; on the basis of which a new coordinate system is established：

With the redundant circumference center F as the origin, a new coordinate system Fx 'y' z 'and the y' axis are established in the vectorOn the z' axis in vector +.>The x ' axis is perpendicular to the plane BSW and is also perpendicular to the z ' and y ' axes; the redundant circumference is now in the plane Fx 'z', and the elbow E 'has a coordinate in the coordinate system Fx' y 'z' of [ m 0 n ] assuming that the arm angle is equal to θ]' then the coordinates of E in the base coordinate system:

x 'in formula (11)' _x 、X′ _y 、X′ _z The projection of the x 'axis of the coordinate system Fx' y 'z' on the x, y and z axes of the base coordinate system respectively; x'. _z 、Y′ _z 、Z′ _z Respectively projecting the three axes of the coordinate system Fx ' y ' z ' on the z axis of the base coordinate system; because Fx 'is perpendicular to the plane BSW, the projection of Fx' axis on the z axis of the base coordinate system is 0, the projection of Fy 'axis on the z axis of the base coordinate system is sin beta, the projection of Fz' axis on the z axis of the base coordinate system is cos beta, and beta is the fixed parameter angle after the terminal pose is determined, namely the known quantity;

[X′ _z Y′ _z Z′ _z ]＝[0 sinβ cosβ] (12)

In the case of introducing the arm angle θ, there is the following relationship:

the following relation is given by the combination of formulas (12) and (13):

E′ _z ＝r*cosβ*cosθ (14)

after the terminal pose is determined, the model is [ (]14 Indicating that the elbow position z-axis coordinate component takes a maximum value when the arm angle θ is 0; similarly, when the maximum and minimum values of the x component of the elbow coordinate are taken, only the vector is needed to be takenIs replaced by vector->The starting point is the point S, and the point E of the elbow joint is the intersection point of the plane SWN and the redundant circumference; similarly, the vector +.>Is replaced by vector->The same starting point is the point S, and the point E of the elbow joint is the intersection point of the plane SWN and the redundant circumference;

and (IV) calculating elbow joint coordinates E when the highest point is obtained:

the coordinates of the center point F of the redundant circle are represented by vectorsThe unit vector of the line segment SF, the length of the line segment SF and the coordinates of the point S are shown, the lengths of the line segment SE and the EW are respectively fixed values d3 and d5 and the coordinates of the point S and the point W are known, and then the coordinates of the point F can be solved in the two right triangles delta EFS and delta EFW;

solving to obtain the coordinates of the highest point E of the elbow joint: from vectorsSum vector->Vertical, vector->On the BSW plane, the vector +.>Is a direction of (2); by vectors- >Normal vector to planar BSW>Judging the direction vertically, and combining the coordinates of the point F to obtain the coordinates of the point E; firstly, obtaining the coordinate of the E point when the redundancy highest point, namely the arm angle theta is 0, and then obtaining the coordinate of the E point when the arm angle theta is at any angle through the generalized of the Rodrigues transformation;

the coordinates of the redundancy highest point E are finally obtained as:

(V) obtaining the rotation matrix of the coordinate system 3 by the coordinates of the E pointThrough the rotation matrix->Solving for theta ₁ 、θ ₂ 、θ ₃ Angle of (2)

The specific solving method of the coordinate system 3 is as follows:

the y-axis of the coordinate system 3 is pointed at S by E, the z-axis is perpendicular to the arm plane τ, and its pointing is determined primarily by equation (10), namelyThe sign selected in (3); when the directions of 2 coordinate axes of the coordinate system 3 are determined, the x-axis is obtained by cross multiplication of positive unit vectors of y-axis and z-axis; the positive directions of the three axes are determined, and the unit column vectors of the positive directions are sequentially arranged to form a rotation matrix of the coordinate system 3 at the highest point of the elbow joint relative to the basic coordinate system, and the rotation matrix is shown in a formula (18):

In formula (18)Is selected and->The signs are opposite;

when the elbow joint moves around the axis SW, namely when the arm angle theta is not equal to 0, the Rodrigos transformation is adopted; at this time, the rotation matrix calculation formula of the coordinate system 3 on the elbow joint is shown in formula (19):

in the formula (19) [ u× ]]Representative vectorAn antisymmetric matrix of unit vectors u, I ₃ A 3x3 identity matrix;

in addition, in the case of the optical fiber,representing a rotation matrix of the first three joints, < >>The other expression is shown in formula (20):

in the formulas (21) to (23),representative is->The ith row and jth column elements in the matrix;

(VI) algebraic method to find θ ₅ 、θ ₆ 、θ ₇ The values, solving method, are as follows:

6. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 1, wherein the method comprises the following steps: in the third step, the specific singular position type of the S-R-S configuration redundant mechanical arm is analyzed, and the following requirements are met:

1) Equivalent three degree of freedom solution

(2) on the premise that the three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the three points are also collinear with the end effector T, and when in the state, the three points are theta ₃ 、θ ₄ 、θ ₅ 、θ ₆ Are all 0; the robot arm in this case can be equivalent to a three-degree-of-freedom robot arm; the formulas of the angles of the joints to be solved are shown as (29) to (31):

2) Equivalent five degree of freedom solution

When three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the redundant circumference radius r is 0, and the large arm SE and the small arm EW of the mechanical arm have no self-movement property; at this time, angle θ of joint 4 ₄ Also equal to 0; the axis collineation of the joint 3 and the joint 5 is equivalent to a revolute pair, the joint space of the mechanical arm is at most 5 degrees of freedom, and the mechanical arm is equivalent to a five-degree-of-freedom mechanical arm;

the formulas of the angles of the joints to be solved are shown in (33) to (38):

7. the redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 6, wherein: in the third step of the process, the process is carried out,

From reverse transportStarting from the dynamic configuration, two singular states of the mechanical arm are analyzed and calculated; singular state analysis is as follows: when the three points of the shoulder joint S, the wrist joint W and the elbow joint E are collinear, the redundant circumference radius r is 0; the large arm SE and the small arm EW of the mechanical arm have no self-movement property, and the angle theta of the joint 4 is at the moment ₄ Also equal to 0; the axis collineation of the joint 3 and the joint 5 is equivalent to a revolute pair, and the joint space of the mechanical arm is at most 5 degrees of freedom; on the premise of ESW three-point collineation, when the straight line SW formed by the whole large arm and the small arm is collinearly with the BS again, then the joints 1, 2, 3, 4 and 5 are equivalent to a rotating pair, and theta ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are equal to 0, and the mechanical arm end effector has three degrees of freedom at most;

the configuration analysis of the iiwa-r800 obtains an odd-abnormal type, and the inverse kinematics model is failed at the moment because the mechanical arm is in the odd-abnormal type; when the inverse kinematics model fails, resolving the singular bit pattern, and establishing a singular bit pattern inverse solution based on equivalent degrees of freedom, wherein the specific process is as follows:

in the case of B, S, E, W being all collinear, the joints 1, 2, 3, 4, 5 are equivalent to one revolute pair, and θ ₁ 、θ ₂ 、θ ₃ 、θ ₄ The values of (2) are all equal to 0, the angles of the joints 5,6 and 7 are joint variables to be solved, namely the mechanical arm equivalent to three degrees of freedom at the moment, and the mechanical arm is constantIs a 3-order identity matrix; the angles of the joints 5,6,7 at this time are as shown in formulas (25) to (27):

when the angle theta of the joint 6 ₆ When=0, the joints 5,6,7 are equivalent to a pair of rotations, so that the angle θ of the joint 5 ₅ =0; only the angle of the joint 7 is the quantity to be solved, the others are all 0; the redundant mechanical arm is equivalent to a free mechanical arm;

equivalent three-degree-of-freedom solution

(1) In the case where B, S, E, W are all collinear, the joints 1,2,3,4,5 are equivalent to a revolute pair, and θ ₁ ，θ ₂ ，θ ₃ ，θ ₄ The values of (2) are all equal to 0; the angles of the joints 5,6,7 are the joint variables to be solved, i.e. the mechanical arm equivalent to the mechanical arm is equivalent to the mechanical arm with three degrees of freedom, and the formulas (24) - (27) are all established at the moment, but in the formula (24), the constantI ₃ Is a 3-order identity matrix; when the angle theta of the joint 6 ₆ When=0, the joints 5,6,7 are equivalent to a pair of rotations, so that the angle θ of the joint 5 ₅ =0, only the angle of joint 7 is the amount to be solved, the others are all 0; the redundant mechanical arm is equivalent to a free mechanical arm;

(2) Another 3-degree-of-freedom mechanism: is also collinear with the end effector T on the premise of being collinear with S, E and W, in this state theta ₃ ，θ ₄ ，θ ₅ ，θ ₆ Are all 0; the mechanical arm in the case can be equivalent to a mechanical arm with three degrees of freedom, and the angles of the joints 1,2 and 7 are joint variables to be solved; by observing the standard D-H coordinate system, the directions of the axes of the coordinate system 2 and the coordinate system 6 are completely the same under the zero joint angle, and the values of 1,2 and 7 can be obtained as shown in the formulas (29) to (31):

(two) solution of equivalent five degrees of freedom

Equivalent five degrees of freedom, i.e. E, S, W three points collinear with SW, and SW is not collinear with BS, WT, at this point θ ₃ ，θ ₄ =0, the angles of joints 1,2, 5, 6, 7 are the quantities to be solved, which can be obtained by observing a standard D-H coordinate system, coordinate system 2 and coordinate system 4 are identical in their axes at zero joint angle, so joints 1,2, 5 are equivalent to the solutions of joints 1,2, 3;

from the following componentsThe five elements of the second column and the third row of (2) give θ ₁ 、θ ₂ 、θ ₅ The calculation formulas of (a) are shown in formulas (33) to (35):

solving the angles of the joints 1,2 and 5 to obtain theta ₆ And theta ₇ As shown in formulas (37), (38):

when the mechanical arm is in an odd abnormal position, whether the equivalent degree of freedom can meet the operation requirement can be judged according to the actual situation; if the condition of specific odd-abnormal type can be met, the operation is completed by utilizing the inverse solution of the equivalent degree of freedom, otherwise, the angle value of the singular state is omitted, the angle value at the previous moment or the program error reporting target point is not reachable, and the danger of the mechanical arm in the singular state is avoided.

8. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 1, wherein the method comprises the following steps: in step four: when a desired Cartesian track is input, judging whether the track is a specific odd-abnormal type or not; if the special type is adopted, an equivalent degree of freedom algorithm is adopted, and if the special type is not adopted, an arm type angle parameterization algorithm is adopted, and finally the track is output, so that the effect of avoiding the special type is achieved.

9. The redundant manipulator kinematics optimization method based on the equivalent degree of freedom method according to claim 8, wherein: step four also meets the following requirements:

the specific requirements of the kinematics model verification are as follows:

(1) positive kinematics verification

The positive kinematics verification method is to compare the calculation result with the positive kinematics solution solved by the robot tool kit to see whether the result is correct; the method comprises the following steps of: d1, d3, d5, d7;

because positive kinematics is a homogeneous matrix of end-to-base coordinates, given joint angle values, several sets of joint angle values are randomly substituted, where the substituted angle values are [0, pi/5,0, pi/4,0], [ pi/2,0, pi/2,0, pi/4,0], [0, pi/2,0, pi/5,0, pi/4,0], [0, pi/5,0, pi/4, pi/2], in units, respectively; substituting the four groups of joint angle values into results T1, T2, T3 and T4 obtained in the kinematic algorithm;

Solving a positive kinematic function xxx.fkine (q) by using the robot toolbox, wherein q represents the joint angle; substituting the four joint angle values into the function to obtain homogeneous matrixes T1_, T2_, T3_, T4_;

(2) inverse kinematics validation

Similarly, the inverse kinematics verifies the calculation principle, so that complete calculation of inverse kinematics solution can be realized, and the position and the gesture at the tail end of the working space domain can be randomly determined;

given a set of joint angle values q= [ 60-20 10 45 0 90 0 ]. Pi/180, unit rad; then, a homogeneous matrix T_S of the end relative to the base coordinate is obtained by utilizing a fkine positive kinematic function, the obtained homogeneous matrix T_D is substituted into a written inverse kinematic algorithm as input, and the adopted inverse solution algorithm is arm angle parameterization due to a redundant mechanical arm, and the range of theta obtained by motion analysis is [0,2 pi ];

firstly, verifying all inverse solutions when the self-motion variable theta is a fixed value, selecting theta=0, and obtaining 8 groups of different inverse solutions by the method;

selecting any group of inverse solutions to be substituted into the positive kinematic function to obtain a new homogeneous matrix T_D_;

the comparison results are identical, and the correctness of inverse kinematics solution when θ=0 is verified;

(3) Odd-type inverse solution verification

When the joint angle is q= [0 45 0 0 0 45 0 ]. Pi/180, the mechanical arm is in a special singular position type, the inverse solution of q= [0 45 0 0 0 45 0 ]. Pi/180 is verified, the kinematic positive solution of q= [0 45 0 0 0 45 0 ]. Pi/180 is firstly obtained, and the fkine positive kinematic function is utilized to obtain a homogeneous matrix T_S of the terminal relative base coordinates;

substituting T_S as input into a written inverse kinematics algorithm to obtain an inverse solution of the redundant mechanical arm, and substituting any group of inverse solutions into a positive kinematics model to obtain a homogeneous matrix T_S_;

10. The redundant manipulator kinematic optimization based on the equivalent degree of freedom method of one of claims 4, 5, 6, 7, 9, wherein: step four also meets the following requirements:

through combining a kinematic model and inverse solution under a special odd-type, the redundant mechanical arm with the S-R-S general structure can normally move under the specific odd-type when the arm angle theta exists or when the arm angle theta does not exist, and at the moment, the mechanical arm has stable and controllable kinematic performance, so that the effect of avoiding the singular is realized.