General servo system control method and system based on special equipment application

Abstract

The application provides a kind of general servo system control method and system based on special equipment application, it is related to special equipment control technical field, first receive the operation instruction sequence corresponding to special equipment application scene, then according to operation instruction sequence construction job flow timing diagram, the time sequence correlation of each operation instruction unit is clear, then obtain the real-time running state data set of special equipment, based on job flow timing diagram and real-time running state data set generate predicted motion trajectory curve, cover the expected position and speed parameter of each time point in future time window, finally call the trajectory tracking controller of general servo system to the deviation correction processing of predicted motion trajectory curve and real-time running state data set, generate the control signal sequence of driving component, drive special equipment to move according to predicted motion trajectory curve, effectively improve the motion precision and dynamic response capability of special equipment, enhance the adaptability and reliability of equipment in complex application scene.

Description

Technical Field

[0001] This invention relates to the field of special equipment control technology, and more specifically, to a general servo system control method and system for special equipment applications. Background Technology

[0002] Traditional methods for controlling special equipment often rely on pre-set fixed programs to drive the equipment to perform tasks. However, the application scenarios of special equipment are complex and diverse, and the operational commands for the equipment vary significantly and dynamically depending on the scenario. For example, in scenarios requiring rapid response and flexible adjustment, such as the attitude adjustment of aircraft in the aerospace field, pre-set fixed programs are difficult to adapt to real-time changing operational requirements.

[0003] Existing control methods, when dealing with complex sequences of operational commands, typically simply execute the commands sequentially, lacking effective analysis and utilization of the temporal relationships between the commands. Furthermore, they fail to fully integrate real-time operational status data of special equipment, such as the position, speed, and load of drive components, with the operational command sequence. This results in difficulties in achieving precise trajectory tracking and dynamic adjustment during equipment movement, easily leading to motion deviations and impacting equipment performance and mission execution effectiveness. Summary of the Invention

[0004] In view of the aforementioned problems, and in conjunction with the first aspect of the present invention, embodiments of the present invention provide a general-purpose servo system control method for special equipment applications, the method comprising:

[0005] Receive an operation instruction sequence corresponding to a special equipment application scenario. The operation instruction sequence contains multiple discrete operation instruction units arranged in chronological order. Each discrete operation instruction unit carries an operation type identifier and an operation parameter set.

[0006] The operation flow timing diagram of the special equipment is constructed based on the operation instruction sequence. The operation flow timing diagram includes multiple operation nodes and timing association edges connecting the operation nodes. Each operation node corresponds to a discrete operation instruction unit. The timing association edges are used to represent the time interval parameter between adjacent discrete operation instruction units.

[0007] Obtain a set of real-time operating status data for the special equipment, which includes the current position parameters, current speed parameters, and current load parameters of the drive components of the special equipment at the current moment.

[0008] Based on the operation process sequence diagram and the real-time operation status data set, a predicted motion trajectory curve of the special equipment is generated. The predicted motion trajectory curve includes the expected position parameters and expected speed parameters of the special equipment at each time point within a future time window.

[0009] The trajectory tracking controller of the general servo system is invoked to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and a control signal sequence of the drive component is generated. The control signal sequence is used to drive the special equipment to perform motion according to the predicted motion trajectory curve.

[0010] Furthermore, embodiments of the present invention also provide a general-purpose servo system control system for special equipment applications, comprising:

[0011] A processor; a machine-readable storage medium for storing machine-executable instructions of the processor; wherein the processor is configured to execute the aforementioned general-purpose servo system control method for special equipment applications by executing the machine-executable instructions.

[0012] In another aspect, embodiments of the present invention also provide a computer program product, the computer program product including machine-executable instructions, the machine-executable instructions being stored in a computer-readable storage medium, a processor of a general-purpose servo system control system for special equipment applications reading the machine-executable instructions from the computer-readable storage medium, the processor executing the machine-executable instructions, causing the general-purpose servo system control system for special equipment applications to execute the aforementioned general-purpose servo system control method for special equipment applications.

[0013] Based on the above, by receiving a sequence of operation instructions containing multiple discrete operation instruction units arranged in chronological order and carrying operation type identifiers and operation parameter sets, a work process timing diagram is constructed based on the operation instruction sequence, and a real-time operating status data set of the special equipment is obtained, enabling the control method to monitor the current state of the equipment in real time. Based on the work process timing diagram and the real-time operating status data set, a predicted motion trajectory curve is generated, allowing for advance planning of the equipment's movement within future time windows. The trajectory tracking controller of the general-purpose servo system is invoked to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, generating a control signal sequence for the drive components. This ensures that the special equipment can accurately execute movement according to the predicted motion trajectory curve, effectively improving the motion accuracy and dynamic response capability of the special equipment, enhancing its adaptability and reliability in complex application scenarios, and thus improving the overall efficiency and success rate of task execution. Attached Figure Description

[0014] Figure 1This is a schematic diagram of the execution flow of a general servo system control method for special equipment applications provided in an embodiment of the present invention.

[0015] Figure 2 This is a schematic diagram of exemplary hardware and software components of a general-purpose servo system control system for special equipment applications provided in an embodiment of the present invention. Detailed Implementation

[0016] Figure 1 This is a flowchart illustrating a general servo system control method for special equipment applications provided in one embodiment of the present invention, which will be described in detail below.

[0017] This embodiment provides a general servo system control method for special equipment applications. This method can be applied to the control systems of special equipment such as high-dynamic-response industrial robotic arms. The following will describe each step of the method in detail with a specific implementation scenario. In this scenario, the special equipment is a six-axis articulated industrial robotic arm, whose driving components are six independent servo motors, which drive the six rotary joints of the robotic arm to achieve precise positioning and movement of the end effector in Cartesian space.

[0018] Step S110: Receive the operation instruction sequence corresponding to the special equipment application scenario. The operation instruction sequence includes multiple discrete operation instruction units arranged in chronological order. Each discrete operation instruction unit carries an operation type identifier and an operation parameter set.

[0019] In this embodiment, an operation instruction sequence issued by the upper-level manufacturing execution system is received. This operation instruction sequence is a set of digital instructions for the robotic arm to complete a complete work process. This operation instruction sequence is organized into a first-in, first-out (FIFO) instruction queue. The first discrete operation instruction unit in the instruction queue corresponds to the operation "rapidly move above the gripping point," the second discrete operation instruction unit corresponds to the operation "vertically descend and close the gripper," the third discrete operation instruction unit corresponds to the operation "lift and quickly move above the placement point," and the fourth discrete operation instruction unit corresponds to the operation "vertically descend and open the gripper." Each discrete operation instruction unit is a structured data packet. Each discrete operation instruction unit's data packet contains at least two fields: the first field is an operation type identifier, which is a unique identifier, using the string "MOVJ" to represent joint interpolation motion, the string "MOVL" to represent linear interpolation motion, and the string "HAND" to represent gripper control; the second field is an operation parameter set, which is a list of key-value pairs. For discrete operation instruction units with the operation type identifier "MOVJ", their operation parameter set includes the target joint angle vector parameter, motion velocity percentage parameter, and transition radius parameter; for discrete operation instruction units with the operation type identifier "HAND", their operation parameter set includes the gripper opening / closing state parameter, gripping force parameter, and action time parameter. The above operation instruction sequence is continuously input in the form of a data stream.

[0020] Step S120: Construct a time sequence diagram of the operation process of the special equipment according to the operation instruction sequence. The time sequence diagram of the operation process includes multiple operation nodes and time sequence association edges connecting the operation nodes. Each operation node corresponds to a discrete operation instruction unit. The time sequence association edge is used to represent the time interval parameter between adjacent discrete operation instruction units.

[0021] Step S121: Parse the operation type identifier of each discrete operation instruction unit in the operation instruction sequence, and retrieve the operation execution parameter template corresponding to the operation type identifier from the preset instruction template library according to the operation type identifier. The operation execution parameter template includes the standard motion trajectory curve template and the standard execution duration parameter corresponding to the operation type identifier.

[0022] For example, the string "MOVJ" is read. Then, "MOVJ" is used as the lookup key to retrieve the information from a pre-stored instruction template library in non-volatile memory. This instruction template library is a pre-compiled data dictionary where the keys are various operation type identifiers and the values ​​are the corresponding operation execution parameter templates. The operation execution parameter template is a predefined data structure containing all the kinematic information required to complete a standard operation of that type. For a discrete operation instruction unit with the operation type identifier "MOVJ", the standard motion trajectory curve template in its corresponding operation execution parameter template is defined as an "S"-shaped velocity curve with continuous acceleration planned in joint space. This standard motion trajectory curve template is stored as a function of joint angles with respect to time. For each joint of the robotic arm, its standard motion trajectory curve template is a fifth-order polynomial, which guarantees that the joint angle, joint angular velocity, and joint angular acceleration are all zero at the start and end points. Simultaneously, the operation execution parameter template also contains a standard execution duration parameter, which is the theoretical time required to complete the joint interpolation motion segment under rated load and standard speed.

[0023] Step S122: Assign parameters to the operation execution parameter template according to the operation parameter set to generate a job node corresponding to each discrete operation instruction unit. The job node includes the standard motion trajectory curve template and the standard execution duration parameter.

[0024] After retrieving the operation execution parameter template, specific parameter values ​​are extracted from the operation parameter set of the current discrete operation instruction unit, and these parameter values ​​are assigned to the formal parameters in the operation execution parameter template. For a discrete operation instruction unit with an operation type identifier of "MOVJ", its operation parameter set provides specific values ​​for the target joint angle vector parameter, which is an array containing six target angle values. The target joint angle vector parameter is assigned to the endpoint parameter in the standard motion trajectory curve template, while the starting point parameter of the standard motion trajectory curve template defaults to the current actual joint angle of the robotic arm or the endpoint angle of the work node corresponding to the previous discrete operation instruction unit. After the assignment is completed, the standard motion trajectory curve template changes from a general template that only defines the motion form to a specific trajectory connecting a specific starting point and a specific endpoint. At the same time, the standard execution duration parameter is also dynamically recalculated based on the angle difference between the starting point and the endpoint, as well as the preset maximum joint velocity constraint and the preset maximum joint acceleration constraint, to generate an execution duration parameter that is more consistent with the actual motion distance. After the above processing, each discrete operation instruction unit is instantiated into a job node, which is a data object containing a specific motion trajectory curve and specific execution time parameters.

[0025] Step S123: Extract the time interval parameter between adjacent discrete operation instruction units in the operation instruction sequence, and use the time interval parameter as a temporal association edge connecting adjacent job nodes. The temporal association edge is used to constrain the time connection relationship between adjacent job nodes.

[0026] After generating each independent task node, the temporal relationship between these nodes needs to be defined. The operation instruction sequence is traversed, and the time interval parameter between every two adjacent discrete operation instruction units is extracted. This time interval parameter is carried within the operation instruction sequence itself; it specifies how long to wait after the previous discrete operation instruction unit finishes execution before starting the next. For example, between the discrete operation instruction unit corresponding to "quickly move above the gripping point" and the discrete operation instruction unit corresponding to "vertically descend and close the gripper," there is a time interval parameter used to wait for the robotic arm to completely stop and eliminate residual vibration. This extracted time interval parameter is used as a temporal correlation edge, with the source node of this edge being the previous task node and the target node being the next task node. This temporal correlation edge has no actual physical motion meaning; it serves only as a time constraint to precisely control the relative positions of the two task nodes on the time axis.

[0027] Step S124: Arrange the work nodes in the order of the operation instruction sequence, and connect adjacent work nodes through the timing association edge to generate the operation flow timing diagram of the special equipment. The order of the work nodes in the operation flow timing diagram is consistent with the order of the discrete operation instruction units in the operation instruction sequence.

[0028] Based on the job nodes and temporal association edges generated in steps S121 to S123, a directed acyclic graph is constructed. The job nodes generated in step S122 are arranged in order of their position in the original operation instruction sequence, forming a linear linked list. Then, the temporal association edges generated in step S123 are used as connecting lines to sequentially connect adjacent job nodes. The first job node has only outgoing edges, the last job node has only incoming edges, and each intermediate job node has one incoming edge and one outgoing edge. The final generated data structure is the job flow timing graph. This job flow timing graph clearly expresses that the entire job task consists of a series of consecutive job nodes, and the time waiting relationship between each job node is uniquely determined by the temporal association edges.

[0029] Step S125: Based on the standard motion trajectory curve template, standard execution duration parameter, and time interval parameter of the time sequence associated edge of the task node in the task flow time sequence diagram, calculate the start execution time point and end execution time point of each task node in the task flow time sequence diagram to obtain the time axis parameter set of the task flow time sequence diagram.

[0030] Step S1251: Obtain the standard execution duration parameter of the first job node in the job flow sequence diagram, take the preset initial time zero point as the start execution time point of the first job node, calculate the end execution time point of the first job node according to the standard execution duration parameter of the first job node, and obtain the start execution time point and end execution time point of the first job node.

[0031] Obtain the first job node in the job flow sequence diagram. Set the start execution time of this first job node to a preset initial time zero, which is the time base for the start of the job. Then, read the standard execution duration parameter of this first job node. The end execution time of this first job node is equal to the start execution time of this first job node plus the standard execution duration parameter of this first job node, that is, zero plus the standard execution duration parameter of this first job node, to obtain the end time of the first job node.

[0032] Step S1252: Add the end execution time of the first job node to the time interval parameter of the time sequence association edge connecting the first job node and the second job node to obtain the start execution time of the second job node. Calculate the end execution time of the second job node according to the standard execution duration parameter of the second job node to obtain the start execution time and end execution time of the second job node.

[0033] Next, process the second job node. Obtain the time interval parameter carried by the temporal association edge connecting the first and second job nodes. The start execution time of the second job node is equal to the end execution time of the first job node plus the aforementioned time interval parameter. Then, read the standard execution duration parameter of the second job node. The end execution time of the second job node is equal to the start execution time of the second job node plus the standard execution duration parameter of the second job node.

[0034] Step S1253: According to the order of the job nodes in the job flow sequence diagram, add the end execution time of the current job node to the time interval parameter of the time sequence association edge connecting the current job node and the next job node in turn to obtain the start execution time of the next job node. Calculate the end execution time of the next job node based on the standard execution duration parameter of the next job node to obtain the start execution time and end execution time of all job nodes.

[0035] The calculation process in step S1252 is iteratively applied to all subsequent job nodes in the job flow sequence diagram. For the k-th job node (where k is an integer greater than 1), its start execution time is equal to the end execution time of the (k-1)-th job node plus the time interval parameter of the time-series association edge connecting the (k-1)-th and k-th job nodes. The end execution time of the k-th job node is equal to its start execution time plus its own standard execution duration parameter. By traversing all job nodes in this way, a unique start and end execution time is calculated for each job node.

[0036] Step S1254: Arrange the start and end execution times of each job node according to the order of the job nodes to generate a time axis parameter set for the job flow sequence diagram. The time axis parameter set includes the start and end execution times of each job node.

[0037] The start and end execution times of all job nodes calculated in steps S1251 to S1253 are organized into a time axis parameter set according to the order of the job nodes in the job flow sequence diagram. This time axis parameter set is an array structure, where each element corresponds to a job node, and each element contains the start and end execution times of that job node.

[0038] Step S1255: Align the start execution time value and end execution time value of each job node in the time axis parameter set with the standard motion trajectory curve template corresponding to the job node on the time axis to obtain the motion trajectory curve segment of each job node on the time axis.

[0039] For each task node, the time domain of its corresponding standard motion trajectory curve template is translated and scaled to align with the start and end execution time values ​​calculated in step S1254. Specifically, the standard motion trajectory curve template is originally defined on the time interval from zero to the standard execution duration parameter of the task node. This time interval is translated as a whole, aligning its starting point with the start execution time value of the task node and its ending point with the end execution time value of the task node. After the above time axis alignment process, each task node obtains a motion trajectory curve segment on a globally unified time axis. This motion trajectory curve segment accurately describes the expected values ​​of the changes in the joint angles of the robotic arm over time within the specified time interval for that task node.

[0040] Step S130: Obtain the real-time operating status data set of the special equipment, which includes the current position parameters, current speed parameters, and current load parameters of the drive components of the special equipment at the current moment.

[0041] Simultaneously or subsequently, the real-time operational status data set of the robotic arm is continuously acquired. This real-time operational status data set is collected by sensors installed on each joint of the robotic arm. Specifically, the current position parameters of each joint are obtained through photoelectric encoders at the rear end of the servo motors of each joint. These current position parameters are an array containing six joint angle values, reflecting the current pose of the robotic arm in the joint space. By performing differential operations on the above current position parameters, the current velocity parameters are obtained, which are an array containing six joint angular velocity values. Current sensors connected in series in the drive circuit of each joint servo motor are used to collect the phase current of the motor in real time. Combined with the motor's torque constant, the current load parameters are calculated. These current load parameters are an array containing six external torque values ​​acting on each joint, reflecting the mapping of the interaction forces between the robotic arm's end effector and the external environment at each joint.

[0042] Step S140: Generate a predicted motion trajectory curve for the special equipment based on the operation process time sequence diagram and the real-time operation status data set. The predicted motion trajectory curve includes the expected position parameters and expected speed parameters of the special equipment at each time point within a future time window.

[0043] Step S141: Extract the current job node from the job flow sequence diagram as the current job node, and obtain the start execution time point, end execution time point, and motion trajectory curve segment corresponding to the current job node.

[0044] Obtain the current time value and compare it with the start and end execution times of each job node in the job flow sequence diagram. Determine which job node the current time value falls within the range of the start and end execution times, and designate that job node as the current job node. Extract the start and end execution times of the current job node, as well as the corresponding motion trajectory curve segment, from the job flow sequence diagram.

[0045] Step S142: Calculate the position deviation on the motion trajectory curve segment at the current moment based on the motion trajectory curve segment of the current working node and the current position parameter in the real-time running status data set, and obtain the trajectory deviation parameter of the current position of the driving component relative to the motion trajectory curve segment.

[0046] Substituting the current time value into the motion trajectory curve segment of the current work node, the expected position parameters of each joint of the robotic arm at the current time are calculated. This expected position parameter is an array containing the expected angle values ​​of the six joints. The current position parameters obtained in step S130 are subtracted from the above expected position parameters joint by joint to obtain the position deviation of each joint. This position deviation is an array containing the position deviation values ​​of the six joints, i.e., the trajectory deviation parameter, which reflects the degree of deviation between the actual position of the robotic arm and the planned trajectory.

[0047] Step S143: Subtract the end execution time of the current job node from the start execution time of the current job node to obtain the remaining execution time parameter of the current job node, and generate the remaining motion trajectory curve segment corresponding to the current job node based on the motion trajectory curve segment of the current job node and the remaining execution time parameter.

[0048] The total execution time of the current job node is obtained by subtracting its start time from its end time. The remaining execution time is then calculated by subtracting the current time from the end time. From the current job node's trajectory curve segment, a portion between the current time and the end time is extracted. The time domain of this extracted segment is then reparameterized, starting from zero and extending to the remaining execution time. After this extraction and reparameterization, the remaining trajectory curve segment corresponding to the current job node is obtained.

[0049] Step S144: Obtain all subsequent work nodes located after the current work node in the work process sequence diagram, and splice the motion trajectory curve segments of the subsequent work nodes according to the order of the work process sequence diagram to generate a set of subsequent work trajectory curve segments.

[0050] Extract all work nodes following the current work node from the work flow sequence diagram, and designate these work nodes as subsequent work nodes. Following the order of these subsequent work nodes in the work flow sequence diagram, sequentially obtain the motion trajectory curve segment corresponding to each subsequent work node. Connect these motion trajectory curve segments end-to-end in chronological order to form a continuous set of trajectory curve segments, i.e., the set of subsequent work trajectory curve segments.

[0051] Step S145: Perform time-series connection processing on the remaining motion trajectory curve segments and the set of subsequent operation trajectory curve segments to generate the initial predicted motion trajectory curve of the special equipment in the future time window. The initial predicted motion trajectory curve includes all motion trajectory curve segments within the time range from the current moment to the end of the last subsequent operation node.

[0052] The remaining motion trajectory curve segment generated in step S143 is temporally connected with the set of subsequent operation trajectory curve segments generated in step S144. Specifically, the endpoint of the remaining motion trajectory curve segment is used as the starting point of the first trajectory curve segment in the set of subsequent operation trajectory curve segments, and their time axes are connected so that the time endpoint of the remaining motion trajectory curve segment is aligned with the time starting point of the first trajectory curve segment in the set of subsequent operation trajectory curve segments. After the above temporal connection process, a continuous trajectory curve is obtained from the current moment to the end of the last subsequent operation node, and this continuous trajectory curve is used as the initial predicted motion trajectory curve.

[0053] Step S146: Extract the current speed parameter from the real-time running status data set, compare the current speed parameter with the expected speed parameter corresponding to the current moment in the initial predicted motion trajectory curve, and obtain the speed deviation parameter of the current speed of the driving component relative to the initial predicted motion trajectory curve.

[0054] The current speed parameter is extracted from the real-time operating status data set obtained in step S130. This current speed parameter is an array containing the angular velocity values ​​of the six joints. The current value is substituted into the initial predicted motion trajectory curve to calculate the expected speed parameter for each joint of the robotic arm at the current moment. This expected speed parameter is an array containing the expected angular velocity values ​​of the six joints. The expected speed parameter is subtracted from the current speed parameter joint by joint to obtain the speed deviation for each joint. This speed deviation is an array containing the speed deviation values ​​of the six joints, i.e., the speed deviation parameter.

[0055] Step S147: Extract the current load parameters from the real-time operating status data set, input the current load parameters into a preset load compensation function for calculation, and obtain the load compensation coefficient. The load compensation coefficient is used to adjust the acceleration response capability of the drive component under different load conditions.

[0056] The current load parameters are extracted from the real-time operating status data set obtained in step S130. These current load parameters are an array containing the external torque values ​​of the six joints. For each joint, its current load parameters are input into a preset load compensation function for calculation. This load compensation function is a mapping relationship constructed based on the robot arm's dynamics model. Its input is the external torque, and its output is the load compensation coefficient for that joint. The load compensation coefficient is a dimensionless scalar value, and its value is negatively correlated with the magnitude of the external torque; that is, the larger the external torque, the smaller the load compensation coefficient, indicating that the joint's acceleration response capability is weakened under that load condition. The specific form of the load compensation function is as follows: first, the standard inertia parameters of the robot arm under rated load are calculated; then, based on the ratio of the current load parameters to the aforementioned standard inertia parameters, the load compensation coefficient is determined through an inverse proportional function.

[0057] Step S148: Based on the speed deviation parameter and the load compensation coefficient, the expected speed parameters at each time point after the current moment in the initial predicted motion trajectory curve are corrected point by point to obtain the corrected expected speed parameter sequence. The expected speed parameter at each time point in the corrected expected speed parameter sequence is negatively correlated with the speed deviation parameter and positively correlated with the load compensation coefficient.

[0058] For each time point after the current moment in the initial predicted motion trajectory curve, the expected velocity parameter corresponding to that time point is obtained. For each joint, the velocity correction amount for that joint at that time point is calculated based on the joint's velocity deviation parameter and load compensation coefficient. The velocity correction amount is calculated by multiplying the velocity deviation parameter by a preset attenuation factor, and then multiplying it by the load compensation coefficient to obtain a correction value that gradually decreases with time and velocity deviation. Then, the above velocity correction amount is superimposed on the expected velocity parameter at that time point to obtain the corrected expected velocity parameter. This correction process ensures that the expected velocity parameter at each time point in the corrected expected velocity parameter sequence is negatively correlated with the current velocity deviation parameter, i.e., the larger the current velocity deviation, the larger the correction magnitude; and positively correlated with the load compensation coefficient, i.e., the larger the load compensation coefficient, the larger the correction magnitude. By traversing all time points after the current moment in the initial predicted motion trajectory curve point by point, the corrected expected velocity parameter sequence is obtained.

[0059] Step S149: Perform integral reconstruction processing on the expected position parameters of each time point after the current moment in the initial predicted motion trajectory curve according to the corrected expected velocity parameter sequence to obtain the corrected expected position parameter sequence. The expected position parameter of each time point in the corrected expected position parameter sequence is equal to the sum of the product of the expected position parameter of the previous moment and the corrected expected velocity parameter of the current moment multiplied by the time interval.

[0060] For each time point after the current moment in the initial predicted trajectory curve, the current position parameter at the current moment is used as the starting point for integration, and numerical integration is performed on the corrected expected velocity parameter sequence obtained in step S148 for reconstruction. Specifically, for the first time point, i.e., the first sampling time point after the current moment, the corrected expected position parameter at this time point is equal to the current position parameter plus the product of the corrected expected velocity parameter at the current moment and the time interval. For the subsequent nth time point, the corrected expected position parameter at this time point is equal to the corrected expected position parameter at the (n-1)th time point plus the product of the corrected expected velocity parameter at the (n-1)th time point and the time interval. Through the above recursive integration calculation, traversing all time points, the corrected expected position parameter sequence is obtained.

[0061] Step S1410: Combine the corrected expected position parameter sequence and the corrected expected velocity parameter sequence in chronological order to generate the predicted motion trajectory curve of the special equipment. Each time point in the predicted motion trajectory curve corresponds to one corrected expected position parameter and one corrected expected velocity parameter.

[0062] The corrected expected position parameter sequence generated in step S149 and the corrected expected velocity parameter sequence generated in step S148 are aligned according to time points, with each time point corresponding to a set of position and velocity parameters. These position and velocity parameters are combined into data pairs and arranged in chronological order to form a new time series data, which is the predicted motion trajectory curve. Each time point in this predicted motion trajectory curve corresponds to an expected position parameter array containing six joint angle values ​​and an expected velocity parameter array containing six joint angular velocity values.

[0063] Step S150: Call the trajectory tracking controller of the general servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and generate the control signal sequence of the drive component. The control signal sequence is used to drive the special equipment to perform motion according to the predicted motion trajectory curve.

[0064] Step S151: Input the predicted motion trajectory curve into the reference trajectory generation module of the trajectory tracking controller, and perform discretization sampling processing on the predicted motion trajectory curve according to the preset time step to obtain a reference trajectory point sequence. The reference trajectory point sequence contains multiple reference trajectory points, and each reference trajectory point corresponds to a sampling time point and the reference position parameters and reference velocity parameters of that sampling time point.

[0065] The predicted motion trajectory curve generated in step S1410 is input into the reference trajectory generation module. This module discretizes the predicted motion trajectory curve according to a preset fixed time step. At each sampling time point, the expected position parameter and expected velocity parameter corresponding to that time point are extracted from the predicted motion trajectory curve. The expected position parameter is used as the reference position parameter, and the expected velocity parameter is used as the reference velocity parameter. The sampling time point, reference position parameter, and reference velocity parameter are combined into a reference trajectory point. All reference trajectory points are arranged in chronological order to obtain a reference trajectory point sequence.

[0066] Step S152: Input the current position parameter and current velocity parameter in the real-time running status data set into the status feedback module of the trajectory tracking controller, and construct a current state vector based on the current position parameter and the current velocity parameter. The current state vector contains a position component and a velocity component.

[0067] The current position parameters and current velocity parameters are extracted from the real-time operating status data set obtained in step S130. The status feedback module combines the current position parameters and current velocity parameters to construct a current state vector. This current state vector is a multi-dimensional vector with a dimension twice that of the robotic arm's degrees of freedom. The first six components of the current state vector are the current position parameters, i.e., the angle values ​​of the six joints; the last six components are the current velocity parameters, i.e., the angular velocity values ​​of the six joints.

[0068] Step S153: Compare the reference position parameter and reference velocity parameter corresponding to the current sampling time point in the reference trajectory point sequence with the current state vector to obtain the position tracking error vector and the velocity tracking error vector. The position tracking error vector is equal to the reference position parameter minus the current position parameter, and the velocity tracking error vector is equal to the reference velocity parameter minus the current velocity parameter.

[0069] Obtain the reference trajectory point corresponding to the current sampling time point from the reference trajectory point sequence, and extract the reference position parameter and reference velocity parameter from the reference trajectory point. Subtract the reference position parameter from the position component in the current state vector element by element to obtain the position tracking error vector. Subtract the reference velocity parameter from the velocity component in the current state vector element by element to obtain the velocity tracking error vector.

[0070] Step S154: Input the position tracking error vector and the velocity tracking error vector into the feedforward control module of the trajectory tracking controller, calculate the feedforward control quantity based on the reference velocity parameter and the reference position parameter, and obtain the feedforward control signal component.

[0071] The position tracking error vector and velocity tracking error vector obtained in step S153 are input into the feedforward control module. Based on the inverse dynamics model of the robotic arm, the feedforward control module calculates the desired torque required for the robotic arm to complete the desired trajectory according to the reference position parameters, reference velocity parameters, and reference acceleration parameters. Specifically, the reference acceleration parameters are first obtained by differentiating the reference velocity parameters. Then, the reference position parameters, reference velocity parameters, and reference acceleration parameters are substituted into the inverse dynamics equation of the robotic arm, which includes inertial force terms, Coriolis force and centrifugal force terms, gravity terms, and friction terms. The output of the above inverse dynamics equation is calculated to obtain the feedforward control signal component, which is an array containing the desired torque values ​​of the six joints.

[0072] Step S155: Input the position tracking error vector and the velocity tracking error vector into the feedback control module of the trajectory tracking controller, and perform a weighted summation calculation on the position tracking error vector and the velocity tracking error vector according to the preset proportional gain parameter and integral gain parameter to obtain the feedback control signal component.

[0073] The position tracking error vector and velocity tracking error vector obtained in step S153 are input into the feedback control module. This feedback control module employs a proportional-integral-derivative (PID) control law. Specifically, the position tracking error vector is multiplied by a preset position loop proportional gain parameter to obtain the position loop proportional control component. The velocity tracking error vector is multiplied by a preset velocity loop proportional gain parameter to obtain the velocity loop proportional control component. Simultaneously, the position tracking error vector is integrated to obtain the position error integral vector, which is then multiplied by a preset integral gain parameter to obtain the integral control component. The position loop proportional control component, velocity loop proportional control component, and integral control component are then summed joint by joint to obtain the feedback control signal component. This feedback control signal component is an array containing torque compensation values ​​for six joints.

[0074] Step S156: Superimpose the feedforward control signal component and the feedback control signal component to generate the initial control signal at the current moment. The initial control signal contains the target control quantity that the drive component needs to output at the next moment.

[0075] The feedforward control signal component obtained in step S154 and the feedback control signal component obtained in step S155 are added joint by joint to obtain the initial control signal. This initial control signal is an array containing the expected output torque values ​​of the six joints, which are the target control quantities that the drive component needs to output at the next moment.

[0076] Step S157: Obtain the set of physical constraints for the drive components of the general servo system. The set of physical constraints for the drive components includes the maximum output torque constraint parameter, the maximum output speed constraint parameter, and the maximum acceleration constraint parameter. Perform amplitude limiting processing on the initial control signal according to the set of physical constraints for the drive components to obtain the amplitude-limited control signal.

[0077] Obtain the pre-stored set of physical constraints for the drive components. This set includes the maximum output torque constraint parameter, maximum output speed constraint parameter, and maximum acceleration constraint parameter for each joint servo motor. For the desired output torque value of each joint in the initial control signal, compare it with the maximum output torque constraint parameter. If the desired output torque value exceeds the maximum output torque constraint parameter, then the desired output torque value of that joint is limited to the maximum output torque constraint parameter. Simultaneously, based on the current speed parameter and the maximum acceleration constraint parameter, calculate the maximum possible speed change at the next moment, and accordingly apply a secondary limit to the desired output torque value to prevent speed overshoot. After the above limiting process, the limited control signal is obtained.

[0078] Step S158: Convert the limited control signal into a pulse width modulation signal format recognizable by the driving component, and generate a control signal unit for the current moment. The control signal unit includes the duty cycle parameter and frequency parameter of the pulse width modulation signal.

[0079] The limited control signal obtained in step S157, i.e., the expected output torque value of each joint, is converted into a pulse width modulation (PWM) signal format. Specifically, based on the torque constant of the servo motor of each joint, the expected output torque value is converted into a corresponding expected current value. Then, based on the expected current value and the rated current value of the motor driver, the duty cycle parameter of the PWM signal is calculated. Simultaneously, based on a preset switching frequency, the frequency parameter of the PWM signal is determined. The duty cycle parameter and the frequency parameter are combined into a control signal unit, which is used to drive the servo motor of the corresponding joint.

[0080] Step S159: Repeat the steps of comparing the reference position parameters and reference velocity parameters corresponding to the current sampling time point in the reference trajectory point sequence with the current state vector according to the preset time step, up to the step of converting the limited control signal into a pulse width modulation signal format recognizable by the driving component, to obtain a control signal unit sequence.

[0081] Steps S153 to S158 are repeated within each control cycle, with a preset time step as the period. At the beginning of each control cycle, the current sampling time point is updated, a new current state vector is obtained, and the position tracking error vector, velocity tracking error vector, feedforward control signal component, feedback control signal component, initial control signal, and limited control signal are repeatedly calculated to finally generate the control signal unit corresponding to that control cycle. The control signal units generated in each control cycle are stored in chronological order to obtain the control signal unit sequence.

[0082] Step S1510: Arrange the control signal unit sequence in chronological order to generate the control signal sequence of the driving component, wherein each control signal unit in the control signal sequence corresponds to a sampling time.

[0083] The control signal unit sequence generated in step S159 is arranged according to the generation time order to form an ordered control signal queue, which is the control signal sequence of the drive component. Each control signal unit in this control signal sequence corresponds to a sampling time, and is output to the servo motor driver of each joint in the order of sampling time, driving the robotic arm to perform motion according to the predicted motion trajectory curve.

[0084] The step of constructing the operation flow sequence diagram of the special equipment based on the operation instruction sequence further includes the following steps.

[0085] Based on step S120 and its sub-steps, the influence of motion constraint parameters in the set of operation parameters on the generation of job nodes is further considered to enhance the physical realizability of the job flow sequence diagram.

[0086] Step S211: Obtain the motion speed limit parameter contained in the operation parameter set of each discrete operation instruction unit in the operation instruction sequence. The motion speed limit parameter is used to constrain the maximum allowable speed of the motion trajectory curve segment corresponding to the discrete operation instruction unit.

[0087] For each discrete operation instruction unit in the operation instruction sequence, a motion speed limit parameter is extracted from its operation parameter set. This motion speed limit parameter is an array containing the maximum permissible angular velocity values ​​of the six joints, used to constrain the maximum velocity of each joint in the motion trajectory curve segment corresponding to the discrete operation instruction unit.

[0088] Step S212: Perform speed limiting processing on the standard motion trajectory curve template corresponding to the discrete operation instruction unit according to the motion speed limit parameter to obtain a speed-limited motion trajectory curve template. The speed parameters at each time point in the speed-limited motion trajectory curve template do not exceed the motion speed limit parameter.

[0089] For each discrete operation instruction unit, the velocity curve in its corresponding standard motion trajectory curve template is compared with the motion speed limit parameter obtained in step S211. If the velocity parameter at a certain time point in the standard motion trajectory curve template exceeds the corresponding motion speed limit parameter, the velocity parameter at that time point is limited to within the motion speed limit parameter. To ensure the continuity of the velocity curve, the limited velocity curve is smoothed, for example, by using low-pass filtering or polynomial interpolation, so that the rate of change of velocity remains within a reasonable range. After the above processing, the speed-limited motion trajectory curve template is obtained.

[0090] Step S213: Obtain the motion acceleration limit parameter contained in the operation parameter set of each discrete operation instruction unit in the operation instruction sequence. The motion acceleration limit parameter is used to constrain the maximum allowable acceleration of the motion trajectory curve segment corresponding to the discrete operation instruction unit.

[0091] For each discrete operation instruction unit in the operation instruction sequence, a motion acceleration limit parameter is extracted from its operation parameter set. This motion acceleration limit parameter is an array containing the maximum permissible angular acceleration values ​​of the six joints, used to constrain the maximum acceleration of each joint in the motion trajectory curve segment corresponding to the discrete operation instruction unit.

[0092] Step S214: Perform acceleration limiting processing on the speed-limited motion trajectory curve template according to the motion acceleration limiting parameters to obtain a speed-limited and acceleration-limited motion trajectory curve template. The acceleration parameters at each time point in the speed-limited and acceleration-limited motion trajectory curve template do not exceed the motion acceleration limiting parameters.

[0093] For each discrete operation instruction unit, the acceleration curve in its corresponding speed-limited motion trajectory curve template is compared with the motion acceleration limit parameters obtained in step S213. If the acceleration parameter at a certain time point in the speed-limited motion trajectory curve template exceeds the corresponding motion acceleration limit parameter, the acceleration parameter at that time point is limited to within the motion acceleration limit parameter. To maintain the continuity of the acceleration curve and the smoothness of the velocity curve, the velocity curve needs to be redesigned so that its derivative satisfies the acceleration constraint. This can be achieved by integrating the acceleration-limited curve to reconstruct the velocity curve. After the above processing, the speed-limited and acceleration-limited motion trajectory curve template is obtained.

[0094] Step S215: Use the motion trajectory curve template after speed and acceleration limitation as the updated standard motion trajectory curve template corresponding to the discrete operation instruction unit.

[0095] The motion trajectory curve template with speed and acceleration limited obtained in step S214 is used to replace the original standard motion trajectory curve template of the discrete operation instruction unit, and is used as the updated standard motion trajectory curve template.

[0096] Step S216: Recalculate the standard execution time parameter corresponding to the discrete operation instruction unit according to the updated standard motion trajectory curve template to obtain the updated standard execution time parameter.

[0097] Because the updated standard motion trajectory curve template has undergone velocity and acceleration limiting, its total motion time may change. Therefore, based on the updated standard motion trajectory curve template, the total time required from the starting point to the ending point is recalculated. This total time is the updated standard execution duration parameter.

[0098] Step S217: Use the updated standard motion trajectory curve template and the updated standard execution duration parameter as the job node corresponding to the discrete operation instruction unit. The job node includes the updated standard motion trajectory curve template and the updated standard execution duration parameter.

[0099] The updated standard motion trajectory curve template obtained in step S215 and the updated standard execution duration parameter obtained in step S216 are used as the content of the job node corresponding to the discrete operation instruction unit, and are used to construct the job flow timing diagram in subsequent steps. Through the above processing, the generated job node can strictly meet the motion speed limit parameter and motion acceleration limit parameter defined in the operation parameter set, ensuring the physical realizability of the planned trajectory.

[0100] The step of generating the predicted motion trajectory curve of the special equipment based on the operation process sequence diagram and the real-time operation status data set also includes the following steps.

[0101] Based on step S140 and its sub-steps, the predicted motion trajectory curve is further generated by splicing discrete point sequences.

[0102] Step S311: Extract the current job node from the job flow sequence diagram as the current job node, and obtain the start execution time point, end execution time point, and motion trajectory curve segment corresponding to the current job node.

[0103] Obtain the current time value and compare it with the start and end execution times of each job node in the job flow sequence diagram. Determine which job node the current time value falls within the range of the start and end execution times, and designate that job node as the current job node. Extract the start and end execution times of the current job node, as well as the corresponding motion trajectory curve segment, from the job flow sequence diagram.

[0104] Step S312: Discretize the motion trajectory curve segment of the current working node according to a preset curve discretization step size to obtain the position discrete point sequence and velocity discrete point sequence corresponding to the current working node. The position discrete point sequence contains the position parameters of each discrete point on the motion trajectory curve segment corresponding to the current working node, and the velocity discrete point sequence contains the velocity parameters of each discrete point on the motion trajectory curve segment corresponding to the current working node.

[0105] Using a preset curve discretization step size as the time interval, the motion trajectory curve segment corresponding to the current task node is discretized and sampled. Starting from the starting time point of the motion trajectory curve segment, a discrete point is sampled every time interval of one curve discretization step size. For each discrete point, the position and velocity parameters at the corresponding time point are extracted from the motion trajectory curve segment. The position parameters of all extracted discrete points are arranged in the order of sampling time to form a position discrete point sequence. The velocity parameters of all extracted discrete points are arranged in the order of sampling time to form a velocity discrete point sequence. The position discrete point sequence and the velocity discrete point sequence have the same time index and together describe the state of the motion trajectory curve segment corresponding to the current task node at discrete time points.

[0106] Step S313: Obtain the current position parameter in the real-time running status data set, and perform matching processing on the current position parameter with each position parameter in the position discrete point sequence to obtain the matching position index parameter of the current position parameter in the position discrete point sequence. The matching position index parameter is used to indicate the discrete point position of the current position parameter on the motion trajectory curve segment corresponding to the current work node.

[0107] The current position parameter is extracted from the real-time running status data set obtained in step S130. This current position parameter is then matched point-by-point with each position parameter in the discrete point sequence generated in step S312. The matching process uses a nearest neighbor matching algorithm to calculate the Euclidean distance between the current position parameter and each position parameter in the discrete point sequence, finding the discrete point with the smallest distance. The index of this discrete point in the discrete point sequence is used as the matching position index parameter. This matching position index parameter indicates the discrete point position of the current position parameter on the motion trajectory curve segment corresponding to the current work node.

[0108] Step S314: Extract all remaining discrete points after the matching position index parameter from the discrete point sequence according to the matching position index parameter, and obtain the set of remaining discrete points.

[0109] Based on the matching position index parameter obtained in step S313, determine the position of the matching position index parameter in the discrete point sequence. Extract all discrete points following the matching position index parameter from the discrete point sequence, i.e., all discrete points with indices greater than the matching position index parameter. Combine the extracted discrete points into a set of remaining discrete points.

[0110] Step S315: Extract all remaining discrete velocity points after the matching position index parameter from the velocity discrete point sequence according to the matching position index parameter, and obtain the set of remaining discrete velocity points.

[0111] Based on the matching position index parameter obtained in step S313, determine the position of the matching position index parameter in the velocity discrete point sequence. From the velocity discrete point sequence, extract all velocity discrete points after the matching position index parameter, that is, all velocity discrete points with indices greater than the matching position index parameter. Combine the extracted velocity discrete points to form the remaining velocity discrete point set.

[0112] Step S316: Arrange the remaining discrete points in the set of remaining discrete points in the original order to generate the sequence of remaining discrete points corresponding to the current working node.

[0113] Arrange the remaining discrete points in the set of remaining discrete points obtained in step S314 according to their order in the original discrete point sequence to form an ordered sequence, and use this ordered sequence as the remaining discrete point sequence corresponding to the current job node.

[0114] Step S317: Arrange the remaining velocity discrete points in the set of remaining velocity discrete points in the original order to generate the sequence of remaining velocity discrete points corresponding to the current working node.

[0115] Arrange the remaining velocity discrete points in the set of remaining velocity discrete points obtained in step S315 according to their order in the original velocity discrete point sequence to form an ordered sequence, and use this ordered sequence as the remaining velocity discrete point sequence corresponding to the current work node.

[0116] Step S318: Obtain all subsequent job nodes located after the current job node in the job flow time sequence diagram, extract the position discrete point sequence and velocity discrete point sequence corresponding to each subsequent job node, and obtain the set of subsequent position discrete point sequences and the set of subsequent velocity discrete point sequences.

[0117] Extract all work nodes following the current work node from the work flow sequence diagram, and designate these work nodes as subsequent work nodes. For each subsequent work node, following the method in step S312, discretize the motion trajectory curve segment corresponding to the subsequent work node according to a preset curve discretization step size to obtain the position discrete point sequence and velocity discrete point sequence corresponding to the subsequent work node. Collect the position discrete point sequences corresponding to all subsequent work nodes to form a set of subsequent position discrete point sequences. Collect the velocity discrete point sequences corresponding to all subsequent work nodes to form a set of subsequent velocity discrete point sequences.

[0118] Step S319: The remaining position discrete point sequence is spliced ​​with each subsequent position discrete point sequence in the subsequent position discrete point sequence set according to the order of the operation flow time sequence diagram to generate the predicted position discrete point sequence.

[0119] The remaining discrete point sequence generated in step S316 is used as the base sequence. Following the order of subsequent job nodes in the job flow time sequence diagram, each subsequent discrete point sequence in the subsequent position discrete point sequence set is sequentially concatenated to the end of the remaining discrete point sequence. During concatenation, it is ensured that adjacent sequences are temporally continuous, meaning the last discrete point of the previous sequence is temporally adjacent to the first discrete point of the next sequence. After this concatenation process, a complete predicted position discrete point sequence is generated.

[0120] Step S3110: The remaining velocity discrete point sequence and each subsequent velocity discrete point sequence in the subsequent velocity discrete point sequence set are spliced ​​together according to the order of the operation flow time sequence diagram to generate the predicted velocity discrete point sequence.

[0121] The remaining velocity discrete point sequence generated in step S317 is used as the base sequence. Following the order of subsequent task nodes in the task flow time sequence diagram, each subsequent velocity discrete point sequence in the subsequent velocity discrete point sequence set is sequentially concatenated to the end of the remaining velocity discrete point sequence. During concatenation, it is ensured that adjacent sequences are temporally continuous, meaning the last discrete point of the previous sequence is temporally adjacent to the first discrete point of the next sequence. After this concatenation process, a complete predicted velocity discrete point sequence is generated.

[0122] Step S3111: Combine the predicted position discrete point sequence and the predicted velocity discrete point sequence in chronological order to generate the predicted motion trajectory curve of the special equipment. The predicted motion trajectory curve includes the expected position parameters of each discrete point in the predicted position discrete point sequence and the expected velocity parameters of each discrete point in the predicted velocity discrete point sequence.

[0123] The predicted position discrete point sequence generated in step S319 is aligned with the predicted velocity discrete point sequence generated in step S3110 using the same time index. For each time index, the position parameter in the predicted position discrete point sequence corresponding to that time index is used as the expected position parameter for that time point, and the velocity parameter in the predicted velocity discrete point sequence corresponding to that time index is used as the expected velocity parameter for that time point. The expected position parameters and expected velocity parameters are organized into a new time series data in chronological order, which is the predicted motion trajectory curve. Each time point in the predicted motion trajectory curve corresponds to one expected position parameter and one expected velocity parameter.

[0124] The step of calling the trajectory tracking controller of the general servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set to generate the control signal sequence of the drive component also includes the following steps.

[0125] Based on step S150 and its sub-steps, further techniques such as look-ahead planning, state observation, torque feedforward, and damping compensation are introduced to improve the accuracy and dynamic response performance of trajectory tracking control.

[0126] Step S411: Input the predicted motion trajectory curve into the look-ahead planning module of the trajectory tracking controller, calculate the acceleration parameter sequence required by the drive component at each time point in the future time window based on the expected velocity parameters at each time point in the predicted motion trajectory curve, and obtain the expected acceleration parameter sequence.

[0127] The predicted motion trajectory curve generated in step S1410 or step S3111, or by other means, is input into the look-ahead planning module. This look-ahead planning module performs time-difference processing on the expected velocity parameters at each time point in the predicted motion trajectory curve. Specifically, for each time point in the predicted motion trajectory curve, the difference between the expected velocity parameter at that time point and the expected velocity parameter at the previous time point is calculated. This difference is then divided by a preset time step to obtain the acceleration parameter at that time point. All time points in the predicted motion trajectory curve are traversed in chronological order, and the acceleration parameter at each time point is calculated. These acceleration parameters are then arranged in chronological order to form a sequence of expected acceleration parameters.

[0128] Step S412: Input the current speed parameter from the real-time operating status data set into the status observation module of the trajectory tracking controller, and calculate the momentum parameter of the driving component at the current moment based on the current speed parameter and the inertial parameter of the driving component to obtain the current momentum parameter.

[0129] Extract the current velocity parameter from the real-time operating status data set obtained in step S130. Obtain the pre-stored inertia parameters of the drive components, which are an array containing the rotational inertia values ​​of the six joints. For each joint, multiply the current velocity parameter of that joint by its inertia parameter to obtain the momentum parameter of that joint at the current moment. Combine the momentum parameters of all joints to form the current momentum parameter, which is an array containing the momentum values ​​of the six joints.

[0130] Step S413: Input the expected acceleration parameter sequence into the torque feedforward module of the trajectory tracking controller, calculate the required inertial torque parameter sequence of the drive component at each time point in the future time window based on the expected acceleration parameter sequence and the inertial parameters of the drive component, and obtain the feedforward torque parameter sequence.

[0131] The expected acceleration parameter sequence generated in step S411 is input into the torque feedforward module. Pre-stored inertial parameters of the driving component are obtained. For each time point in the expected acceleration parameter sequence, the expected acceleration parameter at that time point is multiplied element-wise with the inertial parameter of the driving component to obtain the required inertial torque parameter at that time point. All time points in the expected acceleration parameter sequence are traversed in chronological order, and the inertial torque parameter at each time point is calculated. These inertial torque parameters are then arranged in chronological order to form the feedforward torque parameter sequence.

[0132] Step S414: Input the current momentum parameter into the damping compensation module of the trajectory tracking controller, calculate the damping compensation torque parameter required by the drive component at the current moment based on the current momentum parameter and the damping coefficient of the drive component, and obtain the damping compensation torque parameter.

[0133] The current momentum parameter obtained in step S412 is input into the damping compensation module. The pre-stored damping coefficients of the drive components are retrieved; these coefficients are an array containing the damping coefficients of the six joints. For each joint, the current momentum parameter of that joint is multiplied by its damping coefficient to obtain the damping compensation torque parameter required for that joint at the current moment. The damping compensation torque parameters of each joint are combined to form a damping compensation torque parameter, which is an array containing the damping compensation torque values ​​of the six joints.

[0134] Step S415: Compare the expected position parameter corresponding to the current moment in the predicted motion trajectory curve with the current position parameter in the real-time running status data set to obtain the position deviation parameter.

[0135] Extract the expected position parameters corresponding to the current moment from the predicted motion trajectory curve generated in step S1410, step S3111, or other methods. Extract the current position parameters from the real-time running status data set obtained in step S130. Subtract the expected position parameters from the current position parameters joint by joint to obtain the position deviation parameters, which are arrays containing the position deviation values ​​of the six joints.

[0136] Step S416: Compare the expected velocity parameter corresponding to the current moment in the predicted motion trajectory curve with the current velocity parameter in the real-time running status data set to obtain the velocity deviation parameter.

[0137] Extract the expected velocity parameters corresponding to the current moment from the predicted motion trajectory curve generated in step S1410, step S3111, or other methods. Extract the current velocity parameters from the real-time running status data set obtained in step S130. Subtract the expected velocity parameters from the current velocity parameters joint by joint to obtain the velocity deviation parameters, which are arrays containing the velocity deviation values ​​of the six joints.

[0138] Step S417: Calculate the position loop control output parameters based on the position deviation parameters and the preset position loop proportional coefficient. The position loop control output parameters are physical quantities of torque, thus obtaining the position loop control quantity.

[0139] Obtain the preset position loop proportional coefficient, which is an array containing proportional coefficients for six joints. For each joint, multiply the joint's position deviation parameter by the position loop proportional coefficient to obtain the joint's position loop control output parameter. Since the dimension of the position loop proportional coefficient is the ratio of torque to position, the dimension of this position loop control output parameter is torque. Combine the position loop control output parameters of each joint to form the position loop control quantity, which is an array containing the torque values ​​of the six joints.

[0140] Step S418: The speed deviation parameter is accumulated and summed according to the speed deviation parameter and the preset speed loop integral coefficient to obtain the speed loop integral control quantity, which is a torque physical quantity.

[0141] Obtain the preset speed loop integral coefficients, which are an array containing the integral coefficients of the six joints. Accumulate and sum the speed deviation parameters of each joint over time. Specifically, in each control cycle, multiply the current speed deviation parameter of the joint by the speed loop integral coefficient of that joint, and then multiply by the preset control cycle duration to obtain the speed loop integral increment of that joint in that cycle. Add this speed loop integral increment to the accumulated speed loop integral value saved in the previous cycle to obtain the speed loop integral control quantity for the current cycle. The dimension of this speed loop integral control quantity is torque. Combine the speed loop integral control quantities of each joint to form the speed loop integral control quantity, which is an array containing the torque values ​​of the six joints.

[0142] Step S419: The position loop control quantity and the speed loop integral control quantity are fused to obtain a first fused control quantity; the first fused control quantity, the feedforward torque parameter corresponding to the current moment in the feedforward torque parameter sequence, and the damping compensation torque parameter are superimposed to generate the control signal at the current moment, the control signal containing the electromagnetic torque command parameters that the drive component needs to output.

[0143] The position loop control quantity obtained in step S417 and the speed loop integral control quantity obtained in step S418 are added joint by joint to obtain the first fused control quantity. The feedforward torque parameter corresponding to the current moment is extracted from the feedforward torque parameter sequence generated in step S413. The first fused control quantity, the feedforward torque parameter corresponding to the current moment, and the damping compensation torque parameter obtained in step S414 are added joint by joint to obtain the control signal for the current moment. This control signal is an array containing electromagnetic torque command values ​​for six joints, where each electromagnetic torque command value corresponds to the desired electromagnetic torque that the servo motor of one joint needs to output.

[0144] Step S4110: The control signal is repeatedly generated according to a preset control cycle to obtain a control signal sequence, wherein each control signal in the control signal sequence corresponds to a control cycle.

[0145] Steps S415 to S419 are repeated within each control cycle, using a preset control period as the time interval, to generate the control signal corresponding to that control cycle. The control signals generated in each control cycle are stored in chronological order to obtain a control signal sequence. Each control signal in this sequence corresponds to one control cycle and is sequentially output to the servo motor driver of each joint.

[0146] The step of generating the predicted motion trajectory curve of the special equipment based on the operation process sequence diagram and the real-time operation status data set also includes the following steps.

[0147] Based on step S140 and its sub-steps, a trajectory correction function and a curve parameterization method are further introduced to generate a smoother predicted motion trajectory curve.

[0148] Step S511: Extract the current job node from the job flow sequence diagram as the current job node, and obtain the start execution time point, end execution time point, and motion trajectory curve segment corresponding to the current job node.

[0149] Obtain the current time value and compare it with the start and end execution times of each job node in the job flow sequence diagram. Determine which job node the current time value falls within the range of the start and end execution times, and designate that job node as the current job node. Extract the start and end execution times of the current job node, as well as the corresponding motion trajectory curve segment, from the job flow sequence diagram.

[0150] Step S512: Calculate the position deviation on the motion trajectory curve segment at the current moment based on the motion trajectory curve segment of the current working node and the current position parameter in the real-time running status data set, and obtain the trajectory deviation parameter of the current position of the driving component relative to the motion trajectory curve segment.

[0151] Substituting the current time value into the motion trajectory curve segment of the current work node, the expected position parameters of each joint of the robotic arm at the current time are calculated. These expected position parameters are an array containing the expected angle values ​​of the six joints. The current position parameters obtained in step S130 are subtracted from the above expected position parameters joint by joint to obtain the position deviation of each joint. This position deviation is an array containing the position deviation values ​​of the six joints, i.e., the trajectory deviation parameter.

[0152] Step S513: Obtain all subsequent job nodes located after the current job node in the job flow sequence diagram, extract the start execution time point, end execution time point, and motion trajectory curve segment corresponding to each subsequent job node, and obtain the parameter set of the subsequent job nodes.

[0153] Extract all job nodes following the current job node from the job flow sequence diagram, and designate these as subsequent job nodes. For each subsequent job node, obtain its start execution time, end execution time, and the corresponding motion trajectory curve segment. Organize this information according to the order of the job nodes to form a parameter set for subsequent job nodes.

[0154] Step S514: Input the trajectory deviation parameter into a preset trajectory correction function for calculation to obtain the trajectory correction coefficient. The trajectory correction coefficient is used to adjust the shape of the motion trajectory curve segment corresponding to the current operation node.

[0155] The trajectory deviation parameters obtained in step S512 are input into a preset trajectory correction function for calculation. This trajectory correction function is a nonlinear mapping function based on the deviation amount, and its output is the trajectory correction coefficient. The trajectory correction coefficient is an array containing correction coefficients for six joints, with each correction coefficient corresponding to one joint. The specific form of the trajectory correction function is as follows: First, the ratio between the position deviation of the robotic arm at the current moment and the preset allowable deviation threshold is calculated. Then, the trajectory correction coefficient is determined based on this ratio using an S-shaped function or a piecewise linear function. When the position deviation is small, the trajectory correction coefficient is close to zero, indicating that no significant adjustment to the trajectory is needed; when the position deviation is large, the trajectory correction coefficient approaches one, indicating that significant adjustment to the trajectory is needed to eliminate the deviation.

[0156] Step S515: Perform morphological correction processing on the motion trajectory curve segment corresponding to the current work node according to the trajectory correction coefficient to obtain the corrected motion trajectory curve segment of the current work node.

[0157] Based on the trajectory correction coefficient obtained in step S514, the motion trajectory curve segment corresponding to the current work node is morphologically corrected. The correction method is as follows: for each time point in the motion trajectory curve segment corresponding to the current work node, the expected position parameter at that time point is obtained. The deviation between the expected position parameter and the actual position parameter at the current time is calculated, and this deviation is multiplied by the trajectory correction coefficient to obtain the correction amount. This correction amount is then added to the expected position parameter at that time point to obtain the corrected position parameter. Simultaneously, to ensure the continuity of velocity, the corrected position parameter is differentially analyzed to obtain the corrected velocity parameter. After the above point-by-point correction process, the corrected motion trajectory curve segment of the current work node is obtained.

[0158] Step S516: The corrected current work node motion trajectory curve segment and the motion trajectory curve segments corresponding to each subsequent work node in the parameter set of the subsequent work nodes are spliced ​​together in the order of the work process sequence diagram to generate a corrected complete work trajectory curve segment set.

[0159] The corrected motion trajectory curve segment of the current task node generated in step S515 is used as the base segment. Following the order of subsequent task nodes in the task flow sequence diagram, the motion trajectory curve segments corresponding to each subsequent task node in the parameter set of the subsequent task nodes are sequentially spliced ​​to the end of the corrected motion trajectory curve segment of the current task node. During splicing, it is ensured that the position and velocity of adjacent curve segments are continuous at the connection point. After the above splicing process, a complete set of corrected task trajectory curve segments is generated.

[0160] Step S517: Extract all trajectory curve segments from the current moment to the end of the last subsequent task node from the corrected complete task trajectory curve segment set to obtain the time window trajectory curve segment set.

[0161] From the corrected complete job trajectory curve segment set generated in step S516, determine the time point corresponding to the current moment. Extract all trajectory curve segments within the time range from the current moment until the end of the execution time of the last subsequent job node. Combine the extracted trajectory curve segments into a time window trajectory curve segment set.

[0162] Step S518: The time window trajectory curve segment set is parameterized according to a preset curve parameterization method to obtain a position parameterization function and a velocity parameterization function with time as the independent variable.

[0163] The curves in the time window trajectory curve set obtained in step S517 are merged to form a unified continuous curve. This unified curve is then parameterized and expressed as a function with time as the independent variable. Specifically, a piecewise cubic polynomial function is constructed using cubic spline interpolation, with each discrete time point as a node, such that the function satisfies the conditions of position continuity, velocity continuity, and acceleration continuity at each node. This function is the position parameterization function. The velocity parameterization function is obtained by differentiating this position parameterization function.

[0164] Step S519: Calculate the expected position parameters for each time point within the future time window based on the position parameterization function to obtain the expected position parameter sequence.

[0165] According to the preset sampling time interval, iterate through each time point within the future time window. For each time point, substitute that time point into the position parameterization function obtained in step S518 to calculate the expected position parameters for that time point. Arrange all the calculated expected position parameters in chronological order to form an expected position parameter sequence.

[0166] Step S5110: Calculate the expected velocity parameters at each time point within the future time window according to the velocity parameterization function to obtain the expected velocity parameter sequence.

[0167] According to the preset sampling time interval, iterate through each time point within the future time window. For each time point, substitute that time point into the velocity parameterization function obtained in step S518 to calculate the expected velocity parameter for that time point. Arrange all the calculated expected velocity parameters in chronological order to form an expected velocity parameter sequence.

[0168] Step S5111: Combine the expected position parameter sequence and the expected velocity parameter sequence in chronological order to generate the predicted motion trajectory curve of the special equipment.

[0169] The expected position parameter sequence generated in step S519 and the expected velocity parameter sequence generated in step S5110 are aligned using the same time index. For each time index, the position parameter in the expected position parameter sequence corresponding to that time index is taken as the expected position parameter at that time point, and the velocity parameter in the expected velocity parameter sequence corresponding to that time index is taken as the expected velocity parameter at that time point. The above expected position parameters and expected velocity parameters are organized into a new time series data in chronological order, and this time series data is the predicted motion trajectory curve.

[0170] The step of calling the trajectory tracking controller of the general servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set to generate the control signal sequence of the drive component also includes the following steps.

[0171] Based on step S150 and its sub-steps, a trajectory smoothing module, a three-loop control structure, and a pulse width modulation signal generation are further introduced to form a complete servo drive control closed loop.

[0172] Step S611: Input the predicted motion trajectory curve into the trajectory smoothing module of the trajectory tracking controller, perform differential processing on the expected velocity parameters between adjacent time points in the predicted motion trajectory curve, and obtain a sequence of velocity change rate parameters.

[0173] The predicted motion trajectory curve generated in step S1410 or step S3111, or by other means, is input into the trajectory smoothing module. This module iterates through each time point in the predicted motion trajectory curve. For each pair of adjacent time points, it calculates the difference between the expected velocity parameter at the later time point and the expected velocity parameter at the previous time point. This difference is then divided by the time interval between the two time points to obtain the velocity change rate parameter for that time interval. The velocity change rate parameters between all adjacent time points are calculated in chronological order, and these velocity change rate parameters are arranged in chronological order to form a velocity change rate parameter sequence.

[0174] Step S612: When there is a rate of change parameter in the velocity change rate parameter sequence that exceeds the preset acceleration limit threshold, the expected velocity parameters near the corresponding time point in the predicted motion trajectory curve are re-planned to obtain a smoothed predicted motion trajectory curve.

[0175] A preset acceleration limit threshold is obtained, which is an array containing the maximum allowable acceleration values ​​for the six joints. Each velocity change rate parameter in the velocity change rate parameter sequence generated in step S611 is compared with the aforementioned acceleration limit threshold. If the absolute value of a velocity change rate parameter exceeds the acceleration limit threshold of the corresponding joint, it is determined that the predicted motion trajectory curve segment corresponding to that velocity change rate parameter has an acceleration exceeding the limit. For the aforementioned acceleration exceeding the limit segment, the expected velocity parameters near that segment are replanned. The replanning process uses cubic spline interpolation or polynomial fitting methods to smooth the velocity curve while maintaining position and velocity continuity, so that the replanned velocity change rate parameter does not exceed the acceleration limit threshold. After the above replanning process, a smoothed predicted motion trajectory curve is obtained.

[0176] Step S613: Input the smoothed predicted motion trajectory curve into the reference trajectory generation module of the trajectory tracking controller, and sample the smoothed predicted motion trajectory curve according to the preset control cycle to obtain a reference trajectory point sequence. The reference trajectory point sequence includes reference position parameters and reference velocity parameters for each control cycle.

[0177] The smoothed predicted motion trajectory curve obtained in step S612 is input into the reference trajectory generation module. The smoothed predicted motion trajectory curve is discretized and sampled at a preset control cycle as the sampling time interval. At each sampling moment of the control cycle, the position parameter and velocity parameter corresponding to that sampling moment are extracted from the smoothed predicted motion trajectory curve. The position parameter is used as the reference position parameter for that control cycle, and the velocity parameter is used as the reference velocity parameter for that control cycle. The reference position parameter and reference velocity parameter are arranged according to the control cycle order to form a reference trajectory point sequence.

[0178] Step S614: Input the current position parameter and current speed parameter in the real-time running status data set into the error calculation module of the trajectory tracking controller, calculate the difference between the reference position parameter and the current position parameter as the position error parameter, and calculate the difference between the reference speed parameter and the current speed parameter as the speed error parameter.

[0179] Extract the current position parameter and current velocity parameter from the real-time operating status data set obtained in step S130. Obtain the reference position parameter and reference velocity parameter corresponding to the current control cycle from the reference trajectory point sequence generated in step S613. Subtract the current position parameter from the reference position parameter joint by joint to obtain the position error parameter. Subtract the current velocity parameter from the reference velocity parameter joint by joint to obtain the velocity error parameter.

[0180] Step S615: Input the position error parameter into the position loop controller of the trajectory tracking controller, calculate the velocity loop setpoint parameter based on the position error parameter and the preset position loop control parameter, and obtain the velocity loop setpoint value.

[0181] The position error parameter obtained in step S614 is input into the position loop controller. Preset position loop control parameters, including a position loop proportional gain parameter, are obtained. For each joint, the position error parameter of that joint is multiplied by the position loop proportional gain parameter to obtain the velocity loop setpoint parameter for that joint. The velocity loop setpoint parameters of all joints are combined to form a velocity loop setpoint value, which is an array containing the desired velocity values ​​for the six joints.

[0182] Step S616: Input the speed error parameter and the speed loop setpoint into the speed loop controller of the trajectory tracking controller, calculate the current loop setpoint parameter based on the difference between the speed error parameter and the speed loop setpoint and the preset speed loop control parameters, and obtain the current loop setpoint value.

[0183] The speed error parameter obtained in step S614 and the speed loop setpoint obtained in step S615 are input into the speed loop controller. First, the difference between the speed loop setpoint and the speed error parameter is calculated to obtain the speed loop deviation parameter. Preset speed loop control parameters are obtained, including a speed loop proportional gain parameter and a speed loop integral gain parameter. For each joint, the speed loop deviation parameter of that joint is multiplied by the speed loop proportional gain parameter of that joint to obtain the speed loop proportional control component. Simultaneously, the speed loop deviation parameter of that joint is integrated and accumulated, and the accumulated value is multiplied by the speed loop integral gain parameter of that joint to obtain the speed loop integral control component. The speed loop proportional control component and the speed loop integral control component are added to obtain the current loop setpoint parameter of that joint. The current loop setpoint parameters of all joints are combined to form the current loop setpoint value, which is an array containing the desired current values ​​for the six joints.

[0184] Step S617: Input the current loop setpoint into the current loop controller of the trajectory tracking controller, calculate the voltage control parameters based on the current loop setpoint and the instantaneous phase current value parameters in the real-time operating status data set, and obtain the voltage control quantity.

[0185] The current loop setpoint obtained in step S616 is input to the current loop controller. Instantaneous phase current parameters are extracted from the real-time operating status data set obtained in step S130. These instantaneous phase current parameters are an array containing the phase current values ​​of the six joint motors. For each joint, the difference between the current loop setpoint and the instantaneous phase current parameter is calculated to obtain the current loop deviation parameter. Preset current loop control parameters are obtained, including a current loop proportional gain parameter and a current loop integral gain parameter. The current loop deviation parameter of the joint is multiplied by the current loop proportional gain parameter to obtain the current loop proportional control component. Simultaneously, the current loop deviation parameter of the joint is integrated and accumulated, and the accumulated value is multiplied by the current loop integral gain parameter to obtain the current loop integral control component. The current loop proportional control component and the current loop integral control component are added together, and after back EMF compensation and voltage decoupling processing, the voltage control parameter of the joint is obtained. The voltage control parameters of each joint are combined to form a voltage control quantity, which is an array containing the desired voltage values ​​of the six joints.

[0186] Step S618: Input the voltage control quantity into the pulse width modulation generation module of the trajectory tracking controller, generate a pulse width modulation signal with a corresponding duty cycle according to the voltage control quantity, and obtain a control signal unit.

[0187] The voltage control quantity obtained in step S617 is input to the pulse width modulation (PWM) generation module. This module calculates the duty cycle parameter of the PWM signal corresponding to each joint based on the ratio of the voltage control quantity to the bus voltage. Simultaneously, it determines the frequency parameter of the PWM signal according to a preset switching frequency. The duty cycle parameter and frequency parameter are combined into a control signal unit, which drives the servo motor of the corresponding joint.

[0188] Step S619: Repeat the steps of inputting the smoothed predicted motion trajectory curve into the reference trajectory generation module of the trajectory tracking controller according to the preset control cycle to the step of the pulse width modulation generation module generating a pulse width modulation signal with a corresponding duty cycle according to the voltage control quantity, to obtain a control signal unit sequence.

[0189] Using a preset control cycle as the loop period, steps S613 to S618 are repeated within each control cycle. At the beginning of each control cycle, the time index of the current control cycle is updated, new reference trajectory points are sampled from the smoothed predicted motion trajectory curve, a new real-time operating status data set is obtained, and the position error parameters, velocity error parameters, velocity loop setpoints, current loop setpoints, and voltage control quantities are recalculated to finally generate the control signal unit corresponding to that control cycle. The control signal units generated in each control cycle are stored in chronological order to obtain the control signal unit sequence.

[0190] Step S6110: Arrange the control signal unit sequence in chronological order to generate the control signal sequence of the drive component.

[0191] The control signal unit sequence generated in step S619 is arranged according to the generation time order to form an ordered control signal queue, which is the control signal sequence of the drive component. Each control signal unit in this control signal sequence corresponds to one control cycle and is output sequentially to the servo motor driver of each joint.

[0192] The step of generating the predicted motion trajectory curve of the special equipment based on the operation process sequence diagram and the real-time operation status data set also includes the following steps.

[0193] Based on step S140 and its sub-steps, load inertia identification and sliding mode controller are further introduced to enhance the adaptability and robustness of the predicted motion trajectory curve under variable load conditions.

[0194] For example, step S711: obtain the current load parameters in the real-time operating status data set, input the current load parameters into the preset load inertia identification model for calculation, and obtain the actual rotational inertia parameters of the drive component under the current load conditions.

[0195] The current load parameters are extracted from the real-time operating status data set obtained in step S130. These current load parameters are then input into a preset load inertia identification model for calculation. This load inertia identification model is constructed based on the dynamic equations of the robotic arm and estimates the actual rotational inertia parameters of each joint online using the recursive least squares method. Specifically, the model uses joint acceleration and joint torque as inputs, constructs a regression matrix and parameter vector, and continuously updates the estimated values ​​of the rotational inertia parameters using the recursive least squares algorithm. The actual rotational inertia parameters are an array containing the rotational inertia values ​​of the six joints, reflecting the inertial characteristics of each joint under the current load conditions.

[0196] Step S712: Extract the current job node from the job flow sequence diagram as the current job node, and obtain the start execution time point, end execution time point, and motion trajectory curve segment corresponding to the current job node.

[0197] Obtain the current time value and compare it with the start and end execution times of each job node in the job flow sequence diagram. Determine which job node the current time value falls within the range of the start and end execution times, and designate that job node as the current job node. Extract the start and end execution times of the current job node, as well as the corresponding motion trajectory curve segment, from the job flow sequence diagram.

[0198] Step S713: Perform dynamic response compensation processing on the motion trajectory curve segment corresponding to the current working node according to the actual moment of inertia parameter to obtain the compensated motion trajectory curve segment of the current working node. The acceleration parameter at each time point in the compensated motion trajectory curve segment of the current working node is negatively correlated with the actual moment of inertia parameter.

[0199] Based on the actual moment of inertia parameters obtained in step S711, dynamic response compensation processing is performed on the motion trajectory curve segment corresponding to the current working node. Specifically, for each time point in the motion trajectory curve segment, the desired acceleration parameter at that time point is obtained. Since changes in the actual moment of inertia parameters affect the acceleration response capability of the joint, it is necessary to compensate for the desired acceleration parameter. The compensation method is as follows: calculate the ratio of the rated moment of inertia parameter to the actual moment of inertia parameter, and use this ratio as the acceleration compensation coefficient. Multiply the above desired acceleration parameter by the acceleration compensation coefficient to obtain the compensated desired acceleration parameter. Since the larger the actual moment of inertia parameter is compared with the rated moment of inertia parameter, the smaller the acceleration compensation coefficient is, and the smaller the compensated desired acceleration parameter is. Therefore, the acceleration parameter at each time point in the compensated motion trajectory curve segment is negatively correlated with the actual moment of inertia parameter. Then, the compensated desired acceleration parameter is integrated to obtain the compensated desired velocity parameter; the compensated desired velocity parameter is integrated to obtain the compensated desired position parameter. The compensated expected position parameters and compensated expected velocity parameters are combined in chronological order to obtain the compensated motion trajectory curve segment of the current working node.

[0200] Step S714: Obtain all subsequent work nodes located after the current work node in the work process sequence diagram, and splice the motion trajectory curve segments of the subsequent work nodes according to the order of the work process sequence diagram to generate a set of subsequent work trajectory curve segments.

[0201] Extract all work nodes following the current work node from the work flow sequence diagram, and designate these work nodes as subsequent work nodes. Following the order of these subsequent work nodes in the work flow sequence diagram, sequentially obtain the motion trajectory curve segment corresponding to each subsequent work node. Connect these motion trajectory curve segments end-to-end in chronological order to form a continuous set of trajectory curve segments, i.e., the set of subsequent work trajectory curve segments.

[0202] Step S715: Perform time-series connection processing on the compensated current operation node motion trajectory curve segment and the set of subsequent operation trajectory curve segments to generate the initial predicted motion trajectory curve of the special equipment in the future time window.

[0203] The compensated current work node trajectory curve segment generated in step S713 is temporally connected with the set of subsequent work trajectory curve segments generated in step S714. Specifically, the end point of the compensated current work node trajectory curve segment is used as the starting point of the first trajectory curve segment in the set of subsequent work trajectory curve segments, and their time axes are connected so that the time end point of the compensated current work node trajectory curve segment is aligned with the time start point of the first trajectory curve segment in the set of subsequent work trajectory curve segments. After the above temporal connection process, a continuous trajectory curve is obtained from the current time to the end of the last subsequent work node, and this continuous trajectory curve is used as the initial predicted motion trajectory curve.

[0204] Step S716: Extract the current speed parameter from the real-time running status data set, compare the current speed parameter with the expected speed parameter corresponding to the current moment in the initial predicted motion trajectory curve, and obtain the speed deviation parameter of the current speed of the driving component relative to the initial predicted motion trajectory curve.

[0205] The current speed parameter is extracted from the real-time operating status data set obtained in step S130. This current speed parameter is an array containing the angular velocity values ​​of the six joints. The current value is substituted into the initial predicted motion trajectory curve to calculate the expected speed parameter for each joint of the robotic arm at the current moment. This expected speed parameter is an array containing the expected angular velocity values ​​of the six joints. The expected speed parameter is subtracted from the current speed parameter joint by joint to obtain the speed deviation for each joint. This speed deviation is an array containing the speed deviation values ​​of the six joints, i.e., the speed deviation parameter.

[0206] Step S717: Input the speed deviation parameter into the preset sliding mode controller for calculation to obtain the sliding mode control correction amount, which is used to eliminate the tracking error caused by the speed deviation parameter.

[0207] The velocity deviation parameters obtained in step S716 are input into a preset sliding mode controller for calculation. This sliding mode controller employs a sliding mode variable structure control algorithm, the core of which is to design a sliding surface function and force the system state to move along this sliding surface. Specifically, firstly, a sliding surface function containing the velocity deviation parameters is constructed. Then, based on the sliding mode arrival condition, an equivalent control quantity is calculated, which is used to maintain the system state's movement on the sliding surface. Simultaneously, a switching control quantity is calculated, which is used to overcome system uncertainties and external disturbances. The equivalent control quantity and the switching control quantity are added together to obtain the sliding mode control correction quantity. This sliding mode control correction quantity is an array containing six joint torque correction values, used to eliminate tracking errors caused by the velocity deviation parameters.

[0208] Step S718: Correct the expected velocity parameters at each time point after the current moment in the initial predicted motion trajectory curve according to the sliding mode control correction amount to obtain the expected velocity parameter sequence after sliding mode correction.

[0209] For each time point after the current moment in the initial predicted motion trajectory curve, obtain the expected velocity parameter corresponding to that time point. Map the sliding mode control correction obtained in step S717 to the velocity domain to obtain the velocity correction amount. This mapping process is based on the robotic arm's Jacobian matrix and dynamic model. Superimpose the above velocity correction amount onto the expected velocity parameter at that time point to obtain the sliding mode corrected expected velocity parameter. By traversing all time points after the current moment in the initial predicted motion trajectory curve point by point, obtain the sequence of sliding mode corrected expected velocity parameters.

[0210] Step S719: Based on the expected velocity parameter sequence after the sliding mode correction, perform integral reconstruction processing on the expected position parameters of each time point after the current moment in the initial predicted motion trajectory curve to obtain the expected position parameter sequence after the sliding mode correction.

[0211] For each time point after the current moment in the initial predicted motion trajectory curve, the current position parameter at the current moment is used as the starting point for integration. Numerical integration is performed on the sliding mode corrected expected velocity parameter sequence obtained in step S718 to reconstruct the sequence. Specifically, for the first time point, i.e., the first sampling time point after the current moment, the sliding mode corrected expected position parameter at this time point is equal to the current position parameter plus the product of the sliding mode corrected expected velocity parameter at the current moment and the time interval. For the subsequent nth time point, the sliding mode corrected expected position parameter at this time point is equal to the sliding mode corrected expected position parameter at the (n-1)th time point plus the product of the sliding mode corrected expected velocity parameter at the (n-1)th time point and the time interval. Through the above recursive integration calculation, traversing all time points, the sliding mode corrected expected position parameter sequence is obtained.

[0212] Step S7110: Combine the expected position parameter sequence after sliding mode correction and the expected velocity parameter sequence after sliding mode correction in chronological order to generate the predicted motion trajectory curve of the special equipment.

[0213] The expected position parameter sequence after sliding mode correction generated in step S719 and the expected velocity parameter sequence after sliding mode correction generated in step S718 are aligned according to time points, with each time point corresponding to a set of position and velocity parameters. These position and velocity parameters are combined into data pairs and arranged in chronological order to form a new time series data, which is the predicted motion trajectory curve. Each time point in this predicted motion trajectory curve corresponds to an array of expected position parameters containing six joint angle values ​​and an array of expected velocity parameters containing six joint angular velocity values.

[0214] The method also includes the following steps.

[0215] Based on step S150 and its sub-steps, phase plane analysis, cross-section mapping and trajectory topology adjustment techniques are further introduced to enhance the robustness of the trajectory tracking controller to initial state disturbances and suppress trajectory divergence.

[0216] For example, in step S811: the expected position parameters and corresponding expected velocity parameters of each time point in the predicted motion trajectory curve are mapped according to a preset phase plane coordinate system to obtain the expected phase point sequence of the special equipment in the phase plane. The phase plane coordinate system has the position parameters as the horizontal axis variable and the velocity parameters as the vertical axis variable. The expected phase point sequence includes the position coordinate components and velocity coordinate components of each time point in the phase plane coordinate system.

[0217] A phase plane coordinate system is constructed, with the horizontal axis representing position parameters and the vertical axis representing velocity parameters. For each time point in the predicted motion trajectory curve, the expected position parameter at that time point is used as the horizontal axis value, and the expected velocity parameter at that time point is used as the vertical axis value, forming a coordinate point in the phase plane coordinate system, i.e., the expected phase point. All expected phase points are arranged in chronological order to obtain the expected phase point sequence. Since the robotic arm has multiple joints, and the phase plane of each joint is independent, the above coordinate mapping process is actually performed on each joint separately to obtain the expected phase point sequence corresponding to each joint.

[0218] Step S812: Obtain the current position parameter and current speed parameter from the real-time operating status data set, map the current position parameter and current speed parameter to the phase plane coordinate system, and obtain the actual phase point coordinates of the special equipment at the current moment.

[0219] Extract the current position parameter and current velocity parameter from the real-time operating status data set obtained in step S130. For each joint, use the current position parameter of the joint as the x-axis value and the current velocity parameter of the joint as the y-axis value to form the actual phase point coordinates of the joint at the current moment.

[0220] Step S813: Construct a state transition vector field in the phase plane coordinate system with the actual phase point coordinates as the starting point and the coordinates of each phase point in the expected phase point sequence as the convergence target. The state transition vector field contains a set of transition direction vectors pointing from the actual phase point coordinates to each of the expected phase point coordinates.

[0221] For each joint, in its phase plane coordinate system, a migration direction vector is constructed, starting from the actual phase point coordinates of the joint and ending at the coordinates of each expected phase point in the expected phase point sequence of the joint. The direction of this migration direction vector is from the actual phase point to the expected phase point, and its magnitude is the Euclidean distance between the two points. All of these migration direction vectors are collected to form the state transition vector field of the joint. This state transition vector field describes the possible paths for the actual state to approach the desired state.

[0222] Step S814: Based on the angle parameters between each migration direction vector in the state migration vector field and the preset stable manifold direction in the phase plane coordinate system, calculate the phase trajectory divergence rate parameter between the actual phase point coordinates and the expected phase point sequence. The phase trajectory divergence rate parameter is used to characterize the degree of separation between the actual motion trajectory and the predicted motion trajectory curve in the phase plane.

[0223] In the phase plane coordinate system, a stable manifold direction is pre-defined. This stable manifold direction typically points towards the equilibrium point or a limit cycle. For each joint, the angle between each migration direction vector in the state transition vector field and the aforementioned stable manifold direction is calculated, resulting in a series of angle parameters. Then, the average or maximum value of this series of angle parameters is calculated as the phase trajectory divergence rate parameter for that joint. The larger the value of this phase trajectory divergence rate parameter, the greater the separation between the actual motion trajectory and the predicted motion trajectory curve in the phase plane, and the more unstable the system.

[0224] Step S815: Extract the maximum divergence rate parameter from the phase trajectory divergence rate parameter. When the maximum divergence rate parameter exceeds a preset sensitive threshold parameter, determine that the motion trajectory of the special equipment is in the sensitive response range to the initial state disturbance.

[0225] From the phase trajectory divergence rate parameters of each joint calculated in step S814, the maximum value is extracted as the maximum divergence rate parameter. This maximum divergence rate parameter is compared with a preset sensitivity threshold parameter. If the maximum divergence rate parameter exceeds the sensitivity threshold parameter, it is determined that the current motion trajectory of the robotic arm is in the sensitive response range to the initial state disturbance, that is, a small disturbance may cause a significant deviation in the trajectory.

[0226] Step S816: When it is determined that the motion trajectory of the special equipment is within the sensitive response range to the initial state disturbance, the state migration vector field is input into a preset section interceptor. The section interceptor intercepts a set of section line segments in the phase plane coordinate system along a direction perpendicular to the migration direction vector set to obtain a phase section point set.

[0227] When step S815 determines that the system is in the sensitive response range, for each joint, its state transition vector field is input into a preset section cutter. This section cutter, in the phase plane coordinate system, along a direction perpendicular to the set of transition direction vectors (i.e., perpendicular to the normal direction of the transition direction vectors), cuts a series of section segments. These section segments intersect the state transition vector field, forming a series of intersection points. These intersection points are collected to obtain the phase section point set for that joint. This phase section point set is a set of section mapping points of the system state in phase space.

[0228] Step S817: Extract the mapping parameters between adjacent cross-section points in the phase cross-section point set, and construct the cross-section regression mapping function of the special equipment motion trajectory based on the mapping parameters. The cross-section regression mapping function is used to describe the iterative evolution law of the actual phase point coordinates on the phase cross-section.

[0229] For each joint, the mapping relationship between adjacent cross-section points is extracted from the set of phase cross-section points obtained in step S816. Specifically, for the i-th cross-section point, its coordinates are recorded; for the (i+1)-th cross-section point, its coordinates are also recorded. The coordinate changes between the aforementioned adjacent cross-section points are used as mapping parameters. Based on these mapping parameters, a cross-section regression mapping function is constructed. This cross-section regression mapping function is a nonlinear function that can describe the evolution of the actual phase point coordinates on the phase cross-section after one iteration through polynomial fitting or neural network fitting. That is, the coordinates of the current cross-section point are input, and the coordinates of the cross-section point in the next iteration are output.

[0230] Step S818: Substitute the actual phase point coordinates into the cross-sectional regression mapping function for iterative calculation to obtain a predicted phase point iterative sequence. The predicted phase point iterative sequence includes the convergence position parameters of the actual phase point coordinates on the phase cross section after multiple iterations.

[0231] For each joint, the actual phase point coordinates of that joint are used as initial values ​​and substituted into the cross-sectional regression mapping function constructed in step S817 for iterative calculation. The first iteration yields the cross-sectional point coordinates after the first iteration; the second iteration uses the result of the first iteration as input and substitutes it into the cross-sectional regression mapping function again, and so on. After multiple iterations, the cross-sectional point coordinates will converge to a certain stable value or a stable periodic orbit. The cross-sectional point coordinates obtained from each iteration are arranged in the iteration order to obtain the predicted phase point iteration sequence.

[0232] Step S819: Perform trajectory topology adjustment processing on the predicted motion trajectory curve according to the convergence position parameters in the predicted phase point iteration sequence to obtain the predicted motion trajectory curve after disturbance suppression correction. The expected position parameters and expected velocity parameters at each time point in the predicted motion trajectory curve after disturbance suppression correction are adjusted by phase plane topology transformation.

[0233] For each joint, the original predicted motion trajectory curve is topologically adjusted based on the convergence position parameters in the predicted phase point iteration sequence obtained in step S818. The adjustment involves projecting the original predicted motion trajectory curve onto the phase plane and directing it towards the stable region defined by the convergence position parameters. Specifically, for each time point in the predicted motion trajectory curve, the deviation between the expected phase point and the convergence position parameters at that time point is calculated. A correction vector is generated based on this deviation and superimposed on the expected position and velocity parameters at that time point. Through this topological transformation adjustment, the corrected predicted motion trajectory curve becomes more stable in the phase plane and less sensitive to initial disturbances. The adjusted trajectory curves of each joint are then combined to obtain the disturbance-suppressed corrected predicted motion trajectory curve.

[0234] Step S8110: Input the predicted motion trajectory curve after disturbance suppression correction into the trajectory tracking controller to replace the original predicted motion trajectory curve, and regenerate the control signal sequence of the drive component.

[0235] The disturbance-suppressed and corrected predicted motion trajectory curve generated in step S819 replaces the original predicted motion trajectory curve used in step S150 and subsequent steps. Then, using this disturbance-suppressed and corrected predicted motion trajectory curve as the new desired trajectory, step S150 and its sub-steps are repeated to regenerate the control signal sequence. This control signal sequence is used to drive the robotic arm to perform motion according to the disturbance-suppressed and corrected predicted motion trajectory curve, thereby effectively suppressing trajectory deviations caused by initial disturbances and improving the tracking accuracy and stability of the system.

[0236] In one exemplary embodiment, a general-purpose servo system control system for special equipment applications is provided. This general-purpose servo system control system can be a terminal, server, etc., and its internal structure diagram can be as follows: Figure 2 As shown, this general-purpose servo system control system for special equipment applications includes a processor, memory, input / output interface, communication interface, display unit, and input device. The processor, memory, and input / output interface are connected via a system bus, and the communication interface, display unit, and input device are also connected to the system bus via the input / output interface. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage medium. The input / output interface is used for exchanging information between the processor and external devices. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, mobile cellular networks, near-field communication, or other technologies. When the computer program is executed by the processor, it implements a general-purpose servo system control method for special equipment applications. The display unit is used to form a visually visible image and can be a display screen, projection device, or virtual reality imaging device. The display screen can be an LCD screen or an e-ink screen. The input device can be a touch layer covering the display screen, or a button, trackball, or touchpad set on the housing of a general servo system control system for special equipment applications. It can also be an external keyboard, touchpad, or mouse, etc.

[0237] It should be noted that, in order to simplify the description of the present invention and thus help to understand one or more embodiments of the invention, multiple features may sometimes be grouped into one embodiment, drawing or description thereof in the foregoing description of the embodiments of the present invention.

Claims

1. A general-purpose servo system control method for special equipment applications, characterized in that, The method includes: Receive an operation instruction sequence corresponding to a special equipment application scenario. The operation instruction sequence contains multiple discrete operation instruction units arranged in chronological order. Each discrete operation instruction unit carries an operation type identifier and an operation parameter set. The operation flow timing diagram of the special equipment is constructed based on the operation instruction sequence. The operation flow timing diagram includes multiple operation nodes and timing association edges connecting the operation nodes. Each operation node corresponds to a discrete operation instruction unit. The timing association edges are used to represent the time interval parameter between adjacent discrete operation instruction units. Obtain a set of real-time operating status data for the special equipment, which includes the current position parameters, current speed parameters, and current load parameters of the drive components of the special equipment at the current moment. Based on the operation process sequence diagram and the real-time operation status data set, a predicted motion trajectory curve of the special equipment is generated. The predicted motion trajectory curve includes the expected position parameters and expected speed parameters of the special equipment at each time point within a future time window. The trajectory tracking controller of the general servo system is invoked to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and a control signal sequence of the drive component is generated. The control signal sequence is used to drive the special equipment to perform motion according to the predicted motion trajectory curve. The step of constructing the operation flow sequence diagram of the special equipment based on the operation instruction sequence includes: The operation type identifier of each discrete operation instruction unit in the operation instruction sequence is parsed, and the operation execution parameter template corresponding to the operation type identifier is retrieved from the preset instruction template library according to the operation type identifier. The operation execution parameter template includes the standard motion trajectory curve template and the standard execution duration parameter corresponding to the operation type identifier. The operation execution parameter template is assigned a parameter value according to the operation parameter set to generate a job node corresponding to each discrete operation instruction unit. The job node includes the standard motion trajectory curve template and the standard execution duration parameter. Extract the time interval parameter between adjacent discrete operation instruction units in the operation instruction sequence, and use the time interval parameter as a temporal association edge connecting adjacent job nodes. The temporal association edge is used to constrain the time connection relationship between adjacent job nodes. The operation nodes are arranged in the order of the operation instruction sequence, and adjacent operation nodes are connected by the time-series association edge to generate the operation flow timing diagram of the special equipment. The order of the operation nodes in the operation flow timing diagram is consistent with the order of the discrete operation instruction units in the operation instruction sequence. Based on the standard motion trajectory curve template, standard execution duration parameter, and time interval parameter of the time sequence associated edge of the task node in the task flow sequence diagram, calculate the start execution time point and end execution time point of each task node in the task flow sequence diagram to obtain the time axis parameter set of the task flow sequence diagram; Obtain the standard execution duration parameter of the first job node in the job flow sequence diagram, take the preset initial time zero point as the start execution time point of the first job node, calculate the end execution time point of the first job node according to the standard execution duration parameter of the first job node, and obtain the start execution time point and end execution time point of the first job node; The end execution time of the first job node is added to the time interval parameter of the temporal association edge connecting the first job node and the second job node to obtain the start execution time of the second job node. The end execution time of the second job node is calculated according to the standard execution duration parameter of the second job node to obtain the start execution time and end execution time of the second job node. According to the order of the work nodes in the work process sequence diagram, the end execution time of the current work node is added to the time interval parameter of the time sequence association edge connecting the current work node and the next work node to obtain the start execution time of the next work node. The end execution time of the next work node is calculated according to the standard execution duration parameter of the next work node to obtain the start execution time and end execution time of all work nodes. The start and end execution times of each job node are arranged in the order of the job nodes to generate a time axis parameter set for the job flow sequence diagram. The time axis parameter set includes the start and end execution times of each job node. The start and end execution time values ​​of each task node in the time axis parameter set are aligned with the standard motion trajectory curve template corresponding to the task node on the time axis to obtain the motion trajectory curve segment of each task node on the time axis.

2. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of generating the predicted motion trajectory curve of the special equipment based on the work process sequence diagram and the real-time operating status data set includes: Extract the current job node from the job flow sequence diagram and obtain the start execution time, end execution time, and motion trajectory curve segment corresponding to the current job node. The position deviation of the current position of the drive component relative to the motion trajectory curve segment is calculated based on the motion trajectory curve segment of the current work node and the current position parameter in the real-time operation status data set. Subtract the end execution time of the current job node from the start execution time of the current job node to obtain the remaining execution time parameter of the current job node. Generate the remaining motion trajectory curve segment corresponding to the current job node based on the motion trajectory curve segment of the current job node and the remaining execution time parameter. Obtain all subsequent work nodes located after the current work node in the work process sequence diagram, and splice the motion trajectory curve segments of the subsequent work nodes according to the order of the work process sequence diagram to generate a set of subsequent work trajectory curve segments. The remaining motion trajectory curve segments are connected to the set of subsequent operation trajectory curve segments in a time sequence to generate the initial predicted motion trajectory curve of the special equipment in a future time window. The initial predicted motion trajectory curve includes all motion trajectory curve segments within the time range from the current moment to the end of the last subsequent operation node. Extract the current speed parameter from the real-time operating status data set, compare the current speed parameter with the expected speed parameter corresponding to the current moment in the initial predicted motion trajectory curve, and obtain the speed deviation parameter of the current speed of the driving component relative to the initial predicted motion trajectory curve; Extract the current load parameters from the real-time operating status data set, input the current load parameters into a preset load compensation function for calculation, and obtain the load compensation coefficient. The load compensation coefficient is used to adjust the acceleration response capability of the drive component under different load conditions. Based on the speed deviation parameter and the load compensation coefficient, the expected speed parameters at each time point after the current moment in the initial predicted motion trajectory curve are corrected point by point to obtain the corrected expected speed parameter sequence. The expected speed parameter at each time point in the corrected expected speed parameter sequence is negatively correlated with the speed deviation parameter and positively correlated with the load compensation coefficient. Based on the corrected expected velocity parameter sequence, the expected position parameters at each time point after the current time in the initial predicted motion trajectory curve are integrally reconstructed to obtain the corrected expected position parameter sequence. The expected position parameter at each time point in the corrected expected position parameter sequence is equal to the sum of the products of the expected position parameter at the previous time and the corrected expected velocity parameter at the current time multiplied by the time interval. The corrected expected position parameter sequence and the corrected expected velocity parameter sequence are combined in chronological order to generate the predicted motion trajectory curve of the special equipment. Each time point in the predicted motion trajectory curve corresponds to one corrected expected position parameter and one corrected expected velocity parameter.

3. The general servo system control method for special equipment applications according to claim 1, characterized in that, The process of calling the trajectory tracking controller of the general-purpose servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and generating the control signal sequence of the drive component, includes: The predicted motion trajectory curve is input into the reference trajectory generation module of the trajectory tracking controller. The predicted motion trajectory curve is discretized and sampled according to a preset time step to obtain a reference trajectory point sequence. The reference trajectory point sequence contains multiple reference trajectory points. Each reference trajectory point corresponds to a sampling time point and the reference position parameters and reference velocity parameters of that sampling time point. The current position parameters and current velocity parameters in the real-time operating status data set are input into the status feedback module of the trajectory tracking controller, and a current state vector is constructed based on the current position parameters and current velocity parameters. The current state vector contains position components and velocity components. The reference position parameter and reference velocity parameter corresponding to the current sampling time point in the reference trajectory point sequence are compared with the current state vector to obtain the position tracking error vector and the velocity tracking error vector. The position tracking error vector is equal to the reference position parameter minus the current position parameter, and the velocity tracking error vector is equal to the reference velocity parameter minus the current velocity parameter. The position tracking error vector and the velocity tracking error vector are input into the feedforward control module of the trajectory tracking controller, and the feedforward control quantity is calculated based on the reference velocity parameter and the reference position parameter to obtain the feedforward control signal component; The position tracking error vector and the velocity tracking error vector are input into the feedback control module of the trajectory tracking controller. The position tracking error vector and the velocity tracking error vector are weighted and summed according to the preset proportional gain parameter and integral gain parameter to obtain the feedback control signal component. The feedforward control signal component and the feedback control signal component are superimposed to generate the initial control signal at the current moment. The initial control signal contains the target control quantity that the drive component needs to output at the next moment. Obtain the set of physical constraints for the drive components of the general servo system. The set of physical constraints for the drive components includes the maximum output torque constraint parameter, the maximum output speed constraint parameter, and the maximum acceleration constraint parameter. Perform amplitude limiting processing on the initial control signal according to the set of physical constraints for the drive components to obtain the amplitude-limited control signal. The limited control signal is converted into a pulse width modulation signal format recognizable by the driving component to generate a control signal unit for the current moment. The control signal unit includes the duty cycle parameter and frequency parameter of the pulse width modulation signal. The steps of comparing the reference position parameters and reference velocity parameters corresponding to the current sampling time point in the reference trajectory point sequence with the current state vector are repeated according to the preset time step, up to the step of converting the limited control signal into a pulse width modulation signal format recognizable by the driving component, to obtain a control signal unit sequence. The control signal unit sequence is arranged in chronological order to generate the control signal sequence of the driving component, wherein each control signal unit in the control signal sequence corresponds to a sampling time.

4. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of generating the predicted motion trajectory curve of the special equipment based on the work process sequence diagram and the real-time operation status data set further includes: Extract the current job node from the job flow sequence diagram and obtain the start execution time, end execution time, and motion trajectory curve segment corresponding to the current job node. The motion trajectory curve segment of the current work node is discretized according to a preset curve discretization step size to obtain the position discrete point sequence and velocity discrete point sequence corresponding to the current work node. The position discrete point sequence contains the position parameters of each discrete point on the motion trajectory curve segment corresponding to the current work node, and the velocity discrete point sequence contains the velocity parameters of each discrete point on the motion trajectory curve segment corresponding to the current work node. The current position parameter in the real-time running status data set is obtained, and the current position parameter is matched with each position parameter in the discrete point sequence to obtain the matching position index parameter of the current position parameter in the discrete point sequence. The matching position index parameter is used to indicate the discrete point position of the current position parameter on the motion trajectory curve segment corresponding to the current work node. Based on the matching position index parameter, extract all remaining discrete position points after the matching position index parameter from the discrete position point sequence to obtain the set of remaining discrete position points; Based on the matching position index parameter, extract all remaining velocity discrete points after the matching position index parameter from the velocity discrete point sequence to obtain the set of remaining velocity discrete points; Arrange the remaining discrete points in the set of remaining discrete points in the original order to generate the sequence of remaining discrete points corresponding to the current job node. Arrange the remaining velocity discrete points in the set of remaining velocity discrete points in the original order to generate the sequence of remaining velocity discrete points corresponding to the current working node; Obtain all subsequent work nodes located after the current work node in the work process sequence diagram, extract the position discrete point sequence and velocity discrete point sequence corresponding to each subsequent work node, and obtain the subsequent position discrete point sequence set and the subsequent velocity discrete point sequence set; The remaining position discrete point sequence is spliced ​​with each subsequent position discrete point sequence in the subsequent position discrete point sequence set according to the order of the operation flow time sequence diagram to generate the predicted position discrete point sequence. The remaining velocity discrete point sequence is spliced ​​together with each subsequent velocity discrete point sequence in the subsequent velocity discrete point sequence set according to the order of the operation flow time sequence diagram to generate the predicted velocity discrete point sequence. The predicted position discrete point sequence and the predicted velocity discrete point sequence are combined in chronological order to generate the predicted motion trajectory curve of the special equipment. The predicted motion trajectory curve includes the expected position parameters of each discrete point in the predicted position discrete point sequence and the expected velocity parameters of each discrete point in the predicted velocity discrete point sequence.

5. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of invoking the trajectory tracking controller of the general-purpose servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and generating the control signal sequence of the drive component, further includes: The predicted motion trajectory curve is input into the look-ahead planning module of the trajectory tracking controller. Based on the expected velocity parameters at each time point in the predicted motion trajectory curve, the required acceleration parameter sequence of the drive component at each time point in the future time window is calculated to obtain the expected acceleration parameter sequence. The current speed parameter in the real-time operating status data set is input into the status observation module of the trajectory tracking controller. Based on the current speed parameter and the inertial parameter of the drive component, the momentum parameter of the drive component at the current moment is calculated to obtain the current momentum parameter. The expected acceleration parameter sequence is input into the torque feedforward module of the trajectory tracking controller. Based on the expected acceleration parameter sequence and the inertial parameters of the drive component, the required inertial torque parameter sequence of the drive component at each time point in the future time window is calculated to obtain the feedforward torque parameter sequence. The current momentum parameter is input into the damping compensation module of the trajectory tracking controller. Based on the current momentum parameter and the damping coefficient of the drive component, the damping compensation torque parameter required by the drive component at the current moment is calculated to obtain the damping compensation torque parameter. The expected position parameter corresponding to the current moment in the predicted motion trajectory curve is compared with the current position parameter in the real-time running status data set to obtain the position deviation parameter; The expected velocity parameter corresponding to the current moment in the predicted motion trajectory curve is compared with the current velocity parameter in the real-time running status data set to obtain the velocity deviation parameter. The position loop control output parameters are calculated based on the position deviation parameters and the preset position loop proportional coefficient. The position loop control output parameters are physical quantities of torque, and the position loop control quantities are obtained. Based on the speed deviation parameter and the preset speed loop integral coefficient, the speed deviation parameter is accumulated and summed to obtain the speed loop integral control quantity, which is a torque physical quantity. The position loop control quantity and the velocity loop integral control quantity are fused to obtain a first fused control quantity; the first fused control quantity, the feedforward torque parameter corresponding to the current moment in the feedforward torque parameter sequence, and the damping compensation torque parameter are superimposed to generate the control signal at the current moment, and the control signal includes the electromagnetic torque command parameters that the drive component needs to output; The control signal is repeatedly generated according to a preset control cycle to obtain a control signal sequence, wherein each control signal in the control signal sequence corresponds to a control cycle.

6. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of generating the predicted motion trajectory curve of the special equipment based on the work process sequence diagram and the real-time operation status data set further includes: Extract the current job node from the job flow sequence diagram and obtain the start execution time, end execution time, and motion trajectory curve segment corresponding to the current job node. The position deviation of the current position of the drive component relative to the motion trajectory curve segment is calculated based on the motion trajectory curve segment of the current work node and the current position parameter in the real-time operation status data set. Obtain all subsequent job nodes located after the current job node in the job flow sequence diagram, extract the start execution time point, end execution time point, and motion trajectory curve segment corresponding to each subsequent job node, and obtain the parameter set of the subsequent job nodes; The trajectory deviation parameter is input into a preset trajectory correction function for calculation to obtain the trajectory correction coefficient. The trajectory correction coefficient is used to adjust the shape of the motion trajectory curve segment corresponding to the current operation node. Based on the trajectory correction coefficient, the motion trajectory curve segment corresponding to the current work node is morphologically corrected to obtain the corrected motion trajectory curve segment of the current work node. The corrected current task node motion trajectory curve segment is spliced ​​with the motion trajectory curve segments corresponding to each subsequent task node in the parameter set of the subsequent task node according to the order of the task flow sequence diagram to generate a corrected complete task trajectory curve segment set. Extract all trajectory curve segments from the current moment to the end of the last subsequent task node from the corrected complete set of operation trajectory curve segments to obtain the time window trajectory curve segment set. The time window trajectory curve segment set is parameterized according to a preset curve parameterization method to obtain a position parameterization function and a velocity parameterization function with time as the independent variable; The expected position parameters at each time point within the future time window are calculated based on the position parameterization function to obtain the expected position parameter sequence. The expected velocity parameters at each time point within the future time window are calculated based on the velocity parameterization function to obtain the expected velocity parameter sequence. The expected position parameter sequence and the expected velocity parameter sequence are combined in chronological order to generate the predicted motion trajectory curve of the special equipment.

7. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of invoking the trajectory tracking controller of the general-purpose servo system to perform deviation correction processing on the predicted motion trajectory curve and the real-time operating status data set, and generating the control signal sequence of the drive component, further includes: The predicted motion trajectory curve is input into the trajectory smoothing module of the trajectory tracking controller, and the expected velocity parameters between adjacent time points in the predicted motion trajectory curve are differentially processed to obtain a velocity change rate parameter sequence. When there is a rate of change parameter in the velocity change parameter sequence that exceeds the preset acceleration limit threshold, the expected velocity parameters near the corresponding time point in the predicted motion trajectory curve are re-planned to obtain a smoothed predicted motion trajectory curve. The smoothed predicted motion trajectory curve is input into the reference trajectory generation module of the trajectory tracking controller. The smoothed predicted motion trajectory curve is sampled according to a preset control cycle to obtain a reference trajectory point sequence. The reference trajectory point sequence includes reference position parameters and reference velocity parameters for each control cycle. The current position parameter and current speed parameter in the real-time running status data set are input into the error calculation module of the trajectory tracking controller. The difference between the reference position parameter and the current position parameter is calculated as the position error parameter, and the difference between the reference speed parameter and the current speed parameter is calculated as the speed error parameter. The position error parameter is input into the position loop controller of the trajectory tracking controller, and the velocity loop setpoint parameter is calculated based on the position error parameter and the preset position loop control parameter to obtain the velocity loop setpoint value; The speed error parameter and the speed loop setpoint are input into the speed loop controller of the trajectory tracking controller. The current loop setpoint parameter is calculated based on the difference between the speed error parameter and the speed loop setpoint and the preset speed loop control parameters to obtain the current loop setpoint. The current loop setpoint is input into the current loop controller of the trajectory tracking controller. The voltage control parameters are calculated based on the current loop setpoint and the instantaneous phase current value parameters in the real-time operating status data set to obtain the voltage control quantity. The voltage control quantity is input into the pulse width modulation generation module of the trajectory tracking controller, and a pulse width modulation signal with a corresponding duty cycle is generated according to the voltage control quantity to obtain the control signal unit; The steps of inputting the smoothed predicted motion trajectory curve into the reference trajectory generation module of the trajectory tracking controller and the step of generating a pulse width modulation signal with a corresponding duty cycle according to the voltage control quantity are repeated according to the preset control cycle to obtain a control signal unit sequence. The control signal unit sequence is arranged in chronological order to generate the control signal sequence of the drive component.

8. The general servo system control method for special equipment applications according to claim 1, characterized in that, The step of constructing the operation flow sequence diagram of the special equipment based on the operation instruction sequence further includes: Obtain the motion speed limit parameter contained in the operation parameter set of each discrete operation instruction unit in the operation instruction sequence. The motion speed limit parameter is used to constrain the maximum allowable speed of the motion trajectory curve segment corresponding to the discrete operation instruction unit. Based on the motion speed limit parameter, the standard motion trajectory curve template corresponding to the discrete operation instruction unit is subjected to speed limiting processing to obtain a speed-limited motion trajectory curve template. The speed parameter at each time point in the speed-limited motion trajectory curve template does not exceed the motion speed limit parameter. Obtain the motion acceleration limit parameter contained in the operation parameter set of each discrete operation instruction unit in the operation instruction sequence. The motion acceleration limit parameter is used to constrain the maximum allowable acceleration of the motion trajectory curve segment corresponding to the discrete operation instruction unit. Based on the motion acceleration limit parameters, the motion trajectory curve template after speed limit is processed by acceleration limiting to obtain a motion trajectory curve template after speed and acceleration limit. The acceleration parameters at each time point in the motion trajectory curve template after speed and acceleration limit do not exceed the motion acceleration limit parameters. The motion trajectory curve template after speed and acceleration limitation is used as the updated standard motion trajectory curve template corresponding to the discrete operation instruction unit. The updated standard execution time parameters are obtained by recalculating the standard execution time parameters corresponding to the discrete operation instruction unit based on the updated standard motion trajectory curve template. The updated standard motion trajectory curve template and the updated standard execution time parameter are used as the job node corresponding to the discrete operation instruction unit. The job node includes the updated standard motion trajectory curve template and the updated standard execution time parameter.

9. A general-purpose servo system control system for special equipment applications, characterized in that, include: processor; A machine-readable storage medium for storing machine-executable instructions of the processor; The processor is configured to execute the general servo system control method for special equipment applications as described in any one of claims 1 to 8 by executing the machine-executable instructions.

Images