Series robot and general control method and general control device thereof
By acquiring the DH model parameters and the target pose matrix, and combining Newton's iteration and random walk methods, the limitations of traditional six-axis robot control methods on different models are solved, enabling universal control and efficient computation for different robot models.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-12-29
- Publication Date
- 2026-03-17
AI Technical Summary
Traditional control methods for six-axis serial industrial robots can only be applied to a specific model and cannot meet the needs of general-purpose control.
A general control method for serial robots is provided. By obtaining the structural parameters of the DH model and the pose matrix of the target end effector, the target angles of each joint are solved using the Newton iteration method and the random walk method. The dimensionality reduction method is combined to reduce the amount of iterative computation, thereby achieving general control of different robot models.
It achieves universal control of different models of serial robots, improves computational efficiency and adaptability, reduces iterative computation, and meets the integration needs of robots in different scenarios.
Smart Images
Figure CN121670653A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of industrial robot technology, and more specifically, to a serial robot and its general control method and device. Background Technology
[0002] Traditional control methods for six-axis serial industrial robots are typically applicable only to a specific model; that is, a single control method can only be used for one type of six-axis serial industrial robot. This fails to meet the needs of certain testing or integration fields that require a universal control method. A universal control method, in contrast, refers to changing the control mechanism simply by altering the robot's DH parameter model, thereby achieving universal control of any six-axis serial robot without end-effector X-displacement. Summary of the Invention
[0003] The purpose of this invention is to provide a serial robot and its general control method and device, so as to solve the technical problem that the control methods of existing six-axis serial industrial robots can only be applied to a specific model.
[0004] The present invention provides a general control method for a serial robot, wherein each axis of the serial robot is a rotation axis, and the plurality of rotation axes are numbered from the first end near the base to the last end as 1 to n; The method includes: Obtain the DH model structural parameters of the serial robot and the target end-effector pose matrix of the nth axis of the serial robot; Calculate the target end-effector pose matrix of the (n-1)th axis of the serial robot; The target angles of each joint on the first n-1 axes of the serial robot are solved using Newton's iteration method.
[0005] As one possible implementation, the total number of the rotating axes is 6, and the axial direction of the 6th axis of the serial robot coincides with the TCP-Z direction.
[0006] As one possible implementation method, in the Newton iteration method, The constraint equations include: , , , , , in, Let be the end-effector pose matrix of the fifth axis of the serial robot; The Jacobian matrix is: , Where i takes values from 1 to 6, , , , , These are the joint angles of the first 5 axes of the serial robot.
[0007] As one possible implementation, the target end-effector pose matrix is: ; The z-axis attitude vector of the fifth axis is: ; The end position matrix of the fifth axis is as follows: ;in, This is the preset Z-axis displacement of the 6th axis; In the Newton-Raphson iteration method, the residual formula is: ;in, = [1,1], = [1,1], = [1,2], = [1,2], = [1,3], = [1,3].
[0008] As one possible implementation, the method further includes: The optimal initial point of the Newton-Raphson iteration method is selected using a random walk method.
[0009] As one possible implementation method, the random walk method includes: Obtain the actual end pose matrix of the (n-1)th axis; compare the target end pose matrix of the (n-1)th axis with the current actual pose matrix of the (n-1)th axis to calculate the trajectory parameter points; calculate the number of walk points based on the trajectory parameter points; Obtain the actual angles of each joint on the first n-1 axes, and use the actual angles of each joint as the optimal initial position; perform iterative calculations based on the optimal initial position; If the calculation is successful and the result meets the preset conditions, the result is taken as the optimal initial point. If the number of successful calculations is greater than 0 and none of them meet the preset conditions, the best result is selected from several results and taken as the optimal initial point. If the number of successful calculations is equal to 0, an error signal is output.
[0010] As one possible implementation, the iterative calculation based on the optimal initial point includes: Each time a calculation is completed, the calculation count is incremented by 1; if the calculation is successful, the walkpoint count is incremented by 1, and it is determined whether the calculation result meets the preset conditions; wherein, the preset conditions include: the joint sum difference is less than the sum difference threshold, and / or, the joint variance is less than the variance threshold; If the calculation result does not meet the preset conditions, the number of calculations has not reached the calculation threshold, and the number of walkpoints has not reached the walkpoint threshold, then it is determined whether the current calculation result is better than the previous calculation result. If yes, then replace the optimal initial point with the current calculation result, generate a random walk point based on the optimal initial point, use the random walk point as the optimal initial point, and iterate again; if no, generate a random walk point based on the optimal initial point, use the random walk point as the optimal initial point, and iterate again.
[0011] As one possible implementation method, the formula for calculating the number of walk points is: , where s is the trajectory length, v is the terminal velocity, and dt is the average single-point iteration time.
[0012] The present invention provides a universal control device for a serial robot, wherein each axis of the serial robot is a rotary axis, and the rotary axes are numbered 1 to n from the first end near the base to the last end; The device includes: The acquisition module is used to acquire the DH model structural parameters and target end pose matrix of the serial robot; The calculation module calculates the target end-effector pose matrix of the (n-1)th axis of the serial robot; The solution module uses Newton's iteration method to solve for the target angles of each joint on the first n-1 axes of the serial robot.
[0013] The serial robot provided by the present invention includes a controller, which is used to execute the general control method of the serial robot described above.
[0014] Compared with the prior art, the beneficial effects of the present invention are as follows: The serial robot and its universal control method and device provided by this invention can be adapted to different models of serial robots by simply changing the structural parameters of the DH model. It is applicable to all serial robots without end-effector X-displacement, no longer limited to a single model, and provides an integrated solution for later integration with higher-level control, effectively achieving universal control of serial robots. Furthermore, the universal control method provided in this embodiment uses Newton's iteration method for solution, achieving rapid local convergence. Based on this, combined with dimensionality reduction, the end-effector pose constraints are reduced, retaining only necessary parameters, effectively reducing the amount of iterative computation and improving computational efficiency. Attached Figure Description
[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.
[0016] Figure 1 This is a first schematic flowchart of a general control method for a serial robot provided in an embodiment of the present invention; Figure 2 This is a first schematic flowchart of the random walk method provided in an embodiment of the present invention; Figure 3 This is a second schematic flowchart of the random walk method provided in an embodiment of the present invention; Figure 4 This is a third schematic flowchart of the random walk method provided in an embodiment of the present invention; Figure 5 This is a schematic diagram of the structure of a general control device for a serial robot provided in an embodiment of the present invention. Detailed Implementation
[0017] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0018] The present invention will now be described in further detail with reference to specific embodiments and accompanying drawings.
[0019] Figure 1 A schematic flowchart of a general control method for a serial robot provided in one embodiment of the present invention, wherein each axis of the serial robot is a rotation axis, and a plurality of rotation axes are numbered 1 to n from the first end near the base to the last end; the method includes: S102, obtain the structural parameters of the DH model of the serial robot and the target end pose matrix of the nth axis of the serial robot; Specifically, the structural parameters of the DH model can be obtained through the DH method (Denavit-Hartenberg method). The DH method establishes a link coordinate system on the serial robot and obtains the transformation matrix of adjacent joints. Finally, the pose matrix of the end effector of the serial robot relative to the base can be obtained: Different models of serial robots generally have different DH model structural parameters. Once the model of the serial robot to be controlled is determined, its corresponding DH model structural parameters can also be determined. The target end-effector pose matrix of the nth axis of the serial robot is equivalent to the desired end-effector pose of the serial robot.
[0020] S104, Calculate the target end-effector pose matrix of the (n-1)th axis of the serial robot; Based on the structural parameters of the DH model of the serial robot and the target end pose matrix of the nth axis of the serial robot, the target end pose matrix of the (n-1)th axis of the serial robot is calculated to reduce the dimensionality of the serial robot.
[0021] S106. Using Newton's iteration method, solve for the target angles of each joint on the first n-1 axes of the serial robot.
[0022] The general control method for serial robots provided in this embodiment can adapt to different models of serial robots simply by changing the structural parameters of the DH model. It is applicable to all serial robots without end-effector X displacement, no longer limited to a single model, and can provide an integrated solution for later integration with higher-level control, effectively achieving general control of serial robots. Furthermore, the general control method provided in this embodiment uses Newton's iteration method for solution, achieving rapid local convergence. Based on this, combined with dimensionality reduction, the end-effector pose constraints are reduced, retaining only necessary parameters, which effectively reduces the amount of iterative computation and improves computational efficiency.
[0023] Preferably, the total number of rotation axes of the serial robot can be set to 6, that is, the serial robot is a six-axis serial robot, which can achieve a configuration of full degrees of freedom of spatial motion and has strong end effector flexibility and adaptability. At the same time, the axis of the 6th axis coincides with the TCP-Z direction (the Z direction of the center point of the end effector).
[0024] The transformation matrix of adjacent joints is: ;in, To connect the angles of the various rotation axes of the robot, ， ， These are the structural parameters of the DH model; for example, .
[0025] The end-effector pose matrix of the sixth axis is: .
[0026] In step S106 above, The constraint equations include: , , , , , in, This is the end-effector pose matrix for the 5th axis of the serial robot. For matrix The formula for calculating the element in the 1st row and 3rd column of the table. For matrix The formula for calculating the element in the 1st row and 4th column. For matrix The formula for calculating the element in the 2nd row and 3rd column of the text. For matrix The formula for calculating the element in the 2nd row and 4th column of the text. For matrix The formula for calculating the element in the 3rd row and 3rd column of the text. For matrix The formula for calculating the element in the 3rd row and 4th column.
[0027] The Jacobian matrix is: , Where i takes values from 1 to 6, , , , , These are the joint angles of the first 5 axes of the serial robot.
[0028] The target end pose matrix in step S102 above can be expressed as: Where n, o, and a are unit vectors (attitudes), which can form the end-effector attitude matrix R, satisfying The first three rows of the fourth column are... =[ ]ᵀ (TCP position) is the Cartesian coordinate of the TCP in the robot's base coordinate system. The matrix... All element values in the table are preset known values.
[0029] Since the 6th axis only generates Z-axis rotation and not Z-axis displacement, as well as X-axis displacement and rotation, the robot's dimensions can be reduced when calculating the inverse kinematics. The end-effector pose matrix of the 5th axis is then used. It satisfies the following formula: ,in, Let be the end-point attitude matrix of the sixth axis; then, the z-axis attitude vector of the fifth axis can be obtained as: The end position matrix of the 5th axis is: In the above formula, This is the Z-direction displacement of the 6th axis, which is a preset known value.
[0030] The residual formula is: ;in, = [1,1], = [1,1], = [1,2], = [1,2], = [1,3], = [1,3]. That is, , [1,1], , , , .
[0031] The method provided in this embodiment further includes: selecting the optimal initial position for the Newton-Raphson iteration method using a random walk method. This reduces the probability of the Newton-Raphson iteration getting trapped in local optima, thus meeting the pose requirements for the serial robot in actual operation.
[0032] Figure 2 The above-described random walk method is illustrated in a schematic flowchart. The method includes: S202, obtain the actual end pose matrix of the (n-1)th axis; compare the target end pose matrix of the (n-1)th axis with the current actual pose matrix of the (n-1)th axis to calculate the trajectory parameter points; calculate the number of walk points based on the trajectory parameter points; S204: Obtain the actual angles of each joint on the first n-1 axes, and use the actual angles of each joint as the optimal initial points; perform iterative calculations based on the optimal initial points. S206, If the calculation is successful and the calculation result meets the preset conditions, then the calculation result will be used as the optimal initial point; S208. If the number of successful calculations is greater than 0 and none of them meet the preset conditions, then select the best calculation result from several calculation results as the optimal initial point. S210: If the number of successful calculations is 0, an error signal will be output.
[0033] The core of the above random walk algorithm is as follows: based on the current position, iterate the inverse solution. After the iteration is completed, another set of initial points is randomly generated and iterated again. If the iteration result is better than the initial point of the optimal inverse solution, the optimal initial point is changed, and the initial point is randomly generated again based on the optimal initial point and substituted into the calculation. This continues until the number of calculations or the number of successful walks reaches the set value, or the calculation result directly meets the threshold, at which point it is used directly.
[0034] It should be noted that by using the random walk method, combined with the control cycle, and dynamically adjusting the number of walk points, the output rate of the global optimal solution can be effectively improved; and when the calculation result cannot be obtained, an error can be reported.
[0035] See Figure 3 The above step S204 may specifically include the following steps: S2042, for each calculation completed, the calculation count is incremented by 1; if the calculation is successful, the walkpoint count is incremented by 1, and it is determined whether the calculation result meets the preset conditions; wherein, the preset conditions include: the joint sum difference is less than the sum difference threshold, and / or, the joint variance is less than the variance threshold; S2044, If the calculation result does not meet the preset conditions, the number of calculations has not reached the calculation threshold, and the number of walk points has not reached the walk threshold, then determine whether the current calculation result is better than the previous calculation result. S2046, if so, then replace the optimal initial point with the current calculation result, generate a random walk point based on the optimal initial point, use the random walk point as the optimal initial point, and iterate again; S2048. If not, then based on the optimal initial point, generate a random walk point, use the random walk point as the optimal initial point, and iterate again.
[0036] It should be noted that by using the joint sum minimum strategy (i.e., minimizing the sum of the differences between the actual joint angles in the current pose and the calculated target joint angles), the joint variance minimum strategy, or other optimal solution strategies to constrain the calculation results, joint motion can be optimized.
[0037] A walk coefficient can be added when generating random walk points.
[0038] In step S202 above, the trajectory parameter points include trajectory length s, terminal velocity v, average single-point iteration time dt, and the formula for calculating the number of walk points is: .
[0039] Figure 4 A schematic flowchart of a general control method for a serial robot provided in one embodiment of the present invention, the method comprising: S301, inverse solution begins; S302, based on trajectory parameter points, trajectory length, end velocity, and average single-point iteration time, calculate the number of walk points; S303, initial position, optimal initial position, select the actual angles of each joint on the first n-1 axes; S304, iterative calculation, calculation count incremented by 1; successful calculation, walkpoint count incremented by 1; S305, determine whether the calculation was successful and whether the joint sum is less than the sum threshold; if yes, proceed to step S313; if no, proceed to step S306. S306, determine whether the number of calculations has reached the calculation threshold, or whether the number of walks has reached the walk threshold; if yes, proceed to step S310; if no, proceed to step S307. S307, Determine whether the current calculation result is better than the previous calculation result; if yes, proceed to step S308; if no, proceed to step S309. S308, Current calculation result, replace the optimal initial point; S309, Based on the optimal initial position, randomly generate a unit vector, multiply it by the walk coefficient, and generate a random walk point; return to execute step S304; S310, determine whether the calculated number is greater than 0; S311, if not, an error message will be output; S312, if so, then output the best option; S313, End calculation, output calculation results.
[0040] Figure 5 A schematic diagram of a general control device for a serial robot provided according to an embodiment of the present invention is shown. The device includes: The acquisition module 401 is used to acquire the structural parameters of the DH model of the serial robot and the target end pose matrix; Calculation module 402 calculates the target end-effector pose matrix of the (n-1)th axis of the serial robot; Solver module 403 uses Newton's iteration method to solve for the target angles of each joint on the first n-1 axes of the serial robot.
[0041] The aforementioned device also includes an initial point selection module, which is used to select the optimal initial point using the random walk method and the Newton-Raphson iteration method.
[0042] The aforementioned initial point selection module specifically includes a first calculation unit, a second calculation unit, and a selection unit. The first calculation unit obtains the actual end-effector pose matrix of the (n-1)th axis, compares the target end-effector pose matrix of the (n-1)th axis with the current actual pose matrix of the (n-1)th axis, calculates the trajectory parameter points, and calculates the number of walk points based on the trajectory parameter points. The second calculation unit obtains the actual angles of each joint on the first n-1 axes, uses each joint's actual angle as the optimal initial point, and performs iterative calculations based on the optimal initial point. The selection unit is used to select the optimal initial point when the calculation is successful and the result meets preset conditions; it is also used to select the best calculation result from several calculation results as the optimal initial point when the number of successful calculations is greater than 0 and none of the preset conditions are met; and it is also used to output an error signal when the number of successful calculations is equal to 0.
[0043] Specifically, the second calculation unit increments the calculation count by 1 after each calculation; increments the runner count by 1 after a successful calculation, and determines whether the calculation result meets preset conditions. The second calculation unit is also used to determine whether the current calculation result is better than the previous calculation result when the calculation result does not meet the preset conditions, the calculation count has not reached the calculation threshold, and the runner count has not reached the runner threshold; and when the determination result is yes, it replaces the optimal initial point with the current calculation result, generates a random runner based on the optimal initial point, uses the random runner as the optimal initial point, and iterates again; and when the determination result is no, it generates a random runner based on the optimal initial point, uses the random runner as the optimal initial point, and iterates again.
[0044] The universal control device for serial robots provided in this embodiment can realize the various processes in the embodiments of the universal control method for serial robots described above, and can achieve the same technical effect. To avoid repetition, it will not be described again here.
[0045] This embodiment also provides a serial robot, which includes a controller for executing the general control method for the serial robot described above.
[0046] The serial robot provided in this embodiment can realize all the processes of the above-described serial robot control method embodiment and achieve the same technical effect. To avoid repetition, it will not be described again here.
[0047] Of course, those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by computer-controlled devices. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the above method embodiments. The storage medium can be a memory, a disk, an optical disk, etc.
[0048] While the present invention has been disclosed above, it is not limited thereto. Any person skilled in the art can make various modifications and alterations without departing from the spirit and scope of the invention; therefore, the scope of protection of the present invention should be determined by the scope defined in the claims.
[0049] Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus.
[0050] Finally, it should be noted that the above-described embodiments are merely specific implementations of the present invention, used to illustrate the technical solutions of the present invention, and not to limit it. The scope of protection of the present invention is not limited thereto. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments within the technical scope disclosed in the present invention, or make equivalent substitutions for some of the technical features; and these modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.
Claims
1. A general control method of a series robot, characterized by, Each axis of the serial robot is a rotating axis, and a plurality of the rotating axes are numbered 1-n from a first end close to a base to a last end; The method comprises: obtaining DH model structure parameters of the serial robot and a target end pose matrix of an n-th axis of the serial robot; calculating a target end pose matrix of an n-1-th axis of the serial robot; solving each joint target angle of the first n-1 axes of the serial robot by using a Newton iteration method.
2. The general control method of a series robot according to claim 1, characterized in that, The total number of the rotating axes is 6, and an axial direction of the 6-th axis of the serial robot coincides with a TCP-Z direction.
3. The general control method of a series robot according to claim 2, wherein In the Newton iteration method, The constraint equations include: , , , , , wherein, is the end pose matrix of the 5th axis of the series robot. The Jacobian matrix is: , ; wherein i is 1-6, , , , , are the joint angles of the first 5 axes of the serial robot, respectively.
4. The general control method of a series robot according to claim 3, wherein The target end pose matrix is: ; The z-direction attitude vector of the 5th axis is: ; The end position matrix of the 5th axis is: ; wherein, is the preset Z-direction displacement of the 6th axis; In the Newton iteration method, the residual formula is: ; wherein, = [1, 1], = [1, 1], = [1, 2], = [1, 2], = [1, 3], = [1, 3].
5. The general control method of a series robot according to any one of claims 1 to 4, characterized in that, The method further comprises: selecting an optimal initial point position of the Newton iteration method by using a random walk method.
6. The general control method of a series robot according to claim 5, wherein The random walk method comprises: obtaining an actual end pose matrix of the n-1-th axis; comparing the target end pose matrix of the n-1-th axis with a current actual pose matrix of the n-1-th axis to obtain a trajectory parameter point; and calculating a walk point number based on the trajectory parameter point; obtaining each joint actual angle of the first n-1 axes, taking the each joint actual angle as the optimal initial point position, and performing iterative calculation based on the optimal initial point position; if the calculation is successful and a calculation result meets a preset condition, taking the calculation result as the optimal initial point position; if the number of successful calculations is greater than 0 and none of the calculation results meets the preset condition, selecting a best calculation result from the calculation results as the optimal initial point position; and if the number of successful calculations is equal to 0, outputting an abnormal error signal.
7. The general control method of a series robot according to claim 6, wherein The iterative calculation based on the optimal initial point position comprises: each time a calculation is completed, increasing a calculation number by 1; if the calculation is successful, increasing a walk point number by 1, and judging whether the calculation result meets a preset condition; wherein the preset condition comprises that a joint sum difference is less than a sum difference threshold value, and / or a joint variance is less than a variance threshold value; if the calculation result does not meet the preset condition, the calculation number does not reach a calculation threshold value, and the walk point number does not reach a walk threshold value, judging whether the current calculation result is better than a last calculation result; if yes, replacing the optimal initial point position with the current calculation result, generating a random walk point based on the optimal initial point position, taking the random walk point as the optimal initial point position, and performing iterative calculation again; and if no, generating a random walk point based on the optimal initial point position, taking the random walk point as the optimal initial point position, and performing iterative calculation again.
8. The general control method of a series robot according to claim 6, wherein The calculation formula of the step point number is: wherein s is the track length, v is the terminal speed, and dt is the average single-point iteration time consumption.
9. A general control device for a series robot, characterized by comprising: Each axis of the serial robot is a rotating axis, and a plurality of the rotating axes are numbered 1-n from a first end close to a base to a last end; The device comprises: an obtaining module configured to obtain DH model structure parameters of the serial robot and a target end pose matrix; a calculation module configured to calculate a target end pose matrix of an n-1-th axis of the serial robot; a solving module configured to solve each joint target angle of the first n-1 axes of the serial robot by using a Newton iteration method.
10. A series robot, characterized in that, The device comprises a controller configured to perform the general control method of the serial robot according to any one of claims 1-8.