A macro-micro two-stage hybrid analytical solution method for inverse kinematics of a rope-driven snake-like manipulator
By reconstructing the degree of freedom of the serpentine robot arm and combining a hybrid analytical method with geometric method and algebraic method, the problem of complexity of inverse kinematics is solved, and high-precision and efficient joint angle solution is achieved, satisfying the serpentine machinery Real-time control requirements of arm in complex environments.
Patent Information
- Application Number
- CN202310259085.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-03-14
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2043-03-14
AI Technical Summary
The inverse kinematic solution of serpentine robotic arms is complex, and the existing numerical methods and analytical methods are difficult to meet the requirements in terms of computational complexity and accuracy, especially under real-time control requirements in high degrees of freedom and complex environments.
The macro-micro-double-stage hybrid analytical solution method of inverse kinematics of rope-driven serpentine robot arm is used to reconstruct the degrees of freedom of the robot arm, and it is divided into 'macro' and 'micro' degrees of freedom, using geometric method and algebra respectively. The method performs inverse kinematics solution, reduces the computational complexity and improves the solution accuracy.
The solution process is simplified, the accuracy of the snake robot arm reaching the target is improved, the need to solve joint angles in real time is met, the error is reduced and the control accuracy is improved.
Smart Images

Figure CN116258015B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of mechanical arms, and in particular to a macro-micro two-stage hybrid analytical solution method for inverse kinematics of a rope-driven serpentine mechanical arm. Background Art
[0002] The robotic arm can participate in completing tasks in various complex environments, which makes the robotic arm have a high status in industrial production and social life. As modern industry develops further in the direction of automation and intelligence, the importance of the robotic arm becomes more apparent. The development of science and technology is promoting the progress of many important fields such as aerospace, nuclear engineering, etc. The working environment in these fields is complex, and it is difficult for general robotic arms to complete tasks. As an emerging special robot, the snake robotic arm has many joints, high degrees of freedom, and no power components on the arm body. Therefore, it is lightweight, flexible, and has a large working range. It has unique advantages in aircraft wing and engine maintenance, nuclear power plant maintenance, earthquake relief, etc. How to plan the motion of the snake robotic arm and achieve effective control is a difficult point.
[0003] The inverse kinematics of a robot arm is defined as solving the angles of each joint based on the position and posture of the end effector, which is generally solved by numerical and analytical methods. In general or low-redundancy robot arms, some relatively mature research results have been achieved based on these two methods. For analytical methods, some scholars adopt the idea of posture separation, taking redundant joints as variables, and solving the position and posture of redundant robot arms respectively; or for the idea of elbow self-motion, introduce arm angle variables to obtain a parameter-containing closed solution, and finally determine the solution by selecting the arm angle. For numerical methods, many scholars use the gradient projection method based on the pseudo-inverse of the Jacobian matrix to achieve the effect of avoiding singularities while solving inverse kinematics; some scholars also solve by continuously improving the neural network, which improves the flexibility and versatility of the algorithm.
[0004] The super-redundant characteristics of the serpentine manipulator make the inverse kinematics extremely complex. The same end position corresponds to countless joint angles. Simple numerical or analytical methods cannot meet the solution requirements in the serpentine manipulator. As for the algorithm based on the pseudo-inverse of the Jacobian matrix, the complexity of the algorithm increases with the increase of degrees of freedom. Although some scholars have proposed improved methods, the calculation amount is also huge due to the large degrees of freedom of the serpentine manipulator. To this end, some scholars have proposed a method of augmenting the Jacobian matrix, introducing task constraints into the velocity Jacobian matrix, expanding its row vectors, and thus obtaining the Jacobian square matrix, which simplifies the solution process. However, due to the excessive number of redundant degrees of freedom of the serpentine manipulator, it is difficult to find enough constraint sets to expand the Jacobian matrix into a square matrix. Similarly, for the gradient projection method for joint optimization, due to the large number of extreme points of the optimization objective function in the motion space of the serpentine manipulator, it may move near the local extreme points, and the joint optimization effect is not ideal. Some scholars use intelligent algorithms to solve the motion trajectory, realize motion planning, and thus operate in complex environments. However, although intelligent algorithms can obtain the global optimal solution and can handle different types of constraints, when the number of joints increases, they also face problems such as larger training sets, larger algorithm computational complexity, and long solution time, which cannot meet real-time requirements.
[0005] From the above analysis, we can see that although numerical methods and intelligent algorithms are highly versatile and simple in concept, the computational complexity and amount of operations explode with the increase of degrees of freedom; while analytical methods have certain requirements on the structure of the robot arm, and as the degrees of freedom increase, the analytical analysis will become more complicated. The analytical methods currently applied to snake-shaped robot arms are not yet mature. For example, the method based on geometric constraints has high efficiency and stability, but due to the artificial introduction of more constraints, the optimization ability of the snake-shaped robot arm to the optimization target is affected; the method based on algebra has too many equations and is difficult to solve. Summary of the invention
[0006] In order to solve the above problems, the use of analytical methods has unique advantages in solving the inverse kinematics of the robot arm - high solution accuracy and fast speed, and will not increase the difficulty of subsequent motion optimization and control. It is very suitable for scenarios where joint angles need to be obtained in real time. Therefore, the present invention provides a macro-micro two-stage hybrid analytical solution method for the inverse kinematics of a rope-driven serpentine robot arm. By analyzing the structure of the serpentine robot arm, the robot arm's degrees of freedom are divided into "macro" and "micro" degrees of freedom for the end-joint link of the rope-driven serpentine robot arm, and the inverse kinematics of the two are solved using geometric and algebraic methods respectively. This method ensures that the system characteristics remain unchanged by retaining low redundancy for the "macro" degrees of freedom, simplifies the solution process by reducing the degrees of freedom, and finally uses a hybrid analytical method to solve the joint angles, which can ensure the completeness of the solution while taking into account the speed and accuracy of the solution. The method mainly includes:
[0007] S1: Reconstruct the degrees of freedom of the snake-like robot arm to obtain a "macro" robot arm and a "micro" robot arm;
[0008] S2: Introduce the arm angle variable, and based on the self-motion characteristics of the redundant manipulator, obtain the values of each angle of the "macro" manipulator;
[0009] S3: Based on the values of each angle of the “macro” robotic arm obtained by solving, the values of each angle of the “micro” robotic arm are obtained.
[0010] Furthermore, when reconstructing the degrees of freedom, seven joints are selected as key joints to form a "macro" robotic arm, and the other joints between two adjacent key joints form a "micro" robotic arm. The distance between two adjacent key joints is regarded as an equivalent connecting rod, and the connecting rod length L satisfies:
[0011]
[0012] Where l is the actual single link length of the snake-like robot arm, and n is the actual number of links between two key joints.
[0013] Furthermore, the process of obtaining the values of each angle of the "macro" robot arm is as follows:
[0014] (1) Given the end position T of the snake-like robot arm f , the second, fourth, and sixth joints of the robot are S, E, and W respectively, SE, SW, and EW are vector moduli, θ i is the rotation angle of the i-th joint around its own z-axis, which can be obtained by the cosine theorem:
[0015]
[0016] Where ∠SEW is the angle between the second, fourth, and sixth joints, so the rotation angle of the fourth joint around its own z-axis θ4 = ±(π-arccos(cos∠SEW));
[0017] (2) The first three degrees of freedom are considered as a spherical joint, with the first joint denoted as G and the second joint point S as the coordinate origin. The direction is the positive direction of the x-axis. When the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are all zero, the position of the sixth joint W is:
[0018]
[0019] When the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are not all zero, according to the arm angle φ, the coordinate system ∑ at the second joint S can be obtained from equations (3.2)-(3.3) and the angle of rotation around the origin S s , that is, s =∑ Sφ=0 ×Rot(x s ,φ),∑ S φ=0 is the coordinate system when the arm angle is zero, Rot(x s ,φ) represents the rotation angle φ around the x-axis of point s, the subscript s of x represents the origin of the coordinate system, and the rotation angle around the y or z axis is similar, and then according to the terminal posture T f Get the transformation matrix R of the spherical joint with point S as the center s :
[0020]
[0021] Among them, Rot(z,θ1) means that the first joint rotates around its own z-axis by an angle of θ1, Rot(y,θ2) means that the second joint rotates around its own y-axis by an angle of θ2, and Rot(z,θ3) means that the third joint rotates around its own z-axis by an angle of θ3;
[0022] This gives us three perspectives:
[0023]
[0024] Where R s (i,j) represents the matrix R s The i-th row and j-th column element, i = 1, 2, 3, j = 1, 2, 3, θ1 represents the rotation angle of the first joint around its own z-axis, θ2 represents the rotation angle of the second joint around its own z-axis, and θ3 represents the rotation angle of the third joint around its own z-axis;
[0025] (3) Similarly, the remaining three angles can be solved:
[0026]
[0027] where arctan(·) represents the sigmoidal tangent function, R w is the transformation matrix of the spherical joint with the sixth joint W as the center, R w (i,j) represents the matrix R w The i-th row and j-th column element of , θ5 represents the rotation angle of the fifth joint around its own z-axis, θ6 represents the rotation angle of the sixth joint around its own z-axis, and θ7 represents the rotation angle of the seventh joint around its own z-axis.
[0028] Furthermore, the process of obtaining the values of each angle of the "micro" robotic arm is as follows:
[0029] Since the angles of some joints have been obtained in the solution of the "macro" robotic arm, the position and posture of each key joint can be obtained based on the base coordinate system or the end coordinate system. The angle solution method of all "micro" robotic arms is the same. Taking the angle solution method of the second "micro" robotic arm as an example, its end position and posture are known, and its end transformation matrix is recorded as for:
[0030]
[0031] Where n = [n x ,n y ,n z ] T is the normal vector of the end of the second “micro” robotic arm, o=[o x ,o y ,o z ] T is the direction vector of the second “micro” robotic arm, a=[a x ,a y ,a z ] T is the approach vector of the second “micro” robotic arm, p = [p x ,p y ,p z ] T is the position vector of the second “micro” robotic arm. The joints of the “micro” robotic arm are perpendicular to each other. The joint angles are calculated based on the transformation matrix of each link and the end pose.
[0032] The forward kinematics of the robot arm can be expressed as: is the transformation matrix from the hth to the mth coordinate system, h=0,1,2,3,4,5,m=1,2,3,4,5,6. Combined with the forward kinematics formula of the robotic arm, the process of solving the angle of the second "micro" robotic arm is as follows:
[0033] (1) Using Euler transformation To represent the end transformation matrix of the second "micro" robot arm α,β, represents the pose of the second "micro" robotic arm end,
[0034]
[0035] make The matrix elements correspond one to one, and a total of 12 sets of implicit equations can be obtained. These equations can be combined to solve some angles;
[0036] (2) To solve the remaining angles, use the inverse transformation Multiply both sides of the positive kinematics equation by one of the left sides, such as Let the corresponding elements at both ends of the matrix equation be equal and find the angles;
[0037] (3) Through steps (1)-(2), all angles of the second "micro" robotic arm are obtained. Similarly, all angles of all "micro" robotic arms can be obtained.
[0038] The technical solution provided by the present invention has the following beneficial effects: the solution process is simple, the error of the serpentine robot arm is reduced, and the accuracy of the serpentine robot arm in reaching the target is improved. BRIEF DESCRIPTION OF THE DRAWINGS
[0039] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:
[0040] Figure 1 It is a flow chart of a macro-micro two-stage hybrid analytical solution method for inverse kinematics of a rope-driven serpentine robot arm in an embodiment of the present invention.
[0041] Figure 2 Schematic diagram of degree of freedom reconstruction in an embodiment of the present invention.
[0042] Figure 3 It is a configuration diagram of the “macro” robotic arm in an embodiment of the present invention.
[0043] Figure 4 Schematic diagram of the “macro” robotic arm angle in an embodiment of the present invention.
[0044] Figure 5 Schematic diagram of the angle of the second "microscopic" robotic arm in the embodiment of the present invention.
[0045] Figure 6 Schematic diagram of the terminal termination posture matrix obtained by using this method in an embodiment of the present invention.
[0046] Figure 7 Schematic diagram of the theoretical terminal posture matrix in an embodiment of the present invention. DETAILED DESCRIPTION
[0047] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, specific embodiments of the present invention are now described in detail with reference to the accompanying drawings.
[0048] Considering the real-time requirements of the work, it is necessary to design a suitable analytical algorithm to solve the inverse kinematics problem of the serpentine manipulator. Therefore, an embodiment of the present invention provides a macro-micro two-level hybrid analytical solution method for the inverse kinematics of a rope-driven serpentine manipulator.
[0049] Please refer to Figure 1 , Figure 1 1 is a flow chart of a macro-micro two-stage hybrid analytical solution method for inverse kinematics of a rope-driven snake-shaped manipulator in an embodiment of the present invention, which specifically includes:
[0050] 1.1 Degree of freedom reconstruction
[0051] The serpentine robot has high degrees of freedom. Generally, the serpentine robot has more than 20 degrees of freedom. The joints are universal joints with two degrees of freedom. The connecting rods between adjacent joints have the same length and there is no joint offset. In order to apply the analytical method to the serpentine robot, its degrees of freedom need to be reconstructed, which can also reduce the complexity of the calculation.
[0052] The schematic diagram of the snake-like robot arm when reconstructing its degrees of freedom is as follows Figure 2 As shown in the figure, considering the flexibility brought by the super-redundant characteristics of the snake-like robot arm, seven joints are selected as key joints to form a "macro" robot arm; the distance between two adjacent key joints is regarded as an equivalent connecting rod, and the connecting rod length L satisfies:
[0053]
[0054] Where l is the actual length of a single link of the snake-shaped robot arm, n is the actual number of links between two key joints, and L is variable;
[0055] The other joints between two adjacent key joints (including the key joints close to the base) form a "micro" robotic arm with no more than six degrees of freedom, so it is a non-redundant robotic arm.
[0056] The resulting "macro" robotic arm must retain the redundant characteristics, while the "micro" robotic arm is a general robotic arm. In view of the redundant characteristics, the arm angle idea is used to add constraints to achieve the seven-degree-of-freedom inverse solution; the general robotic arm is sufficient to solve the inverse kinematics using algebraic methods. Finally, the hybrid analytical method combining geometric and algebraic methods is used to realize the inverse kinematics algorithm design.
[0057] 1.2 Macroscopic Robotic Arm Solution
[0058] The method of the present invention is proposed for the inverse kinematics of the snake-like manipulator and does not involve motion optimization, so any k value can be selected and different key joint positions can be selected. Obviously, the seven-degree-of-freedom "macro" manipulator meets the conditions of the closed solution, so this embodiment takes the seven-degree-of-freedom "macro" manipulator as an example.
[0059] First, since each joint is a universal joint, only the degrees of freedom that conform to the general configuration of a seven-degree-of-freedom robotic arm are selected as the joints of the “macro” robotic arm, e.g. Figure 3 As shown, the remaining degrees of freedom below seven degrees of freedom are classified as low-level "microscopic" robotic arms.
[0060] Then, the arm angle variable is introduced. Based on the self-motion characteristics of the redundant manipulator, the method for calculating each angle can be designed. The specific contents are as follows:
[0061] (1) Given the snake-like robot end position Tf , the second, fourth, and sixth joints of the robot are S, E, and W respectively, SE, SW, and EW are vector moduli, θ i is the rotation angle of the i-th joint around its own z-axis, which can be obtained by the cosine theorem:
[0062]
[0063] In the formula, ∠SEW has two solutions, which are the angles formed by the second, fourth, and sixth joints. Therefore, the rotation angle of the fourth joint around its own z-axis, θ4 = ±(π-arccos(cos∠SEW)), also has two solutions;
[0064] (2) For the first three degrees of freedom, consider it as a spherical joint, denote the first joint as G, and take the second joint point S as the coordinate origin. The direction is the positive direction of the x-axis. After coordinate transformation, it can be obtained that when the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are all zero, the position of the sixth joint W is:
[0065]
[0066] When the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are not all zero, the coordinate system ∑ at the second joint S can be obtained from equations (3.2)-(3.3) and the angle of rotation around the origin S s , that is, s =∑ S φ=0 ×Rot(x s ,φ),∑ S φ=0 is the coordinate system when the arm angle is zero, Rot(x s ,φ) represents the rotation angle φ around the x-axis of point s, and the subscript s of x represents the origin of the coordinate system. The same is true for rotating around the y or z axis at a certain angle, and then according to the terminal posture T f Get the transformation matrix R of the spherical joint with point S as the center s :
[0067]
[0068] Among them, Rot(z,θ1) means that the first joint rotates around its own z-axis by an angle of θ1, Rot(y,θ2) means that the second joint rotates around its own y-axis by an angle of θ2, and Rot(z,θ3) means that the third joint rotates around its own z-axis by an angle of θ3. The * in the formula represents an irrelevant term and will not affect the calculation result.
[0069] This gives us three perspectives:
[0070]
[0071] Among them, R s (i,j) represents the matrix R s The i-th row and j-th column element, i=1,2,3, j=1,2,3, θ1 represents the rotation angle of the first joint around its own z-axis, θ2 represents the rotation angle of the second joint around its own z-axis, and θ3 represents the rotation angle of the third joint around its own z-axis; in this embodiment, θ1 also has two solutions;
[0072] (3). The remaining three angles can be solved in the same way:
[0073]
[0074] where arctan(·) represents the sigmoidal tangent function, R w is the transformation matrix of the spherical joint with the sixth joint W as the center, R w (i,j) represents the matrix R w The i-th row and j-th column element, θ5 represents the rotation angle of the fifth joint around its own z-axis, θ6 represents the rotation angle of the sixth joint around its own z-axis, θ7 represents the rotation angle of the seventh joint around its own z-axis. In this embodiment, θ5 also has two solutions.
[0075] Through the above analysis, we can get the values of each angle of the "macro" robot arm. It should be noted that when k takes different values, it will lead to the local transformation matrix R s ,R w Based on this, we can start to find the joint angles of each "micro" robotic arm.
[0076] 1.3 Microscopic Robotic Arm Solution
[0077] Since the "micro" robotic arms are all general robotic arms, Figure 2 Taking the second "micro" robotic arm as an example, it is a six-degree-of-freedom (i.e. six-joint) robotic arm that meets the Pieper criterion. Consider the algebraic method for the inverse kinematics of this robotic arm.
[0078] Since the angles of some joints have been obtained in the solution of the "macro" robotic arm, the position and posture of each key joint can be obtained based on the base coordinate system or the end coordinate system. The angle solution method of all "micro" robotic arms is the same. Take the angle solution method of the second "micro" robotic arm as an example, that is, the second "micro" robotic arm as an example, its end position (key joint 3) is known, and its end transformation matrix 06T is recorded as:
[0079]
[0080] Where n = [n x ,n y ,n z ] Tis the normal vector of the end of the second “micro” robotic arm, o=[o x ,o y ,o z ] T is the direction vector of the second “micro” robotic arm, a=[a x ,a y ,a z ] T is the approach vector of the second “micro” robotic arm, p = [p x ,p y ,p z ] T is the position vector of the second “micro” robotic arm. The joints of the “micro” robotic arm are also perpendicular to each other. The joint angles are calculated based on the transformation matrix of each link and the end pose.
[0081] The forward kinematics of the robot arm can be expressed as: is the transformation matrix from the hth to the mth coordinate system, h=0,1,2,3,4,5,m=1,2,3,4,5,6. Combined with the forward kinematics formula of the robotic arm, the process of solving the angle of the second "micro" robotic arm is as follows:
[0082] (1) Using Euler transformation To represent the end transformation matrix of the second "micro" robot arm α,β, represents the pose of the second "micro" robotic arm end,
[0083]
[0084] make The matrix elements correspond one to one, and a total of 12 sets of implicit equations can be obtained. These equations can be combined to solve some angles;
[0085] (2) For other angles, you can try to use inverse transformation Multiply both sides of the positive kinematics equation by one of the left sides, such as Make the elements at both ends of the matrix equation equal, and use techniques such as trigonometric substitution to find the angles;
[0086] (3) Through the above steps (1)-(2), all angles of the second "micro" robotic arm are obtained. Similarly, all angles of all "micro" robotic arms can be obtained. For a solution that has two solutions, you can choose any one solution and then solve the other solutions (the number of solutions for a six-degree-of-freedom robotic arm is limited).
[0087] The values of each angle of the "macro" robotic arm and the values of each angle of the "micro" robotic arm obtained by solving are the solution results of the inverse kinematics of the snake-like robotic arm.
[0088] 2. Effect after implementation
[0089] In order to verify the accuracy of the proposed method in obtaining the inverse solution of the snake-like robot, a simulation verification was performed using a 28-degree-of-freedom snake-like robot.
[0090] Take the actual single link length l = 0.1m, the 1st, 3rd, 6th, 8th, 10th, 12th, and 14th joints as key joints, and multiple links and degrees of freedom between any two key joints to form a "micro" robotic arm. During simulation, the end position of the entire snake-like robotic arm is given as End = [-0.6551, 0.2, 0.6037], Ori_end = [0, pi / 3, 0]; the simulation results are as follows Figure 4-7 As shown, Figure 4 This is a schematic diagram of the "macro" robotic arm angle. Figure 5 This is a schematic diagram of the angle of the second "micro" robotic arm. Figure 6 The end position matrix is obtained by substituting the angle obtained by this method into the forward kinematics formula of the robot arm. The 3×1 vector composed of the first three rows of the fourth column of the matrix is the end position vector, and the 3×3 matrix composed of the first three rows and first three columns is the end position vector. Figure 6 The matrix shown is the theoretical transformation matrix obtained by giving the end position and attitude vector through the end termination posture (End and Ori_end) (i.e., the theoretical end posture matrix, such as Figure 7 It can be seen that given a continuous trajectory, the snake-like robot arm based on the proposed algorithm can accurately reach the target point.
[0091] The beneficial effects of the present invention are: the solution process is simple, the error of the snake-like mechanical arm is reduced, and the accuracy of the snake-like mechanical arm in reaching the target is improved.
[0092] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims
1. A macro-micro two-stage hybrid analytical solution method for inverse kinematics of a rope-driven snake-like manipulator. Features: include: S1: Reconstruct the degrees of freedom of the snake-like robot arm to obtain the "macro" robot arm and the "micro" robot arm; When reconstructing the degrees of freedom, seven joints are selected as key joints to form a "macro" robotic arm, and the other joints between two adjacent key joints form a "micro" robotic arm. The distance between two adjacent key joints is regarded as an equivalent connecting rod, and the connecting rod length L satisfies: Where l is the actual length of a single link of the snake-shaped robot arm, and n is the actual number of links between two key joints; S2: Introduce the arm angle variable, and based on the self-motion characteristics of the redundant manipulator, obtain the values of each angle of the "macro" manipulator; The process of getting the values of each angle of the "macro" robot arm is as follows: (1) Given the end position T of the snake-shaped robot arm f , the second, fourth, and sixth joints of the robot are S, E, and W respectively, SE, SW, and EW are vector moduli, θ i is the rotation angle of the i-th joint around its own z-axis, which can be obtained by the cosine theorem: Where ∠SEW is the angle between the second, fourth, and sixth joints, so the rotation angle of the fourth joint around its own z-axis is θ 4 =±(π-arccos(cos∠SEW)); (2) The first three degrees of freedom are considered as a spherical joint, with the first joint denoted as G and the second joint point S as the coordinate origin. The direction is the positive direction of the x-axis. When the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are all zero, the position of the sixth joint W is: When the rotation angles of the first joint, the second joint, and the third joint around their own z-axis are not all zero, according to the arm angle φ, the coordinate system ∑ at the second joint S can be obtained from equations (3.2)-(3.3) (3.10) and the angle of rotation around the origin S s , that is, s =∑ S φ=0 ×Rot(x s ,φ),∑ S φ=0 is the coordinate system when the arm angle is zero, Rot(x s ,φ) represents the rotation angle φ around the x-axis of point s, and the subscript s of x represents the origin of the coordinate system. The same is true for rotating around the y or z axis at a certain angle, and then according to the terminal posture T f Get the transformation matrix R of the spherical joint with point S as the center s : Among them, Rot(z,θ 1 ) indicates that the first joint rotates around its own z-axis θ 1 Angle, Rot(y,θ 2 ) indicates that the second joint rotates around its own y-axis θ 2 Angle, Rot(z,θ 3 ) indicates that the third joint rotates around its own z-axis θ 3 angle; This gives us three perspectives: Where R s (i,j) represents the matrix R s The element in the i-th row and j-th column, i=1,2,3, j=1,2,3, θ 1 Represents the rotation angle of the first joint around its own z-axis, θ 2 Represents the rotation angle of the second joint around its own z-axis, θ 3 Represents the rotation angle of the third joint around its own z-axis; (3) Similarly, the remaining three angles can be solved: where arctan(·) represents the sigmoidal tangent function, R w is the transformation matrix of the spherical joint with the sixth joint W as the center, R w (i,j) represents the matrix R w The i-th row and j-th column element, θ 5 Represents the rotation angle of the fifth joint around its own z-axis, θ 6 Represents the rotation angle of the sixth joint around its own z-axis, θ 7 Indicates the rotation angle of the seventh joint around its own z-axis; S3: According to the values of each angle of the "macro" robotic arm obtained by solving, the values of each angle of the "micro" robotic arm are obtained; The process of getting the values of each angle of the "micro" robotic arm is as follows: Since the angles of some joints have been obtained in the solution of the "macro" robotic arm, the position and posture of each key joint can be obtained based on the base coordinate system or the end coordinate system. The angle solution method of all "micro" robotic arms is the same. Taking the angle solution method of the second "micro" robotic arm as an example, its end position and posture are known, and its end transformation matrix is recorded as for: Where n = [n x ,n y ,n z ] T is the normal vector of the end of the second "micro" robotic arm, o = [o x ,o y ,o z ] T is the direction vector of the second "micro" robotic arm, a=[a x ,a y ,a z ] T is the approach vector of the second "micro" robot arm, p = [p x ,p y ,p z ] T is the position vector of the second "micro" robotic arm. The joints of the "micro" robotic arm are perpendicular to each other. The joint angles are calculated based on the transformation matrix of each link and the end pose. The forward kinematics of the robot arm can be expressed as: is the transformation matrix from the hth to the mth coordinate system, h=0,1,2,3,4,5,m=1,2,3,4,5,6. Combined with the forward kinematics formula of the robotic arm, the process of solving the angle of the second "micro" robotic arm is as follows: (1) Using Euler transformation To represent the end transformation matrix of the second "micro" robot arm α,β, represents the pose of the second "micro" robotic arm end, make The matrix elements correspond one to one, and a total of 12 sets of implicit equations can be obtained. These equations can be combined to solve some angles; (2) To solve the remaining angles, use the inverse transformation Multiply both sides of the positive kinematics equation by one of the left sides, make the elements at both ends of the matrix equation equal, and find the angles; (3) Through the above steps (1)-(2), all angles of the second "micro" robotic arm are obtained. Similarly, all angles of all "micro" robotic arms can be obtained.
Citation Information
Patent Citations
Inverse-kinematics analytic-solution calculating method of articulated manipulator
CN107589934A
Position level inverse solution method of seven-degree-of-freedom series mechanical arm
CN109702751A
Seven-degree-of-freedom mechanical arm limiting optimization method based on position-level inverse kinematics
CN112091979A
Inverse kinematics analytical solution control method for six-degree-of-freedom mechanical arm
CN113127989A
Seven-degree-of-freedom redundant mechanical arm inverse kinematics solving method and system
CN113722864A
