Automation Equipment Control Method and System Based on a Six-Axis Manipulator

Abstract

The present invention relates to the technical field of manipulators, and discloses an automatic equipment control method and system based on a six-axis manipulator. The method includes: planning the target pose of the six-axis manipulator by using the end coordinate points, and calculating the pose cost from the current pose to the target pose; constructing the transfer function of the six-axis manipulator, constructing a PID controller of the six-axis manipulator by using the pose error, and calculating the gain parameters of the PID controller; based on the gain parameters and the pose error, outputting a planned control signal of the six-axis manipulator by using the PID controller; calculating the signal control risk from the current control signal to the planned control signal, screening the equipment control signal from the planned control signal according to the signal control risk, and performing equipment control on the six-axis manipulator by using the equipment control signal to obtain the equipment control result of the six-axis manipulator. The present invention can clarify the movement actions and movement trajectories of the manipulator and joints in front of the end, and replace the fuzzy PID control method to control the six-axis manipulator.

Description

Technical Field

[0001] The present invention relates to the technical field of manipulators, and in particular, to an automated equipment control method and system based on a six-axis manipulator. Background Art

[0002] A six-axis manipulator refers to a manipulator with six degrees of freedom, which are the degrees of freedom of the x-axis, y-axis, z-axis, the degree of rotation around the x-axis, the degree of rotation around the y-axis, and the degree of rotation around the z-axis. The control of automated equipment based on a six-axis manipulator refers to the process of controlling the motion actions and motion trajectories of the six-axis manipulator.

[0003] Currently, before controlling a six-axis manipulator, it is necessary to first determine the position where the six-axis manipulator will move next. Generally, the end of the six-axis manipulator needs to touch the goods to pick up the goods. The prior art generally only controls the motion actions and motion trajectories of the end of the six-axis manipulator, and the control of the end of the six-axis manipulator depends on the robotic arm and joints in front of the end to achieve. However, the prior art does not clarify the motion actions and motion trajectories of the robotic arm and joints in front of the end. Secondly, the conversion from the current action of the manipulator to the action in other positions needs to be carried out slowly at intervals, rather than a one-time action, but is divided into small actions to complete the conversion of the current action to the action in other positions. In this process, it is also necessary to consider whether the manipulator maintains stability, that is, to prevent the manipulator from jittering due to external interference. The prior art often uses the fuzzy PID method to maintain the stability of the manipulator. The fuzzy PID control method requires adjustment of control parameters, selection of fuzzy sets, and a large amount of data calculation. The adjustment of parameters requires certain experience and professional knowledge, and needs to be decided and selected by artificial experts. A large amount of data calculation requires a large amount of computing resources, and the selection of fuzzy sets has a great influence on the control effect. If the selection is improper, it may lead to a decline or instability of the control effect. Therefore, there is an urgent need for a solution that can clarify the motion actions and motion trajectories of the robotic arm and joints in front of the end and replace the fuzzy PID control method to control the six-axis manipulator. Summary of the Invention

[0004] To solve the above problems, the present invention provides an automated equipment control method and system based on a six-axis manipulator, which can clarify the motion actions and motion trajectories of the robotic arm and joints in front of the end and replace the fuzzy PID control method to control the six-axis manipulator.

[0005] In a first aspect, the present invention provides an automated equipment control method based on a six-axis manipulator, including:

[0006] Determine the end coordinate point of the end of the six-axis manipulator, and collect the current pose of the six-axis manipulator. Among them, the six-axis manipulator includes an end, a first link, a second link, a starting point of the first link, and an end point of the first link. The current pose includes a current coordinate point and a current angle value;

[0007] Use the end coordinate point to plan the target pose of the six-axis manipulator. Among them, the target pose includes a target coordinate point and a target angle value. Based on the current coordinate point and the current angle value, calculate the pose cost from the current pose to the target pose, and select the final pose from the target poses using the pose cost;

[0008] Calculate the pose error between the current pose and the final pose, construct the transfer function of the six-axis manipulator, use the pose error to construct the PID controller of the six-axis manipulator, and calculate the gain parameters of the PID controller based on the transfer function;

[0009] Based on the gain parameters and the pose error, use the PID controller to output the planned control signal of the six-axis manipulator, and collect the current control signal of the six-axis manipulator;

[0010] Calculate the signal control risk from the current control signal to the planned control signal. According to the signal control risk, screen the device control signal from the planned control signal, and use the device control signal to control the six-axis manipulator to obtain the device control result of the six-axis manipulator.

[0011] In a possible implementation manner of the first aspect, the using the end coordinate point to plan the target pose of the six-axis manipulator includes:

[0012] Obtain the rotation angle range and link length of the six-axis manipulator;

[0013] Based on the rotation angle range and the link length, use the following formula to construct the objective function of the end coordinate point:

[0014]

[0015]

[0016]

[0017] l x =l 1 cosθ x

[0018] l y =l 1 cosθ y

[0019] l z = l 1 cosθ z

[0020]

[0021]

[0022]

[0023] l′ x = l 2 cosθ′ x

[0024] l′ y = l 2 cosθ′ y

[0025] l′ z = l 2 cosθ′ z

[0026] l″ x = l x + l′ x

[0027] l″ y = l y + l′ y

[0028] l″ z = l z + l′ z

[0029] l″ x = l 1 cosθ x + l 2 cosθ′ x

[0030] l″ y = l 1 cosθ y + l 2 cosθ′ y

[0031] l″ z = l 1 cosθ z + l 2 cosθ′ z

[0032]

[0033] Among them, F represents the objective function, and F includes min|l″ x -l″′ x |, min|l″ y -l″′ y |, and min|l″ z -l″′ z |. (l″′ x , l″′ y , l″′ z ) represents the end coordinate point, (l″ x , l″ y , l″ z ) represents the coordinate point to be optimized of the end coordinate point, l 1 represents the length of the first link among the link lengths, l 2 represents the length of the second link among the link lengths, (θ x , θ y , θ z ) represents the deflection angle of the first link relative to the rectangular coordinate system in the rectangular coordinate system with the starting point of the first link as the origin, (θ′ x , θ′ y , θ′ z ) represents the deflection angle of the second link relative to the rectangular coordinate system in the rectangular coordinate system with the end point of the first link as the origin, l x represents the mapped length of the first link on the x-axis in the rectangular coordinate system with the starting point of the first link as the origin, l y represents the mapped length of the first link on the y-axis in the rectangular coordinate system with the starting point of the first link as the origin, l z represents the mapped length of the first link on the z-axis in the rectangular coordinate system with the starting point of the first link as the origin, l′ x represents the mapped length of the second link on the x-axis in the rectangular coordinate system with the end point of the first link as the origin, l′ y represents the mapped length of the second link on the y-axis in the rectangular coordinate system with the end point of the first link as the origin, l′ z represents the mapped length of the second link on the z-axis in the rectangular coordinate system with the end point of the first link as the origin, θ x , θ y , θ z , θ′ x , θ' y , θ' z belong to the rotation angle range;

[0034] Use the following formula to update the angle parameters in the objective function to obtain the updated angle:

[0035] v′ ij = v ij+c 1 ×rand×(pbest ij -x ij )+c 2 ×rrand×(gbest ij -x ij )

[0036] x′ ij =x ij +v ij

[0037] where x′ ij represents the updated angle, v′ ij represents the velocity after updating the original update velocity, i represents the sequence number of θ, j represents the sequence number of θ', v ij represents the original update velocity of θ', c 1 and c 2 represent learning factors, c 1 =c 2 =2, rand represents a random number between 0 and 1, x ij represents the current value of θ', pbest ij represents the neighboring historical value with F value of 0 among the neighboring historical values of θ', gbest ij represents the historical value with F value of 0 among all historical values of θ', θ' includes θ' x , θ' y , θ' z , θ includes θ x , θ y , θ z ;

[0038] Determine whether the objective function value corresponding to the updated angle in the objective function is a preset threshold;

[0039] When the objective function value corresponding to the updated angle in the objective function is the preset threshold, use the updated angle to calculate the target coordinate point and target angle value of the six-axis manipulator;

[0040] Take the target coordinate point and the target angle value as the target pose.

[0041] In a possible implementation manner of the first aspect, calculating the pose cost from the current pose to the target pose based on the current coordinate point and the current angle value includes:

[0042] Obtain the target coordinate point and target angle value in the target pose;

[0043] Use the following formula to calculate the coordinate cost from the current coordinate point to the target coordinate point:

[0044] U = u 1 + u 2

[0045] wherein, U represents the coordinate cost, and u 1 represents the absolute value of the difference between the coordinate of the end point of the first link in the current coordinate point and the coordinate of the end point of the first link in the target coordinate point, and u 2 the absolute value of the difference between the coordinate of the end in the current coordinate point and the coordinate of the end point;

[0046] The angular cost from the current angular value to the target coordinate point is calculated using the following formula:

[0047] V = v 1 + v 2

[0048] wherein, V represents the angular cost, and v 1 represents the absolute value of the difference between the angle of the end point of the first link in the current angular value and the angle of the end point of the first link in the target angular value, and v 2 the absolute value of the difference between the angle of the end in the current angular value and the end angle value;

[0049] The sum of the coordinate cost and the angular cost is used as the pose cost from the current pose to the target pose.

[0050] In a possible implementation manner of the first aspect, constructing the transfer function of the six-axis manipulator includes:

[0051] Construct the transfer function of the six-axis manipulator using the following formula:

[0052]

[0053] p(s)*(s 2 + αs + β) = γ * q(s)

[0054] s 2 p(s) + αsp(s) + βp(s) = γq(s)

[0055]

[0056] wherein, Denote the transfer function of the six-axis manipulator, \(t\) represents the time independent variable in the time domain, \(p(t)\) represents the pose of the six-axis manipulator after being adjusted under the control of the PID controller, \(q(t)\) represents the pose control signal input by the PID controller to the six-axis manipulator, \(\alpha\), \(\beta\), \(\gamma\) represent constant parameters, \(s\) represents the independent variable obtained by transforming \(t\) when performing the Laplace transform on \(p(t)\), \(p(s)\) represents the Laplace transform form of \(p(t)\), and \(q(s)\) represents the Laplace transform form of \(q(t)\).

[0057] In a possible implementation manner of the first aspect, calculating the gain parameters of the PID controller based on the transfer function includes:

[0058] Obtain the output error corresponding to the PID controller;

[0059] Based on the output error and the transfer function, construct the state equation matrix of the six-axis manipulator;

[0060] Construct the Lyapunov original model under the PID controller;

[0061] Derive the model of the Lyapunov original model to obtain the derivative of the Lyapunov original model;

[0062] Calculate the inverse function of the PID controller under the output error;

[0063] Substitute the state equation matrix and the inverse function into the derivative of the Lyapunov original model to obtain the Lyapunov derivative model;

[0064] Determine whether a positive definite matrix of the Lyapunov original model can be constructed;

[0065] When a positive definite matrix of the Lyapunov original model can be constructed, determine that the Lyapunov original model is greater than a preset threshold;

[0066] When the Lyapunov original model is greater than the preset threshold, construct a negative definite matrix of the Lyapunov derivative model;

[0067] When the proportional gain, integral gain, and derivative gain of the PID controller conform to the negative definite matrix, determine the gain parameters of the PID controller.

[0068] In a possible implementation manner of the first aspect, outputting the planned control signal of the six-axis manipulator by using the PID controller based on the gain parameters and the pose error includes:

[0069] Adjust the PID controller by using the gain parameters to obtain an adjusted controller;

[0070] Substitute the pose error into the adjustment controller;

[0071] Output a planned control signal corresponding to the pose error in the adjustment controller.

[0072] In a possible implementation manner of the first aspect, calculating the signal control risk from the current control signal to the planned control signal includes:

[0073] Query the signal control intervals between every two adjacent control signals in the current control signal and the planned control signal;

[0074] Randomly merge the signal control intervals according to the adjacent relationship between each signal control interval to obtain merged control intervals;

[0075] Identify whether there is an interval not less than the merged control interval in the historical period;

[0076] When there is an interval not less than the merged control interval in the historical period, take no risk as the signal control risk;

[0077] When there is no interval not less than the merged control interval in the historical period, calculate the interval length of the merged control interval;

[0078] Identify whether there is a target length consistent with the interval length in the historical period;

[0079] Query the interval scale at the interval length and the target scale at the target length;

[0080] Calculate the scale standard deviation between the interval scale and the target scale;

[0081] Construct a standard deviation threshold of the interval scale using the scale standard deviation;

[0082] When the interval scale is greater than the standard deviation threshold, take there is risk as the signal control risk;

[0083] When the interval scale is not greater than the standard deviation threshold, take no risk as the signal control risk.

[0084] In a possible implementation manner of the first aspect, calculating the scale standard deviation between the interval scale and the target scale includes:

[0085] Obtain the interval angular difference and interval coordinate distance in the interval scale;

[0086] Calculate the first angle - coordinate sum between the interval angular difference and the interval coordinate distance;

[0087] Obtain the target angular difference and target coordinate distance in the target scale;

[0088] Calculate the second angle - coordinate sum between the target angular difference and the target coordinate distance;

[0089] Calculate the sum mean between the first angle - coordinate sum and the second angle - coordinate sum;

[0090] Calculate the scale standard deviation between the interval scale and the target scale according to the first angle - coordinate sum, the second angle - coordinate sum, and the sum mean.

[0091] In a possible implementation manner of the first aspect, the screening of the device control signal from the planning control signal according to the signal control risk includes:

[0092] When the signal control risk is risk - free, retain the merged control interval corresponding to the risk - free to obtain a retained interval;

[0093] Query whether the intervals between each adjacent retained interval in the retained interval are continuous;

[0094] When the intervals between each adjacent retained interval in the retained interval are not continuous, shorten the interval length of the retained interval according to the signal control interval in the retained interval to obtain a shortened interval, so as to align each adjacent retained interval in the retained interval through the shortened interval to obtain an aligned interval;

[0095] Calculate the number of intervals of the aligned interval;

[0096] Select the minimum number from the number of intervals;

[0097] Obtain the target aligned interval corresponding to the minimum number from the aligned interval;

[0098] Take the planning control signal on the boundary of the target aligned interval as the device control signal.

[0099] In a second aspect, the present invention provides an automated equipment control system based on a six - axis manipulator, and the system includes:

[0100] A pose acquisition module, configured to determine the end coordinate point of the end of the six - axis manipulator and collect the current pose of the six - axis manipulator. Wherein, the six - axis manipulator includes an end, a first link, a second link, a starting point of the first link, and an end point of the first link, and the current pose includes a current coordinate point and a current angle value;

[0101] The pose selection module is used to plan the target pose of the six-axis manipulator by using the end coordinate points. Among them, the target pose includes the target coordinate point and the target angle value. Based on the current coordinate point and the current angle value, calculate the pose cost from the current pose to the target pose, and select the final pose from the target poses by using the pose cost;

[0102] The parameter calculation module is used to calculate the pose error between the current pose and the final pose, construct the transfer function of the six-axis manipulator, use the pose error to construct the PID controller of the six-axis manipulator, and calculate the gain parameters of the PID controller based on the transfer function;

[0103] The signal acquisition module is used to output the planned control signal of the six-axis manipulator by using the PID controller based on the gain parameters and the pose error, and collect the current control signal of the six-axis manipulator;

[0104] The device control module is used to calculate the signal control risk from the current control signal to the planned control signal, screen the device control signal from the planned control signal according to the signal control risk, and use the device control signal to control the six-axis manipulator to obtain the device control result of the six-axis manipulator.

[0105] Compared with the prior art, the technical principle and beneficial effects of this solution are as follows:

[0106] In the embodiments of the present invention, the target pose of the six-axis manipulator is planned by using the end coordinate points to clarify the movement actions and trajectories of the first link, the second link, and the end point of the first link in front of the end. Further, in the embodiments of the present invention, the pose cost from the current pose to the target pose is calculated based on the current coordinate point and the current angle value to select the target pose with the minimum pose cost. Further, in the embodiments of the present invention, the transfer function of the six-axis manipulator is constructed to establish the transfer relationship between the output control signal of the PID controller and the pose output by the six-axis manipulator. Further, in the embodiments of the present invention, the gain parameters of the PID controller are calculated based on the transfer function to perform stability control on the six-axis manipulator by using the Lyapunov model instead of the fuzzy PID control method. In the embodiments of the present invention, the signal control risk from the current control signal to the planned control signal is calculated to determine whether a mechanical obstacle will be caused to the six-axis manipulator during the process of calculating each current control signal to the subsequent control signal after the next control signal under the premise that the PID controller outputs a series of planned control signals. Further, in the embodiments of the present invention, the device control signals are screened from the planned control signals according to the signal control risk to expand the risk-free planned control signals in the continuous planned control signals originally output by the PID controller, which can reduce the number of times of frequently adjusting the control signals. Therefore, an automated equipment control method and system based on a six-axis manipulator proposed in the embodiments of the present invention can clarify the movement actions and trajectories of the manipulator and joints in front of the end and control the six-axis manipulator by replacing the fuzzy PID control method. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] The accompanying drawings herein are incorporated into the specification and form a part of the specification, showing embodiments consistent with the present invention and, together with the specification, are used to explain the principles of the present invention.

[0108] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0109] Figure 1 It is a schematic flow chart of an automated equipment control method based on a six-axis manipulator provided by an embodiment of the present invention;

[0110] Figure 2 It is a schematic diagram of the cargo space of an automated equipment control method based on a six-axis manipulator provided by an embodiment of the present invention;

[0111] Figure 3It is a schematic diagram of modules of an automated equipment control system based on a six-axis manipulator provided by an embodiment of the present invention. Detailed implementation manners

[0112] It should be understood that the specific implementation manners described herein are only used to explain the present invention and are not used to limit the present invention.

[0113] An embodiment of the present invention provides an automated equipment control method based on a six-axis manipulator. The execution subject of the automated equipment control method based on the six-axis manipulator includes, but is not limited to, at least one of electronic devices such as a server, a terminal, etc. that can be configured to execute the method provided by the embodiment of the present invention. In other words, the automated equipment control method based on the six-axis manipulator can be executed by software or hardware installed on a terminal device or a server device, and the software can be a blockchain platform. The server includes, but is not limited to: a single server, a server cluster, a cloud server, or a cloud server cluster, etc. The server can be an independent server or a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network services, cloud communications, middleware services, domain name services, security services, Content Delivery Network (CDN), and big data and artificial intelligence platforms.

[0114] Refer to Figure 1 As shown, it is a flowchart of an automated equipment control method based on a six-axis manipulator provided by an embodiment of the present invention. Among them, Figure 1 The automated equipment control method described in

[0115] S1. Determine the end coordinate point of the end of the six-axis manipulator, and collect the current pose of the six-axis manipulator. Among them, the six-axis manipulator includes an end, a first link, a second link, a starting point of the first link, and an end point of the first link, and the current pose includes a current coordinate point and a current angle value.

[0116] In an embodiment of the present invention, the six-axis manipulator refers to a manipulator with six degrees of freedom, and these six degrees of freedom are respectively the degree of freedom of the x-axis, the degree of freedom of the y-axis, the degree of freedom of the z-axis, the degree of freedom of rotation around the x-axis, the degree of freedom of rotation around the y-axis, and the degree of freedom of rotation around the z-axis. The end coordinate point refers to the coordinate position that the end needs to reach in the next step, and the end is not currently at the end coordinate point.

[0117] Refer to Figure 2 As shown, it is a schematic diagram of a six-axis manipulator of an automated equipment control method based on a six-axis manipulator provided by an embodiment of the present invention. In Figure 2Among them, 1 represents the end, 5 represents the first link, 4 represents the second link, 3 represents the starting point of a link, and 2 represents the end point of the first link.

[0118] It should be noted that the current coordinate points include the coordinate of the starting point of the first link, the coordinate of the end point of the first link, and the coordinate of the end. The current angle value is the angle value of the first link and the angle value of the second link.

[0119] S2. Plan the target pose of the six-axis manipulator by using the end coordinate point. Among them, the target pose includes a target coordinate point and a target angle value. Based on the current coordinate point and the current angle value, calculate the pose cost from the current pose to the target pose, and select the final pose from the target poses by using the pose cost.

[0120] In the embodiment of the present invention, by using the end coordinate point to plan the target pose of the six-axis manipulator, it is used to clarify the motion actions and motion trajectories of the first link, the second link, and the end point of the first link in front of the end.

[0121] Among them, the target pose refers to the coordinate position and rotation angle that the six-axis manipulator needs to reach in the next step, and the six-axis manipulator is not currently at the target pose.

[0122] In an embodiment of the present invention, the planning of the target pose of the six-axis manipulator by using the end coordinate point includes: obtaining the rotation angle range and link length of the six-axis manipulator; based on the rotation angle range and the link length, constructing the objective function of the end coordinate point by using the following formula:

[0123]

[0124]

[0125]

[0126] l x =l 1 cosθ x

[0127] l y =l 1 cosθ y

[0128] l z =l 1 cosθ z

[0129]

[0130]

[0131]

[0132] l′ x = l 2 cosθ′ x

[0133] l′ y = l 2 cosθ′ y

[0134] l′ Z = l 2 cosθ′ z

[0135] l″ x = l x + l′ x

[0136] l″ y = l y + l′ y

[0137] l ″ z = l z + l′ z

[0138] l″ x = l 1 cosθ x + l 2 cosθ′ x

[0139] l″ y = l 1 cosθ y + l 2 cosθ′ y

[0140] l″ z = l 1 cosθ z + l 2 cosθ′ z

[0141]

[0142] where F represents the objective function, and F includes min|l″ x - l″' x |, min|l″ y - l″' y |, and min|l″ z - l″' z (l″' x,l”' y ,l”' z ) represents the said end coordinate point, (l” x ,l” y ,l” z ) represents the coordinate point to be optimized of the said end coordinate point, l 1 represents the length of the first link in the said link lengths, l 2 represents the length of the second link in the said link lengths, (θ x ,θ y ,θ z ) represents the deflection angle of the first link relative to the rectangular coordinate system in the rectangular coordinate system with the starting point of the first link as the origin, (θ' x ,θ' y ,θ' z ) represents the deflection angle of the second link relative to the rectangular coordinate system in the rectangular coordinate system with the end point of the first link as the origin, l x represents the mapped length of the first link on the x-axis in the rectangular coordinate system with the starting point of the first link as the origin, l y represents the mapped length of the first link on the y-axis in the rectangular coordinate system with the starting point of the first link as the origin, l z represents the mapped length of the first link on the z-axis in the rectangular coordinate system with the starting point of the first link as the origin, l' x represents the mapped length of the second link on the x-axis in the rectangular coordinate system with the end point of the first link as the origin, l' y represents the mapped length of the second link on the y-axis in the rectangular coordinate system with the end point of the first link as the origin, l' z represents the mapped length of the second link on the z-axis in the rectangular coordinate system with the end point of the first link as the origin, θ x ,θ y ,θ z ,θ' x ,θ' y ,θ' z belongs to the said rotation angle range;

[0143] Use the following formula to update the angle parameter in the said objective function to obtain the updated angle:

[0144] v′ ij = v ij + c 1 × rand × (pbest ij - x ij ) + c 2 × rand × (gbest ij - x ij )

[0145] x′ ij = xij +v ij

[0146] where x' ij represents the updated angle, v' ij represents the velocity after updating the original update velocity, i represents the sequence number of θ, j represents the sequence number of θ', v ij represents the original update velocity of θ', c 1 and c 2 represent the learning factor, c 1 = c 2 = 2, rand represents a random number between 0 and 1, x ij represents the current value of θ', pbest ij represents the neighboring historical value with F value of 0 among the neighboring historical values of θ', gbest ij represents the historical value with F value of 0 among all historical values of θ', θ' includes θ' x , θ' y , θ' z , θ includes θ x , θ y , θ z ;

[0147] Judge whether the objective function value corresponding to the updated angle in the objective function is a preset threshold; when the objective function value corresponding to the updated angle in the objective function is the preset threshold, calculate the target coordinate point and the target angle value of the six-axis manipulator by using the updated angle; use the target coordinate point and the target angle value as the target pose.

[0148] Among them, the rotation angle range refers to the maximum angle interval in which the first link and the second link of the six-axis manipulator can rotate, and the link length includes the length of the first link and the length of the second link. It should be noted that the above (l” x , l” y , l” z ) represents the coordinate points to be optimized of the end coordinate point. Among them, (l” x , l” y , l” z ) has the same meaning as the end coordinate point, (l” x , l” y , l” z ) are the values to be optimized by the objective function. The neighboring historical values refer to other θ' values neighboring to θ' in the historical values. The value of the preset threshold is 0, indicating (l” x , l” y , l” z) has the same value as the value of the end coordinate point. The updated angle refers to the value of θ' under θ. Therefore, θ and θ' can be updated through the updated angle, and the target coordinate point and the target angle value can be calculated through θ, θ' and the link length. The specific calculation method is the same as the above calculation of l x 、l y 、l z The calculation principle is similar and will not be elaborated further here.

[0149] Furthermore, in the embodiment of the present invention, the pose cost from the current pose to the target pose is calculated based on the current coordinate point and the current angle value, so as to select the target pose with the minimum pose cost.

[0150] In an embodiment of the present invention, calculating the pose cost from the current pose to the target pose based on the current coordinate point and the current angle value includes: obtaining the target coordinate point and the target angle value in the target pose; calculating the coordinate cost from the current coordinate point to the target coordinate point by using the following formula:

[0151] U = u 1 + u 2

[0152] where U represents the coordinate cost, and u 1 represents the absolute value of the difference between the coordinate of the end point of the first link in the current coordinate point and the coordinate of the end point of the first link in the target coordinate point, and u 2 The absolute value of the difference between the coordinate of the end in the current coordinate point and the end coordinate point;

[0153] Calculating the angle cost from the current angle value to the target coordinate point by using the following formula:

[0154] V = v 1 + v 2

[0155] where V represents the angle cost, and v 1 represents the absolute value of the difference between the angle of the end point of the first link in the current angle value and the angle of the end point of the first link in the target angle value, and v 2 The absolute value of the difference between the angle of the end in the current angle value and the end angle value;

[0156] Taking the sum of the coordinate cost and the angle cost as the pose cost from the current pose to the target pose.

[0157] Optionally, the process of selecting the final pose from the target poses by using the pose cost refers to selecting the target pose with the minimum pose cost.

[0158] S3. Calculate the pose error between the current pose and the final pose, construct the transfer function of the six-axis manipulator, use the pose error to construct the PID controller of the six-axis manipulator, and calculate the gain parameters of the PID controller based on the transfer function.

[0159] In the embodiment of the present invention, the pose error refers to the pose difference between the current pose and the final pose. It should be noted that the principle of calculating the pose error between the current pose and the final pose is similar to the principle of calculating the pose cost from the current pose to the target pose based on the current coordinate point and the current angle value, and will not be further elaborated here.

[0160] Further, in the embodiment of the present invention, by constructing the transfer function of the six-axis manipulator, it is used to construct the transfer relationship between the output control signal of the PID controller and the pose output by the six-axis manipulator.

[0161] In an embodiment of the present invention, constructing the transfer function of the six-axis manipulator includes: constructing the transfer function of the six-axis manipulator using the following formula:

[0162]

[0163] p(s)*(s 2 +αs + β) = γ*q(s)

[0164] s 2 p(s)+αsp(s)+βp(s)=γq(s)

[0165]

[0166] where, represents the transfer function of the six-axis manipulator, t represents the time independent variable in the time domain, p(t) represents the pose of the six-axis manipulator after being adjusted under the control of the PID controller, q(t) represents the pose control signal input by the PID controller to the six-axis manipulator, α, β, γ represent constant parameters, s represents the independent variable obtained by transforming t when performing the Laplace transform on p(t), p(s) represents the Laplace transform form of p(t), and q(s) represents the Laplace transform form of q(t).

[0167] Optionally, the process of using the pose error to construct the PID controller of the six-axis manipulator is: based on the pose error, construct the PID controller of the six-axis manipulator using the following formula:

[0168]

[0169]

[0170] Among them, q(t) represents the pose control signal input by the PID controller to the six-axis manipulator, and k 1 represents the proportional gain, k 2 represents the integral gain, k 3 represents the derivative gain, e(t) represents the output error between p(t) and the final pose, e 0 (t) represents the pose error between the current pose and the final pose, q 0 (t) represents the pose control signal input by the PID controller to the six-axis manipulator under the pose error, and t represents the time independent variable in the time domain.

[0171] It should be noted that after inputting e 0 (t) into the PID controller, the PID controller outputs q 0 (t) based on e 0 (t), q 0 (t) acts on the six-axis manipulator, and the six-axis manipulator outputs p(t) under the action of q 0 (t), and the PID controller outputs q(t) under the control of the pose error e(t) between p(t) and the final pose.

[0172] Furthermore, in an embodiment of the present invention, the gain parameters of the PID controller are calculated based on the transfer function for performing stability control on the six-axis manipulator by using the Lyapunov model to replace the fuzzy PID control method.

[0173] Among them, the gain parameters refer to the optimized values of the proportional gain, integral gain, and derivative gain in the PID controller.

[0174] In an embodiment of the present invention, calculating the gain parameters of the PID controller based on the transfer function includes: obtaining the output error corresponding to the PID controller; constructing the state equation matrix of the six-axis manipulator by using the following formula based on the output error and the transfer function:

[0175]

[0176] Among them, represents the state equation matrix, e n (t) represents the nth derivative of e(t), p n (t) represents the nth derivative of p(t), e(t) represents the output error, p(t) represents the pose after the six-axis manipulator is adjusted under the control of the PID controller, t represents the time independent variable in the time domain, represents the deformation formula of the transfer function of the six-axis manipulator, and δ' and ε' represent substituting into The known parameters calculated after q(t) in it, where q(t) represents the pose control signal input by the PID controller to the six-axis manipulator, and δ and ε represent unknown parameters;

[0177] Use the following formula to construct the Lyapunov original model under the PID controller:

[0178] I(e n (t))=(e n (t)) T H(e n (t))

[0179] Among them, I(e n (t)) represents the Lyapunov original model, e n (t) represents the nth derivative of e(t), and H represents the parameter used to judge the stability of the Lyapunov original model and the Lyapunov derivative model;

[0180] Derive the model of the Lyapunov original model to obtain the derivative of the Lyapunov original model; calculate the inverse function of the PID controller under the output error; substitute the state equation matrix and the inverse function into the derivative of the Lyapunov original model to obtain the Lyapunov derivative model; judge whether the positive definite matrix of the Lyapunov original model can be constructed; when the positive definite matrix of the Lyapunov original model can be constructed, determine that the Lyapunov original model is greater than the preset threshold; when the Lyapunov original model is greater than the preset threshold, construct the negative definite matrix of the Lyapunov derivative model; when the proportional gain, integral gain and differential gain of the PID controller meet the negative definite matrix, determine the gain parameters of the PID controller.

[0181] Among them, the pose after the six-axis manipulator is adjusted under the control of the PID controller is p(t), and the inverse function of the PID controller under the output error is derived from q(t)=f(e n (t)) to e n (t)=

[0182] f -1 (q(t)) is obtained, and the value of the preset threshold is 0.

[0183] Optionally, the formula for substituting the state equation matrix and the inverse function into the derivative of the Lyapunov original model to obtain the Lyapunov derivative model is:

[0184]

[0185] Among them, represents the Lyapunov derivative model, e n(t) represents the nth derivative of e(t), and δ' and ε' represent known parameters obtained by substituting into for q(t). H represents a matrix used to determine the stability of the Lyapunov original model and the Lyapunov derivative model. represents the output error derivative model. represents the state equation matrix, and f(e n (t)) represents a function with the independent variable e n (t).

[0186] Optionally, the formula for constructing the negative definite matrix of the Lyapunov derivative model is:

[0187]

[0188]

[0189] where Q(H) represents the negative definite matrix of the Lyapunov derivative model. represents the Lyapunov derivative model, and H represents a matrix used to determine the stability of the Lyapunov original model and the Lyapunov derivative model.

[0190] It should be noted that Q(H) is derived from H, k 1 , k 2 , k 3 , δ' and ε' in the Lyapunov derivative model.

[0191] Optionally, the process of determining whether the positive definite matrix of the Lyapunov original model can be constructed refers to the process of determining whether there is a positive definite H matrix. In this process, parameters need to be selected and substituted into the H matrix. The process of selecting parameters is similar to the principle of calculating and updating the angle described above, and will not be elaborated further here. The principle of determining the gain parameters of the PID controller when the proportional gain, integral gain, and derivative gain of the PID controller meet the negative definite matrix is similar to the principle of constructing the positive definite matrix of the Lyapunov original model described above, and will not be elaborated further here.

[0192] S4. Based on the gain parameters and the pose error, use the PID controller to output the planned control signal of the six-axis manipulator, and collect the current control signal of the six-axis manipulator.

[0193] In one embodiment of the present invention, outputting a planned control signal for the six-axis manipulator by using the PID controller based on the gain parameter and the pose error includes: adjusting the PID controller by using the gain parameter to obtain an adjusted controller; substituting the pose error into the adjusted controller; and outputting a planned control signal corresponding to the pose error in the adjusted controller.

[0194] Optionally, the process of adjusting the PID controller by using the gain parameter to obtain an adjusted controller refers to adjusting the proportional gain, integral gain, and derivative gain in the PID controller to the gain parameter.

[0195] S5. Calculate the signal control risk from the current control signal to the planned control signal. According to the signal control risk, screen out a device control signal from the planned control signals, and use the device control signal to perform device control on the six-axis manipulator to obtain a device control result of the six-axis manipulator.

[0196] In an embodiment of the present invention, by calculating the signal control risk from the current control signal to the planned control signal, it is used to calculate whether there will be a mechanical obstacle to the six-axis manipulator during the process from each current control signal to the subsequent control signal following the next control signal on the premise that the PID controller outputs a series of planned control signals.

[0197] Wherein, the signal control risk refers to the risk of mechanical obstacles caused to the six-axis manipulator during signal control.

[0198] In one embodiment of the present invention, calculating the signal control risk from the current control signal to the planned control signal includes: querying a signal control interval between every two adjacent control signals in the current control signal and the planned control signal; randomly merging the signal control intervals according to the adjacent relationship between each signal control interval to obtain a merged control interval; identifying whether there is an interval not less than the merged control interval in the historical period; when there is an interval not less than the merged control interval in the historical period, taking no risk as the signal control risk; when there is no interval not less than the merged control interval in the historical period, calculating the interval length of the merged control interval; identifying whether there is a target length consistent with the interval length in the historical period; querying the interval scale under the interval length and the target scale under the target length; calculating the scale standard deviation between the interval scale and the target scale; constructing a standard deviation threshold of the interval scale by using the scale standard deviation; when the interval scale is greater than the standard deviation threshold, taking there is risk as the signal control risk; when the interval scale is not greater than the standard deviation threshold, taking no risk as the signal control risk.

[0199] Optionally, according to the adjacent relationship between each signal control interval, randomly combining the signal control intervals to obtain the combined control interval is as follows: for example, each control signal in the current control signal and the planned control signal is D t , D t represents the control signal at the t-th moment, and the signal control interval is D t-2 ~D t-1 , D t-1 ~D t , D t ~D t+1 , D t+1 ~D t+2 , and the combined control interval is D t-2 ~D t , D t-1 ~D t+2 , that is, multiple individual signal control intervals are combined into one combined control interval. The multiple individual signal control intervals to be combined need to be adjacent or continuous, and the number of combined intervals is random and not fixed. The interval scale refers to the magnitude of the change in the angle and coordinates of the six-axis manipulator during the process of adjusting the signal from the start point to the end point of this interval length. The meaning of the target scale is similar to the meaning of the interval scale.

[0200] In another embodiment of the present invention, calculating the scale standard deviation between the interval scale and the target scale includes: obtaining the interval angle difference and interval coordinate distance in the interval scale; calculating the first angle - coordinate sum between the interval angle difference and the interval coordinate distance; obtaining the target angle difference and target coordinate distance in the target scale; calculating the second angle - coordinate sum between the target angle difference and the target coordinate distance; calculating the sum mean between the first angle - coordinate sum and the second angle - coordinate sum; and calculating the scale standard deviation between the interval scale and the target scale according to the first angle - coordinate sum, the second angle - coordinate sum, and the sum mean.

[0201] Optionally, the process of constructing the standard deviation threshold of the interval scale using the scale standard deviation refers to reducing or enlarging the scale standard deviation by using a certain constant as needed to obtain the standard deviation threshold.

[0202] Further, in the embodiment of the present invention, by screening the device control signals from the planned control signals according to the signal control risk, the risk - free planned control signals in the originally continuous planned control signals output by the PID controller are expanded, which can reduce the number of times of frequently adjusting the control signals.

[0203] In one embodiment of the present invention, screening device control signals from the planning control signals according to the signal control risk includes: when the signal control risk is risk-free, retaining the merged control interval corresponding to the risk-free to obtain a retained interval; querying whether there is interval continuity between each adjacent retained interval in the retained interval; when there is no interval continuity between each adjacent retained interval in the retained interval, shortening the interval length of the retained interval according to the signal control interval in the retained interval to obtain a shortened interval, so as to align each adjacent retained interval in the retained interval through the shortened interval to obtain an aligned interval; calculating the number of intervals of the aligned interval; selecting the minimum number from the number of intervals; obtaining a target aligned interval corresponding to the minimum number from the aligned interval; and using the planning control signals on the boundaries of the target aligned interval as the device control signals.

[0204] Wherein, the aligned interval refers to multiple retained intervals and shortened intervals with continuous boundaries.

[0205] Exemplarily, the process of shortening the interval length of the retained interval according to the signal control interval in the retained interval to obtain a shortened interval means that: when the retained interval is D t-2 ~D t 、D t-1 ~D t+2 ,shorten D t-2 ~D t to D t-2 ~D t-1 ,so that D t-2 ~D t-1 is continuous with D t-2 ~D t-1 (the boundaries of these two intervals are aligned and are an aligned interval), or shorten D t-1 ~D t+2 to D t ~D t+2 ,so that D t-2 ~D t is continuous with D t ~D t+2 (the boundaries of these two intervals are aligned and are an aligned interval). It can be seen from this that the aligned intervals are not the same and there can be multiple. The purpose of subsequently obtaining the target aligned interval corresponding to the minimum number from the aligned intervals is to select the minimum number of times and reduce the number of signal adjustments. Because each boundary in the aligned interval is a value of a control signal, if the number of intervals in the aligned interval is large, the number of signal adjustments will also increase.

[0206] It can be seen that in the embodiment of the present invention, the target pose of the six-axis manipulator is planned by using the end coordinate points to clarify the motion actions and trajectories of the first link, the second link, and the end point of the first link in front of the end. Further, in the embodiment of the present invention, the pose cost from the current pose to the target pose is calculated based on the current coordinate point and the current angle value to select the target pose with the minimum pose cost. Further, in the embodiment of the present invention, the transfer function of the six-axis manipulator is constructed to establish the transfer relationship between the output control signal of the PID controller and the pose output by the six-axis manipulator. Further, in the embodiment of the present invention, the gain parameters of the PID controller are calculated based on the transfer function to perform stability control on the six-axis manipulator by using the Lyapunov model instead of the fuzzy PID control method. In the embodiment of the present invention, the signal control risk from the current control signal to the planned control signal is calculated to determine whether mechanical obstacles will be caused to the six-axis manipulator during the process of calculating from each current control signal to the subsequent control signal of the next control signal on the premise that the PID controller outputs a series of planned control signals. Further, in the embodiment of the present invention, the device control signals are screened from the planned control signals according to the signal control risk to expand the risk-free planned control signals in the continuous planned control signals originally output by the PID controller, which can reduce the number of times of frequently adjusting the control signals. Therefore, an automated equipment control method and system based on a six-axis manipulator proposed in the embodiment of the present invention can clarify the motion actions and trajectories of the manipulator and joints in front of the end and control the six-axis manipulator by replacing the fuzzy PID control method.

[0207] As Figure 3 shown, it is a functional module diagram of the automated equipment control system based on a six-axis manipulator of the present invention.

[0208] The automated equipment control system 300 based on a six-axis manipulator of the present invention can be installed in an electronic device. According to the implemented functions, the automated equipment control system based on a six-axis manipulator may include a pose acquisition module 301, a pose selection module 302, a parameter calculation module 303, a signal acquisition module 304, and a device control module 305. The modules in the present invention may also be referred to as units, which refer to a series of computer program segments that can be executed by a processor of an electronic device and can complete fixed functions, and are stored in the memory of the electronic device.

[0209] In the embodiment of the present invention, the functions of each module / unit are as follows:

[0210] The pose acquisition module 301 is configured to determine the end coordinate point of the end of the six-axis manipulator and acquire the current pose of the six-axis manipulator. The six-axis manipulator includes an end, a first link, a second link, a starting point of the first link, and an ending point of the first link. The current pose includes a current coordinate point and a current angle value.

[0211] The pose selection module 302 is configured to plan the target pose of the six-axis manipulator by using the end coordinate point. The target pose includes a target coordinate point and a target angle value. Based on the current coordinate point and the current angle value, the pose cost from the current pose to the target pose is calculated, and the final pose is selected from the target poses by using the pose cost.

[0212] The parameter calculation module 303 is configured to calculate the pose error between the current pose and the final pose, construct the transfer function of the six-axis manipulator, construct the PID controller of the six-axis manipulator by using the pose error, and calculate the gain parameter of the PID controller based on the transfer function.

[0213] The signal acquisition module 304 is configured to output the planned control signal of the six-axis manipulator by using the PID controller based on the gain parameter and the pose error, and acquire the current control signal of the six-axis manipulator.

[0214] The device control module 305 is configured to calculate the signal control risk from the current control signal to the planned control signal, screen the device control signal from the planned control signal according to the signal control risk, and perform device control on the six-axis manipulator by using the device control signal to obtain the device control result of the six-axis manipulator.

[0215] Specifically, each module in the automated equipment control system 300 based on a six-axis manipulator in the embodiments of the present invention adopts the same technical means as those in the Figure 1 above-mentioned automated equipment control method based on a six-axis manipulator and can produce the same technical effects, which will not be elaborated here.

[0216] In addition, in each embodiment of the present invention, the functional modules can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware, or in the form of a combination of hardware and software functional modules.

[0217] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above-mentioned exemplary embodiments, and without departing from the spirit or basic characteristics of the present invention, the present invention can be implemented in other specific forms.

[0218] Therefore, from any perspective, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Thus, all changes falling within the meaning and scope of the equivalent elements of the claims are intended to be encompassed within the present invention. Any reference signs in the claims should not be construed as limiting the claims concerned.

[0219] It should be noted that in this text, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprising", "including" or any other variant thereof are intended to cover non-exclusive inclusion, such that a process, method, article or device comprising a series of elements not only includes those elements but also includes other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an..." does not exclude the presence of additional identical elements in the process, method, article or device comprising the element.

[0220] The above are only specific embodiments of the present invention, enabling those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but rather to the broadest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for controlling an automated device based on a six-axis manipulator, characterized in that: The method comprises: Determine the terminal coordinate point of the terminal of the six-axis manipulator, and collect the current posture of the six-axis manipulator, wherein the six-axis manipulator includes the terminal, the first connecting rod, the second connecting rod, the starting point of the first connecting rod, and the end point of the first connecting rod, and the current posture includes the current coordinate point and the current angle value, wherein the terminal coordinate point refers to the coordinate position that the terminal needs to reach in the next step; The target posture of the six-axis manipulator is planned using the terminal coordinate point, wherein the target posture includes a target coordinate point and a target angle value, and based on the current coordinate point and the current angle value, a posture cost from the current posture to the target posture is calculated, and a final posture is selected from the target posture using the posture cost, wherein the target posture of the six-axis manipulator is planned using the terminal coordinate point, including: Obtaining the rotation angle range and connecting rod length of the six-axis manipulator; Based on the rotation angle range and the connecting rod length, the objective function of the end coordinate point is constructed using the following formula: l x =l1cosθ x l y =l1cosθ y l z =l1cosθ z the x =l2cosθ' x the y =l2cosθ' y the z =l2cosθ' z l” x ＝l x +l' x l” y ＝l y +l' y l” z ＝l z +l' z l” x =l1cosθ x +l2cosθ' x l” y =l1cosθ y +l2cosθ' y l” z =l1cosθ z +l2cosθ' z Among them, F represents the objective function, and F includes min|l” x -l”' x |, min|l” y -l”' y |, and min|l” z -l”' z |, (l”' x , l”' y , l”' z ) represents the end coordinate point, (l” x , l” y , l” z ) represents the coordinate point to be optimized of the end coordinate point, l1 represents the length of the first link in the link lengths, l2 represents the length of the second link in the link lengths, (θ x , θ y , θ z ) represents the deflection angle of the first link relative to the rectangular coordinate system in the rectangular coordinate system with the starting point of the first link as the origin, (θ' x , θ' y , θ' z ) represents the deflection angle of the second link relative to the rectangular coordinate system in the rectangular coordinate system with the end point of the first link as the origin, l x represents the mapped length of the first link on the x-axis in the rectangular coordinate system with the starting point of the first link as the origin, l y represents the mapped length of the first link on the y-axis in the rectangular coordinate system with the starting point of the first link as the origin, l z represents the mapped length of the first link on the z-axis in the rectangular coordinate system with the starting point of the first link as the origin, l' x represents the mapped length of the second link on the x-axis in the rectangular coordinate system with the end point of the first link as the origin, l' y represents the mapped length of the second link on the y-axis in the rectangular coordinate system with the end point of the first link as the origin, l' z represents the mapped length of the second link on the z-axis in the rectangular coordinate system with the end point of the first link as the origin, θ x , θ y , θ z , θ' x , θ' y , θ' z belong to the range of the rotation angles; The angle parameter in the objective function is updated using the following formula to obtain an updated angle: v' ij =v ij +c1×rand×(pbest ij -x ij )+c2×rand×(gbest ij -x ij ) x' ij =x ij +v ij Among them, x' ij Indicates the updated angle, v' ij represents the speed after the original update speed is updated, i represents the serial number of θ, j represents the serial number of θ', and v ij represents the original update speed of θ', c1 and c2 represent learning factors, c1=c2=2, rand represents a random number between 0 and 1, x ij Indicates the current value of θ', pbest ij Indicates the neighboring historical values ​​of θ' whose F value is 0, gbest ij Indicates the historical values ​​of θ' where F is 0, θ' includes θ' x ,θ' y ,θ' z , θ includes θ x ,θ y ,θ z ; Determine whether the objective function value corresponding to the update angle in the objective function is a preset threshold; When the objective function value corresponding to the updated angle in the objective function is the preset threshold, the target coordinate point and the target angle value of the six-axis manipulator are calculated using the updated angle; Taking the target coordinate point and the target angle value as the target posture; Calculate the posture error between the current posture and the final posture, construct the transfer function of the six-axis manipulator, construct the PID controller of the six-axis manipulator using the posture error, and calculate the gain parameter of the PID controller based on the transfer function, wherein the calculation of the gain parameter of the PID controller based on the transfer function includes: Get the output error corresponding to the PID controller; Based on the output error and the transfer function, construct a state equation matrix of the six-axis manipulator; Constructing the Lyapunov original model under the PID controller; Derivative the Lyapunov original model to obtain the derivative of the Lyapunov original model; Calculating the inverse function of the PID controller under output error; Substituting the state equation matrix and the inverse function into the derivative of the Lyapunov original model to obtain a Lyapunov derivative model; Determine whether a positive definite matrix of the Lyapunov original model can be constructed; When the positive definite matrix of the Lyapunov original model can be constructed, determining that the Lyapunov original model is greater than a preset threshold; When the Lyapunov original model is greater than a preset threshold, constructing a negative definite matrix of the Lyapunov derivative model; When the proportional gain, integral gain and differential gain of the PID controller conform to the negative definite matrix, determining the gain parameters of the PID controller; Based on the gain parameter and the posture error, the PID controller is used to output a planned control signal of the six-axis manipulator, and a current control signal of the six-axis manipulator is collected; The signal control risk from the current control signal to the planned control signal is calculated, and according to the signal control risk, a device control signal is screened from the planned control signal, and the device control signal is used to perform device control on the six-axis manipulator to obtain a device control result of the six-axis manipulator, wherein the signal control risk refers to the risk of mechanical obstacles caused to the six-axis manipulator during signal control, and the signal control risk from the current control signal to the planned control signal is calculated, including: Querying the signal control interval between the current control signal and every two adjacent control signals in the planned control signal; According to the adjacent relationship between each signal control interval, the signal control intervals are randomly merged to obtain a merged control interval; Identify whether there is an interval in the historical period that is not less than the combined control interval; When there is an interval not less than the combined control interval in the historical period, no risk is used as the signal control risk; When there is no interval not less than the combined control interval in the historical period, calculating the interval length of the combined control interval; Identify whether there is a target length consistent with the interval length in the historical period; Querying the interval size under the interval length and the target size under the target length; Calculating the standard deviation of the size between the interval size and the target size; constructing a standard deviation threshold of the interval size using the size standard deviation; When the interval size is greater than the standard deviation threshold, the risk is taken as the signal to control the risk; When the interval size is not greater than the standard deviation threshold, taking risk-free as the signal to control risk; The calculating the size standard deviation between the interval size and the target size includes: Obtaining the interval angle difference and the interval coordinate distance in the interval scale; Calculating a first angle-coordinate sum between the interval angle difference and the interval coordinate distance; Obtaining a target angle difference and a target coordinate distance in the target scale; Calculating a second angle-coordinate sum between the target angle difference and the target coordinate distance; Calculate the mean of the first angle-coordinate sum and the second angle-coordinate sum; Calculating a scale standard deviation between the interval scale and the target scale according to the first angle-coordinate sum, the second angle-coordinate sum and the sum mean; The step of controlling the risk according to the signal and screening the device control signal from the planned control signal comprises: When the signal control risk is no risk, retaining the combined control interval corresponding to the no risk to obtain a retained interval; Querying whether each adjacent reserved interval in the reserved interval is continuous; When each adjacent reserved interval in the reserved interval is not continuous, shorten the interval length of the reserved interval according to the signal control interval in the reserved interval to obtain a shortened interval, so as to align each adjacent reserved interval in the reserved interval through the shortened interval to obtain an aligned interval; Calculating the number of intervals of the alignment interval; Selecting a minimum number from the number of intervals; Acquire a target alignment interval corresponding to the minimum number from the alignment interval; The planned control signal on the boundary of the target alignment interval is used as the device control signal.

2. The method according to claim 1, characterized in that: The step of calculating the posture cost from the current posture to the target posture based on the current coordinate point and the current angle value includes: Obtaining target coordinate points and target angle values ​​in the target posture; The coordinate cost from the current coordinate point to the target coordinate point is calculated using the following formula: U=u1+u2 Where U represents the coordinate cost, u1 represents the absolute value of the difference between the coordinate of the end point of the first link in the current coordinate point and the coordinate of the end point of the first link in the target coordinate point, and u2 represents the absolute value of the difference between the coordinate of the end in the current coordinate point and the coordinate of the end point; The angle cost from the current angle value to the target coordinate point is calculated using the following formula: V=v1+v2 Where V represents the angle cost, v1 represents the absolute value of the difference between the angle of the first link end point in the current angle value and the angle of the first link end point in the target angle value, and v2 represents the absolute value of the difference between the angle of the end point in the current angle value and the angle value of the end point; The sum of the coordinate cost and the angle cost is used as the pose cost from the current pose to the target pose.

3. The method according to claim 1, characterized in that The transfer function of the six-axis manipulator is constructed, comprising: The transfer function of the six-axis manipulator is constructed using the following formula: p(s)*(s 2 +αs+β)=γ*q(s) s 2 p(s)+αsp(s)+βp(s)=γq(s) in, represents the transfer function of the six-axis manipulator, t represents the time independent variable in the time domain, p(t) represents the posture of the six-axis manipulator after adjustment under the control of the PID controller, q(t) represents the posture control signal input by the PID controller to the six-axis manipulator, α, β, γ represent constant parameters, s represents the independent variable obtained by transforming t when performing Laplace transform on p(t), p(s) represents the Laplace transform form of p(t), and q(s) represents the Laplace transform form of q(t).

4. The method according to claim 1, characterized in that: The method of outputting a planning control signal of the six-axis manipulator by using the PID controller based on the gain parameter and the posture error includes: The PID controller is adjusted using the gain parameter to obtain an adjusted controller; Substituting the posture error into the adjustment controller; A planning control signal corresponding to the posture error is output in the adjustment controller.

5. An automation equipment control system based on a six-axis manipulator, characterized in that: The system comprises: A posture acquisition module is used to determine the terminal coordinate point of the terminal of the six-axis manipulator, and to acquire the current posture of the six-axis manipulator, wherein the six-axis manipulator includes the terminal, the first connecting rod, the second connecting rod, the starting point of the first connecting rod, and the end point of the first connecting rod, and the current posture includes the current coordinate point and the current angle value, wherein the terminal coordinate point refers to the coordinate position that the terminal needs to reach in the next step; A posture selection module is used to plan the target posture of the six-axis manipulator using the terminal coordinate point, wherein the target posture includes a target coordinate point and a target angle value, and based on the current coordinate point and the current angle value, calculate the posture cost from the current posture to the target posture, and select the final posture from the target posture using the posture cost, wherein the planning of the target posture of the six-axis manipulator using the terminal coordinate point includes: Obtaining the rotation angle range and connecting rod length of the six-axis manipulator; Based on the rotation angle range and the connecting rod length, the objective function of the end coordinate point is constructed using the following formula: l x =l1cosθ x l y =l1cosθ y l z =l1cosθ z the x =l2cosθ' x the y =l2cosθ' y the z =l2cosθ' z l” x ＝l x +l' x l” y ＝l y +l' y l” z ＝l z +l' z l” x =l1cosθ x +l2cosθ' x l” y =l1cosθ y +l2cosθ' y l” z =l1cosθ z +l2cosθ' z Among them, F represents the objective function, and F includes min|l” x -l”' x |, min|l” y -l”' y |, and min|l” z -l”' z |. (l”' x , l”' y , l”' z ) represents the end coordinate point, (l” x , l” y , l” z ) represents the coordinate point to be optimized of the end coordinate point. l1 represents the length of the first link among the link lengths, l2 represents the length of the second link among the link lengths. (θ x , θ y , θ z ) represents the deflection angle of the first link relative to the rectangular coordinate system in the rectangular coordinate system with the starting point of the first link as the origin. (θ' x , θ' y , θ' z ) represents the deflection angle of the second link relative to the rectangular coordinate system in the rectangular coordinate system with the end point of the first link as the origin. l x represents the mapped length of the first link on the x-axis in the rectangular coordinate system with the starting point of the first link as the origin. l y represents the mapped length of the first link on the y-axis in the rectangular coordinate system with the starting point of the first link as the origin. l z represents the mapped length of the first link on the z-axis in the rectangular coordinate system with the starting point of the first link as the origin. l' x represents the mapped length of the second link on the x-axis in the rectangular coordinate system with the end point of the first link as the origin. l' y represents the mapped length of the second link on the y-axis in the rectangular coordinate system with the end point of the first link as the origin. l' z represents the mapped length of the second link on the z-axis in the rectangular coordinate system with the end point of the first link as the origin. θ x , θ y , θ z , θ' x , θ' y , θ' z belong to the range of the rotation angle; The angle parameter in the objective function is updated using the following formula to obtain an updated angle: v' ij =v ij +c1×rand×(pbest ij -x ij )+c2×rand×(gbest ij -x ij ) x' ij =x ij +v ij Among them, x' ij Indicates the updated angle, v' ij represents the speed after the original update speed is updated, i represents the serial number of θ, j represents the serial number of θ', and v ij represents the original update speed of θ', c1 and c2 represent learning factors, c1=c2=2, rand represents a random number between 0 and 1, x ij Indicates the current value of θ', pbest ij Indicates the neighboring historical values ​​of θ' whose F value is 0, gbest ij Indicates the historical values ​​of θ' where F is 0, θ' includes θ' x ,θ' y ,θ' z , θ includes θ x ,θ y ,θ z ; Determine whether the objective function value corresponding to the update angle in the objective function is a preset threshold; When the objective function value corresponding to the updated angle in the objective function is the preset threshold, the target coordinate point and the target angle value of the six-axis manipulator are calculated using the updated angle; Taking the target coordinate point and the target angle value as the target posture; A parameter calculation module is used to calculate the posture error between the current posture and the final posture, construct the transfer function of the six-axis manipulator, use the posture error to construct the PID controller of the six-axis manipulator, and calculate the gain parameter of the PID controller based on the transfer function, wherein the calculation of the gain parameter of the PID controller based on the transfer function includes: Get the output error corresponding to the PID controller; Based on the output error and the transfer function, construct a state equation matrix of the six-axis manipulator; Constructing the Lyapunov original model under the PID controller; Derivative the Lyapunov original model to obtain the derivative of the Lyapunov original model; Calculating the inverse function of the PID controller under output error; Substituting the state equation matrix and the inverse function into the derivative of the Lyapunov original model to obtain a Lyapunov derivative model; Determine whether a positive definite matrix of the Lyapunov original model can be constructed; When the positive definite matrix of the Lyapunov original model can be constructed, determining that the Lyapunov original model is greater than a preset threshold; When the Lyapunov original model is greater than a preset threshold, constructing a negative definite matrix of the Lyapunov derivative model; When the proportional gain, integral gain and differential gain of the PID controller conform to the negative definite matrix, determining the gain parameters of the PID controller; A signal acquisition module, used to output a planned control signal of the six-axis manipulator using the PID controller based on the gain parameter and the posture error, and to acquire a current control signal of the six-axis manipulator; The device control module is used to calculate the signal control risk from the current control signal to the planned control signal, and according to the signal control risk, screen the device control signal from the planned control signal, and use the device control signal to control the six-axis manipulator to obtain the device control result of the six-axis manipulator, wherein the signal control risk refers to the risk of mechanical obstacles caused to the six-axis manipulator during signal control. The calculation of the signal control risk from the current control signal to the planned control signal includes: Querying the signal control interval between the current control signal and every two adjacent control signals in the planned control signal; According to the adjacent relationship between each signal control interval, the signal control intervals are randomly merged to obtain a merged control interval; Identify whether there is an interval in the historical period that is not less than the combined control interval; When there is an interval not less than the combined control interval in the historical period, no risk is used as the signal control risk; When there is no interval not less than the combined control interval in the historical period, calculating the interval length of the combined control interval; Identify whether there is a target length consistent with the interval length in the historical period; Querying the interval size under the interval length and the target size under the target length; Calculating the standard deviation of the size between the interval size and the target size; constructing a standard deviation threshold of the interval size using the size standard deviation; When the interval size is greater than the standard deviation threshold, the risk is taken as the signal to control the risk; When the interval size is not greater than the standard deviation threshold, taking risk-free as the signal to control risk; The calculating the size standard deviation between the interval size and the target size includes: Obtaining the interval angle difference and the interval coordinate distance in the interval scale; Calculating a first angle-coordinate sum between the interval angle difference and the interval coordinate distance; Obtaining a target angle difference and a target coordinate distance in the target scale; Calculating a second angle-coordinate sum between the target angle difference and the target coordinate distance; Calculate the mean of the first angle-coordinate sum and the second angle-coordinate sum; Calculating a scale standard deviation between the interval scale and the target scale according to the first angle-coordinate sum, the second angle-coordinate sum and the sum mean; The step of controlling the risk according to the signal and screening the device control signal from the planned control signal comprises: When the signal control risk is no risk, retaining the combined control interval corresponding to the no risk to obtain a retained interval; Querying whether each adjacent reserved interval in the reserved interval is continuous; When each adjacent reserved interval in the reserved interval is not continuous, shorten the interval length of the reserved interval according to the signal control interval in the reserved interval to obtain a shortened interval, so as to align each adjacent reserved interval in the reserved interval through the shortened interval to obtain an aligned interval; Calculating the number of intervals of the alignment interval; Selecting a minimum number from the number of intervals; Acquire a target alignment interval corresponding to the minimum number from the alignment interval; The planned control signal on the boundary of the target alignment interval is used as the device control signal.

Images