Driving and control integrated control method and system

Abstract

The invention discloses a driving and control integrated control method and system, relates to the technical field of industrial robot control, is applied to a mechanical arm, and provides a driving and control integrated control method and system through prediction of future current requirements, evaluation of current saturation risks, construction of rolling optimization problems and dynamic adjustment of joint position instructions. Multi-axis collaborative optimization is realized while current abrupt change is avoided, so that trajectory tracking precision is improved, mechanical vibration is reduced, and dynamic current optimization is realized.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of industrial robot control, in particular to a drive-control integrated control method and system. BACKGROUND

[0002] Drive-control integrated technology closely integrates the driver and the controller, which is an important direction of the development of modern manipulators. However, the existing technology faces multiple technical bottlenecks in multi-axis cooperative motion control. First, the traditional current loop limiting method directly truncates the instruction when the joint motor reaches the maximum output current, resulting in sudden torque output, causing trajectory deviation and mechanical vibration. Second, there is a problem of information transmission lag in multi-axis cooperative control, and it is difficult to adjust the trajectory planning in time when the joint current saturation is predicted. Third, there is a lack of effective dynamic coordination mechanism among the joint drivers, which cannot realize the global optimization distribution of current demand. The existing solutions are mostly static compensation or local adjustment, which cannot accurately predict the current demand changes in the future multiple control periods, and lack the cooperative optimization ability based on the prediction results, resulting in problems such as trajectory distortion, vibration intensification and performance waste of the manipulator in high-speed high-precision operation.

[0003] The above content is only used to assist in understanding the technical solutions of the present application, and does not represent the acknowledgement of the above content as prior art. SUMMARY

[0004] The main purpose of the present application is to provide a drive-control integrated control method and system, which aims to improve the trajectory tracking accuracy, reduce mechanical vibration and realize dynamic current optimization.

[0005] To achieve the above purpose, the present application provides a drive-control integrated control method, which comprises: generating a manipulator end desired trajectory according to an externally input task instruction, and obtaining desired position data and desired speed data of each joint in the current control period and future multiple control periods based on the manipulator end desired trajectory; simultaneously obtaining actual position data, actual speed data of each joint encoder and actual current data output to each joint motor; obtaining current demand prediction data of each joint in the future multiple control periods based on the desired position data, the desired speed data, the actual position data, the actual speed data and the actual current data; calculating real-time current margin data of each joint based on the actual current data and pre-stored maximum allowable current data of each joint; and combining the current demand prediction data, evaluating the current saturation risk of each joint in the future multiple control periods; constructing a rolling optimization problem data package aiming at minimizing trajectory tracking error based on the desired position data, the current demand prediction data and the current saturation risk, wherein the optimization variable is an optimized position command of each joint, and the constraint conditions include current upper limit constraint and dynamics model constraint; solving the rolling optimization problem data package by quadratic programming, outputting an optimized position command sequence of multiple control cycles in the future, and extracting the optimized position command data of the first cycle in the optimized position command sequence as the current control command and outputting to the joint driver.

[0006] In an embodiment, the step of obtaining current demand prediction data of each joint in multiple control cycles in the future based on the desired position data, the desired speed data, the actual position data, the actual speed data and the actual current data comprises: performing time difference calculation on the desired speed data to obtain desired acceleration data of each joint; based on a preset joint dynamics model library, calling the rotational inertia data of the corresponding joint, and based on the actual speed data, estimating the friction torque data; based on the desired acceleration data, the rotational inertia data and the friction torque data, obtaining the predicted required torque data through Newton-Euler equation calculation processing; based on the preset motor torque constant, converting the predicted required torque data into current demand prediction data through proportional conversion processing.

[0007] In an embodiment, the step of estimating the friction torque data based on the actual speed data comprises: based on the actual speed data, obtaining speed direction identification data through sign function processing; calling the pre-stored joint Coulomb friction coefficient and viscous friction coefficient; multiplying the Coulomb friction coefficient by the speed direction identification data, and adding the product of the viscous friction coefficient and the actual speed data to obtain the friction torque data.

[0008] In an embodiment, the step of combining the current demand prediction data to evaluate the current saturation risk of each joint in multiple control cycles in the future comprises: based on the current demand prediction data, obtaining compensated current demand data through time decay factor compensation processing; the time decay factor is set according to the current loop response bandwidth; comparing the compensated current demand data with the maximum allowed current data, when the compensated current demand data exceeds the maximum allowed current, calculating the current oversaturation degree through difference calculation; Outputting a multi-level current saturation risk through threshold grading processing based on the current oversaturation degree.

[0009] In an embodiment, the current saturation risk includes a low risk level, a medium risk level and a high risk level, and the step of outputting a multi-level current saturation risk through threshold grading processing based on the current oversaturation degree includes: When the current oversaturation degree data is less than a preset first threshold value, outputting low risk level identification data; When the current oversaturation degree data is greater than the preset first threshold value and less than a preset second threshold value, outputting medium risk level identification data; When the current oversaturation degree data is greater than the preset second threshold value, outputting high risk level identification data and corresponding joint identification; wherein the preset first threshold value is less than the preset second threshold value.

[0010] In an embodiment, the step of constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data and the current saturation risk includes: For the joint identified with the high risk level, increasing the position change rate limit in the dynamics model constraint, and achieving by setting the maximum allowed acceleration constraint; For the joint identified with the low risk level, increasing its position tracking error weight in the optimization target.

[0011] In an embodiment, the step of solving the rolling optimization problem data packet through quadratic programming to output an optimized position instruction sequence for a plurality of future control cycles includes: Converting the rolling optimization problem data packet into a standard quadratic programming matrix; Processing the standard quadratic programming matrix through an active set iteration algorithm to obtain a numerical solution of the optimized position instruction sequence; Performing instruction smoothing filtering processing on the numerical solution of the optimized position instruction sequence to output an optimized position instruction sequence that meets the continuity of the robot motion.

[0012] In an embodiment, the step of performing instruction smoothing filtering processing on the numerical solution of the optimized position instruction sequence to output an optimized position instruction sequence that meets the continuity of the robot motion includes: Setting a filtering coefficient based on the rated acceleration parameter of the robot; Performing smoothing filtering processing on the optimized position instruction sequence based on the filtering coefficient; Performing slope verification on the optimized position instruction sequence after the smoothing filtering processing to output an optimized position instruction sequence that meets the continuity of the robot motion.

