CN113510690B - Inverse kinematics solving method and system for four-degree-of-freedom series robot - Google Patents
Inverse kinematics solving method and system for four-degree-of-freedom series robot Download PDFInfo
- Publication number
- CN113510690B CN113510690B CN202110341066.7A CN202110341066A CN113510690B CN 113510690 B CN113510690 B CN 113510690B CN 202110341066 A CN202110341066 A CN 202110341066A CN 113510690 B CN113510690 B CN 113510690B
- Authority
- CN
- China
- Prior art keywords
- joint
- robot
- solution
- module
- solving
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/08—Programme-controlled manipulators characterised by modular constructions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J17/00—Joints
- B25J17/02—Wrist joints
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J18/00—Arms
- B25J18/02—Arms extensible
- B25J18/025—Arms extensible telescopic
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
Abstract
The invention provides an inverse kinematics solving method and system for a four-degree-of-freedom series robot, which comprises the steps of establishing a coordinate system of each connecting rod, solving a kinematics positive solution of the robot, solving the rotation angle/displacement of each joint of the robot according to the tail end pose of the robot, alarming for the over travel error of the joint, and carrying out travel constraint on each joint of the robot. The method can uniquely determine the inverse solution of the robot according to the position form of the robot, judge the accessibility of the target position and pose, solve the corresponding inverse solution according to the expected position form of the robot, realize the controllability of the inverse solution under the condition of multiple solutions, and be beneficial to path planning and obstacle avoidance.
Description
Technical Field
The invention relates to the technical field of robot kinematics and robot motion control, in particular to a four-degree-of-freedom series robot inverse kinematics solving method and system.
Background
The inverse kinematics solving problem of the robot is an important problem in the field of robotics, and is a premise and basis for researching problems such as robot trajectory planning, motion control, dynamics analysis and the like. The solution method of the robot inverse kinematics is generally divided into a numerical solution and a closed solution. The numerical solution is a general solution for solving the inverse kinematics of the robot, but due to the iterative nature of the numerical solution, the numerical solution has large calculation amount and slow solving speed, and cannot ensure the solving precision and solve all possible inverse kinematics. The closed solution, namely, an analytic solution of the kinematic inverse solution of the robot is obtained, the calculation speed is high, the precision is high, all possible inverse solutions can be obtained, but the closed solution is only suitable for the robot with a specific configuration, and the universality is poor.
Patent document CN111113425A (application number: CN201911407238.5) discloses an inverse solution method for kinematics of a five-degree-of-freedom series-parallel robot with parasitic motion, which establishes an equivalent mechanism of a three-degree-of-freedom parallel mechanism, converts an original series-parallel robot into a five-degree-of-freedom series mechanism consisting of the equivalent mechanism and two series joints, sets a degree of freedom of a tail end tool in a direction of Z-axis rotation of a base coordinate system as parasitic motion, makes the parasitic motion not participate in calculation, reduces the number of equations, obtains equations of equivalent joint variables and series joint driving variables, position coordinates of the tail end tool and 2 euler angles by solving a nonlinear equation set, and finally obtains the relationship between the equivalent joint variables and the driving variables of the parallel three-degree-of-freedom mechanism by using a vector analysis method.
At present, an inverse kinematics closed solution for a six-degree-of-freedom series robot meeting the Pieper criterion is mature, but the method cannot be directly applied to a four-degree-of-freedom series robot. According to the configuration characteristics of the four-degree-of-freedom robot, multiple solutions exist in the inverse kinematics solution, and a plurality of specific motion constraint conditions exist in the operation space. Therefore, in order to realize the fast, accurate and reliable inverse kinematics solution of the four-degree-of-freedom robot, an inverse kinematics closed solution considering the position and the motion constraint of the robot needs to be researched.
Disclosure of Invention
Aiming at the defects in the prior art, the invention aims to provide a four-degree-of-freedom series robot inverse kinematics solving method and system.
The inverse kinematics solving method of the four-freedom-degree series robot provided by the invention comprises the following steps:
the method comprises the following steps: establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint, wherein the four-degree-of-freedom series robot is formed by connecting four connecting rods in series to form four joints, and the four joints are sequentially as follows: the device comprises a rotary arm-joint 1, a big arm-joint 2, an external knuckle arm-joint 3 and a telescopic arm-joint 4, wherein the joint 1, the joint 2 and the joint 3 are rotary joints, and the joint 4 is a translation joint;
step two: solving a kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of a robot end coordinate system expressed by each joint kinematics parameter relative to a base coordinate system;
step three: solving a corresponding homogeneous transformation matrix according to the input robot end pose vector;
step four: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the constraint conditions are met, performing the subsequent steps; otherwise, the input terminal pose is not reachable, and the inverse solution is not solved;
step five: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta of the joint 1 1 ；
Step six: substituting theta obtained in the step five 1 Separating variable processing is carried out on the homogeneous transformation matrix expression in the step two to obtain displacement d related to the joint 4 4 If the equation has no real number solution, the input terminal pose is unreachable, and the inverse solution has no solution; if the equation has a real solution, the corresponding solution is selected as the displacement d of the joint 4 according to the desired robot configuration 4 ；
Step seven: substituting theta obtained in the step five 1 And d obtained in step six 4 And calculating the rotation angle theta of the joint 2 from the homogeneous transformation matrix expression in the step two 2 The sine value and the cosine value of the joint 2 are solved, and the rotation angle theta of the joint 2 is further solved 2 ；
Step eight: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta and the angle theta of the joint 2 and the joint 3 2 +θ 3 ；
Step nine: theta obtained in the seventh step 2 Substituting theta obtained in the step eight 2 +θ 3 Solving the rotation angle theta of the joint 3 3 ；
Step ten: judging the theta obtained by solving 1 、θ 2 、θ 3 And d 4 And if the robot exceeds the range, alarming the over-travel error of the joint, and restricting the travel of each joint of the robot.
Preferably, the homogeneous transformation matrix expression of the end coordinate system relative to the base coordinate system is:
wherein, c i Represents cos θ i ，s i Denotes sin θ i ，s 23 Denotes sin (θ) 2 +θ 3 )，c 23 Represents cos (θ) 2 +θ 3 )，a 1 、a 2 、d 1 、d 3 、d 4 、d 5 Is a constant.
Preferably, the third step includes: the input robot end pose vector is: [ p ] x p y p z γ β α]Then, the homogeneous transformation matrix corresponding to the pose vector is calculated according to the following formula:
p x 、p y 、p z respectively representing x, y and z position coordinates of the tail end of the robot under a base coordinate system; gamma, beta and alpha respectively represent attitude angles of the tail end of the robot around x, y and z axes of the base coordinate system; n is x 、n y 、n z Respectively representing the direction cosine of the x axis of the robot tail end coordinate system under the base coordinate system; o x 、o y 、o z Respectively representing the direction cosine of the y axis of the robot tail end coordinate system under the base coordinate system; a is x 、a y 、a z Respectively representing the direction cosine of the z axis of the robot tail end coordinate system under the base coordinate system; c denotes the cosine function cos (); s represents a sine functionsin()。
Preferably, the motion constraint of the four-degree-of-freedom series robot is as follows:
the input position and posture satisfy the constraint condition of equation (3).
Preferably, it is obtained from formula (1) and formula (2):
thus theta 1 Directly solved by:
θ 1 ＝Atan2(-o x ,o y )……(5)
wherein Atan2(x, y) is a two-parameter arctangent function.
Preferably, with respect to d 4 The first order quadratic equation of (a) is as follows:
wherein:
K 2 ＝d 1 +d 5 a z -p z ……(8)
if the equation has no real number solution, the input end pose is not reachable; if there is a real number solution, d can be obtained according to the root-solving formula of the quadratic equation of one element 4 The solution of (1);
according to the given robot configuration, selecting the corresponding solution as d 4 Is determined uniquely, let solution of equation be d' 4 And d ″) 4 If the shape is a short arm shape:
if the arm is long, then:
at d 4 In the case of a real solution, continue to calculate θ 2 And theta 3 If d is a value of 4 If no solution exists, the solution is quitted and an error is reported.
Preferably, d to be determined 4 Substituting formula (1) to obtain:
thus calculating theta 2 The value of (c):
θ 2 ＝Atan2(s 2 ,c 2 )……(12)。
preferably, the combined type (1) and the formula (2) have the following components:
thus, θ 2 +θ 3 The value of (d) is calculated by:
θ 2 +θ 3 ＝Atan2(s 23 ,c 23 )＝Atan2(a z ,n z )……(14)。
preferably, theta 3 The value of (d) is calculated by:
θ 3 ＝Atan2(a z ,n z )-Atan2(s 2 ,c 2 )……(15)。
the four-degree-of-freedom series robot inverse kinematics solving system provided by the invention comprises the following modules:
module M1: establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint, wherein the four-degree-of-freedom series robot is formed by connecting four connecting rods in series to form four joints, and the four joints are sequentially as follows: the device comprises a rotary arm-joint 1, a large arm-joint 2, an external knuckle arm-joint 3 and a telescopic arm-joint 4, wherein the joint 1, the joint 2 and the joint 3 are rotary joints, and the joint 4 is a translation joint;
module M2: solving a kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of a robot end coordinate system relative to a base coordinate system represented by each joint kinematics parameter;
module M3: solving a corresponding homogeneous transformation matrix according to the input robot end pose vector;
module M4: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the constraint conditions are met, performing a subsequent module; otherwise, the input terminal pose is not reachable, and the inverse solution is not solved;
module M5: simultaneous module M2 and module M3 are used for solving the rotation angle theta of the joint 1 1 ；
Module M6: substituting the obtained theta into a module M5 1 Separating variable processing is carried out on the homogeneous transformation matrix expression of the module M2 to obtain the displacement d of the joint 4 4 If the equation has no real number solution, the input terminal pose is not reachable, and the inverse solution has no solution; if the equation has a real number solution, then the corresponding solution is selected as the displacement d of the joint 4 according to the expected robot configuration 4 ；
Module M7: substituting the determined theta in the module M5 1 D determined by module M6 4 The rotation angle theta of the joint 2 is obtained from the homogeneous transformation matrix expression of the module M2 2 The sine value and the cosine value of the joint 2 are further solved 2 ；
Module M8: simultaneous module M2 and module M3 are used for solving the rotation angle sum theta of joint 2 and joint 3 2 +θ 3 ；
Module M9: theta obtained by module M7 2 Substituting the obtained theta into a module M8 2 +θ 3 Solving the rotation angle theta of the joint 3 3 ；
Module M10: judging the solved theta 1 、θ 2 、θ 3 And d 4 And if the robot exceeds the range, alarming the over-travel error of the joint, and restricting the travel of each joint of the robot.
Compared with the prior art, the invention has the following beneficial effects:
(1) the method can solve the closed solutions of all possible inverse solutions of the four-degree-of-freedom series robot, and has high solving speed and high precision;
(2) the method can uniquely determine the inverse solution of the robot according to the position form of the robot, judge the accessibility of the target pose, solve the corresponding inverse solution according to the expected position form of the robot, realize the controllability of the inverse solution under the condition of multiple solutions, and be beneficial to path planning and obstacle avoidance;
(3) the invention considers the motion constraint of the four-degree-of-freedom robot in the operation space, and can eliminate the influence of unreasonable target poses on the algorithm;
(4) the invention considers the travel constraint of each joint of the robot and can alarm the over travel error of the joint.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments with reference to the following drawings:
FIG. 1 is a schematic structural diagram of a four-degree-of-freedom series robot of the type to which the present invention is applicable for solving;
FIG. 2 is a schematic diagram of a link coordinate system establishing method and D-H kinematic parameter definition adopted in the present invention;
fig. 3 is a schematic diagram of a link coordinate system of a four-degree-of-freedom tandem robot established by the present invention.
Detailed Description
The present invention will be described in detail with reference to specific examples. The following examples will aid those skilled in the art in further understanding the present invention, but are not intended to limit the invention in any manner. It should be noted that variations and modifications can be made by persons skilled in the art without departing from the concept of the invention. All falling within the scope of the present invention.
Example 1:
the invention is realized by the following technical scheme: a solving method for inverse kinematics of a four-freedom-degree series robot is a method for solving the rotation angle/displacement of each joint of the robot according to the tail end pose of the robot by considering the pose and the motion constraint of the robot, and is a closed solution capable of solving all inverse solutions of a class of four-freedom-degree series robots.
The four-degree-of-freedom serial robot is characterized in that four joints are formed by connecting four connecting rods in series, and sequentially comprise a rotary arm (joint 1), a large arm (joint 2), an outer joint arm (joint 3) and a telescopic arm (joint 4), wherein the joint 1, the joint 2 and the joint 3 are rotary joints, and the joint 4 is a translation joint.
The inputs to the method are the desired pose of the robot's end coordinate system relative to the base coordinate system, and the desired pose of the robot (short arm pose or long arm pose).
The method comprises the following steps:
the method comprises the following steps: establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint, as shown in figure 2;
step two: solving a kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of a robot end coordinate system relative to a base coordinate system represented by each joint kinematics parameter;
step three: solving a corresponding homogeneous transformation matrix according to the input robot terminal pose vector;
step four: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the input terminal pose meets the constraint conditions, performing the subsequent steps; otherwise, the input terminal pose is not reachable, and the inverse solution is not solved;
step five: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta of the joint 1 1 ；
Step six: substituting theta obtained in the step five 1 Separating variable processing is carried out on the homogeneous transformation matrix expression in the step twoObtaining a displacement d about the joint 4 4 If the equation has no real number solution, the input terminal pose is unreachable, and the inverse solution has no solution; if the equation has a real number solution, an appropriate solution is selected as the displacement d of the joint 4 according to the expected robot configuration 4 ；
Step seven: substituting theta obtained in the step five 1 And d obtained in step six 4 And calculating the rotation angle theta of the joint 2 from the homogeneous transformation matrix expression in the step two 2 The sine value and the cosine value of the joint 2 are further solved 2 ；
Step eight: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta and the angle theta of the joint 2 and the joint 3 2 +θ 3 ；
Step nine: theta obtained in the seventh step 2 Substituting theta obtained in the step eight 2 +θ 3 Solving the rotation angle theta of the joint 3 3 ；
Step ten: judging the theta obtained by solving 1 、θ 2 、θ 3 And d 4 Whether over travel is to be exceeded.
Example 2:
example 2 is a preferred example of example 1.
The inverse kinematics solving method of the four-degree-of-freedom series robot is suitable for the four-degree-of-freedom series robot shown in figure 1. Wherein, the joint 1 is a rotary joint rotating around a vertical shaft; the joint 2 is parallel to the joint 3 and is a rotary joint pitching around a horizontal shaft; the joint 4 is a translational joint, and the translational direction is perpendicular to the rotation axis of the joint 3.
The invention relates to an inverse kinematics solving method of a four-degree-of-freedom series robot, which comprises the following steps of:
the method comprises the following steps: and establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint.
A connecting rod coordinate system of the four-degree-of-freedom serial robot is established, as shown in fig. 3, wherein a coordinate system 1 is a base coordinate system of the robot, a coordinate system f is a terminal coordinate system of the robot, and a coordinate system 5 is a transition coordinate system.
The D-H parameters corresponding to each link coordinate system are shown in Table 1, wherein the parameter theta 1 、θ 2 、θ 3 、d 4 Are variables and the other parameters are constants.
TABLE 1
Step two: and solving the kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of the robot end coordinate system relative to the base coordinate system represented by each joint kinematics parameter.
According to the D-H kinematic parameters shown in the table 1, a homogeneous transformation matrix of the terminal coordinate system relative to the base coordinate system is obtained by calculation:
wherein, c i Represents cos θ i ，s i Denotes sin θ i ，s 23 Denotes sin (θ) 2 +θ 3 )，c 23 Represents cos (θ) 2 +θ 3 )。
Step three: and solving a corresponding homogeneous transformation matrix according to the input robot terminal pose vector.
Let the input robot end pose vector be [ p ] x p y p z γ β α]Then, the homogeneous transformation matrix corresponding to the pose vector is calculated according to the following formula:
step four: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the input terminal pose meets the constraint conditions, performing the subsequent steps; otherwise, the input terminal pose is not reachable, and the inverse solution has no solution.
The motion constraints of the four-degree-of-freedom series robot shown in fig. 1 are:
i.e. the input position and posture should satisfy the constraint of equation (3).
Step five: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the corner theta of the joint 1 1 。
The following formulas (1) and (2) show that:
thus theta 1 This can be solved directly by:
θ 1 ＝Atan2(-o x ,o y )……(5)
wherein Atan2(x, y) is a two-parameter arctangent function.
Step six: substituting theta obtained in the step five 1 Separating variable processing is carried out on the homogeneous transformation matrix expression in the step two to obtain displacement d related to the joint 4 4 If the equation has no real number solution, the input terminal pose is not reachable, and the inverse solution has no solution; if the equation has a real solution, then a suitable solution is selected as the displacement d of the joint 4 according to the desired robot configuration 4 。
With respect to d 4 The first order quadratic equation of (a) is as follows:
wherein:
K 2 ＝d 1 +d 5 a z -p z ……(8)
if the equation has no real number solution, the input end pose is not reachable; if there is a real number solution, d can be obtained according to the root-solving formula of the quadratic equation of one element 4 The solution of (c).
According to the given robot configuration, selecting the corresponding solution as d 4 Is uniquely determined. Let the solutions of equations be d' 4 And d ″) 4 If the robot is a short arm configuration (i.e., the telescopic arm of the robot is short), then:
if the robot has a long arm configuration (i.e. the telescopic arm of the robot is long), then:
at d 4 With a real solution, the calculation of θ can continue 2 And theta 3 The value of (c). If d is 4 If no solution exists, the solution is quitted and an error is reported.
Step seven: substituting theta obtained in the step five 1 And d obtained in step six 4 And calculating the rotation angle theta of the joint 2 from the homogeneous transformation matrix expression in the step two 2 The sine value and the cosine value of the joint 2 are further solved 2 。
D to be obtained 4 Substituting formula (1) to obtain:
therefore, θ can be calculated 2 The value of (c):
θ 2 ＝Atan2(s 2 ,c 2 )……(12)
step eight: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta and the rotation angle theta of the joint 2 and the joint 3 2 +θ 3 。
The united type (1) and formula (2) have:
thus, θ 2 +θ 3 The value of (d) can be calculated by:
θ 2 +θ 3 ＝Atan2(s 23 ,c 23 )＝Atan2(a z ,n z )……(14)
step nine: theta obtained in the seventh step 2 Substituting theta obtained in the step eight 2 +θ 3 Solving for the rotation angle theta of the joint 3 3 。
θ 3 Is calculated by the following formula:
θ 3 ＝Atan2(a z ,n z )-Atan2(s 2 ,c 2 )……(15)
step ten: judging the solved theta 1 、θ 2 、θ 3 And d 4 Whether over travel is to be exceeded.
In the description of the present application, it is to be understood that the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", and the like indicate orientations or positional relationships based on those shown in the drawings, and are only for convenience in describing the present application and simplifying the description, but do not indicate or imply that the referred device or element must have a specific orientation, be constructed in a specific orientation, and be operated, and thus, should not be construed as limiting the present application.
Those skilled in the art will appreciate that, in addition to implementing the systems, apparatus, and various modules thereof provided by the present invention in purely computer readable program code, the same procedures can be implemented entirely by logically programming method steps such that the systems, apparatus, and various modules thereof are provided in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
The foregoing description has described specific embodiments of the present invention. It is to be understood that the present invention is not limited to the specific embodiments described above, and that various changes or modifications may be made by one skilled in the art within the scope of the appended claims without departing from the spirit of the invention. The embodiments and features of the embodiments of the present application may be combined with each other arbitrarily without conflict.
Claims (5)
1. A four-degree-of-freedom series robot inverse kinematics solving method is characterized by comprising the following steps:
the method comprises the following steps: establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint, wherein the four-degree-of-freedom series robot is formed by connecting four connecting rods in series to form four joints, and the four joints are sequentially as follows: the device comprises a rotary arm-joint 1, a large arm-joint 2, an external knuckle arm-joint 3 and a telescopic arm-joint 4, wherein the joint 1, the joint 2 and the joint 3 are rotary joints, and the joint 4 is a translation joint;
step two: solving a kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of a robot end coordinate system relative to a base coordinate system represented by each joint kinematics parameter;
step three: solving a corresponding homogeneous transformation matrix according to the input robot terminal pose vector;
step four: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the constraint conditions are met, performing the subsequent steps; otherwise, the input end pose is not reachable, and the inverse solution is not available;
step five: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta of the joint 1 1 ；
Step six: substituting theta obtained in the step five 1 Separating variable processing is carried out on the homogeneous transformation matrix expression in the step two to obtain displacement d related to the joint 4 4 If the equation has no real number solution, the input terminal pose is unreachable, and the inverse solution has no solution; if the equation has a real number solution, then the corresponding solution is selected as the displacement d of the joint 4 according to the expected robot configuration 4 ；
Step seven: substituting theta obtained in the step five 1 And d obtained in step six 4 And calculating the rotation angle theta of the joint 2 from the homogeneous transformation matrix expression in the step two 2 The sine value and the cosine value of the joint 2 are solved, and the rotation angle theta of the joint 2 is further solved 2 ；
Step eight: simultaneously establishing a homogeneous transformation matrix expression in the second step and a homogeneous transformation matrix in the third step, and solving the rotation angle theta and the angle theta of the joint 2 and the joint 3 2 +θ 3 ；
Step nine: theta obtained in the seventh step 2 Substituting theta obtained in the step eight 2 +θ 3 Solving the rotation angle theta of the joint 3 3 ；
Step ten: judging the theta obtained by solving 1 、θ 2 、θ 3 And d 4 Whether the robot exceeds the range or not, alarming for the over-range error of the joints and carrying out stroke constraint on each joint of the robot;
the homogeneous transformation matrix expression of the end coordinate system relative to the base coordinate system is:
wherein, c i Represents cos θ i ，s i Denotes sin θ i ，s 23 Denotes sin (θ) 2 +θ 3 )，c 23 Represents cos (θ) 2 +θ 3 )，a 1 、a 2 、d 1 、d 3 、d 4 、d 5 Is a constant;
the third step comprises: the input robot end pose vector is: [ p ] x p y p z γ β α]Then, the homogeneous transformation matrix corresponding to the pose vector is calculated according to the following formula:
p x 、p y 、p z respectively representing x, y and z position coordinates of the tail end of the robot under a base coordinate system; gamma, beta and alpha respectively represent attitude angles of the tail end of the robot around x, y and z axes of the base coordinate system; n is x 、n y 、n z Respectively representing the direction cosine of the x axis of the robot tail end coordinate system under the base coordinate system; o. o x 、o y 、o z Respectively representing the direction cosines of the y axis of the robot tail end coordinate system under the base coordinate system; a is x 、a y 、a z Respectively representing the direction cosine of the z axis of the robot tail end coordinate system under the base coordinate system; c represents the cosine function cos (); s denotes a sine function sin ();
the motion constraint of the four-freedom-degree series robot is as follows:
the input position and the input posture meet the constraint condition of the formula (3);
obtained from formula (1) and formula (2):
thus theta 1 Directly solved by:
θ 1 ＝Atan2(-o x ,o y )……(5)
wherein Atan2(x, y) is a two-parameter arctangent function;
with respect to d 4 The first order quadratic equation of (a) is as follows:
wherein:
K 2 ＝d 1 +d 5 a z -p z ……(8)
if the equation has no real number solution, the input end pose is not reachable; if a real number solution exists, d can be obtained according to a root equation of a quadratic equation with one element 4 The solution of (1);
according to the given robot configuration, selecting the corresponding solution as d 4 Is determined uniquely, let solution of equation be d' 4 And d ″) 4 If the shape is a short arm shape:
if the arm is long, then:
at d 4 In the case of a real solution, continue to calculate θ 2 And theta 3 If d is a value of 4 If no solution exists, the solution is quitted and an error is reported.
2. The inverse kinematics solution method according to claim 1, wherein the determined d is 4 Substituting formula (1) to obtain:
thus calculating theta 2 The value of (c):
θ 2 ＝Atan2(s 2 ,c 2 )……(12)。
3. the inverse kinematics solution for a four degree-of-freedom series robot according to claim 2, wherein the equations (1) and (2) are jointly established by:
thus, θ 2 +θ 3 The value of (d) is calculated by:
θ 2 +θ 3 ＝Atan2(s 23 ,c 23 )＝Atan2(a z ,n z )……(14)。
4. the inverse kinematics solution method according to claim 3, wherein θ is 3 Is calculated by the following formula:
θ 3 ＝Atan2(a z ,n z )-Atan2(s 2 ,c 2 )……(15)。
5. the four-degree-of-freedom series robot inverse kinematics solving system is characterized in that the four-degree-of-freedom series robot inverse kinematics solving method of any one of claims 1 to 4 is adopted, and comprises the following modules:
module M1: establishing a coordinate system of each connecting rod and determining D-H kinematic parameters of each joint, wherein the four-freedom-degree series robot is formed by connecting four connecting rods in series to form four joints, and the four joints are sequentially as follows: the device comprises a rotary arm-joint 1, a large arm-joint 2, an external knuckle arm-joint 3 and a telescopic arm-joint 4, wherein the joint 1, the joint 2 and the joint 3 are rotary joints, and the joint 4 is a translation joint;
module M2: solving a kinematics positive solution of the robot to obtain a homogeneous transformation matrix expression of a robot end coordinate system expressed by each joint kinematics parameter relative to a base coordinate system;
module M3: solving a corresponding homogeneous transformation matrix according to the input robot terminal pose vector;
module M4: judging whether the input terminal pose can be reached or not according to the constraint conditions of the terminal position and the pose, and if the constraint conditions are met, performing a subsequent module; otherwise, the input terminal pose is not reachable, and the inverse solution is not solved;
module M5: simultaneous module M2 and module M3 are used for solving the rotation angle theta of the joint 1 1 ；
Module M6: substituting the determined theta in the module M5 1 Separating variable processing is carried out on the homogeneous transformation matrix expression of the module M2 to obtain the displacement d of the joint 4 4 If the equation has no real number solution, the input terminal pose is unreachable, and the inverse solution has no solution; if the equation has a real solution, the corresponding solution is selected as the displacement d of the joint 4 according to the desired robot configuration 4 ；
Module M7: substituting the determined theta in the module M5 1 D determined by module M6 4 The rotation angle theta of the joint 2 is obtained from the homogeneous transformation matrix expression of the module M2 2 The sine value and the cosine value of the joint 2 are solved, and the rotation angle theta of the joint 2 is further solved 2 ；
Module M8: simultaneous module M2 and module M3 are used for solving the rotation angle sum theta of joint 2 and joint 3 2 +θ 3 ；
Module M9: theta obtained by module M7 2 Substituting the determined theta in the module M8 2 +θ 3 Solving the rotation angle theta of the joint 3 3 ；
Module M10: judging the theta obtained by solving 1 、θ 2 、θ 3 And d 4 And if the robot is over-travel, alarming the joint over-travel error, and restricting the travel of each joint of the robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110341066.7A CN113510690B (en) | 2021-03-30 | 2021-03-30 | Inverse kinematics solving method and system for four-degree-of-freedom series robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110341066.7A CN113510690B (en) | 2021-03-30 | 2021-03-30 | Inverse kinematics solving method and system for four-degree-of-freedom series robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113510690A CN113510690A (en) | 2021-10-19 |
| CN113510690B true CN113510690B (en) | 2022-07-26 |
Family
ID=78061283
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110341066.7A Active CN113510690B (en) | 2021-03-30 | 2021-03-30 | Inverse kinematics solving method and system for four-degree-of-freedom series robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113510690B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113925742B (en) * | 2021-10-20 | 2022-06-21 | 南通大学 | Control method and control system of target-driven upper limb exoskeleton rehabilitation robot |
| CN113997288B (en) * | 2021-11-01 | 2023-11-21 | 武汉科技大学 | Numerical algorithm for solving inverse kinematics of aspheric wrist 6R industrial robot |
| CN114750153B (en) * | 2022-04-13 | 2024-03-19 | 上海电气集团股份有限公司 | Motion control system for robot arm, cooperative robot and storage medium |
| CN117077498B (en) * | 2023-10-16 | 2024-02-09 | 季华实验室 | Modeling method and device for compliant parallel mechanism, electronic equipment and storage medium |
| CN117428791B (en) * | 2023-12-21 | 2024-03-01 | 江西求是高等研究院 | Inverse kinematics solving method and system for shoulder four-axis rehabilitation robot |
| CN117681213A (en) * | 2024-02-04 | 2024-03-12 | 之江实验室 | Method and device for planning and evaluating motion trail of industrial robot |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104535027A (en) * | 2014-12-18 | 2015-04-22 | 南京航空航天大学 | Robot precision compensation method for variable-parameter error recognition |
| CN106228260A (en) * | 2016-01-26 | 2016-12-14 | 西北工业大学 | A kind of planar three freedom robot for space inverse kinematics method |
| CN110457647A (en) * | 2019-07-29 | 2019-11-15 | 上海机电工程研究所 | A kind of rotary missile bullet mesh experience time estimation method |
| CN111360825A (en) * | 2020-02-28 | 2020-07-03 | 上海航天控制技术研究所 | Inverse kinematics resolving method and medium for serial six-degree-of-freedom mechanical arm |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8903546B2 (en) * | 2009-08-15 | 2014-12-02 | Intuitive Surgical Operations, Inc. | Smooth control of an articulated instrument across areas with different work space conditions |
-
2021
- 2021-03-30 CN CN202110341066.7A patent/CN113510690B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104535027A (en) * | 2014-12-18 | 2015-04-22 | 南京航空航天大学 | Robot precision compensation method for variable-parameter error recognition |
| CN106228260A (en) * | 2016-01-26 | 2016-12-14 | 西北工业大学 | A kind of planar three freedom robot for space inverse kinematics method |
| CN110457647A (en) * | 2019-07-29 | 2019-11-15 | 上海机电工程研究所 | A kind of rotary missile bullet mesh experience time estimation method |
| CN111360825A (en) * | 2020-02-28 | 2020-07-03 | 上海航天控制技术研究所 | Inverse kinematics resolving method and medium for serial six-degree-of-freedom mechanical arm |
Non-Patent Citations (1)
| Title |
|---|
| 曾远帆等.面向飞机装配的机器人定位误差和残差补偿.《航空学报》.2017,第38卷(第4期),420538-1-420538-11 . * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113510690A (en) | 2021-10-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113510690B (en) | Inverse kinematics solving method and system for four-degree-of-freedom series robot | |
| Gong et al. | Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator | |
| EP1728600A1 (en) | Controlling the trajectory of an effector | |
| CN108908347B (en) | Fault-tolerant repetitive motion planning method for redundant mobile mechanical arm | |
| CN107791248B (en) | Control method of six-degree-of-freedom series robot based on criterion of not meeting Pieper | |
| CN116038702B (en) | Seven-axis robot inverse solution method and seven-axis robot | |
| CN113119109A (en) | Industrial robot path planning method and system based on pseudo-distance function | |
| Tang et al. | Hierarchical design, laboratory prototype fabrication and machining tests of a novel 5-axis hybrid serial-parallel kinematic machine tool | |
| CN114055467B (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
| CN111673739A (en) | Robot pose reachability judgment method based on RBF neural network | |
| Wang et al. | Inverse kinematics of a 7-DOF spraying robot with 4R 3-DOF non-spherical wrist | |
| Brandstötter | Adaptable serial manipulators in modular design | |
| CN112643658A (en) | Calibration method for adaptive error modeling of series robot based on SIR dimension reduction DH model | |
| CN113255111B (en) | Inverse kinematics solving method and system for two-degree-of-freedom parallel mechanism | |
| Misra et al. | Kinematic and dynamic analysis of an industrial six-axis robotic manipulator | |
| CN116276990A (en) | Two-degree-of-freedom parallel structure kinematics positive solution method based on neural network training | |
| JP2019093487A (en) | Robot control device and robot reverse conversion processing method | |
| JP6897520B2 (en) | Robot control device and robot inverse transformation processing method | |
| Xia et al. | ABB-IRB120 Robot Modeling and Simulation Based on MATLAB | |
| CN110555240A (en) | automatic generation method from robot assembly model to simulation model | |
| CN113878583B (en) | Underwater mechanical arm control method and system | |
| Li et al. | Image moments-based visual servoing control of bagged agricultural materials handling robot | |
| Wang et al. | Inverse kinematics of 6-DOF robot manipulator via analytic solution with conformal geometric algebra | |
| Cuan-Urquizo et al. | Mobility analysis and inverse kinematics of a novel 2R1T parallel manipulator | |
| CN116533241A (en) | Kinematic calibration method and system for redundant drive over-constrained parallel robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |