Modular kinematics solving method

Abstract

The application discloses a kind of modular kinematics solving method, comprising: constructing single-arm lever module, the mapping relationship between end point and angle parameter is obtained;Double-arm lever module is constructed, and intermediate key point solving model is obtained;Mechanical arm is equivalent to double-arm lever module, the position coordinates of intermediate key point are solved;Two equivalent arm rods are equivalent to single-arm lever module, and the angle parameter of single-arm lever module is obtained according to the mapping relationship and intermediate key point position coordinates;The number of arm rod contained in single-arm lever module obtained in the previous step is greater than 1, and single-arm lever module is equivalent to double-arm lever module, and the position coordinates of intermediate key point are solved;Two equivalent arm rods are equivalent to single-arm lever module, and its angle parameter is obtained;The above steps are repeated until the number of arm rod contained in single-arm lever module is equal to 1, and the angle parameter and end point position coordinates of each arm rod are obtained.The application can realize kinematics modularization high-efficiency solution, and has important guiding significance for real-time inverse kinematics solution.

Description

Technical Field

[0001] This invention relates to a kinematics solution method, specifically a modular kinematics solution method, and belongs to the field of robotics technology. Background Technology

[0002] Kinematic control is the most fundamental problem for redundant robotic arms, which can be reduced to solving the inverse kinematics of the robotic arm. Solving the inverse kinematics of robotic arms is of great significance in practical applications and has attracted widespread attention from researchers in the field of robotics. The number of inverse kinematic solutions depends on the number of links and joints in the robotic arm, and is also related to the link parameters and range of motion. The more joints the robotic arm has, the more non-zero parameters the links have, and the more ways there are to achieve a specific pose, i.e., the more inverse kinematic solutions there are.

[0003] The Jacobian matrix of a hyperredundant robotic arm is a rectangular matrix, and regardless of whether it is full rank, it has no conventional inverse, only a generalized inverse. Therefore, many researchers have adopted pseudo-inverse-based methods to solve the inverse kinematics of hyperredundant robotic arms. However, the joint velocities solved by pseudo-inverse-based methods near the singular configuration of the robotic arm are too large, exceeding the joint's driving capability, and cannot be applied to robotic arm control. Furthermore, the solution process is entirely numerical, which introduces accumulated errors, affecting control accuracy.

[0004] Besides using the numerical methods described above for inverse kinematics solutions, analytical methods can also be used to solve position-level inverse kinematics. While this method is simple in concept, the computational process is complex. Therefore, for the special geometric relationships inherent in the robotic arm structure, geometric simplification can be employed to reduce the computational complexity of solving closed-form solutions. Furthermore, the super-redundant robotic arm can be equivalent to multiple serial robotic arms, further reducing the complexity of kinematic solutions; for example, the robotic arm can be divided into three parts: shoulder, elbow, and wrist. However, to achieve better kinematic results, the segmentation method and the number of arms in each of the shoulder, elbow, and wrist parts are uncertain. In summary, in methods for solving the segmented kinematics of robotic arms, the segmentation type is difficult to select, and no researchers have yet proposed a universal solution method. Summary of the Invention

[0005] The purpose of this invention is to overcome the above-mentioned defects and provide a modular kinematics solution method to solve the technical problem that traditional multi-degree-of-freedom robotic arm kinematics model solutions are complex and difficult to achieve precise control of the robotic arm. This invention can achieve modular and efficient kinematics solutions and has important guiding significance for real-time inverse kinematics solutions.

[0006] To achieve the above-mentioned objectives, the present invention provides the following technical solution:

[0007] A modular kinematics solution method includes:

[0008] S1 constructs a single-arm lever module and obtains the mapping relationship between the end point of the single-arm lever module and the angle parameters;

[0009] S2 constructs a double-arm module and obtains the solution model for the intermediate key point of the double-arm module. The intermediate key point is the end point of the first arm in the double-arm module.

[0010] S3 equates a robotic arm containing n arms to a dual-arm module, where both arms in the module are equivalent arms. The position coordinates of the intermediate key points are obtained by solving the model based on the intermediate key points.

[0011] The first and second equivalent booms are respectively equivalent to two single boom modules. Based on the mapping relationship between the end point and angle parameters of the single boom module and the position coordinates of the intermediate key points, the angle parameters of the two single boom modules are obtained.

[0012] When the number of arms in the single-arm module obtained in the previous step is greater than 1, S4 will convert the single-arm module into a double-arm module and obtain the position coordinates of the intermediate key points based on the solution model of the intermediate key points.

[0013] The first and second equivalent arms of the double-arm module are respectively equivalent to two single-arm modules. Based on the mapping relationship between the end point and angle parameters of the single-arm module and the position coordinates of the intermediate key points, the angle parameters of the two single-arm modules are obtained.

[0014] S5 Repeat step S4 until all single-arm modules obtained in the previous step contain an arm with a number of arms equal to 1, and obtain the angle parameters and end point position coordinates of each arm.

[0015] S6 obtains the motor rotation angle data based on the angle parameters of each boom and the position coordinates of the end point.

[0016] Furthermore, the angular parameters of the single-arm module include the yaw angle α of the i-th arm. i Pitch angle θ i Position angle ψ i and arm angle γ i i≥1;

[0017] The mapping relationship between the end point of the single-arm module and the angle parameters includes:

[0018]

[0019]

[0020] Among them, L i Let x be the length of the i-th boom, which is the starting point (x) of the i-th boom. i ,y i,z i ) and the end point (x) of the i-th arm i+1 ,y i+1 ,z i+1 The distance between them.

[0021] Furthermore, the dual-arm module includes two adjacent single-arm arms, which are denoted as the i-th single-arm arm and the (i+1)-th single-arm arm, respectively, where i≥1;

[0022] The solution model for the key points in the middle of the double-arm module includes:

[0023]

[0024]

[0025]

[0026] Where, r i+1 Let γ be the radius of the spatial circle formed by the reachable position of the i-th single-arm rod end in three-dimensional space. i_i+1_o Let ψ be the vector of the line connecting the center of the spatial circle formed by the reachable positions of the i-th single-arm end in three-dimensional space and the starting point of the i-th arm. i+1 α i+1 θ i+1 These represent the position angle, yaw angle, and pitch angle of the (i+1)th single arm, respectively, and L i and L i+1 γ represents the lengths of the i-th and (i+1)-th single-arm rods, respectively. i_i+2 The end point (x) of the (i+1)th single-arm rod i+2 ,y i+2 ,z i+2 ) and the starting point of the i-th single-arm pole (x) i ,y i ,z i The vector of the line connecting γ i_i+2 =[x i+2 -x i ,y i+2 -y i ,z i+2 -z i ] T .

[0027] Furthermore, in the dual-arm module, when step S3 converts the robotic arm containing n arms into a dual-arm module, if n is an even number, the number of arms in the first equivalent arm and the second equivalent arm in the dual-arm module is n / 2.

[0028] When n is odd, the number of arms in the first and second equivalent arms of the double-arm module are (n-1) / 2 and (n+1) / 2, respectively.

[0029] When S4 converts a single-arm module containing w arms into a double-arm module, if w is an even number, the number of arms in the first and second equivalent arms of the double-arm module is w / 2.

[0030] When w is odd, the number of arms in the first and second equivalent arms of the double-arm module are (w-1) / 2 and (w+1) / 2, respectively; w>1.

[0031] Furthermore, in step S4, assume that the double-arm module contains w arms out of n arms, where w arms are the i-th to j-th arms, 1≤i≤j≤n, and w=j+1-i; the starting point in the double-arm module is (x i ,y i ,z i ), the endpoint is (x j+1 ,y j+1 ,z j+1 );

[0032] When w is even, the first equivalent arm is considered from the i-th arm to the (i+j-1) / 2-th arm, and the second equivalent arm is considered from the (i+j+1) / 2-th arm to the j-th arm. The coordinates of the intermediate key points are (x... (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 When w is odd, the first equivalent arm is considered as the arm from the i-th arm to the (i+j) / 2-1-th arm, and the second equivalent arm is considered as the arm from the (i+j) / 2-th arm to the j-th arm. The coordinates of the intermediate key points are (x... (i+j) / 2 ,y (i+j) / 2 ,z (i+j) / 2 );

[0033] In the dual-arm module, the methods for obtaining the position coordinates of the intermediate key points from the solution model include:

[0034] When w is even

[0035] When w is odd

[0036]

[0037] Among them, (x (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 ) and (x (i+j) / 2_i-1 ,y(i+j) / 2_i-1 ,z (i+j) / 2_i-1 R represents the position coordinates of the intermediate key point in the body coordinate system of the (i-1)th arm. i-1_j Let γ be the rotation matrix of the j-th arm relative to the body coordinate system of the (i-1)-th arm. i_(i+j+1) / 2_o Let r be the vector connecting the center of the spatial circle formed by the optional positions of the end point of the first equivalent arm (i+j+1) / 2 in three-dimensional space) and the starting point of the i-th arm. (i+j+1) / 2 Let τ be the radius of the spatial circle formed by the reachable positions of the end points of the first equivalent arm in three-dimensional space. i_j+1_(i+j+1) / 2 For the first equivalent arm end point relative to vector γ i_j+1 Phase angle; R 0_i-1 Let α be the rotation matrix of the (i-1)th boom relative to the natural coordinate system. Consider the line connecting the end point (i-1)th boom (i.e., the i-th point) and the starting point (i.e., the 1st point) of the 1st boom as an equivalent boom. The corresponding yaw angle α can be calculated from the position coordinates. 1_i-1 and pitch angle θ 1_i-1 R 0_i-1 The solution formula is shown below:

[0038]

[0039] Furthermore, in step S4, when i = 1, R 0_i-1 It is the identity matrix;

[0040]

[0041] Where, α i+1_j and θ i+1_j These are the yaw angle and pitch angle of the line connecting the end point of the j-th boom and the starting point of the (i+1)-th boom relative to the i-th boom.

[0042] Furthermore, when w is even,

[0043] When w is odd

[0044]

[0045] γ i_j+1 =[x j+1 -x i ,y j+1 -y i ,z j+1 -z i ] T .

[0046] Furthermore, when the first or second equivalent boom contains only one boom, the length of the equivalent boom is equal to the length of one boom.

[0047] When the first or second equivalent boom contains more than one boom, the lengths of the two equivalent booms are determined as follows:

[0048] Solve for the starting point (x) of the double-arm module. i ,y i ,z i ) and endpoint (x) j+1 ,y j+1 ,z j+1 Distance between:

[0049]

[0050] Set the proportional allocation coefficients k1 and k2:

[0051]

[0052] According to L i_j The lengths of the two equivalent booms are obtained by using the proportional distribution coefficient:

[0053] When w is even

[0054]

[0055] When w is odd

[0056] L m Let m be the length of the m-th arm.

[0057] Furthermore, τ i_j+1_(i+j+1) / 2 or τ i_j+1_(i+j) / 2 Determined using the following method:

[0058] When w is even, construct the evaluation function. Where, x obs and y obs Let τ be the position coordinates of the obstacle; make τ i_j+1_(i+j+1) / 2 Varying within the range of 0° to 360°, and solving for each τ i_j+1_(i+j+1) / 2 The corresponding x (i+j+1) / 2 and y (i+j+1) / 2 , each x (i+j+1) / 2 and y (i+j+1) / 2 Substituting into the evaluation function, the output makes the evaluation function J... i_j τ at its maximum i_j+1_(i+j+1) / 2 , complete τ i_j+1_(i+j+1) / 2 The determination;

[0059] When w is odd, construct the evaluation function. Where, x obs and y obs Let τ be the position coordinates of the obstacle; make τ i_j+1_(i+j) / 2 Varying within the range of 0° to 360°, and solving for each τ i_j+1_(i+j) / The corresponding x (i+j) / 2 and y (i+j) / 2 , each x (i+j) / 2 and y (i+j) / 2 Substituting into the evaluation function, the output makes the evaluation function J... i_j τ at its maximum i_j+1_(i+j) / 2 , complete τ i_j+1_(i+j) / 2 The determination.

[0060] Furthermore, in step S4, the method for determining the coordinates of the intermediate key points includes:

[0061] When w is even:

[0062] Based on the known coordinates (x1, y1, z1) and the calculated coordinates (x... i ,y i ,z i ), thus obtaining α 1_i-1 and θ 1_i-1 According to α 1_i-1 and θ 1_i-1 Get R 0_i-1 ;

[0063] Based on the obtained coordinates (x i ,y i ,z i ) and coordinates (x) j+1 ,y j+1 ,z j+1 ), thus obtaining α i_j and θ i_j According to α i_j and θ i_j Get R i-1_j ;

[0064] According to R i-1_j and the determined τ i_j+1_(i+j+1) / 2 Obtain the relative position coordinates (x, y) of the intermediate key point in the first i-1 arm coordinate systems. (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 );

[0065] According to R 0_i-1 and relative position coordinates (x) (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 Solve for the natural coordinates (x, y) of the intermediate key points. (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 );

[0066] When w is odd:

[0067] Based on the known coordinates (x1, y1, z1) and the calculated coordinates (x... i ,y i ,z i ), thus obtaining α 1_i-1 and θ 1_i-1 According to α 1_i-1 and θ 1_i-1 Get R 0_i-1 ;

[0068] Based on the obtained coordinates (x i ,y i ,z i ) and coordinates (x) j+1 ,y j+1 ,z j+1 ), thus obtaining α i_j and θ i_j According to α i_j and θ i_j Get R i-1_j ;

[0069] According to R i-1_j and the determined τ i_j+1_(i+j) / 2 Obtain the relative position coordinates (x, y) of the intermediate key point in the first i-1 arm coordinate systems. (i+j) / 2_i-1 ,y (i+j) / 2_i-1 ,z (i+j) / 2_i-1 );

[0070] According to R 0_i-1 and relative position coordinates (x) (i+j) / 2_i-1 ,y (i+j) / 2_i-1 ,z (i+j) / 2_i-1 Solve for the natural coordinates (x, y) of the intermediate key points. (i+j) / 2 ,y (i+j) / 2 ,z (i+j) / 2 ).

[0071] Furthermore, when the dual-arm module obtains the position coordinates of the intermediate key points based on the solution model, it includes six cases:

[0072] Scenario 1: In the dual-arm module, the lengths of the two equivalent arms are unknown. At this time, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is first solved by using the known angle parameters of the previous equivalent arm. Then, the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm are obtained by using the rotation matrix. Finally, the coordinates of the intermediate key point in the body coordinate system of the i-1 arms are converted into the coordinates of the intermediate key point in the natural coordinate system.

[0073] Scenario 2: The lengths of the two equivalent arms in the double-arm module are unknown, so the coordinates of the intermediate key point in the natural coordinate system are obtained directly.

[0074] Scenario 3: The lengths of the two equivalent arms in the double-arm module are determined. At this time, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is first solved by using the known angle parameters of the previous equivalent arm. Then, the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm are obtained by using the rotation matrix. Finally, the coordinates of the intermediate key point in the body coordinate system of the i-1 arms are converted into the coordinates of the intermediate key point in the natural coordinate system.

[0075] Scenario 4: The coordinates of the intermediate key points in the natural coordinate system can be obtained directly;

[0076] Scenario 5: In the dual-arm module, the length of one arm is determined, while the length of the other equivalent arm is unknown. In this case, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is first solved using the known angle parameters of the previous equivalent arm. Then, the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm are obtained using this rotation matrix. Finally, the coordinates of the intermediate key point in the body coordinate system of the i-1 arms are converted into the coordinates of the intermediate key point in the natural coordinate system.

[0077] Situation 6: The coordinates of the intermediate key points in the natural coordinate system can be obtained directly.

[0078] Compared with the prior art, the present invention has at least one of the following advantages:

[0079] (1) This invention innovatively constructs a modular system, dividing a complex robotic arm model into multiple sub-modules with fewer degrees of freedom, thus solving the complex problem of inverse kinematics solution for multi-degree-of-freedom robotic arms. This invention enables efficient modular kinematics solution and has significant guiding significance for real-time inverse kinematics solution;

[0080] (2) This invention constructs a novel single-arm lever module and a double-arm lever module, establishes parameter transformation relationships, reduces the complexity of the system by reducing the number of elements, and transforms the constrained nonlinear optimization problem into an unconstrained nonlinear optimization problem, which is convenient for solving inverse kinematics.

[0081] (3) This invention plans an obstacle avoidance path by analyzing the relationship between the starting and ending points of the module and the obstacles, thereby increasing the obstacle avoidance capability of the robotic arm;

[0082] (4) The traditional inverse kinematics solution method solves the values ​​of each degree of freedom by the end pose. The multi-task planning method proposed in this invention not only achieves the above functions, but also completes other tasks through redundant degrees of freedom, such as obstacle avoidance, optimal mechanical properties, and smooth operation. Attached Figure Description

[0083] Figure 1 This is a schematic diagram of a continuous, serial, redundant robotic arm;

[0084] Figure 2 This is a flowchart of a modular kinematics solution method according to the present invention;

[0085] Figure 3 This is a schematic diagram of the parameters between adjacent booms;

[0086] Figure 4 A schematic diagram of the range of motion of a single arm lever (ψ) max =40°), where (a) and (b) are the oblique view and the view along the -x axis, respectively;

[0087] Figure 5 A schematic diagram of the range of motion of the double-arm lever (ψ) max =40°), where (a), (b), and (c) are the oblique view, the view along the -x-axis, and the view along the y-axis, respectively;

[0088] Figure 6 This is a parameter diagram of a double-arm module consisting of the i-th arm to the (i+1)-th arm.

[0089] Figure 7 This is a diagram showing the segmentation types of the robotic arm;

[0090] Figure 8 This is a diagram illustrating the segmentation process of the bisection method.

[0091] Figure 9 This is a parameter diagram of a double-arm module consisting of the i-th to j-th arms (the actual number of arms is even).

[0092] Figure 10 This is a segmented process diagram of an eight-arm robotic arm;

[0093] Figure 11 The kinematics solution process for an eight-arm robotic arm is shown in (a), (b), (c), and (d), which are respectively the initial conditions, the first set of key point coordinates, the second set of key point coordinates, and the third set of key point coordinates.

[0094] Figure 12 For the motion trajectory of each boom end point;

[0095] Figure 13The closest distance d between the robotic arm and the obstacle min The curve showing the change. Detailed Implementation

[0096] The features and advantages of the present invention will become clearer and more apparent from the following detailed description.

[0097] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments. Although various aspects of embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated otherwise.

[0098] This invention proposes a general solution method for ultra-redundant robotic arms: a modular kinematics solution method.

[0099] This invention is based on Figure 1 Taking a continuous serial robotic arm as an example, the technical solution of the present invention will be described in detail. The continuous serial robotic arm of the present invention includes n arms, and the i-th arm has two degrees of freedom relative to the previous arm: yaw angle α. i and pitch angle θ i The starting coordinates of the i-th boom are (x... i ,y i ,z i The coordinates of the endpoint are (x i+1 ,y i+1 ,z i+1 ).

[0100] In solving the forward motion of the robotic arm, the angle (α) is used. i ,θ i The rotation matrix R corresponding to the i-th arm can be obtained. i-1_i Therefore, it is only necessary to change the degree of freedom value (α) i ,θ i Substituting the values ​​into the formula, the coordinates (x, y) of the robotic arm's end effector can be obtained. n+1 ,y n+1 ,z n+1 The only solution to ).

[0101] Three constraint methods can be constructed using the three coordinate values ​​of the robotic arm's end point. If we consider the coordinates of the robotic arm's end point (x... n+1 ,y n+1 ,z n+1 Solve for the deflection angle (α) i ,θ i That is, solving for 2n unknowns (ignoring the overall translational degrees of freedom) through 3 constraint equations. If the position of the robotic arm's end effector (x... n+1 ,y n+1,z n+1 ,dx n ,dy n ,dz n Solve for the deflection angle (α) i ,θ i This involves solving for multiple degrees of freedom using six constraint equations. As n increases, the inverse kinematics solution becomes increasingly complex. When n>2 and a solution exists, there are infinitely many solutions. Selecting the optimal solution from these infinite solutions is the most challenging aspect of this field. Therefore, this invention proposes a generalized solution method: a modular kinematics solution method.

[0102] The flowchart of the method of the present invention is as follows Figure 2 As shown, the following describes the inverse kinematics solution process for a robotic arm with n connected links. The specific steps are as follows:

[0103] Step 1: Input initial data.

[0104] Input obstacle data, the initial point of the motion path, and the ending point. The obstacle data includes the shape and position data of the obstacles. Taking a continuous robotic arm as an example, it includes the following data: the number of arms n in the continuous robotic arm, and the length L of the i-th arm. i The total length of all booms is denoted by the symbol L. all This means that we can obtain Obstacle data includes the obstacle's shape and position. For dynamic obstacles, the data needs to be updated in real time to prevent moving objects from colliding with them. Taking a cylindrical obstacle as an example, if the obstacle's axis is parallel to the z-axis, its position data passes through (x...). obs ,y obs ) represents shape data through r obs h obs Indicates that x obs and y obs These are the projected coordinates of the cylinder's axis on the x-axis and y-axis, respectively, r. obs Let h be the radius of the cylinder. obs Let x be the height of the cylinder. The initial point of the motion path is the position that the end point of the moving object needs to reach at the initial moment, and the ending point is the position that the end point of the moving object needs to reach at the final moment. The significance of this step is that the starting point (x1, y1, z1) of the robotic arm is fixed, and the end point (x... n+1 ,y n+1 ,z n+1 The motion path of the robotic arm's end point is unknown, and it is necessary to plan the motion path of the end point based on the starting and ending points of the motion path. At any given moment, the coordinates of the robotic arm's midpoint are adjusted based on the coordinates of the starting point, the end point, and the position of obstacles on the arm.

[0105] This invention defines some generalized parameters, as follows. An equivalent boom module is formed from the i-th boom to the j-th boom, denoted by the symbol i_j, where i ≤ j. The starting point of the i-th boom (x... i ,y i ,z i ) and the j-th arm end point (x j+1 ,y j+1 ,z j+1 The distance between them is represented by L. i_j This indicates that the starting point of the i-th boom (x) i ,y i ,z i ) and the j-th arm end point (x j+1 ,y j+1 ,z j+1 The line connecting the two arms can be considered as an equivalent single-arm lever, with the corresponding arm angle and position angle being γ. i_j ψ i_j The corresponding yaw angle and pitch angle are α and α, respectively. i_j θ i_jj Here, yaw angle and pitch angle are technical terms. The boom angle is the angle between the axis of this boom and the axis of the previous boom. The position angle is the angle between the line segment projected from the axis of this boom onto the end face of the previous boom and the y-axis of the previous boom's coordinate system. For example, L... 1_4 Let L represent the distance between the start and end points in the double-arm module consisting of the first, second, third, and fourth arms. The first and second arms are the first equivalent arms in the double-arm module, and the third and fourth arms are the second equivalent arms. The lengths of these two equivalent arms, i.e., the distances between the start and end points, are denoted by L. 1_2 and L 3_4 Represents the coordinate value (x) i_j ,y i_j ,z i_j ) is a point (x) i ,y i ,z i The relative coordinates of the j-th coordinate system.

[0106] Step 2, construct the single-arm module.

[0107] In steps 2 and 3, a single-arm module and a double-arm module are constructed respectively. The characteristics of a module consisting of a single equivalent arm and two equivalent tandem arms are analyzed, as well as the mapping relationship between the angle parameters and the position of the end point of each arm, laying the groundwork for subsequent kinematic solutions.

[0108] Single-arm module such as Figure 3 As shown, the i-th arm has two rotational degrees of freedom relative to the (i-1)-th arm: αi and θ i To facilitate the analysis of the positional relationship between adjacent booms, these two angles can be converted into boom angles ψ. i With position angle γ i Based on spatial geometry, the angle parameter α can be derived. i θ i ψ i γ i The conversion relationship between them is shown in the following formula:

[0109] cos(ψ i )=cos(θ i )·cos(α i )

[0110]

[0111] The single-arm module has two degrees of freedom / variables. The length of the single arm is L. i The distance from the end point of the single arm to the start point of the single arm is L. i Through the Monte Carlo method, we can obtain the following: Figure 4 The diagram shows the range of motion of a single lever. The position of the end of the lever, obtained through random sampling, is represented by a point in the diagram. The circle in the diagram represents the boundary of the extreme position of the end of the lever's motion.

[0112] The specific mapping relationship between angle and position is as follows. The coordinates of the starting position of the single arm are (x... i ,y i ,z i The coordinates of the endpoint are (x... i+1 ,y i+1 ,z i+1 If α is known i θ i x i y i z i Then x can be solved using the following formula. i+1 y i+1 z i+1 .

[0113]

[0114] If x is known i y i z i x i+1 y i+1 z i+1 α can then be solved using the following formula. i θ i :

[0115]

[0116] Through ψ i γ i The formula for characterizing the position of the endpoint is as follows:

[0117]

[0118] If x is known i y i z i x i+1 y i+1 z i+1 Then ψ can be solved using the following formula. i γ i :

[0119]

[0120] Analysis of the single-arm lever module reveals that there is no transition point in the middle of the module. If the two endpoints are determined, the solution is unique or there is no solution, and the direction cannot be adjusted.

[0121] In summary, this step constructed a single-arm lever module and obtained the transformation relationships between the various parameters within the module. The transformation relationships between the four parameters of the single-arm lever module and the position coordinates are shown below:

[0122]

[0123] The conversion relationships between the four parameters are shown below:

[0124]

[0125] Step 3: Construct the double-arm module.

[0126] The twin-arm module has four degrees of freedom. Using the Monte Carlo method, we can obtain... Figure 5 The diagram shows the range of motion of the double-arm lever. The lengths of the double-arm levers are L and L', respectively. i L i+1 The range of variation of the distance from the end point of the boom to the initial point can be derived as follows: ψ max It is the maximum deflection angle between the i-th boom axis and the (i+1)-th boom axis.

[0127] A schematic diagram of the parameters of the dual-arm lever module is shown below. Figure 6 As shown in the figure, the mapping relationship between angle and position in the double-arm module is analyzed.

[0128] Input the position coordinates (x, y) of the end point of the (i+1)th arm. i+2 ,y i+2 ,z i+2By constructing basic kinematic modules, the following relationships can be derived:

[0129] x i+2 =L i cosα i cosθ i -L i+1 (cosα i sinθ i sinθ i+1 +sinα i sinα i+1 cosθ i+1 -cosα i cosθ i cosα i+1 cosθ i+1 )

[0130] y i+2 =L i sinα i cosθ i +L i+1 (cosα i sinθ i cosθ i+1 -sinα i sinθ i sinθ i+1 +sinα i cosθ i cosα i+ 1cosθ i+1 )

[0131] z i+2 =-L i sinθ i -L i+1 (cosθ i sinθ i+1 +sinθ i cosα i+1 cosθ i+1 )

[0132] As the formula shows, the double-arm module involves solving four unknowns using three equations, and this solution module is quite complex. This invention reduces the complexity of the system by using a method of reducing the number of unknowns, transforming the constrained nonlinear optimization problem into an unconstrained nonlinear optimization problem. In other words, to simplify the solution module, the following improvements are made to the spatial geometry modeling of the double-arm module.

[0133] Using spatial geometry, the position vector ζ of the end point of the i-th arm can be obtained. i+1 for:

[0134] ζ i+1 =L i [cos(α i cos(θ) i ),sin(α i cos(θ) i ),-sin(θ i )] T

[0135] In the same coordinate system {O i-1 In the diagram, the position vector ζ of the end point of the (i+1)th arm is... i+2 for:

[0136] ζ i+2 =R i-1_i ·L i+1 [cos(α i+1 cos(θ) i+1 ),sin(α i+1 cos(θ) i+1 ),-sin(θ i+1 )] T

[0137] In the formula R i-1_i Let be the rotation matrix from the body coordinate system of the i-th arm to the body coordinate system of the (i-1)-th arm.

[0138] By using vectors and matrices, the end point (x) of the (i+1)th arm can be solved. i+2 ,y i+2 ,z i+2 ) and the starting point (x) of the i-th arm i ,y i ,z i The vector γ corresponding to the line connecting ) i_i+2 for:

[0139] γ i_i+2 =ζ i+1 +ζ i+2

[0140] The vector γ of the line connecting the end point of the (i+1)th arm and the starting point of the ith arm. i_i+2 It can also be calculated using the following formula:

[0141] γ i_i+2 =[x i+2 -x i ,y i+2 -y i ,z i+2 -z i ] T (2)

[0142] The position of the end point of the i-th arm (x i+1,y i+1 ,z i+1 The location is uncertain. A spatial circle is formed at a reachable position in three-dimensional space, and the position vector of the center of this circle is denoted as γ. i_i+1_o Based on spatial geometry, we can obtain formula (3), where r i+1 Let be the radius of the circle, and γ be an unknown quantity. i_i+1_o Direction and γ i_i+2 Since the directions are the same, the solution for γ is obtained by rearranging formula (3) as shown in formula (4). i_i+1_o The formula for size. After determining the position γ of the spatial circle. i_i+1_o Then, the radius r of the spatial circle as shown in formula (5) can be calculated. i+1 .

[0143]

[0144]

[0145]

[0146] The position coordinates (x, y) of the end point of the (i+1)th arm. i+2 ,y i+2 ,z i+2 Given the given information, the vector γ can be obtained using formula (2). i_i+2 As a constant, r can be obtained through formula (5). i+1 It is also a constant. The two arms can form a plane, and the included angle ψ between the two arms is... i+1 If it remains unchanged, the corresponding solution formula is as follows:

[0147]

[0148] In constructing the dual-arm module, four variables were simplified into one: the position of the i-th arm's end point on the circle. This simplifies the mapping relationship between angles and positions in the dual-arm module, facilitating the solution. By adjusting this variable in conjunction with the robotic arm's motion scenarios, tasks such as obstacle avoidance can be achieved.

[0149] Step 4: Construct the segmentation method.

[0150] Based on the number of links in the robotic arm, analyze the possible segmentation methods. Taking a robotic arm consisting of n links connected in series as an example, divide the robotic arm into k segments, where k ranges from 1 to n. Under different classification scenarios, taking an eight-link robotic arm as an example, its segmentation types are shown in the diagram below. Figure 7As shown. A robotic arm with eight arms can be divided into 1, 2, 3, 4, 5, 6, 7, and 8 segments, with 1, 7, 21, 35, 35, 21, 7, and 1 ways respectively. In summary, a robotic arm with n arms can be divided into k segments, where k can be any integer from 1 to n; a robotic arm with n arms can be divided into k segments in various ways. Classification method.

[0151] If a portion of a robotic arm is divided into a five-link module, a six-link module, or a module containing even more links, the module becomes quite complex, with many redundant parameters, making kinematic solutions and task planning difficult. To balance redundancy and adjustable parameters, this invention prioritizes decomposing a multi-link cascaded robotic arm into a two-link module or a single-link module, where the two-link module contains one adjustable parameter.

[0152] This invention employs a binary search method to construct segmentation types, the segmentation process as follows: Figure 8 As shown. The segmentation principle for a robotic arm with n cascaded arms is as follows: If n is even, the first n / 2 arms are considered as one equivalent arm, and the next n / 2 arms are considered as one equivalent arm, thus constructing a double-arm module; if n is odd, the first (n-1) / 2 arms are considered as one equivalent arm, and the next (n+1) / 2 arms are considered as one equivalent arm, thus constructing a double-arm module. Subsequent segmentation follows the same principle, segmenting level by level until each segment contains only one actual arm. At this point, further segmentation is impossible, and segmentation stops. In summary, this step constructs a segmentation type by progressively segmenting using the binary search principle.

[0153] The following section uses the solver module in step 5 to describe in detail the process of solving for key data such as the coordinates of intermediate points.

[0154] Step 5, Inverse Kinematics Solving Module.

[0155] The inverse kinematics solution requires the use of the single-arm and double-arm modules described in steps 2 and 3. In the single-arm module, after inputting the end-effector position information, the corresponding rotational degree of freedom α can be obtained. i θ i and arm angle ψ i With position angle γ i .

[0156] In the dual-arm lever module, the inverse kinematics solution is divided into six types based on whether the arm length is known and whether a rotation matrix is ​​needed. Specifically, if the equivalent arm in the dual-arm lever module consists of only one actual arm, its length is determined; if the equivalent arm in the dual-arm lever module consists of two or more actual arms, its equivalent arm length is unknown. When the starting point in the dual-arm lever module is the starting point (x1, y1, z1) of the entire robotic arm, no rotation matrix is ​​needed.

[0157] The six solution modules are shown below:

[0158] (1) Solving module 1 refers to the double-arm module where the lengths of the two equivalent arms are unknown. It requires solving for the rotation matrix of the previous equivalent arm relative to the natural coordinate system using the known angle parameters of the previous equivalent arm, thereby solving for the intermediate coordinates in the coordinate system corresponding to the previous equivalent arm. The parameters i and j in this double-arm module must satisfy the following conditions:

[0159]

[0160] (2) Solving module 2 refers to a module where the lengths of the two equivalent arms in the double-arm module are unknown. No rotation matrix is ​​needed; the intermediate coordinates of the double-arm module can be solved in the natural coordinate system. The parameters i and j in this double-arm module must satisfy the following conditions:

[0161]

[0162] (3) Solving module 3 refers to the double-arm module where the lengths of both arms are determined. It requires solving for the rotation matrix of the previous equivalent arm relative to the natural coordinate system using the known angle parameters of the previous equivalent arm. This allows the intermediate coordinates of the double-arm module to be determined in the coordinate system corresponding to the previous equivalent arm. The parameters i and j in this double-arm module must satisfy the following conditions:

[0163]

[0164] (4) Solving module 4 refers to the double-arm module where the lengths of both arms are fixed, requiring no rotation matrix; the intermediate coordinates of the double-arm module can be solved in the natural coordinate system. The parameters i and j in this double-arm module must satisfy the following conditions:

[0165]

[0166] (5) Solving module 5 refers to a double-arm module where the length of one arm is determined, while the length of the other equivalent arm is unknown. It requires solving for the rotation matrix of the previous equivalent arm relative to the natural coordinate system using the known angle parameters of the previous equivalent arm, thereby solving for the intermediate coordinates in the coordinate system corresponding to the previous equivalent arm. The parameters i and j in this double-arm module must satisfy the following conditions:

[0167]

[0168] (6) Solving module 6 refers to a double-arm module where the length of one arm is determined, while the length of the other equivalent arm is unknown. No rotation matrix is ​​needed; the intermediate coordinates of the double-arm module can be solved in the natural coordinate system. The parameters i and j in this double-arm module must satisfy the following conditions:

[0169]

[0170] The following describes the specific process of each step in the six solution modules.

[0171] Step 5.1: Solve module 1.

[0172] Solving module 1 refers to the case where the lengths of the two equivalent arms in the double-arm module are unknown. It is necessary to solve the rotation matrix using the angle parameters of the previous arm, and then solve for the intermediate coordinates in the double-arm module in the coordinate system corresponding to the previous arm.

[0173] Step 5.1.1, solve for the arm length.

[0174] A double-arm module consisting of arms from the i-th arm to the j-th arm can be represented by the symbols i to j. This double-arm module consists of w arms, where w = j + 1 - i. The starting point (x) in the double-arm module... i ,y i ,z i ) and endpoint (x) j+1 ,y j+1 ,z j+1 Let be a known quantity, and let L be the distance between the two points. i_j The solution formula is as follows:

[0175]

[0176] Next, in L i_j Given the given information, find the lengths L of the two equivalent arms in the double-arm module. i_(i+j-1) / 2 L (i+j+1) / 2_j (w is an even number) or L i_(i+j) / 2-1 and L (i+j) / 2_j (w is an odd number).

[0177] When w is even, according to the segmentation method in step 4, the section from the i-th arm to the (i+j-1) / 2-th arm is considered the first equivalent arm, and the section from the (i+j+1) / 2-th arm to the j-th arm is considered the second equivalent arm. The length symbol of the corresponding equivalent arm is: L. i_(i+j-1) / 2 and L (i+j+1) / 2_j .

[0178] To specifically calculate the equivalent boom length, two proportional distribution coefficients, k1 and k2, are introduced in this invention. k1 is the cumulative sum of the actual lengths of each boom segment. The corresponding scaling factor; k2 is the distance L between the start and end points in the double-arm module. i_j The corresponding proportionality coefficient. The specific equation for calculating the proportionality coefficient is as follows:

[0179]

[0180] From the above formula, we can obtain that the two proportional allocation coefficients k1 and k2 satisfy the following relationship:

[0181] k1+k2=1

[0182] The lengths L of the two equivalent booms i_(i+j-1) / 2 and L (i+j+1) / 2_j The solution formula is as follows:

[0183]

[0184] The lengths L of the two equivalent arms i_(i+j-1) / 2 and L (i+j+1) / 2_j Adding them together, we get:

[0185]

[0186] Substituting k1 and k2, we can simplify to get:

[0187]

[0188] From the above formula, we can see that... Under the premise that this holds true, the following conclusion must also hold: the first equivalent boom length L i_(i+j-1) / 2 Adding the second equivalent boom length L (i+j+1) / 2_j The distance L between the start and end points in the double-arm module is greater than or equal to the distance between the start and end points. i_j Less than or equal to the cumulative actual length of each boom.

[0189] When w is odd, according to the segmentation method in step 4, the section from the i-th arm to the (i+j) / 2-1-th arm is considered the first equivalent arm, and the section from the (i+j) / 2-th arm to the j-th arm is considered the second equivalent arm. The length symbol of the corresponding equivalent arm is: L.i_(i+j) / 2-1 and L (i+j) / 2_j The process for determining the equivalent boom length is the same as when w is an even number, and the proportional distribution coefficients k1 and k2 can be obtained:

[0190]

[0191] The lengths L of the two equivalent booms i_(i+j) / 2-1 and L (i+j) / 2_j The solution formula is as follows:

[0192]

[0193] exist Under the premise that this holds true, the following conclusion must also hold: the first equivalent boom length L i_(i+j) / 2-1 Adding the second equivalent boom length L (i+j) / 2_j The distance L between the start and end points in the double-arm module is greater than or equal to the distance between the start and end points. i_j Less than or equal to the cumulative actual length of each boom.

[0194] In summary, step 5.1.1 derived the lengths of the two equivalent arms in the dual-arm module when the actual number of arms w (where w = j + 1 - i) is even or odd. Next, after obtaining the lengths of the two equivalent arms, considering the influence of obstacles and other factors, the angle parameter γ of the dual-arm module is solved. i_j ψ i_j α i_j θ i_j And the coordinates of the midpoint: (x (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 ) or (x (i+j) / 2 ,y (i+j) / 2 ,z (i+j) / 2 ).

[0195] Step 5.1.2: Solve for the rotation matrix of the transformed coordinate system.

[0196] Based on the angle parameter α of the previous boom (denoted as the (i-1)th boom). i-1 θ i-1 Solve for the rotation matrix R corresponding to the arm. i-2_i-1 :

[0197]

[0198] Its inverse matrix as follows:

[0199]

[0200] The rotation matrix R of the previous arm relative to the natural coordinate system 0_i-1 The calculation formula is as follows:

[0201] R 0_i-1 =R 0_1 R 1_2 …R i-2_i-1

[0202] R here 0_i-1 It is obtained through the equivalent boom. For example, in Figure 11 (a) yields the midpoint (x5, y5, z5), thus allowing us to calculate ψ. 1_4 and γ 1_4 The equivalent rotation matrix R can be obtained through the joint angles. 0_4 Rotation matrix R 0_4 It can be calculated directly according to formula (6).

[0203] Corresponding inverse matrix The calculation formula is as follows:

[0204]

[0205] Through the inverse matrix The coordinates (x) in the natural coordinate system can be... j ,y j ,z j Convert to relative coordinates (x, y) in the coordinate system corresponding to the previous arm. j_i-1 ,y j_i-1 ,z j_i-1 ):

[0206]

[0207] Through rotation matrix R 0_i-1 The relative coordinates (x, y) in the coordinate system corresponding to the previous arm can be used to... j_i-1 ,y j_i-1 ,z j_i-1 Convert to coordinates in the natural coordinate system (x) j ,y j ,z j ):

[0208]

[0209] The rotation matrix R obtained in this step 0_i-1 and inverse matrix It can realize the mutual conversion between the coordinate values ​​of all points in the natural coordinate system and the relative coordinate values ​​in the coordinate system of the previous arm in the dual-arm model.

[0210] Step 5.1.3: Solve for the relative coordinates and natural coordinates of the intermediate point.

[0211] Taking an even number of booms (w) as an example, we can obtain the following: Figure 9 The diagram shows the parameters of the double-arm module. Based on step 5.1.2, the relative coordinates of the two endpoints of the double-arm model can be obtained. In this step, the relative coordinates (x, y, y) of the midpoint of the double-arm module in the coordinate system corresponding to the previous arm are solved. (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 ).

[0212] Based on formulas (4) and (5) in step 3, the magnitude |γ| of the position vector of the center of the spatial circle in the double-arm module can be obtained respectively. i_(i+j+1) / 2_o | and the radius r of the space circle (i+j+1) / 2 Vector γ i_(i+j+1) / 2_o Direction and vector γ i_j+1 The directions are the same, and the vector γ can be determined by the start and end points of the double-arm module. i_(i+j+1) / 2_o The direction. It is necessary to solve for the intermediate key point (x) in the double-arm module. (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 On which point of the space circle does it fall, that is, the arm angle ψ of the first equivalent arm in the double-arm module? i_(i+j-1) / 2 Solve for the position angle γ given a constant value. i_(i+j-1) / 2 The numerical values ​​are then calculated. The coordinates of the intermediate key points are then determined using an evaluation function.

[0213] Rotation matrix R i-1_j Multiply by vector (|γ) i_(i+j+1) / 2_o |,0,0) is the vector γ i_(i+j+1) / 2_o Rotation matrix R i-1_j The calculation formula is as follows:

[0214]

[0215] α in the formula i_j and θ i_j The line connecting the end point of the j-th boom and the starting point of the i-th boom, i.e., the vector γ, represents the line connecting these two points. i_j+1 and vector γ i_(i+j+1) / 2_o The yaw and pitch angles relative to the (i-1)th boom.

[0216] Rotation matrix R i-1_j Multiply by vector (|γ) i_(i+j+1) / 2_o |,r (i+j+1) / 2 ·sin(γ i_j ),r (i+j+1) / 2 ·cos(γ i_j The vector ζ corresponding to the axis of the first arm in the dual-arm module is ζ. (i+j+1) / 2 Vector ζ (i+j+1) / 2 The relative coordinates of the endpoint are (x (i+j+1) / 2_i-1,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 This point is the intermediate critical point, and the solution formula is as follows:

[0217]

[0218] τ in the formula i_j+1_(i+j+1) / 2 For the first equivalent arm end point relative to vector γ i_j+1 The phase angle.

[0219] Based on step 5.1.2, the transformation relationship between the relative coordinates and natural coordinates of the midpoint of the double-arm module can be obtained:

[0220]

[0221] Vector ζ (i+j+1) / 2 End point natural coordinates (x) (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 There is an unknown parameter γ in ) i_j The parameter γ is solved using an optimization algorithm. i_j Evaluation metrics are defined based on the task, such as furthest distance from the obstacle, minimum cumulative deflection angle, and minimum maximum deflection angle. Taking the furthest distance from the obstacle as an example, the evaluation function J in the optimization algorithm is... i_j The settings are as follows:

[0222]

[0223] Analysis parameter γ i_j When varying within the range of 0° to 360°, the evaluation function J i_j Where is the maximum value obtained? What is the evaluation function J? i_j When the value is at its maximum, the corresponding parameter γ i_j Substituting into formula (1), we obtain α. i_j and θ i_j Substituting these two angle parameters into formula (6) yields the rotation matrix R. i-1_j Then, the vector ζ can be solved using formula (7). (i+j+1) / 2 The relative coordinates (x, y) of the end point, i.e., the midpoint of the double-arm module, in the coordinate system corresponding to the previous arm are as follows: (i+j+1) / 2_i-1 ,y (i+j+1) / 2_i-1 ,z (i+j+1) / 2_i-1 The natural coordinates (x, y) of the midpoint of the double-arm module in the natural coordinate system are obtained by formula (8). (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 ).

[0224] Step 5.1.4: Output key data.

[0225] Based on steps 5.1.1, 5.1.2, and 5.1.3, the natural coordinates (x, y, y) of the midpoint of the double-arm module can be obtained. (i+j+1) / 2 ,y (i+j+1) / 2 ,z (i+j+1) / 2 Based on the single-arm module in step 2, the yaw angle α of the first equivalent arm in the double-arm module can be calculated. i_(i+j+1) / 2 Pitch angle θ i_(i+j+1) / 2 arm angle γ i_(i+j+1) / 2 Position angle ψ i_(i+j+1) / 2 Similarly, the yaw angle α of the second equivalent boom in the dual-boom module can be obtained. (i+j+1) / 2_j Pitch angle θ (i+j+1) / 2_j arm angle γ (i+j+1) / 2_j Position angle ψ (i+j+1) / 2_j .

[0226] In this step, the key data mentioned above is solved, saved, and output to lay the groundwork for other solution processes, such as the angle parameters of the front boom needed in step 5.1.2.

[0227] Step 5.2: Solve module 2.

[0228] Solution module 2 refers to a module where the lengths of the two equivalent arms in the dual-arm module are unknown. No rotation matrix is ​​needed; the intermediate coordinates of the dual-arm module can be solved in the natural coordinate system. In solution module 2, parameter i = 1, the starting point of the dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm, and the corresponding rotation matrix R of the previous arm is... 0_i-1 and its inverse matrix All are identity matrices.

[0229] Step 5.2.1 is to solve for the boom length, and the solution process is the same as step 5.1.1.

[0230] Step 5.2.2 is to solve for the rotation matrix of the transformed coordinate system. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm and there is no previous arm, the identity matrix can be output in this step.

[0231] Step 5.2.3 involves solving for the relative and natural coordinates of the intermediate point. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm, the natural and relative coordinates in this dual-arm module are equal. The solution process is the same as in step 5.1.3, with the special point being the rotation matrix R. 0_i-1 and its inverse matrix All are identity matrices.

[0232] Step 5.2.4 is to output key data, which is the same as step 5.1.4.

[0233] Step 5.3: Solve module 3.

[0234] Solution module 3 refers to the double-arm module where the lengths of both arms are determined. It requires solving the rotation matrix using the angle parameters of the previous arm to find the intermediate coordinates within the coordinate system corresponding to the previous arm. In solution modules 1 and 2, parameters i and j satisfy the condition j ≥ i + 3. Dividing the i-th to j-th arms evenly into two parts creates two equivalent arms. Each equivalent arm includes at least two arms, therefore the lengths of these two equivalent arms are unknown. In solution modules 3 and 4, parameters i and j satisfy the condition j = i + 1, meaning the double-arm module has exactly two actual arms. Dividing it into two parts results in each part corresponding to one actual arm, therefore the lengths of these two arms are known.

[0235] Step 5.3.1 is to solve for the length of the boom. Since the lengths of both booms are known, we only need to substitute the initial known conditions and there is no need to solve for the length.

[0236] Step 5.3.2 is to solve for the rotation matrix of the transformed coordinate system. The solution process is the same as in step 5.1.2.

[0237] Step 5.3.3 is to solve for the relative coordinates and natural coordinates of the intermediate point. The solution process is the same as in step 5.1.3.

[0238] Step 5.3.4 is to output key data, which is the same as step 5.1.4.

[0239] Step 5.4: Solve module 4.

[0240] Solving module 4 refers to the fact that the lengths of the two arms in the double-arm module are determined, and the intermediate coordinates of the double-arm module can be solved in the natural coordinate system without the need for a rotation matrix.

[0241] Step 5.4.1 is to solve for the length of the boom. Since the lengths of both booms are known, we only need to substitute the initial known conditions and there is no need to solve for the length.

[0242] Step 5.4.2 is to solve for the rotation matrix of the transformed coordinate system. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm and there is no previous arm, the identity matrix can be output in this step.

[0243] Step 5.4.3 involves solving for the relative and natural coordinates of the intermediate point. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm, the natural and relative coordinates in this dual-arm module are equal. The solution process is the same as in step 5.1.3, with the special point being the rotation matrix R. 0_i-1 and its inverse matrix All are identity matrices.

[0244] Step 5.4.4 outputs key data, which is the same as step 5.1.4.

[0245] Step 5.5: Solve module 5.

[0246] Solution module 5 refers to a double-arm module where the length of one arm is determined, while the length of the other equivalent arm is unknown. It requires solving for the rotation matrix using the angle parameters of the previous arm, thereby determining the intermediate coordinates of the double-arm module within the coordinate system corresponding to the previous arm. Parameters i and j satisfy the condition j≥i+3 in solution modules 1 and 2, j=i+1 in solution modules 3 and 4, and j=i+2 in solution modules 5 and 6. The double-arm modules in solution modules 5 and 6 have exactly three actual arms. Dividing them into two parts, one part corresponds to one actual arm, and the other part corresponds to two actual arms. Therefore, one arm's length is determined, while the length of the other equivalent arm is unknown.

[0247] Step 5.5.1 involves solving for the boom length. The length of the first boom is known and does not need to be solved. The calculation method for the length of the second equivalent boom is the same as in step 5.1.1.

[0248] Step 5.5.2 is to solve for the rotation matrix of the transformed coordinate system. The solution process is the same as in step 5.1.2.

[0249] Step 5.5.3 is to solve for the relative coordinates and natural coordinates of the intermediate point. The solution process is the same as in step 5.1.3.

[0250] Step 5.5.4 is to output key data, which is the same as step 5.1.4.

[0251] Step 5.6: Solve module 6.

[0252] Solving module 6 refers to a double-arm module where the length of one arm is determined and the length of the other equivalent arm is unknown. Without the need for a rotation matrix, the intermediate coordinates of the double-arm module can be solved in the natural coordinate system.

[0253] Step 5.6.1 involves solving for the boom length. The length of the first boom is known and does not need to be solved. The calculation method for the length of the second equivalent boom is the same as in step 5.1.1.

[0254] Step 5.6.2 is to solve for the rotation matrix of the transformed coordinate system. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm and there is no previous arm, the identity matrix can be output in this step.

[0255] Step 5.6.3 involves solving for the relative and natural coordinates of the intermediate point. Since the starting point in this dual-arm module is the starting point (x1, y1, z1) of the entire robotic arm, the natural and relative coordinates in this dual-arm module are equal. The solution process is the same as in step 5.1.3, with the special point being the rotation matrix R. 0_i-1 and its inverse matrix All are identity matrices.

[0256] Step 5.6.4 is to output key data, which is the same as step 5.1.4.

[0257] Step 6: Output real-time data to complete the desired task.

[0258] After inputting the target point, obstacles, and motion environment, the position data of each key point is obtained according to the above steps. Through the basic kinematic transformation principle, the key point position is converted into deflection angle. Finally, the real-time data of each deflection angle is converted into motor rotation angle data to realize the path planning and obstacle avoidance motion planning of the robotic arm, so that the robotic arm can complete the expected task.

[0259] This invention solves the complex problem of inverse kinematics calculation for multi-degree-of-freedom robotic arms. The effectiveness of the method in this invention is verified through simulation as follows:

[0260] Example 1:

[0261] Taking the movement of the end point of a robotic arm comprising eight levers from the initial point (3100,0,0) to the termination point (2000,100,100) as an example, the method of this invention is introduced. The lengths of the eight levers are 480, 480, 480, 480, 480, 200, 200, and 300 mm, respectively, and the movement time is from 0 to 100 seconds. A cylindrical obstacle exists in this movement environment, and the radius of the cylindrical obstacle is r. obs =200, its axis is parallel to the z-axis, and the axis is located at x obs =1000, y obs =500 locations.

[0262] Figure 10 This is a segmented process diagram of an eight-arm robotic arm. Figure 11 The following is the kinematics solution process for an eight-arm robotic arm, where Figure (a) shows the initial conditions, Figure (b) shows the coordinates of the first set of key points, Figure (c) shows the coordinates of the second set of key points, and Figure (d) shows the coordinates of the third set of key points.

[0263] The specific solution process is as follows:

[0264] (1) First group solution. Divide the 8-arm rod into two parts, each part including 4 arms, to construct a double-arm rod module. Based on the starting point (x1, y1, z1) and the ending point (x9, y9, z9), solve L through solution module 2. 1_4 L 5_8 Then, solve for the intermediate point (x5, y5, z5). Treating the line connecting point (x5, y5, z5) and point (x1, y1, z1) as an equivalent single-arm rod, we obtain γ. 1_4 ψ 1_4 α 1_4 θ 1_4 .

[0265] (2) Second group solution. Divide the first four booms into two parts, each part including two booms, to construct a double boom module. Based on the point (x1, y1, z1) and the obtained point (x5, y5, z5), solve for L using solution module 2. 1_2 L 3_4 Then, solve for the intermediate point (x3, y3, z3). Treating the line connecting point (x3, y3, z3) and point (x1, y1, z1) as an equivalent single-arm rod, we obtain γ. 1_2 ψ 1_2 α 1_2 θ 1_2 .

[0266] (3) Second group solution. Divide the last four booms into two parts, each part including two booms, constructing a double boom module. Using the obtained γ... 1_4 ψ 1_4 α 1_4 θ 1_4 The coordinate system rotation matrix is ​​obtained, and then L is solved using solver module 1 based on points (x5, y5, z5) and (x9, y9, z9). 5_6 L 7_8 Then, according to α 1_4 θ 1_4 Solve for the rotation matrix used for coordinate transformation, and then solve for the intermediate point (x7, y7, z7). Treat the line connecting points (x7, y7, z7) and (x5, y5, z5) as an equivalent single-arm rod, and obtain γ. 5_6 ψ 5_6 α 5_6 θ 5_6 .

[0267] (4) Third group solution. Divide the first two booms into two parts, each part including one actual boom, to construct a double boom module. Based on the points (x1, y1, z1), L1, L2 and the obtained point (x3, y3, z3), solve the intermediate point (x2, y2, z2) through the solution module 4, and solve the four angles γ1, ψ1, α1, θ1 of the first single boom.

[0268] (5) Solving the third set of problems. Divide the third and fourth booms into two parts, each consisting of one actual boom, to construct a double boom module. Using the obtained γ... 1_2 ψ 1_2 α 1_2 θ 1_2 The coordinate system rotation matrix is ​​obtained. Then, based on L3, L4, the obtained points (x3, y3, z3) and (x5, y5, z5), the intermediate point (x4, y4, z4) is solved through the solution module 3, and the four angles γ3, ψ3, α3, and θ3 of the third single arm are solved.

[0269] (6) Third group solution. Divide the 5th and 6th booms into two parts, each including one actual boom, constructing a double boom module. Using the obtained γ... 1_4 ψ 1_4 α 1_4 θ 1_4 The coordinate system rotation matrix is ​​obtained. Then, based on L5, L6, the obtained points (x5, y5, z5) and (x7, y7, z7), the intermediate point (x6, y6, z6) is solved through the solution module 3, and the four angles γ5, ψ5, α5, and θ5 of the fifth single arm are solved.

[0270] (7) Solving the third set of problems. Divide the 7th and 8th booms into two parts, each consisting of one actual boom, to construct a dual-boom module. Using the obtained γ... 5_6 ψ 5_6 α 5_6 θ 5_6 The coordinate system rotation matrix is ​​obtained. Then, based on L7, L8, (x9, y9, z9) and the obtained point (x7, y7, z7), the intermediate point (x8, y8, z8) is solved through the solution module 3, and the four angles γ7, ψ7, α7, and θ7 of the seventh single arm are solved.

[0271] Through simulation using the method of this invention, the following results can be obtained. Figure 12 The motion trajectories of the end points of each boom shown in the figure demonstrate that each boom will not collide with obstacles and that the end points can move along the desired trajectory, thus confirming the feasibility of the present invention.

[0272] The closest distance d between the robotic arm and the central axis of the obstacle during its movement. min The change curve is as follows Figure 13 As shown in the figure, the closest distance between the robotic arm and the obstacle is always greater than 200, meaning that a collision will not occur. This demonstrates that the method in this invention can not only achieve the desired motion but also perform additional tasks such as obstacle avoidance.

[0273] This invention can be used for kinematic transformation and solution of multi-degree-of-freedom moving objects such as surgical robots, continuous robotic arms, and snake-like robotic arms, laying the foundation for control. The methods in this invention can be applied to modularization, segmented planning, and other fields, which is beneficial for achieving high real-time and high-precision control.

[0274] The present invention has been described in detail above with reference to specific embodiments and exemplary examples; however, these descriptions should not be construed as limiting the present invention. Those skilled in the art will understand that various equivalent substitutions, modifications, or improvements can be made to the technical solutions and embodiments of the present invention without departing from the spirit and scope of the invention, and all such modifications and improvements fall within the scope of the present invention. The scope of protection of the present invention is defined by the appended claims.

[0275] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A modular kinematics solution method, characterized in that, include: S1 constructs a single-arm lever module and obtains the mapping relationship between the end point of the single-arm lever module and the angle parameters; S2 constructs a double-arm module and obtains the solution model for the intermediate key point of the double-arm module. The intermediate key point is the end point of the first arm in the double-arm module. S3 will include n The robotic arm with one arm is equivalent to a double-arm module. The two arms in the double-arm module are both equivalent arms. The position coordinates of the intermediate key points are obtained by solving the model based on the intermediate key points. The first and second equivalent booms are respectively equivalent to two single boom modules. Based on the mapping relationship between the end point and angle parameters of the single boom module and the position coordinates of the intermediate key points, the angle parameters of the two single boom modules are obtained. When the number of arms in the single-arm module obtained in the previous step is greater than 1, S4 will convert the single-arm module into a double-arm module and obtain the position coordinates of the intermediate key points based on the solution model of the intermediate key points. The first and second equivalent arms of the double-arm module are respectively equivalent to two single-arm modules. Based on the mapping relationship between the end point and angle parameters of the single-arm module and the position coordinates of the intermediate key points, the angle parameters of the two single-arm modules are obtained. S5 Repeat step S4 until all single-arm modules obtained in the previous step contain an arm with a number of arms equal to 1, and obtain the angle parameters and end point position coordinates of each arm. S6 obtains the motor rotation angle data based on the angle parameters of each boom and the coordinates of the end point position; The dual-arm module consists of two adjacent single-arm arms.

2. The modular kinematics solution method according to claim 1, characterized in that, The angle parameters of the single-arm module include the first i Yaw angle of the boom α i Pitch angle θ i Position angle ψ i and arm angle γ i ; i ≥1; The mapping relationship between the end point of the single-arm module and the angle parameters includes: in, L i For the first i The length of the first boom, i.e., the length of the second boom. i The starting point of the boom ( x i , y i , z i ) and the i The end point of each boom ( x i+1 , y i+1 , z i+1 The distance between them; The boom angle is the angle between the boom axis and the axis of the previous boom, and the position angle is the line segment projected onto the end face of the previous boom and the coordinate system of the previous boom. y The angle between axes.

3. The modular kinematics solution method according to claim 1, characterized in that, Let the two adjacent single-arm poles be labeled as the first. i The single arm pole and the first i +1 single-arm pole, i ≥1; The solution model for the key points in the middle of the double-arm module includes: ； ； ； in, r i+1 For the first i The radius of the spatial circle formed by the reachable positions of the ends of a single-arm lever in three-dimensional space. γ i_i+1_o For the first i The center of the spatial circle formed by the reachable positions of the ends of the single-arm levers in three-dimensional space and the first i The vector of the line connecting the starting points of each arm. ψ i+1 , α i+1 , θ i+1 The first i +1 single-arm position angle, yaw angle, and pitch angle L i and L i+1 The first i The single arm pole and the first i +1 unit length of single arm pole, γ i_i+2 For the first i +1 end point of a single arm ( x i+2 , y i+2 , z i+2 ) and the i A single-arm pole starting point ( x i , y i , z i The vector of the connecting line, .

4. The modular kinematics solution method according to claim 3, characterized in that, In the dual-arm lever module, step S3 will include n When a single-arm robotic arm is equivalent to a dual-arm module, n If the number is even, then the number of arms in the first and second equivalent arms of the double-arm module is both 1 / 2. n / 2; when n If the number is odd, then the number of arms in the first and second equivalent arms of the double-arm module are respectively ( n -1) / 2 and ( n +1) / 2; S4 will include w When a single-arm module of a boom is equivalent to a double-arm module, w If the number is even, then the number of arms in the first and second equivalent arms of the double-arm module is both 1 / 2. w / 2; when w If the number is odd, then the number of arms in the first and second equivalent arms of the double-arm module are respectively ( w -1) / 2 and ( w +1) / 2; w >1.

5. The modular kinematics solution method according to claim 1, characterized in that, In step S4, it is assumed that the dual-arm lever module contains n One of the booms w One boom, w The first boom is respectively the first i ~ j One boom, 1≤ i ≤ j ≤ n , w = j +1- i The starting point in the double-arm module is ( x i , y i , z i ), the destination is ( x j+1 , y j+1 , z j+1 ); when w When it is even, the first i The boom to the ( i + j -1) / 2 booms are considered as the first equivalent boom, and the ( i + j +1) / 2 booms to the first j The first boom is considered as the second equivalent boom, and the coordinates of the intermediate key point are ( x (i+j+1) / 2 , y (i+j+1) / 2 , z (i+j+1) / 2 );when w When it is an odd number, the first... i The boom to the ( i + j The first () / 2-1 boom is considered the first equivalent boom, and the () i + j ) / 2 booms to the first j The first boom is considered as the second equivalent boom, and the coordinates of the intermediate key point are ( x (i+j) / 2 , y (i+j) / 2 , z (i+j) / 2 ); In the dual-arm module, the methods for obtaining the position coordinates of the intermediate key points from the solution model include: when w When it is even, ； when w When it is an odd number, ； in,( x (i+j+1) / 2_i-1 , y (i+j+1) / 2_i-1 , z (i+j+1) / 2_i-1 )and( x (i+j) / 2_i-1 , y (i+j) / 2_i-1 , z (i+j) / 2_i-1 () is the intermediate key point in the first i -1 The position coordinates of the boom in the body coordinate system. R i-1_j For the first j The first boom relative to the first i -1 Rotation matrix of the body coordinate system of the arm. γ i_(i+j+1) / 2_o The end point of the first equivalent boom, i.e., the ( i + j The center of the spatial circle formed by the optional positions of +1) / 2 points in three-dimensional space is parallel to the first... i The vector of the line connecting the starting points of each arm. r (i+j+1) / 2 Let be the radius of the spatial circle formed by the reachable positions of the end points of the first equivalent arm in three-dimensional space. τ i_j+1_(i+j+1) / 2 For the first equivalent arm end point relative to the vector γ i_j+1 The phase angle; R 0_i-1 For the first i -1 is the rotation matrix of the boom relative to the natural coordinate system, which will be the first... i -1 boom end point, i.e. i The line connecting the first point to the starting point of the first boom (i.e., the first point) is considered as an equivalent boom. The corresponding yaw angle can be calculated based on the position coordinates. α 1_i-1 and pitch angle θ 1_i-1 , R 0_i-1 The solution formula is shown below:

6. The modular kinematics solution method according to claim 5, characterized in that, In step S4, when i When =1, R 0_i-1 It is the identity matrix; ； in, α i+1_j and θ i+1_j The first j The end point of the boom and the first i The line connecting the +1 boom start points relative to the first... i The yaw and pitch angles of the boom.

7. The modular kinematics solution method according to claim 5, characterized in that, when w When it is even, ; ； when w When it is an odd number, ; ； 。 8. The modular kinematics solution method according to claim 7, characterized in that, When the first or second equivalent boom contains only one boom, the length of the equivalent boom is equal to the length of one boom. When the first or second equivalent boom contains more than one boom, the lengths of the two equivalent booms are determined as follows: Solve for the starting point of the double-arm module ( x i , y i , z i ) and endpoint ( x j+1 , y j+1 , z j+1 Distance between: ； Set the proportional allocation coefficient k 1 and k 2: ； according to L i_j The lengths of the two equivalent booms are obtained by using the proportional distribution coefficient: when w When it is even, ； when w When it is an odd number, ; L m For the first m The length of the arm.

9. The modular kinematics solution method according to claim 5, characterized in that, τ i_j+1_(i+j+1) / 2 or τ i_j+1_(i+j) / 2 Determined using the following method: when w When the number is even, construct the evaluation function. ; in, x obs and y obs Let the coordinates of the obstacle be... τ i_j+1_(i+j+1) / 2 Varying within the range of 0° to 360°, and solving for each... τ i_j+1_(i+j+1) / 2 The corresponding ones and , will each and Substitute into the evaluation function, and output the result that makes the evaluation function... J i_j Maximum τ i_j+1_(i+j+1) / 2 ,Finish τ i_j+1_(i+j+1) / 2 The determination; when w When the number is odd, construct the evaluation function. ;in, x obs and y obs Let the coordinates of the obstacle be... τ i_j+1_(i+j) / 2 Varying within the range of 0° to 360°, and solving for each... τ i_j+1_(i+j) / The corresponding ones and , will each and Substitute into the evaluation function, The output makes the evaluation function J i_j Maximum τ i_j+1_(i+j) / 2 ,Finish τ i_j+1_(i+j) / 2 The determination.

10. A modular kinematics solution method according to claim 5, characterized in that, In step S4, the method for determining the coordinates of the intermediate key points includes: when w Even numbers: Based on the known coordinates ( x 1, y 1, z 1) and the obtained coordinates ( x i , y i , z i ),get α 1_i-1 and θ 1_i-1 ,according to α 1_i-1 and θ 1_i-1 get R 0_i-1 ; Based on the obtained coordinates ( x i , y i , z i ) and coordinates ( x j+1 , y j+1 , z j+1 ),get α i_j and θ i_j ,according to α i_j and θ i_j get R i-1_j ; according to R i-1_j and certain τ i_j+1_(i+j+1) / 2 Get the intermediate key points first i -1 relative position coordinates in the boom coordinate system ( x (i+j+1) / 2_i-1 , y (i+j+1) / 2_i-1 , z (i+j+1) / 2_i-1 ); according to R 0_i-1 and relative position coordinates ( x (i+j+1) / 2_i-1 , y (i+j+1) / 2_i-1 , z (i+j+1) / 2_i-1 Solve for the natural coordinates of the intermediate key points. x (i+j+1) / 2 , y (i+j+1) / 2 , z (i+j+1) / 2 ); when w Odd number: Based on the known coordinates ( x 1, y 1, z 1) and the obtained coordinates ( x i , y i , z i ),get α 1_i-1 and θ 1_i-1 ,according to α 1_i-1 and θ 1_i-1 get R 0_i-1 ; Based on the obtained coordinates ( x i , y i , z i ) and coordinates ( x j+1 , y j+1 , z j+1 ),get α i_j and θ i_j ,according to α i_j and θ i_j get R i-1_j ; according to R i-1_j and certain τ i_j+1_(i+j) / 2 Get the intermediate key points first i -1 relative position coordinates in the boom coordinate system ( x (i+j) / 2_i-1 , y (i+j) / 2_i-1 , z (i+j) / 2_i-1 ); according to R 0_i-1 and relative position coordinates ( x (i+j) / 2_i-1 , y (i+j) / 2_i-1 , z (i+j) / 2_i-1 Solve for the natural coordinates of the intermediate key points. x (i+j) / 2 , y (i+j) / 2 , z (i+j) / 2 ).

11. The modular kinematics solution method according to claim 5, characterized in that, When the dual-arm module obtains the position coordinates of the intermediate key points from the solution model, there are six possible cases: Scenario 1: In the dual-arm module, the lengths of both equivalent arms are unknown. First, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is calculated using the known angle parameters of the previous equivalent arm. Then, this rotation matrix is ​​used to obtain the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm. Finally, the coordinates of the intermediate key point are... i -1 The coordinates of the boom in the body coordinate system are converted to the coordinates of the intermediate key points in the natural coordinate system; Scenario 2: The lengths of the two equivalent arms in the double-arm module are unknown, so the coordinates of the intermediate key point in the natural coordinate system are obtained directly. Scenario 3: In the dual-arm module, the lengths of the two equivalent arms are determined. First, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is calculated using the known angle parameters of the previous equivalent arm. Then, this rotation matrix is ​​used to obtain the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm. Finally, the coordinates of the intermediate key point are... i -1 The coordinates of the boom in the body coordinate system are converted to the coordinates of the intermediate key points in the natural coordinate system; Scenario 4: This directly yields the coordinates of the intermediate key points in the natural coordinate system; Scenario 5: In a dual-arm module, the length of one arm is determined, while the length of the other equivalent arm is unknown. First, the rotation matrix of the previous equivalent arm relative to the natural coordinate system is calculated using the known angle parameters of the previous arm. Then, this rotation matrix is ​​used to obtain the coordinates of the intermediate key point in the body coordinate system of the previous equivalent arm. Finally, the coordinates of the intermediate key point are... i -1 The coordinates of the boom in the body coordinate system are converted to the coordinates of the intermediate key points in the natural coordinate system; Situation 6: This directly yields the coordinates of the intermediate key points in the natural coordinate system.

Images