[0013] In an embodiment, when the robot is in a heavy-load turning condition, before the step of constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data, and the current saturation risk, the method further comprises: identifying an active turning joint based on historical motion data and obtaining corresponding dynamic load inertia data; According to the dynamic load inertia data, the maximum allowable current data of the corresponding joint is proportionally increased.

[0014] In addition, to achieve the above-mentioned purpose, the application also proposes a drive-control integrated control system, which comprises: A data acquisition module is configured to generate an end-of-arm expected trajectory according to an externally input task instruction, and obtain joint expected position data and expected speed data of a current control period and a plurality of future control periods based on the end-of-arm expected trajectory; and simultaneously obtain actual position data, actual speed data of joint encoders, and actual current data output to joint motors; A current demand prediction module is configured to obtain joint current demand prediction data in a plurality of future control periods based on the expected position data, the expected speed data, the actual position data, the actual speed data, and the actual current data; A current saturation risk assessment module is configured to calculate joint real-time current margin data based on the actual current data and pre-stored joint maximum allowable current data, and simultaneously assess current saturation risks of the joints in a plurality of future control periods in combination with the current demand prediction data; A joint position command optimization module is configured to construct a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data, and the current saturation risk, wherein the optimization variable is joint optimization position command, and the constraint condition includes current upper limit constraint and dynamics model constraint; An output control module is configured to perform quadratic programming solution on the rolling optimization problem data packet, output an optimization position command sequence of a plurality of future control periods, and extract the optimization position command data of the first period in the optimization position command sequence as a current control command and output to the joint driver.

[0015] The drive-control integrated control method and system proposed in the application are applied to a robot, which predicts future current demand, assesses current saturation risk, and constructs a rolling optimization problem to dynamically adjust joint position commands, thereby avoiding current surge, realizing multi-axis collaborative optimization, improving trajectory tracking accuracy, reducing mechanical vibration, and achieving dynamic current optimization. Attached Figure Description

[0016] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0017] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is a flowchart illustrating an embodiment of the integrated drive and control method of this application. Figure 2 For this application Figure 1 A detailed flowchart of step S200; Figure 3 For this application Figure 2 A detailed flowchart of step S220; Figure 4 For this application Figure 1 Detailed flowchart of step S300; Figure 5 For this application Figure 4 A detailed flowchart of step S330; Figure 6 For this application Figure 1 Detailed flowchart of step S400; Figure 7 For this application Figure 1 Detailed flowchart of step S500; Figure 8 For this application Figure 7 A detailed flowchart of step S530; Figure 9 This is a flowchart illustrating another embodiment of the integrated drive and control method of this application. Figure 10 This is a schematic diagram of a structure provided for an embodiment of the integrated drive and control system of this application.

[0019] Explanation of icon numbers: 100. Integrated drive and control system; 110. Data acquisition module; 120. Current demand prediction module; 130. Current saturation risk assessment module; 140. Joint position command optimization module; 150. Output control module.

[0020] The purpose, features, and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0021] The technical solutions in the present application will be clearly and completely described below with reference to the drawings in the present application. Obviously, the described embodiments are only some of the embodiments of the present application, rather than all the embodiments. The components of the present application described and shown in the drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work fall within the scope of the present application.

[0022] It should be understood that similar reference numbers and letters represent similar items in the following drawings, and therefore, once an item is defined in one drawing, it does not need to be further defined and explained in subsequent drawings. Meanwhile, in the description of the present application, the terms "first", "second", and the like are only used to distinguish the description, and cannot be understood as indicating or implying relative importance.

[0023] In the prior art, the drive-control integration technology integrates the driver and the controller, reducing the volume and communication delay. However, the traditional scheme faces the rigid limit of current saturation in multi-axis high-speed cooperative motion, and multiple joints need large current output at the same time, which easily touches the physical limit. The existing control architecture has the problem of information transmission lag, and the trajectory adjustment layer cannot respond to the change of current constraint in time, resulting in insufficient instantaneous torque or trajectory deviation. The conservative strategy can avoid saturation, but at the cost of sacrificing the potential performance of the system. For example, when the mechanical arm performs high-speed arc drawing action, the shoulder joint and the elbow joint may accelerate simultaneously, which may exceed the current upper limit of each joint. The traditional limiting method directly truncates the current instruction, causing the distortion of the end trajectory and mechanical vibration.

[0024] In order to solve the above problems, the inventor found that the current saturation risk has dynamic and multi-joint coupling characteristics. The traditional static limiting cannot adapt to the load change, and the multi-axis cooperative control needs a forward-looking prediction mechanism. By analyzing the dynamic relationship between the current demand and the position instruction, it is proposed to include the current prediction of multiple control cycles in the future into the optimization framework. Considering the balance between trajectory tracking and current constraint, a rolling optimization model needs to be established to adjust the joint position instruction under the premise of meeting the dynamics constraint. In order to solve the delay problem, a quadratic programming method in the prediction time domain is used to generate an optimized instruction sequence in advance, ensuring the real-time and coordination of the control system.

[0025] Based on this, the embodiment of the present application provides a drive-control integrated control method applied to a mechanical arm, referring to Figure 1 , the drive-control integrated control method comprises steps S100-S500, wherein: Step S100, generating a robot end effector desired trajectory according to an externally input task instruction, and obtaining joint desired position data and desired speed data of a current control period and future control periods based on the robot end effector desired trajectory; meanwhile, obtaining actual position data, actual speed data of joint encoders, and actual current data output to joint motors; Step S200, obtaining joint current demand prediction data in future control periods based on the desired position data, the desired speed data, the actual position data, the actual speed data, and the actual current data; Step S300, calculating joint real-time current margin data based on the actual current data and pre-stored joint maximum allowable current data; meanwhile, combining the current demand prediction data, evaluating current saturation risks of joints in future control periods; Step S400, constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the desired position data, the current demand prediction data, and the current saturation risks, wherein the optimization variable is a joint optimization position instruction, and the constraint conditions include current upper limit constraints and dynamic model constraints; Step S500, performing quadratic programming solution on the rolling optimization problem data packet, outputting an optimization position instruction sequence in future control periods, and extracting the optimization position instruction data of the first period in the optimization position instruction sequence as a current control instruction and outputting to a joint driver.

[0026] In the embodiment, the future control periods refer to control stages of a plurality of discrete time intervals after the current time, which can be implemented by fixed step or variable step prediction time domain, and are used to establish a multi-step prediction model. The current demand prediction data are current values required in future periods calculated by a dynamic model and friction estimation, which can be obtained by converting Newton-Euler equation combined with motor torque constant, and are used to predict the possibility of current saturation. The current saturation risk is a probability of exceeding the maximum allowable current of a joint in future control periods, which can be determined by comparing the predicted current with a threshold, and is used to dynamically adjust the optimization constraint conditions. The rolling optimization problem data packet includes mathematical expressions of objective functions and constraint conditions, which can be constructed as a quadratic programming problem with inequality constraints, and is used to solve optimal position instructions online. The quadratic programming solution is to convert the optimization problem into a standard matrix form, which can be calculated by an effective set algorithm or an interior point method to ensure that a feasible instruction sequence is obtained within a limited time.

[0027] In this embodiment, the integrated control system 100 decomposes the end trajectory into a series of desired positions and velocities of each joint at the beginning of each control cycle through inverse kinematics calculation. At the same time, the actual motion state of the encoder feedback and the current value of the driver output are collected. Based on the dynamics model and friction compensation algorithm, the current value required to maintain the desired acceleration in the future multiple control cycles is predicted. Real-time monitoring of the current margin of each joint, combined with the predicted value to assess whether there will be a current overrun in the future. When detecting potential saturation risk, increase the acceleration constraint of the corresponding joint in the optimization model or adjust the tracking error weight. By solving the quadratic programming problem with constraints, the optimized position command sequence that takes into account trajectory accuracy and current limit is obtained. Finally, the optimization result of the first cycle is selected as the current control output, realizing dynamic adjustment and early avoidance of current saturation.

[0028] In this embodiment, the association model between current demand and position command is established through multi-step prediction, and the saturation risk is actively avoided in the trajectory generation layer. The constraint conditions are dynamically adjusted according to the real-time load to realize adaptive optimization. Compared with the local compensation method, the application coordinates the actions of multiple joints through global optimization, and maintains the balanced distribution of each axis current during high-speed motion. Thus, the current saturation problem during multi-axis cooperative motion is effectively solved, while maintaining the trajectory tracking accuracy and avoiding torque sudden change. Through forward-looking prediction and rolling optimization, the position command sequence is adjusted in advance to eliminate the delay of traditional control architecture. The dynamic constraint adjustment mechanism not only guarantees the safety of the system, but also fully utilizes the current output capacity of the driver, avoiding the performance waste caused by conservative strategy. Multi-joint cooperative optimization ensures that the axes move in coordination in complex motion scenarios, reducing the risk of mechanical vibration.

[0029] In one possible implementation, with reference to Figure 2 , the step S200 includes steps S210-S240, in which: Step S210, time difference calculation is performed on the desired speed data to obtain desired acceleration data of each joint; Step S220, based on the preset joint dynamics model library, the moment of inertia data of the corresponding joint is called, and the friction torque data is estimated based on the actual speed data; Step S230, based on the desired acceleration data, the moment of inertia data and the friction torque data, the predicted required torque data is obtained through Newton-Euler equation calculation processing; Step S240, based on the preset motor torque constant, the predicted required torque data is converted into current demand prediction data through proportional conversion processing.

[0030] In this embodiment, the time difference calculation refers to calculating the acceleration by the numerical differentiation method, which can be realized by the central difference method or the backward difference method, and is used to extract the acceleration information from the discrete speed sampling data. The rotational inertia data refers to the joint dynamic parameters stored in the preset model library, and the inertia value under the corresponding pose can be realized by querying in real time through the joint angle feedback, and reflects the dynamic change of the load inertia in the movement process of the mechanical arm. The friction force estimation refers to the nonlinear friction modeling based on the speed signal, which can be realized by using the Coulomb-viscous hybrid friction model, and the speed direction is extracted by using the sign function and the real-time friction torque is calculated by combining the preset friction coefficient. The Newton-Euler equation refers to the rigid body dynamics calculation equation, which can be realized by forward dynamics calculation, and the acceleration, inertia and external torque are related to solve the torque required by the driving joint. The motor torque constant refers to the conversion coefficient of motor torque and current, which can be obtained by the parameter manual of the mechanical arm shutdown motor, and is used to convert the theoretical torque demand into actual current instruction.

[0031] In this embodiment, in the process of acquiring the current demand prediction data, first, the expected speed sequence is subjected to discrete difference operation, for example, the expected acceleration corresponding to each control period is calculated by using a three-point difference formula. Then, the rotational inertia parameters corresponding to the current joint angle are extracted from the pre-established joint dynamics database, and the composite friction torque including static friction and viscous friction is calculated by judging the motion direction through the sign function according to the actual speed signal collected in real time. The expected acceleration, rotational inertia and friction torque are substituted into the Newton-Euler dynamics equation to solve the predicted torque required by the driving joint. Finally, the predicted torque is linearly mapped into the corresponding current demand value according to the torque-current proportional coefficient in the motor characteristic parameter, and the current demand prediction sequence of the future multiple control periods is formed.

[0032] In this embodiment, by dynamically calling the rotational inertia data and accurately calculating the friction torque, the load characteristics of the mechanical arm under different poses and motion states can be accurately reflected, and the friction model established in combination with the real-time speed data significantly improves the torque prediction accuracy. In addition, the application realizes accurate prediction of the future current demand of the multi-joint system, which provides reliable input data for subsequent current saturation risk assessment and trajectory optimization. Based on the prediction method of dynamic inertia calling and real-time friction force compensation, the prediction deviation of the traditional static model under the variable load and variable direction working conditions is effectively overcome, so that the current demand prediction value is closer to the actual control demand. By combining the dynamics calculation and the motor parameters, a complete mapping model from the motion instruction to the current demand is established, which lays a foundation for identifying and avoiding the current saturation risk in advance.

[0033] In a possible implementation, with reference to Figure 3 , step S220 includes steps S221-S223, in which: Step S221, based on the actual speed data, the speed direction identification data is obtained by sign function processing; Step S222, the pre-stored Coulomb friction coefficient and viscous friction coefficient are called; Step S223, the Coulomb friction coefficient is multiplied by the speed direction identification data, and the product of the viscous friction coefficient and the actual speed data is added to obtain the friction torque data.

[0034] In the embodiment, the sign function processing refers to converting the positive and negative values of the speed into direction identification, which can specifically map the speed data into +1 or -1 by using the sign function, and is used to distinguish the direction of the friction force. The Coulomb friction coefficient refers to the static friction component irrelevant to the speed direction, which can be specifically obtained by experimental calibration or theoretical calculation, and is used to represent the friction resistance when the joint is static or low-speed motion. The viscous friction coefficient refers to the dynamic friction component proportional to the speed, which can be specifically obtained by dynamic test or model fitting, and is used to represent the friction resistance when the joint is high-speed motion. The speed direction identification data refers to the binary data reflecting the direction of the joint motion, which can be specifically generated by judging the positive and negative of the actual speed by using the sign function, and is used to determine the direction of the Coulomb friction.

[0035] In the embodiment, when calculating the friction torque, the positive and negative of the actual speed is first converted into direction identification by using the sign function, for example, +1 is output when the speed is positive, and -1 is output when the speed is negative. Then the pre-stored Coulomb friction coefficient and viscous friction coefficient are called, the Coulomb friction coefficient is multiplied by the direction identification to obtain the direction-related static friction component, and the viscous friction coefficient is multiplied by the actual speed to obtain the dynamic friction component proportional to the speed. Finally, the two components are added to obtain the total friction torque data under the current speed. The friction torque data is used for subsequent torque prediction calculation to more accurately reflect the friction influence on the joint during the motion.

[0036] In the embodiment, by introducing the direction identification data, the direction of the Coulomb friction is dynamically associated with the speed direction, and the speed proportional characteristic of the viscous friction is combined, which can more accurately describe the friction characteristics of the joint under different motion states, so as to accurately distinguish the contribution of the static friction and the dynamic friction, avoid the torque prediction deviation caused by the misjudgment of the friction force direction, improve the accuracy of the current demand prediction, reduce the risk of current saturation misjudgment caused by the friction estimation error, reduce the trajectory tracking error of the mechanical arm in the start-stop or direction-changing stage, and suppress the mechanical vibration caused by the torque mutation.

[0037] In a feasible implementation manner, with reference to Figure 4 Step S300 includes steps S310-S330, wherein: At step S310, the compensated current demand data is obtained through time decay factor compensation processing based on the current demand prediction data; the time decay factor is set according to the current loop response bandwidth; At step S320, the compensated current demand data is compared with the maximum allowed current data, and when the compensated current demand data exceeds the maximum allowed current, the current oversaturation degree is obtained through difference calculation; At step S330, the multi-level current saturation risk is output through threshold classification processing based on the current oversaturation degree.

[0038] In this embodiment, the time decay factor refers to a dynamic compensation coefficient for correcting the current demand prediction value, which can be specifically implemented by an exponential decay function, the decay rate of which is matched with the closed-loop response speed of the current loop, and is used to compensate for the time difference between the control period and the current loop response delay. The compensated current demand data refers to the prediction current demand value after dynamic correction, which can be specifically implemented by multiplying the original prediction data by the time decay factor, and is used to more accurately reflect the actual current loop tracking capability. The current oversaturation degree refers to the amplitude of the prediction current demand exceeding the maximum allowed current, which can be specifically implemented by difference absolute value calculation, and is used to quantify the severity of current overrun. The threshold classification processing refers to a classification method for dividing risk levels according to the oversaturation degree, which can be specifically implemented by using preset multi-level threshold values, and is used to distinguish different levels of constraint violation.

[0039] In this embodiment, the current demand prediction data is first subjected to time decay factor compensation processing, for example, the prediction current is multiplied by a decay factor in the range of 0.8 to 0.95, and the coefficient is dynamically adjusted according to the current loop bandwidth. The compensated current demand is compared in real time with the pre-stored maximum allowed current, and when an overrun is detected, the absolute value of the overrun is calculated as an oversaturation degree indicator. The indicator is input into a three-level threshold classifier, for example, the first threshold is set to 5% of the maximum current, and the second threshold is set to 15%, corresponding to low, medium and high risk levels respectively. When a high risk level is detected, the system will trigger a preventive control strategy adjustment.

[0040] In this embodiment, by introducing time decay factor compensation and multi-level threshold classification, both the dynamic response characteristics of the current loop and the quantitative grading evaluation of future risks are considered, so that the possible current saturation risk level can be identified in advance, and grading warning information is provided for subsequent optimized control. Through dynamic compensation of prediction data error and grading risk evaluation, false positives or false negatives caused by single threshold judgment are avoided, and different risk levels are matched with differentiated control strategies, which maximizes the maintenance of trajectory tracking performance while ensuring system safety.

[0041] In a feasible implementation manner, reference is made to Figure 5, step S330 includes steps S331-S333, wherein: Step S331, when the current oversaturation degree data is less than a preset first threshold value, output low risk level identification data; Step S332, when the current oversaturation degree data is greater than the preset first threshold value and less than a preset second threshold value, output medium risk level identification data; Step S333, when the current oversaturation degree data is greater than the preset second threshold value, output high risk level identification data and corresponding joint identification; wherein the preset first threshold value is less than the preset second threshold value.

[0042] In this embodiment, the current oversaturation degree data refers to the difference between the predicted current demand and the maximum allowable current, which can be realized by a real-time difference calculation module and is used to quantify the degree of current exceeding the safe range. The preset first threshold value refers to the critical value for triggering low risk level judgment, which can be specifically set to 10%-20% of the maximum allowable current and is used to identify slight over-limit working conditions. The preset second threshold value refers to the critical value for triggering high risk level judgment, which can be specifically set to 30%-50% of the maximum allowable current and is used to identify serious over-limit working conditions. The joint identification refers to the joint number information of the joint that occurs high risk saturation, which can be realized by an address encoder and is used to accurately locate the abnormal joint.

[0043] In this embodiment, during the high-speed motion of the mechanical arm, when it is predicted that the current demand of a certain joint in the future control period exceeds its maximum allowable current, the absolute difference between the two is first calculated as the oversaturation degree. If the difference is below the first threshold value, only a low risk level is marked and the original control strategy is maintained; if the difference is between the first and second threshold values, a medium risk level is marked and a current compensation algorithm is started; if the difference exceeds the second threshold value, a high risk identification and joint number are immediately output, triggering an emergency constraint mechanism. For example, when the mechanical arm executes a fast circular arc trajectory, the second joint causes the predicted current demand to reach 1.4 times the maximum allowable current due to load mutation, at which time the oversaturation degree exceeds the second threshold value, the system immediately generates a high risk identification and locks the joint number, providing accurate fault positioning for subsequent optimization.

[0044] In this embodiment, two threshold values are set to realize three-level risk division, maintain control performance in low risk, start compensation mechanism in medium risk, and accurately locate problem joints and apply strong constraints in high risk, so as to accurately identify different risk levels before current saturation occurs and take targeted hierarchical control strategies. In the low risk stage, the trajectory tracking accuracy is maintained, in the medium risk stage, the current demand is reduced through dynamic compensation, and in the high risk stage, the problem joint is quickly located and motion constraint is applied, so as to maximize the motion performance while ensuring the safety of the motion of the mechanical arm.

[0045] In a feasible implementation, referenceFigure 6 Step S400 includes steps S410-S420, in which: Step S410, for the joint identified with a high risk level, a position rate of change limit is added in the dynamics model constraint, which is realized by setting a maximum allowed acceleration constraint; Step S420, for the joint identified with a low risk level, a position tracking error weight thereof is increased in the optimization objective.

[0046] In this embodiment, according to the above description, the joint identified with a high risk level refers to the joint whose difference between the predicted current demand and the maximum allowed current exceeds the second threshold value, which can be realized by comparing the predicted current demand value with the preset threshold value in real time, and its role is to identify the joint that may have serious current saturation in advance. The position rate of change limit refers to the hard constraint on the acceleration of the joint, which can be specifically limited by the upper limit value of the acceleration term in the dynamics equation, and is used to prevent the high-risk joint from causing the current to exceed the physical limit due to excessive acceleration demand. The joint identified with a low risk level refers to the joint whose predicted current demand value does not exceed the first threshold value, which is specifically realized by threshold value classification, and its role is to identify the joint that still has current margin. The position tracking error weight refers to the coefficient of the joint position deviation term in the optimization objective function, which can be specifically realized by adjusting the corresponding element of the weight matrix in the quadratic programming problem, and is used to improve the trajectory tracking accuracy of the low-risk joint.

[0047] In this embodiment, when constructing the rolling optimization problem, differential processing is implemented according to the current saturation risk level of each joint. For the high-risk joint, an acceleration upper limit is embedded in the dynamics constraint condition, for example, the acceleration term in the dynamics equation is limited to not more than 80% of the rated value; for the low-risk joint, the weight coefficient of the position error term in the objective function is increased to 1.5 times of the standard value. In the optimization solving process, the acceleration limit of the high-risk joint directly acts on the constraint condition matrix, and the error weight adjustment of the low-risk joint is reflected in the Hessian matrix of the objective function, thereby realizing dynamic adjustment of the optimization strategy.

[0048] In this embodiment, precise local constraint adjustment is realized through risk level division, and based on real-time risk assessment and optimization parameter linkage, the current margin of the low-risk joint is fully utilized to improve the trajectory accuracy while ensuring that the high-risk joint does not exceed the limit, which can effectively balance the current constraint and trajectory tracking performance in the multi-joint cooperative motion scene. For the high-risk joint, the acceleration limit avoids the torque jump and vibration caused by current saturation; for the low-risk joint, the error weight increase can reduce the cumulative trajectory deviation, for example, in the circular arc trajectory motion, the cooperative error between joints can be reduced, thereby improving the smoothness of the end trajectory.

[0049] In a feasible implementation manner, referring to Figure 7, step S500 includes steps S510-S530, wherein: Step S510, converting the rolling optimization problem data packet into a standard quadratic programming matrix; Step S520, processing the standard quadratic programming matrix by an active set iteration algorithm to obtain a numerical solution of the optimization position instruction sequence; Step S530, performing instruction smoothing filter processing on the numerical solution of the optimization position instruction sequence to output an optimization position instruction sequence satisfying the continuity of the manipulator motion.

[0050] In this embodiment, the standard quadratic programming matrix refers to the standard mathematical expression form of converting the trajectory tracking error minimization objective function and the constraint condition into a quadratic programming, which can be realized by converting the objective function into a quadratic form matrix and converting the constraint condition into a linear inequality matrix. The function is to convert the complex multivariable optimization problem into a mathematical form that can be efficiently solved. The active set iteration algorithm refers to a numerical calculation method for solving the constrained quadratic programming problem, which is realized by dynamically identifying the active constraint set and iteratively updating the candidate solution. The function is to accurately process the current limit of the current and the dynamic model constraint while ensuring the calculation efficiency. The instruction smoothing filter processing refers to filtering the optimized position instruction to eliminate the sudden signal, which can be realized by using a moving average filter or a low-pass filter. The function is to ensure the stability of the manipulator motion by suppressing high-frequency noise.

[0051] In this embodiment, after the rolling optimization problem data packet is converted into a standard quadratic programming matrix, the active set iteration algorithm dynamically identifies the current active constraint set and updates the candidate solution, gradually approaching the optimal solution that satisfies all constraint conditions. In each iteration, the algorithm prioritizes processing the constraint conditions corresponding to the joints with high risk of causing current saturation, thereby quickly adjusting the optimization position instruction. Subsequently, the numerical solution obtained by solving is subjected to instruction smoothing filter processing, and the filter strength is adjusted by adjusting the filter coefficient, so that the optimized position instruction can satisfy the current constraint and avoid mechanical vibration caused by sudden change of the instruction.

[0052] In this embodiment, by combining multi-period prediction with quadratic programming, the joint optimization of current constraint and trajectory tracking is realized, effectively reducing the control delay. In addition, the gradient descent method is often used in the prior art for optimization solution, which has low calculation efficiency and is difficult to process sudden constraints. The active set iteration algorithm can dynamically adjust the active constraint set, significantly improve the solving speed, so as to quickly generate an optimization position instruction that satisfies the current constraint in a multi-axis cooperative motion scene, avoid torque mutation and trajectory deviation caused by current saturation. At the same time, the high-frequency noise in the optimization instruction is eliminated by the instruction smoothing filter processing, ensuring the continuous and smooth motion of the manipulator, reducing the risk of mechanical vibration, thereby improving the control accuracy and system reliability in high dynamic tasks.

[0053] In an implementable embodiment, referring to Figure 8 , step S530 comprises steps S531-S533, in which: In step S531, a filter coefficient is set based on a mechanical arm rated acceleration parameter; In step S532, the optimized position instruction sequence is subjected to a smoothing filter processing based on the filter coefficient; In step S533, the optimized position instruction sequence after the smoothing filter processing is subjected to a slope check to output an optimized position instruction sequence satisfying mechanical arm motion continuity.

[0054] In the embodiment, the filter coefficient refers to a parameter for adjusting the smoothing degree of the instruction sequence, which can be specifically set by using a rated acceleration parameter of a mechanical arm joint, for example, a ratio of the rated acceleration to the square of the sampling period is taken as a filter coefficient reference value. The parameter setting manner can ensure that the change rate of the filtered instruction does not exceed the bearing capacity of the mechanical structure. The smoothing filter processing refers to a low-pass filter operation on the discrete optimized instruction sequence, which can be specifically implemented by using a moving average filter or a Butterworth filter, and is used to eliminate possible stepwise mutations in the quadratic programming solving process. The slope check refers to verifying whether the difference between adjacent points of the filtered instruction sequence exceeds the allowed range, which can be specifically implemented by calculating the ratio of the position difference between adjacent instruction points to the time interval, and comparing it with a preset maximum allowed slope threshold, thereby avoiding introducing new mutation points due to the filter processing.

[0055] In the embodiment, after the discrete optimized position instruction sequence is obtained by quadratic programming solving, the filter coefficient is first determined according to the rated acceleration parameters of the joints of the mechanical arm. For example, for a joint with a rated acceleration of 5 m / s², the filter coefficient can be set to 0.005 under a 1 ms control period. Subsequently, the instruction sequence is subjected to a sliding window average processing by using the coefficient, so that the acceleration change between adjacent instruction points tends to be gentle. After the filter processing is completed, the processed instruction sequence is subjected to a point-by-point check, and the deviation of the position instruction increment in each control period from the current actual position is calculated. When it is found that the increment of a certain point exceeds a preset threshold, the instruction value of the point is automatically adjusted to satisfy the maximum allowed jump range, thereby ensuring that the position instruction received by the driver not only meets the optimization target but also has actual executability.

[0056] In this embodiment, the filter coefficient setting with the rated acceleration constraint ensures that the command smoothness matches the dynamic characteristics of the mechanical system, and the double protection mechanism formed by combining the slope check effectively suppresses mechanical impact while maintaining trajectory tracking accuracy, so as to eliminate high-frequency jitter components in the optimized command sequence, avoid the end-of-arm tremor phenomenon caused by the mutation of the discrete optimization solution, and ensure smooth connection between the smoothed command and the real-time position state through the dynamic check mechanism, thereby significantly improving the motion stability and control safety under heavy load and variable direction conditions.

[0057] In a feasible implementation, when the manipulator is in a heavy load and variable direction condition, before step S400, the method further includes steps S610-S620, wherein: Step S610, identifying the active variable direction joint based on historical motion data and obtaining corresponding dynamic load inertia data; Step S620, proportionally increasing the maximum allowable current data of the corresponding joint according to the dynamic load inertia data.

[0058] In this embodiment, the heavy load and variable direction condition refers to a working state in which the load mass at the end of the manipulator exceeds the rated value and the motion direction changes abruptly, which can be triggered and judged by the acceleration change rate threshold of the end effector. The active variable direction joint refers to the joint that undertakes the main motion direction adjustment task in this condition, which can be identified by analyzing the correlation between the joint angular velocity change and the end trajectory curvature. The dynamic load inertia data refers to the equivalent moment of inertia change value caused by load inertia, which can be estimated by the product of the load mass parameter and the joint acceleration. The proportional increase of the maximum allowable current data refers to the linear expansion of the current upper limit according to the load inertia increase, which can be realized by using a predefined proportional coefficient for dynamic adjustment.

[0059] In this embodiment, when the manipulator performs a heavy load and variable direction action, the second derivative of the angular velocity of each joint is calculated by analyzing the joint motion trajectory in the past several control periods, and the joint with a sudden change in angular acceleration exceeding a set threshold is selected as the active variable direction joint. For the identified joint, the dynamic load inertia is calculated based on the current acceleration data and the load mass parameter, for example, when the load mass is 5 kg and the instantaneous acceleration of the joint reaches 20 rad / s², the dynamic load inertia can be estimated to be 100 kg·m². According to the ratio of this inertia value to the reference inertia, for example, when the inertia is increased by 1.5 times, the maximum allowable current of the corresponding joint is temporarily increased to 1.2 times the original value, thereby providing current margin for sudden load changes.

[0060] In some specific embodiments, the historical motion data can be selected from the joint position sequence in the last three control periods for differential calculation, and the acceleration characteristics can be extracted after fitting the joint motion curve by cubic spline interpolation. The dynamic load inertia calculation can be combined with the end force sensor data and the Jacobian matrix for inverse dynamics compensation, for example, when the end measures 10N of inertial force, the equivalent moment of joint is calculated by the transpose of the Jacobian matrix. The adjustment ratio of the maximum allowable current can set a nonlinear mapping relationship, for example, when the load inertia exceeds twice the rated value, an exponential growth strategy is started.

[0061] In this embodiment, by dynamically identifying the joint with the most severe load change and increasing the current upper limit of the joint, the motion control bandwidth is effectively expanded under the premise of ensuring system safety, and energy waste caused by global increase of current upper limit is avoided. In this way, the application can accurately identify the joint with the most significant load mutation and dynamically adjust the current capacity of the joint when the robot arm performs heavy load change action, which not only prevents the amplification of trajectory deviation caused by current saturation, but also avoids the loss of motion performance caused by conservative current limit. This scheme is particularly suitable for scenarios with sudden load changes in handling operations, for example, when grabbing objects of unknown mass, the driving capacity can be adjusted adaptively to maintain trajectory accuracy.

[0062] In this embodiment, the drive control integrated control method is applied to a robot arm, which predicts future current demand, evaluates current saturation risk, and builds a rolling optimization problem to dynamically adjust joint position instructions, avoid current mutation, and realize multi-axis collaborative optimization, thereby improving trajectory tracking accuracy, reducing mechanical vibration, and realizing dynamic current optimization.

[0063] It should be noted that the above examples are only used to understand the application and do not constitute a limitation on the drive control integrated control method of the application. Further simple transformations based on this technical concept are within the scope of protection of the application.

[0064] The application also provides a drive control integrated control system 100, referring to Figure 10 , the drive control integrated control system 100 comprises: The data acquisition module 110 is configured to generate a desired trajectory of the end of the robot according to an externally input task instruction, and acquire desired position data and desired speed data of each joint in the current control period and a plurality of future control periods based on the desired trajectory of the end of the robot. The actual position data, actual speed data, and actual current data output to the motor of each joint of each joint encoder are also acquired. The current demand prediction module 120 is connected with the data acquisition module 110 and is configured to acquire current demand prediction data of each joint in the plurality of future control periods based on the desired position data, the desired speed data, the actual position data, the actual speed data, and the actual current data. The current saturation risk evaluation module 130 is connected with the current demand prediction module 120, and is used for calculating real-time current margin data of each joint based on the actual current data and pre-stored maximum allowable current data of each joint; and evaluating the current saturation risk of each joint in a plurality of future control periods in combination with the current demand prediction data. The joint position instruction optimization module 140 is connected with the current saturation risk evaluation module 130, and is used for constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data and the current saturation risk, wherein the optimization variable is an optimized position instruction of each joint, and the constraint condition includes a current upper limit constraint and a dynamics model constraint. The output control module 150 is connected with the joint position instruction optimization module 140, and is used for performing quadratic programming solution on the rolling optimization problem data packet, outputting an optimized position instruction sequence in a plurality of future control periods, and extracting the optimized position instruction data in the first period of the optimized position instruction sequence as a current control instruction and outputting to a joint driver.

[0065] In the embodiment, the data acquisition module 110 is a combination of hardware and software for real-time acquisition of the motion state and control instruction of the robot arm, and can be implemented by using an FPGA chip with high-speed communication capability, and is used for synchronously acquiring encoder feedback signals and planned trajectory data. The current demand prediction module 120 is a current estimation unit based on a dynamics model and friction compensation, and can be implemented by using a recurrent neural network algorithm, and is used for predicting the current demand of each joint in a plurality of future control periods in advance. The current saturation risk evaluation module 130 is a dynamic evaluation unit combining real-time current margin and predicted demand, and can be implemented by using a sliding window statistical method, and is used for quantifying the possibility of current overrun of each joint. The joint position instruction optimization module 140 is a calculation unit for constructing and solving a multi-constraint optimization problem, and can be implemented by using a quadratic programming solver based on Lagrange multipliers, and is used for generating optimized instructions considering tracking accuracy and current constraints. The output control module 150 is an execution unit for converting the optimized results into driving signals, and can be implemented by using a DAC circuit with filtering function, and is used for ensuring the smoothness and real-time performance of the instruction output.

[0066] In this embodiment, the data acquisition module 110 decomposes the end desired trajectory into the desired position and velocity sequence of each joint through inverse kinematics calculation, and synchronously collects the actual position, velocity and actual current value of the current loop output of the encoder feedback through the high-speed bus. The current demand prediction module 120 calculates the joint acceleration demand according to the dynamics model, estimates the torque demand in combination with the friction compensation model, and then converts it into the current prediction value through the motor parameters. The current saturation risk assessment module 130 performs time sequence comparison between the predicted current and the preset maximum current, and identifies the joints that may continuously exceed the limit through sliding window statistics. The joint position instruction optimization module 140 applies acceleration constraints to high-risk joints, and increases tracking weights for low-risk joints, and constructs a multi-objective optimization model containing current constraints and dynamics equations. The output control module 150 adopts quadratic programming to obtain future multi-period optimization instructions, which are output to the driver after slope limit filtering, so as to ensure that the instruction jump is within the allowable range of the mechanical system.

[0067] The drive-control integrated control system 100 provided by the present application adopts the drive-control integrated control method in the above embodiment, and can improve trajectory tracking accuracy, reduce mechanical vibration and realize dynamic current optimization. Compared with the prior art, the drive-control integrated control system 100 provided by the present application has the same beneficial effects as the drive-control integrated control method provided by the above embodiment, and other technical features in the drive-control integrated control system 100 are the same as the features disclosed in the above embodiment method, which will not be repeated here.

[0068] The above only describes some embodiments of the present application, and does not limit the patent scope of the present application. Any equivalent structural transformation, direct / indirect application in other related technical fields based on the technical concept of the present application, and the content of the present application specification and drawings are included in the patent protection scope of the present application.

Claims

1. A drive-control integrated control method, applied to a robotic arm, characterized in that, The integrated control method comprises the following steps: According to the task instruction inputted from outside, the end-of-arm desired trajectory of the manipulator is generated, and the desired position data and the desired speed data of each joint in the current control period and in the future multiple control periods are obtained based on the end-of-arm desired trajectory of the manipulator; meanwhile, the actual position data, the actual speed data of each joint encoder and the actual current data output to each joint motor are obtained; Based on the desired position data, the desired speed data, the actual position data, the actual speed data and the actual current data, the current demand prediction data of each joint in the future multiple control periods is obtained; Based on the actual current data and the pre-stored maximum allowable current data of each joint, the real-time current margin data of each joint is calculated; meanwhile, in combination with the current demand prediction data, the current saturation risk of each joint in the future multiple control periods is evaluated; Based on the desired position data, the current demand prediction data and the current saturation risk, a rolling optimization problem data package is constructed, with the minimum trajectory tracking error as the target, wherein the optimization variable is the optimized position instruction of each joint, and the constraint conditions include the current upper limit constraint and the dynamic model constraint; The rolling optimization problem data package is solved by quadratic programming, and the optimized position instruction sequence in the future multiple control periods is output, and the optimized position instruction data in the first period of the optimized position instruction sequence is extracted as the current control instruction and output to the joint driver.

2. The drive-integrated control method of claim 1, wherein The step of obtaining the current demand prediction data of each joint in the future multiple control periods based on the desired position data, the desired speed data, the actual position data, the actual speed data and the actual current data comprises the following steps: The desired speed data is subjected to time difference calculation to obtain the desired acceleration data of each joint; Based on the preset joint dynamic model library, the rotational inertia data of the corresponding joint is called, and the friction torque data is obtained based on the actual speed data; Based on the desired acceleration data, the rotational inertia data and the friction torque data, the predicted required torque data is obtained through Newton-Euler equation calculation processing; Based on the preset motor torque constant, the predicted required torque data is converted into current demand prediction data through proportional conversion processing.

3. The drive-integrated control method of claim 2, wherein The step of estimating the friction torque data based on the actual speed data comprises the following steps: Based on the actual speed data, the speed direction identification data is obtained through sign function processing; The pre-stored Coulomb friction coefficient and viscous friction coefficient of the joint are called; The Coulomb friction coefficient is multiplied by the speed direction identification data, and the product of the viscous friction coefficient and the actual speed data is added to obtain the friction torque data.

4. The integrated control method of claim 1, wherein The step of evaluating the current saturation risk of each joint in the future multiple control periods in combination with the current demand prediction data comprises the following steps: Based on the current demand prediction data, the compensated current demand data is obtained through time decay factor compensation processing; the time decay factor is set according to the current loop response bandwidth. comparing the compensated current demand data with the maximum allowed current data, and when the compensated current demand data exceeds the maximum allowed current, obtaining a current oversaturation degree by difference calculation; based on the current oversaturation degree, outputting a multi-level current saturation risk through threshold grading processing.

5. The integrated control method of claim 4, wherein The current saturation risk includes a low risk level, a medium risk level and a high risk level, and the step of outputting a multi-level current saturation risk based on the current oversaturation degree through threshold grading processing includes: when the current oversaturation degree data is less than a preset first threshold value, outputting low risk level identification data; when the current oversaturation degree data is greater than the preset first threshold value and less than a preset second threshold value, outputting medium risk level identification data; when the current oversaturation degree data is greater than the preset second threshold value, outputting high risk level identification data and corresponding joint identification; wherein the preset first threshold value is less than the preset second threshold value.

6. The drive-integrated control method of claim 5, wherein, The step of constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data and the current saturation risk includes: for the joint identified with the high risk level, increase the position change rate limit in the dynamics model constraint, and realize it by setting the maximum allowed acceleration constraint; for the joint identified with the low risk level, increase its position tracking error weight in the optimization target.

7. The drive-integrated control method of claim 1, wherein The step of solving the rolling optimization problem data packet by quadratic programming to output an optimized position instruction sequence of multiple control cycles in the future includes: convert the rolling optimization problem data packet into a standard quadratic programming matrix; process the standard quadratic programming matrix through an active set iteration algorithm to obtain a numerical solution of the optimized position instruction sequence; perform instruction smoothing filtering processing on the numerical solution of the optimized position instruction sequence to output an optimized position instruction sequence that meets the continuity of the robot motion.

8. The drive-integrated control method of claim 1, wherein, The step of performing instruction smoothing filtering processing on the numerical solution of the optimized position instruction sequence to output an optimized position instruction sequence that meets the continuity of the robot motion includes: set a filtering coefficient based on the rated acceleration parameter of the robot; perform smoothing filtering processing on the optimized position instruction sequence based on the filtering coefficient; perform slope verification on the optimized position instruction sequence after smoothing filtering processing to output an optimized position instruction sequence that meets the continuity of the robot motion.

9. The drive-integrated control method of claim 1, wherein, When the robot is in a heavy load and direction changing condition, before the step of constructing a rolling optimization problem data packet with the minimum trajectory tracking error as the target based on the expected position data, the current demand prediction data and the current saturation risk, wherein the optimization variable is the optimized position instruction of each joint, and the constraint condition includes the current upper limit constraint and the dynamics model constraint, the method further includes: identify the active direction changing joint based on the historical motion data and obtain the corresponding dynamic load inertia data; according to the dynamic load inertia data, proportionally increase the maximum allowed current data of the corresponding joint.

10. An integrated control system for driving a control system, characterized by The drive and control integrated control system includes: The data acquisition module is configured to generate an end-of-robot manipulator desired trajectory according to an externally input task instruction, and acquire joint desired position data and joint desired speed data for a current control period and a plurality of future control periods based on the end-of-robot manipulator desired trajectory; and simultaneously acquire actual position data, actual speed data of joint encoders, and actual current data output to joint motors; The current demand prediction module is configured to acquire joint current demand prediction data in the plurality of future control periods based on the joint desired position data, the joint desired speed data, the actual position data, the actual speed data, and the actual current data; The current saturation risk evaluation module is configured to calculate joint real-time current margin data based on the actual current data and pre-stored joint maximum allowable current data, and simultaneously evaluate joint current saturation risks in the plurality of future control periods in combination with the current demand prediction data; The joint position instruction optimization module is configured to construct a rolling optimization problem data packet with the minimum trajectory tracking error as a target based on the joint desired position data, the current demand prediction data, and the current saturation risks, wherein an optimization variable is an optimized joint position instruction, and constraint conditions include a current upper limit constraint and a dynamics model constraint; The output control module is configured to perform quadratic programming solving on the rolling optimization problem data packet, output an optimized position instruction sequence for the plurality of future control periods, and extract optimized position instruction data in a first period of the optimized position instruction sequence as a current control instruction and output the current control instruction to a joint driver.

Images