A heavy-load robot arm trajectory tracking and control method and system under dynamic working conditions

Abstract

The application belongs to the field of industrial automation and mechanical control, and discloses a heavy-load mechanical arm trajectory tracking and control method and system under dynamic working conditions, which comprises dynamic parameter identification, multi-model predictive control and fault-tolerant control, real-time acquisition of mechanical arm motion state data, real-time updating and optimization of the dynamic parameters of the mechanical arm by combining the recursive least square method and the particle swarm optimization algorithm, construction of a dynamic prediction model set under different dynamic load working conditions and motion states, dynamic selection of the optimal model and adjustment of the mechanical arm trajectory tracking strategy by using the model predictive control algorithm, real-time monitoring of the mechanical arm motion state, simultaneous fault detection and diagnosis, activation of redundant control once a fault is detected, dynamic adjustment of the control strategy by combining the redundant degrees of freedom of the mechanical arm and the distributed control architecture, maintenance of the normal operation of the system, and improvement of the robustness and reliability of the system to adapt to the requirements of complex operation scenarios.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of industrial automation and mechanical control, and particularly relates to a heavy-load mechanical arm trajectory tracking and control method and system under dynamic working conditions. BACKGROUND

[0002] With the rapid development of industrial automation and intelligent technology, heavy-load mechanical arms are increasingly widely used in complex work scenarios, such as maintenance of nuclear fusion equipment, material handling of oil drilling platforms, heavy industrial assembly, etc. However, under dynamic working conditions, the trajectory tracking and control of such mechanical arms still face many technical difficulties, mainly in the following aspects:

[0003] (1) Insufficient real-time and accuracy of dynamic parameter identification: The dynamic model parameters of heavy-load mechanical arms, such as inertia matrix and friction coefficient, are difficult to accurately describe due to the influence of dynamic load changes, nonlinear disturbances and environmental factors during the work process. Existing dynamic parameter identification methods mostly rely on static models or offline optimization, which cannot meet the demand for real-time parameter update under dynamic working conditions, resulting in a decline in control performance.

[0004] (2) Trajectory tracking accuracy affected by complex working conditions: Nonlinear disturbances, external load changes and coupling characteristics of the system under dynamic working conditions all interfere with the trajectory tracking of the mechanical arm. The robustness of the current mainstream control strategies, such as traditional PID control and model-based optimization control, is limited when dealing with complex working conditions, and they cannot balance high precision and fast response of trajectory tracking.

[0005] (3) Insufficient fault-tolerant control capability: In long-term high-intensity work, the actuators and sensors of the mechanical arm are prone to local faults. Existing fault-tolerant control techniques mostly use simple redundancy strategies, which cannot fully utilize the redundant degrees of freedom of the mechanical arm, making it difficult to quickly respond to faults and maintain system stability under complex working conditions, affecting work efficiency and safety.

[0006] Based on the above problems, there is an urgent need for a comprehensive solution that can achieve real-time dynamic parameter identification, high-precision trajectory tracking control and fault-tolerant control under dynamic working conditions, to meet the demand for high-precision and high-reliability work of heavy-load mechanical arms in complex scenarios. SUMMARY

[0007] In view of the problems of poor real-time dynamic parameter identification, trajectory tracking accuracy affected by disturbances, and insufficient fault-tolerant control capability of heavy-load mechanical arms under dynamic working conditions, a comprehensive technical solution integrating dynamic parameter identification, multi-model predictive control and fault-tolerant control is proposed to improve the robustness and reliability of the system and meet the demand of complex work scenarios.

[0008] To achieve the above object, according to one aspect of the present application, a heavy-load robot arm trajectory tracking and control method under dynamic working conditions is provided, comprising the following steps:

[0009] S1 Kinematics parameter identification: real-time acquisition of robot arm motion state data, combination of recursive least square method and particle swarm optimization algorithm, real-time update and optimization of robot arm kinematics parameters, and provision of high-precision parameter support for subsequent control;

[0010] S2 Multi-model predictive control: based on the kinematics parameters provided by step S1 kinematics parameter identification, a set of dynamic prediction models under different dynamic load working conditions and motion states is constructed, the optimal model is dynamically selected, and the model predictive control algorithm is used to adjust the robot arm trajectory tracking strategy, the control input is real-time rolling optimization, the trajectory tracking error is minimized, and the stability and efficiency of trajectory execution are improved;

[0011] S3 Fault-tolerant control: after control and optimization by step S2 multi-model predictive control, the robot arm motion state is continuously monitored in real time, fault detection and diagnosis are simultaneously performed, and once a fault is detected, redundant control is enabled, the control strategy is dynamically adjusted in combination with the redundant degrees of freedom of the robot arm and the distributed control architecture, and the normal operation of the system is maintained.

[0012] As a preferred, the data real-time acquisition device in step S1 is a plurality of sensors installed on the robot arm, the sensors including position encoders, torque sensors and acceleration sensors, for real-time acquisition of displacement, speed, acceleration and torque of each joint of the robot arm;

[0013] The sampling frequency of each sensor is set to 100 Hz to ensure the real-time and accuracy of the data; at the same time, an adaptive filter is introduced to process the sensor data, to reduce the influence of dynamic load changes and measurement noise on the parameter identification accuracy;

[0014] The kinematics parameters include inertia matrix, friction coefficient and gravity compensation parameters.

[0015] As a preferred, according to the kinematics model of the robot arm, the motion equation in step S1 is:

[0016]

[0017] Wherein, τ(t) is the joint torque vector, M(θ) is the inertia matrix, θ is the position matrix of the joint, is the velocity matrix of the joint, is the acceleration matrix of the joint, is the Coriolis and centrifugal force matrix, G(θ) is the gravity matrix, is the friction torque;

[0018] The formula of the recursive least square method in step S1 is:

[0019]

[0020] where θ k is the parameter estimation at current time step k, P k is the covariance matrix, x k is the input data vector, y k is the measurement output at current time step k.

[0021] By continuously updating the kinetic parameters θ k , the system can adapt to dynamic load changes and improve the accuracy of parameter identification.

[0022] The objective function of the particle swarm optimization algorithm in step S1 is:

[0023]

[0024] where p is the parameter vector to be optimized, is the predicted output based on the current parameters, y i is the actual measurement value, and n is the sample number.

[0025] The particle swarm optimization algorithm optimizes this objective function to achieve global optimization of the kinetic parameters, thereby improving the identification accuracy of the system.

[0026] As a preferred embodiment, step S2 specifically includes the following steps:

[0027] S21 Constructing a multi-model system: based on the dynamic model of the robot arm, constructing sub-dynamic models M1(θ), M2(θ), …, M n (θ) under different dynamic load conditions and motion states, and the multiple sub-models form a multi-model system; each sub-dynamic model is trained under different load conditions and adapts to different working environments through different parameters, ensuring that the control system can adapt to complex working conditions.

[0028] S22 Model selection mechanism: in each control cycle, the multi-model system of step S21 selects the most suitable sub-dynamic model for control according to the current load and working state.

[0029] S23 Model predictive control: after selecting the optimal sub-dynamic model in step S22, the model predictive control algorithm is used to solve the optimal robot arm trajectory tracking strategy and calculate the control input in real time.

[0030] As a preferred embodiment, the principle of selecting the sub-dynamic model in step S22 is:

[0031]

[0032] Among them, y m The measurement data is at the current moment, u is the control input, and M is the control input. s The optimal model selected;

[0033] The model predictive control algorithm in step S23 is as follows:

[0034]

[0035] Among them, y k For the output at the current prediction time, y ref,k Let u be the reference trajectory, λ be the regularization parameter, and u be the reference trajectory. k To control the input.

[0036] Preferably, step S3 specifically includes the following steps:

[0037] S31 Fault Detection and Diagnosis: Based on multi-sensor fusion technology, it monitors the status of the robotic arm actuators and sensors in real time, estimates the status of the robotic arm under normal conditions through time series analysis, detects whether there are any abnormalities, distinguishes different fault types of the robotic arm through Bayesian inference algorithm, and detects and locates faults.

[0038] The time series was modeled using an autoregressive integral moving average model, and its modeling equation is as follows:

[0039]

[0040] Where: y t φ is the current observation value; c is the constant term; p is the autoregression order, which determines how many past time steps have influenced the current state; q is the moving average order, which determines the influence of past error terms; φ is the value at the current moment. i and θ j These are model coefficients; ∈ t It is a noise term;

[0041] The predicted output value for future time steps is obtained through an autoregressive integral moving average model. Compare with the actual measured value y m If the residual exceeds the set threshold, a fault is considered to exist; therefore, the fault detection formula is:

[0042]

[0043] Among them, y r Let y be the residual value. m These are actual measured values. For predicting output;

[0044] If the absolute value of the residual exceeds a certain threshold, it is considered that an anomaly has occurred, and further fault diagnosis is performed using a Bayesian inference algorithm. The Bayesian inference calculates the posterior probability P(H i | D) corresponding to the fault, which is calculated as follows:

[0045]

[0046] where n is the number of different faults of the robot arm; H i represents different fault types; D represents observed abnormal data; P(H i ) is the prior probability, representing the historical occurrence rate of a certain fault; P(D|H i ) is the likelihood probability, representing the possibility of abnormal data under a certain fault;

[0047] According to the calculated posterior probability P(H i | D), the fault type with the highest probability is selected to determine whether the anomaly belongs to a fault and to infer the type of the fault;

[0048] S32 Redundancy control and fault recovery: Once a fault is detected in step S31, redundancy control is enabled, and a dynamic adjustment method of distributed redundancy control is used to achieve optimal control input distribution, so that the redundant degrees of freedom of the heavy-duty robot arm can effectively compensate after a fault occurs, maintaining the stability and control performance of the system. At the same time, for critical task, set fault-tolerant priority, ensure the efficient completion of system task.

[0049] As preferred, the dynamic adjustment method of distributed redundancy control in step S32 is specifically:

[0050] Assuming that the robot arm has m redundant joints, the redundant degree of freedom control optimization formula is:

[0051]

[0052] where u j is the redundant control input; y k is the current joint state of the robot arm at time k; y ref,k is the reference trajectory; and N is the control time window length.

[0053] Suppose the fault occurs at the jth joint, and the control input of this joint is set to zero. Then the torque balance equation of the entire system is rewritten as:

[0054]

[0055] where τ res represents an additional error term due to the fault.

[0056] To maintain the stable operation of the system, the control input of the remaining joints needs to be adjusted to compensate τ res , i.e., to satisfy the following optimization objective:

[0057]

[0058] where θ ref,k is the reference trajectory, λ is the regularization parameter to control the smoothness of the input, and τ i is the input of the compensation joint;

[0059] The constraint condition is introduced, i.e., the Jacobian matrix J and the pseudo-inverse control method:

[0060]

[0061] where J is the Jacobian matrix of the heavy manipulator, representing the mapping relationship from the joint space to the task space;

[0062] After the fault occurs, the control input of part of the joints is limited, and the optimization problem is rewritten as:

[0063]

[0064] where J + represents the generalized inverse (pseudo-inverse) matrix, and J j is the Jacobian matrix corresponding to the fault joint;

[0065] By utilizing the remaining redundant degrees of freedom to redistribute the control input, the trajectory tracking accuracy is maintained as much as possible;

[0066] In the process of redundancy control, the saturation constraint of the actuator needs to be further considered to avoid new faults caused by the compensation overload of the remaining joints, and the torque constraint condition is introduced:

[0067] τ min ≤τ opt ≤τ max

[0068] where τ min and τ max are the minimum and maximum allowed torques of the actuator, respectively;

[0069] When the calculated τ opt exceeds this range, it needs to be corrected by the quadratic programming method to minimize the impact on the overall motion of the system:

[0070] s.t.τ min ≤τ≤τ max

[0071] By Lagrange multiplier method, the optimal control input satisfying the constraint condition is obtained.

[0072] As a preferred, the fault-tolerant priority adjustment process in step S32 is to define a task error function:

[0073]

[0074] wherein M represents the number of tasks currently to be executed, w i represents the priority weight of the i-th task, θ ref,i represents the current joint state of task i, θ ref,i represents the target trajectory thereof;

[0075] A fault severity weight γ j is introduced to represent the fault degree of each joint:

[0076]

[0077] wherein d j represents the fault influence degree of joint j, d thr is the threshold of fault influence, and β is the parameter of control fault weight variation rate;

[0078] In combination with the task error weight w i and the fault severity weight γ j , the adjusted control input optimization problem is represented as:

[0079]

[0080] wherein the first term ensures the minimization of task error, and the second term is used to regularize the influence of fault on control input, so as to prevent the damaged joint from being overused;

[0081] In the optimization process, the task priority weight w i needs to be dynamically adjusted according to the influence of fault, wherein the weight update amount of task i is Δw i , and the update formula is:

[0082]

[0083] wherein e i is the current error of task i, e max,i is the maximum error allowed by the task, and α is the weight adjustment step;

[0084] When the task error is large, the system will automatically increase the weight of the task, so that the task obtains more redundant control resources in the optimization process, and when the task error is small, the weight of the task is reduced, so as to release the control resources for other tasks;

[0085] Further, in order to make the weight adjustment more smooth, avoid mutation leading to system instability, an exponential weighted moving average is used for smooth updating:

[0086]

[0087] wherein β is a smoothing factor, controlling the dynamic response speed of weight adjustment; a larger β value means slower weight adjustment to ensure system stability, while a smaller β value allows faster weight adjustment to adapt to sudden failure conditions;

[0088] After the failure occurs, not only the priority of the task needs to be adjusted, but also the allocation of redundant control resources needs to be optimized, and then the final optimization problem is represented as:

[0089]

[0090] wherein the third term is used to regularize the control input of the redundant degree of freedom, and λ is a regularization parameter;

[0091] The optimal redundant control input is obtained by solving the quadratic programming method.

[0092] To achieve the above purpose, according to one aspect of the present application, a heavy-duty robot arm trajectory tracking and control system under dynamic working conditions is provided, comprising:

[0093] A dynamics parameter identification module, comprising a plurality of sensors arranged on the robot arm, the plurality of sensors real-time collect robot motion state data, and combine the recursive least squares method and the particle swarm optimization algorithm to real-time update and optimize the dynamics parameters of the robot arm, providing high-precision parameter support for subsequent control;

[0094] A multi-model predictive control module, by constructing a set of dynamics prediction models based on different dynamic load working conditions and motion states, dynamically selecting the optimal model for trajectory tracking control, real-time rolling optimization of control input, minimizing trajectory tracking error and improving the stability and efficiency of trajectory execution;

[0095] A fault-tolerant control module, the fault-tolerant control module comprises a fault detection unit and a redundant control unit, the fault detection unit based on multi-sensor fusion technology, real-time monitoring the state of the robot executor and sensor, through time series analysis and Bayesian inference algorithm, fault detection, positioning and diagnosis are carried out;

[0096] The redundant control unit combines the redundant degree of freedom of the robot arm and the distributed control architecture, dynamically allocates control resources, adjusts the control strategy after local failure occurs, and maintains normal operation of the system; at the same time, for key task, set fault-tolerant priority to ensure efficient completion of the task.

[0097] The kinetic parameter identification module further comprises an adaptive filter, which processes the sensor data to reduce the influence of dynamic load changes and measurement noise on the parameter identification accuracy.

[0098] Overall, compared with the prior art, the above technical solutions conceived by the present application have the following beneficial effects:

[0099] 1. Real-time update of kinetic parameters: through improved RLS and PSO algorithms, efficient identification of kinetic parameters is realized, dynamic load changes are adapted to, and model accuracy is improved;

[0100] 2. High-precision control of trajectory tracking: based on the multi-model predictive control algorithm, the trajectory tracking precision and response speed are significantly improved, and complex nonlinear working conditions are adapted to;

[0101] 3. Enhanced fault tolerance: multi-sensor fusion and redundant control strategies are adopted to quickly respond to local faults and ensure the stability and reliability of the system;

[0102] 4. Strong applicability: the present technology can be widely applied to industrial operation scenes under dynamic load and complex working conditions such as nuclear fusion equipment maintenance and oil drilling platform transportation, and has good engineering application prospect. BRIEF DESCRIPTION OF DRAWINGS

[0103] Figure 1 is a schematic diagram of the overall structure of the system of the present application.

[0104] Figure 2 is a schematic diagram of the working principle of the kinetic parameter identification module.

[0105] Figure 3 is a structural block diagram of the multi-model predictive control module.

[0106] Figure 4 is a workflow diagram of the fault-tolerant control module. DETAILED DESCRIPTION

[0107] In order to make the purpose, technical solutions and advantages of the present application clearer and more apparent, the present application will be further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the present application and do not limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0108] Please refer to Figures 1-4The embodiment provides a heavy-load mechanical arm trajectory tracking and control method under a dynamic working condition, and through integration of a dynamics parameter identification module, a trajectory control module and a fault-tolerant control module, how to realize the application technology in a typical industrial environment is shown. The mechanical arm in the embodiment adopts a six-degree-of-freedom heavy-load mechanical arm, and the task is to perform accurate trajectory tracking and load carrying under a dynamic working condition.

[0109] The method comprises the following steps:

[0110] S1: dynamics parameter identification: real-time acquisition of mechanical arm motion state data, combination of recursive least square method and particle swarm optimization algorithm, real-time updating and optimization of dynamics parameters of the mechanical arm, and provision of high-precision parameter support for subsequent control;

[0111] The mechanical arm in the embodiment is equipped with multiple sensors, including a position encoder, a torque sensor and an acceleration sensor, which are used for real-time acquisition of displacement, speed, acceleration and torque of each joint of the mechanical arm; the sampling frequency of each sensor is set to 100 Hz, so that the real-time performance and accuracy of data are ensured; meanwhile, an adaptive filter is introduced to process sensor data, so as to reduce the influence of dynamic load change and measurement noise on parameter identification accuracy; the dynamics parameters include an inertia matrix, a friction coefficient and a gravity compensation parameter.

[0112] According to a dynamics model of the mechanical arm, a motion equation thereof is set as:

[0113]

[0114] Wherein, τ(t) is a joint torque vector, M(θ) is an inertia matrix, θ is a position matrix of a joint, is a velocity matrix of the joint, is an acceleration matrix of the joint, is a Coriolis and centrifugal force matrix, G(θ) is a gravity matrix, is a friction torque;

[0115] On this basis, a recursive least square method (RLS) is used for dynamic updating of the parameters in the above dynamics model, and a formula of the recursive least square method is as follows:

[0116]

[0117] Wherein, θ k is a parameter estimation at a current moment, k represents a time step at the current moment, P k is a covariance matrix, x k is an input data vector, y k is a measurement output at the current moment;

[0118] By continuously updating the dynamics parameters θ kSo that the system can adapt to dynamic load changes, improve the accuracy of parameter identification.

[0119] To further optimize the accuracy of dynamic parameter identification, the particle swarm optimization (PSO) algorithm is introduced to optimize the parameter estimation in the RLS algorithm, and the objective function of the particle swarm optimization algorithm in step S1 is:

[0120]

[0121] Where p is the parameter vector to be optimized, is the predicted output based on the current parameters, y i is the actual measurement value, and n is the sample number.

[0122] The particle swarm optimization (PSO) algorithm optimizes the objective function to achieve global optimization of the dynamic parameters, thereby improving the identification accuracy of the system.

[0123] S2 Multi-model predictive control: based on the dynamic parameters provided by step S1 dynamic parameter identification, a set of dynamic prediction models under different dynamic load conditions and motion states is constructed, the optimal model is dynamically selected and the model predictive control algorithm is used to adjust the robot trajectory tracking strategy, the control input is optimized in real time, the trajectory tracking error is minimized and the stability and efficiency of trajectory execution are improved.

[0124] Step S2 specifically includes the following steps:

[0125] S21 Construct a multi-model system: based on the dynamic model of the robot arm, construct a sub-dynamic model M1(θ), M2(θ), …, M n (θ) under different dynamic load conditions and motion states, and a plurality of sub-models form a multi-model system; each sub-dynamic model is trained under different load conditions (such as no load, light load, heavy load, etc.), and different parameters are used to adapt to different working environments to ensure that the control system can adapt to complex working conditions.

[0126] S22 Model selection mechanism: in each control period, the multi-model system of step S21 selects the most suitable sub-dynamic model for control according to the current load and working state at the moment.

[0127] The principle of selecting a sub-dynamic model is:

[0128]

[0129] Where y m is the measurement data at the current moment, u is the control input, and M s is the selected optimal model.

[0130] S23 Model predictive control: after selecting the optimal sub-dynamics model through step S22, a model predictive control algorithm is used to solve the optimization of the robot trajectory tracking strategy, and the control input is calculated in real time.

[0131] The model predictive control algorithm is:

[0132]

[0133] Where y k is the output at the current prediction time, y ref,k is the reference trajectory, λ is the regularization parameter, and u k is the control input.

[0134] By solving this optimization problem, MPC can minimize the trajectory tracking error and ensure the smoothness of the control input.

[0135] S3 Fault-tolerant control: after control and optimization by step S2 multi-model predictive control, the robot motion state is continuously monitored in real time, and fault detection and diagnosis are simultaneously performed. Once a fault is detected, redundant control is enabled, and the control strategy is dynamically adjusted in combination with the robot's redundant degrees of freedom and distributed control architecture to maintain normal operation of the system.

[0136] Step S3 specifically includes the following steps:

[0137] S31 Fault detection and diagnosis: based on multi-sensor fusion technology, the state of the robot actuator and sensor is monitored in real time, such as motor speed, torque, etc. The robot state under normal conditions is estimated through time series analysis, and whether there is an abnormality is detected. Different fault types of the robot are distinguished through Bayesian inference algorithm, and the fault is detected and located.

[0138] The execution state of the robot is a typical time series data, including: joint position θ(t), joint speed Joint acceleration Joint torque τ(t), load m(t), sensor current I(t). These variables constitute a multi-dimensional time series:

[0139]

[0140] In order to detect the fault, a time series prediction model needs to be established to estimate the robot state under normal conditions and detect whether there is an abnormality.

[0141] In the present application, ARIMA (Autoregressive Integrated Moving Average Model) based method is used to model the time series. The equation of ARIMA model is as follows:

[0142]

[0143] where: y t is the observation value at the current time (such as joint torque); c is the constant term; p is the autoregressive order, which determines how many time steps in the past affect the current state; q is the moving average order, which determines the influence of past error terms; φ i and θ j are model coefficients; ∈ t is the noise term (usually assumed to be white noise). Using the ARIMA prediction model, the predicted value at the future time step is obtained. Its detection formula is:

[0144]

[0145] where: y r is the residual value, y m is the actual measurement value, is the predicted output; if the residual exceeds the set threshold, it is considered to be a fault; if the absolute value of the residual exceeds a certain threshold (such as 3 times the standard deviation, i.e. 3σ principle), it is considered to be an anomaly.

[0146] When the time series analysis detects an anomaly, further use the Bayesian inference algorithm for fault diagnosis to determine whether the anomaly is a fault and to infer the type of fault.

[0147] The Bayesian inference algorithm of the present application is used to distinguish different fault types of the robotic arm, such as: sensor failure (Sensor Failure), motor overload (Motor Overload), mechanical joint stiction (Joint Stiction), and end-effector load change (End-Effector Load Change). Assuming there are 4 types of faults (H1, H2, H3, H4), and the time series feature vector D of the sensor data, the posterior probability of each fault is calculated using Bayesian inference:

[0148]

[0149] where: n is the number of different faults of the robotic arm; H i represents different fault types (such as sensor failure, motor overload, etc.); D represents the observed abnormal data; P(H i ) is the prior probability, indicating the historical occurrence rate of a certain fault; P(D|H i ) is the likelihood probability, indicating the possibility of abnormal data under a certain fault;

[0150] The system selects the fault type with the highest probability according to the calculated P(H i |D) and triggers the corresponding fault-tolerant control strategy.

[0151] S32 Redundancy control and fault recovery: After detecting the fault in step S31, redundancy control is enabled. A dynamic adjustment method of distributed redundancy control is used to optimize control input distribution, so that the redundant degrees of freedom of the heavy-duty robot arm can effectively compensate after a fault occurs, maintaining the stability and control performance of the system. At the same time, fault-tolerant priorities are set for key tasks to ensure efficient completion of system tasks.

[0152] Specifically, assuming there are m redundant joints, the redundancy control optimization formula is:

[0153]

[0154] where u j is the redundancy control input; y k is the current joint state (position, velocity) of the robot arm at time k; y ref,k is the reference trajectory (target trajectory); and N is the control time window length. The goal of this optimization problem is to minimize the trajectory error, i.e., to keep the robot arm's trajectory as close to the target trajectory as possible. Minimizing the redundancy control input avoids unnecessary control energy consumption and improves the stability of the robot arm.

[0155] In the trajectory tracking and fault-tolerant control of heavy-duty robot arms, the core goal of distributed redundancy control is to dynamically adjust the redundant degrees of freedom to maintain normal operation of the heavy-duty robot arm after a local fault occurs and ensure trajectory tracking accuracy. The control system of the heavy-duty robot arm uses a distributed optimization method to adjust the control input in real time, taking into account task requirements, the redundant structure of the heavy-duty robot arm, and its dynamic characteristics, to minimize the impact of faults on the system.

[0156] When the system is operating normally, the dynamics equation of the heavy-duty robot arm is represented as:

[0157]

[0158] where M(θ) is the inertia matrix, is the Coriolis force and centrifugal force matrix, G(θ) is the gravity matrix, τ is the joint torque vector, θ, θ, and are the joint angle, velocity, and acceleration, respectively. When a fault occurs, some joints may not work normally, and the control input of the entire system needs to be redistributed. Let the fault occur at the jth joint, and set the control input of this joint to zero. The torque balance equation of the entire system is rewritten as:

[0159]

[0160] where τ res represents the additional error term due to the fault. To maintain stable operation of the system, the control input of the remaining joints needs to be adjusted so that it can compensate for τ resi.e., to satisfy the following optimization objective:

[0161]

[0162] where θ ref,k is the reference trajectory, λ is a regularization parameter to control the smoothness of the input, and τ i is the input to compensate for the joint. To solve this optimization problem, we introduce a constraint condition, i.e., the Jacobian matrix J and the pseudo-inverse control method:

[0163]

[0164] where J is the Jacobian matrix of the redundant manipulator, representing the mapping relationship from joint space to task space. After the fault occurs, the control input of some joints is limited, so the optimization problem is rewritten as:

[0165]

[0166] where J + represents the generalized inverse (pseudo-inverse) matrix, and J j is the Jacobian matrix corresponding to the fault joint. This formula shows that the system uses the remaining redundant degrees of freedom to redistribute the control input to maintain the trajectory tracking accuracy as much as possible.

[0167] In addition, during the redundant control process, the actuator saturation constraint needs to be considered to avoid new faults caused by overloading of the remaining joints. Therefore, the torque constraint condition is introduced:

[0168] τ min ≤τ opt ≤τ max

[0169] where τ min and τ max are the minimum and maximum allowable torques of the actuator, respectively. When the calculated τ opt exceeds this range, it needs to be corrected by a quadratic programming method to minimize the impact on the overall motion of the system:

[0170] s.t.τ min ≤τ≤τ max

[0171] This optimization problem is solved by the Lagrange multiplier method, and the optimal control input that satisfies the constraint condition is finally obtained. The entire adjustment process is dynamically performed within each control period to ensure that the system can still operate stably after a fault occurs and maintain a high trajectory tracking accuracy.

[0172] The dynamic adjustment method of distributed redundancy control can make the redundant degrees of freedom of the heavy-duty robot arm compensate effectively after the fault occurs, and maintain the stability and control performance of the system through optimal control input distribution. By introducing optimization method, Jacobian matrix pseudo-inverse, actuator constraint and other technical means, the method can ensure that the heavy-duty robot arm still has high reliability and adaptability under complex working conditions.

[0173] Meanwhile, fault-tolerant priorities are set for key operation tasks to ensure efficient completion of the tasks. In the optimization process, not only trajectory tracking error is considered, but also minimization of redundant control input is introduced to ensure efficient use of redundant degrees of freedom.

[0174] During the operation of the heavy-duty robot arm, various joints, actuators and sensors of the heavy-duty robot arm may fail to different degrees in complex environment. In order to ensure that the heavy-duty robot arm can continue to perform tasks, the application proposes a dynamic adjustment method based on fault-tolerant priority, so that the system can dynamically adjust the control strategy according to the importance of the task, the severity of the fault and the availability of the redundant resources after detecting the fault, thereby maximizing the stability and task completion rate of the heavy-duty robot arm. The core idea of the method is to make the heavy-duty robot arm compensate high-priority tasks first and reduce the execution requirements of low-priority tasks under different fault conditions, so as to ensure the completion efficiency of the overall task.

[0175] The dynamic equation of the heavy-duty robot arm under fault-free condition is represented as:

[0176]

[0177] where M(θ) is an inertia matrix, is a Coriolis force and centrifugal force matrix, G(θ) is a gravity matrix, τ is a joint torque vector, θ, θ and are joint angle, velocity and acceleration, respectively. When a certain actuator or sensor fails, part of the torque input may be limited, resulting in an increase in system trajectory tracking error.

[0178] In order to maintain system stability, after detecting the fault, the control input of the system needs to be dynamically adjusted to adapt to the new constraint condition. In the fault-tolerant priority adjustment process, the application first defines a task error function:

[0179]

[0180] where M represents the number of tasks currently to be executed, w i represents the priority weight of the i-th task, θ ref,i represents the current joint state of task i, and θ ref,i represents its target trajectory. The weights wi Reflects the importance of the task, the need for dynamic adjustment after the fault occurs to ensure the execution accuracy of the key task.

[0181] In the fault-tolerant adjustment process, the invention introduces a fault severity weight γ j to represent the fault degree of each joint:

[0182]

[0183] where d j represents the fault influence degree of joint j, d thr is the threshold of fault influence, and β is the parameter controlling the change rate of fault weight. The formula ensures that when the fault influence is small, γ j is still small, and when the fault influence exceeds a certain degree, γ j increases rapidly.

[0184] Combined with the task error weight w i and the fault severity weight γ j , the adjusted control input optimization problem is represented as:

[0185]

[0186] The first term ensures the minimization of task error, and the second term is used to regularize the influence of fault on control input to prevent the damaged joint from being overused. The optimization problem is solved by Lagrange multiplier method to obtain the optimal control input that meets the current task priority.

[0187] In the optimization process, the task priority weight w i needs to be dynamically adjusted according to the influence of the fault. Let the weight update amount of task i be Δw i , then the update formula is:

[0188]

[0189] where e i is the current error of task i, e max,i is the maximum error allowed for the task, and α is the weight adjustment step size. When the task error is large, the system will automatically increase the weight of the task, so that the task gets more redundant control resources in the optimization process, and when the task error is small, the weight of the task is reduced to release control resources for other tasks.

[0190] In order to make the weight adjustment more smooth and avoid sudden changes that cause system instability, the invention uses Exponential Weighted Moving Average (EWMA) for smooth update:

[0191]

[0192] where β is a smoothing factor that controls the dynamic response speed of weight adjustment. A larger β value means slower weight adjustment to ensure system stability, while a smaller β value allows faster weight adjustment to adapt to sudden fault conditions.

[0193] In addition, after the fault occurs, not only the priority of the task needs to be adjusted, but also the allocation of redundant control resources needs to be optimized. Suppose the system has m redundant degrees of freedom, and the control input is τ redun The final optimization problem is represented as:

[0194]

[0195] where the third term is used to regularize the control input of the redundant degree of freedom, and λ is the regularization parameter. The optimization problem is solved by a quadratic programming (QP) method to obtain the optimal redundant control input.

[0196] The fault-tolerant priority adjustment dynamic adjustment method proposed in the present application can dynamically adjust the fault severity weight and task error weight, so that the system can prioritize the execution accuracy of critical tasks, and through optimization of the control input of the redundant degree of freedom, the system can still operate stably when a fault occurs. This method can still have good adaptive ability under sudden faults through weight adjustment, redundancy optimization, and exponential smoothing processing, etc., to improve the overall task completion rate.

[0197] Referring to Figures 1-4 The dynamic working condition heavy-duty robot arm trajectory tracking and control system provided in the embodiment comprises:

[0198] The dynamics parameter identification module comprises a plurality of sensors arranged on the robot arm, which collect real-time motion state data of the robot arm, and combines the recursive least squares method and the particle swarm optimization algorithm to update and optimize the dynamics parameters of the robot arm in real time, providing high-precision parameter support for subsequent control;

[0199] The multi-model predictive control module dynamically selects the optimal model for trajectory tracking control by constructing a set of dynamics prediction models based on different dynamic load working conditions and motion states, and realizes real-time rolling optimization of control input to minimize trajectory tracking error and improve the stability and efficiency of trajectory execution;

[0200] The fault-tolerant control module comprises a fault detection unit and a redundant control unit. The fault detection unit is based on multi-sensor fusion technology and monitors the state of the robot arm actuator and sensor in real time. Through time series analysis and Bayesian inference algorithm, fault detection, positioning and diagnosis are performed.

[0201] The redundancy control unit combines the redundancy degree of freedom of the mechanical arm and the distributed control architecture, dynamically allocates control resources, adjusts the control strategy after a local fault occurs, and maintains normal operation of the system; meanwhile, for key work tasks, fault-tolerant priorities are set to ensure efficient completion of the tasks.

[0202] The dynamic parameter identification module further comprises an adaptive filter, which processes sensor data to reduce the influence of dynamic load changes and measurement noise on parameter identification accuracy.

[0203] Specifically, Figure 1 The main modules of the trajectory tracking and fault-tolerant control system of the heavy-duty mechanical arm under dynamic working conditions are shown in the figure, including the dynamic parameter identification module, the multi-model predictive control module, and the fault-tolerant control module, and the mutual connection and information flow path between the modules are shown.

[0204] Figure 2 In the figure, how to use recursive least squares (RLS) combined with particle swarm optimization algorithm (PSO) to update the dynamic parameters of the mechanical arm in real time, and perform data filtering and optimization processing are described in detail.

[0205] Figure 3 In the figure, the model selection mechanism under different working conditions and how it dynamically adjusts the trajectory tracking strategy of the mechanical arm through the model predictive control algorithm (MPC) to minimize tracking error and improve control accuracy are shown.

[0206] Figure 4 In the figure, the cooperation mode of the fault detection unit and the redundancy control unit is shown, how to identify faults based on multi-sensor data, and ensure that the system can still operate normally under fault conditions through control resource reallocation.

[0207] Experimental test

[0208] The experiment tests the heavy-duty mechanical arm in a simulation environment, simulating trajectory tracking tasks under different load conditions; the mechanical arm used in the experiment is a six-degree-of-freedom heavy-duty arm, and the task is to complete a series of trajectory tracking and material handling work within 5 minutes.

[0209] (1) Trajectory tracking performance

[0210] The experimental results show that after using the control scheme of the invention, the mechanical arm can keep the trajectory tracking error within ±2mm under different load conditions, and compared with the traditional PID control, the error is reduced by more than 50%, especially the precision under heavy load working condition is improved significantly.

[0211] (2) Fault-tolerant control performance

[0212] When a local fault occurs, the redundant control module can respond in time and restore the normal operation of the manipulator by adjusting the joint load distribution. The recovery time after the fault occurs is 2 seconds on average, and the manipulator can continue to perform tasks without affecting the trajectory tracking accuracy.

[0213] (3) Energy optimization

[0214] By optimizing the control input through MPC, the experimental results show that the energy consumption of the manipulator in the trajectory tracking process is reduced by about 20% compared with the traditional control method, and the working efficiency of the system is improved.

[0215] Application in nuclear fusion device maintenance

[0216] (1) Experimental steps

[0217] The method and system are used in the implementation of nuclear fusion device maintenance scenarios.

[0218] (2) Experimental results

[0219] In the maintenance of nuclear fusion devices, according to the load change and working state of the device, the appropriate dynamic model is dynamically selected and real-time optimization is performed. At the same time, the fault detection and redundant control module ensures that the manipulator can still maintain high efficiency in harsh environments. The method and system realize the precise trajectory tracking and load carrying tasks of the manipulator in the complex environment of nuclear fusion, and have high fault tolerance and real-time self-adaptation ability.

[0220] Those skilled in the art will readily understand that the above description is only a preferred embodiment of the present application and is not intended to limit the present application. Any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A method for trajectory tracking and control of a heavy-duty manipulator under dynamic conditions, characterized in that, The method comprises the following steps: S1 Kinematics parameter identification: Real-time acquisition of mechanical arm motion state data, combined with recursive least squares method and particle swarm optimization algorithm, real-time update and optimization of the kinematics parameters of the mechanical arm, providing high-precision parameter support for subsequent control; S2 Multi-model predictive control: Based on the kinematics parameters provided by step S1 kinematics parameter identification, a set of dynamic prediction models under different dynamic load conditions and motion states is constructed, the optimal model is dynamically selected, and the model predictive control algorithm is used to adjust the mechanical arm trajectory tracking strategy, real-time rolling optimization of control input is realized, the trajectory tracking error is minimized, and the stability and efficiency of trajectory execution are improved; S3 Fault-tolerant control: After control and optimization by step S2 multi-model predictive control, the mechanical arm motion state is continuously monitored in real time, fault detection and diagnosis are simultaneously performed, and once a fault is detected, redundant control is enabled, the control strategy is dynamically adjusted in combination with the redundant degrees of freedom of the mechanical arm and the distributed control architecture, and the system is maintained in normal operation.

2. The trajectory tracking and control method for heavy-duty robotic arms under dynamic conditions according to claim 1, characterized in that, The data real-time acquisition device in step S1 is a plurality of sensors installed on the mechanical arm, including position encoders, torque sensors and acceleration sensors, for real-time acquisition of displacement, speed, acceleration and torque of each joint of the mechanical arm; The sampling frequency of each sensor is set to 100Hz to ensure the real-time and accuracy of the data; At the same time, an adaptive filter is introduced to process the sensor data, to reduce the influence of dynamic load changes and measurement noise on the parameter identification accuracy; The kinematics parameters include inertia matrix, friction coefficient and gravity compensation parameters.

3. The trajectory tracking and control method for heavy-duty robotic arms in dynamic conditions according to claim 1, characterized in that, In step S1, according to the kinematics model of the mechanical arm, the motion equation is: wherein, is a joint torque vector, is an inertia matrix, is a position matrix of the joint, is a velocity matrix of the joint, is an acceleration matrix of the joint, is a Coriolis and centrifugal force matrix, is a gravity matrix, is a friction torque; In step S1, the formula of recursive least squares method is: wherein, is the parameter estimate at the current time instant, k represents the time step at the current time instant, is the covariance matrix, is the input data vector, is the measurement output at the current time instant; by continuously updating the kinetic parameters so that the system can adapt to dynamic load changes and improve the accuracy of parameter identification; In step S1, the objective function of particle swarm optimization algorithm is: wherein, is a parameter vector to be optimized, is an output predicted based on current parameters, is an actual measurement value, n is the number of samples; The particle swarm optimization algorithm optimizes the objective function to achieve global optimization of the kinematics parameters, thereby improving the identification accuracy of the system.

4. The trajectory tracking and control method for heavy-duty robotic arms under dynamic conditions according to claim 1, characterized in that, Step S2 specifically comprises the following steps: S21 Construct a multi-model system: based on the dynamics model of the robot arm, construct sub-dynamics models under different dynamic load conditions and motion states , a plurality of sub-models form a multi-model system; each sub-dynamics model is trained under different load conditions and adapts to different working environments through different parameters to ensure that the control system can adapt to complex working conditions; S22 Model selection mechanism: In each control period, the multi-model system of step S21 selects the most suitable sub-kinematics model for control according to the current load and working state; S23 Model predictive control: After selecting the optimal sub-kinematics model in step S22, the model predictive control algorithm is used to solve the optimized mechanical arm trajectory tracking strategy, and the control input is calculated in real time.

5. The trajectory tracking and control method for heavy-duty robotic arms in dynamic conditions according to claim 4, characterized in that, The selection principle of the sub-kinematics model in step S22 is: wherein, is the measurement data at the current time instant, is the control input, is the selected optimal model; The model predictive control algorithm in step S23 is: wherein, is the output at the current prediction time, is the reference trajectory, is a regularization parameter, is the control input.

6. The method of claim 1, wherein, Step S3 specifically comprises the following steps: S31 Fault detection and diagnosis: Based on multi-sensor fusion technology, the state of the mechanical arm actuator and sensor is monitored in real time, the state of the mechanical arm under normal conditions is estimated through time series analysis, and whether there is an abnormality is detected, different fault types of the mechanical arm are distinguished through Bayesian inference algorithm, and the fault is detected and located; The time series is modeled based on the autoregressive integrated moving average model, and the modeling equation is: where: is the observed value at the current time step; is the constant term; is the autoregressive order, determining how many past time steps influence the current state; is the moving average order, determining how much past error terms influence the current state; and are the model coefficients; is the noise term; The predicted output value of the future time step is obtained by an autoregressive integrated moving average model , the actual measured value , if the residual exceeds the set threshold, it is considered that there is a fault; then the fault detection formula is: wherein, is a residual value, is an actual measurement value, is a prediction output; If the absolute value of the residual exceeds a certain threshold, it is considered that an anomaly has occurred, and a Bayesian inference algorithm is further used for fault diagnosis, and the Bayesian inference calculates the posterior probability corresponding to the fault The formula is: where n is the number of different faults of the robot arm; represents different fault types; represents observed abnormal data; is a prior probability, representing the historical occurrence rate of a certain fault; is a likelihood probability, representing the possibility of abnormal data under a certain fault; According to the calculated posterior probability selecting the fault type with the highest probability to determine whether the anomaly belongs to a fault and to infer the type of fault; S32 Redundancy control and fault recovery: after step S31 detects a fault, redundancy control is enabled, and a dynamic adjustment method of distributed redundancy control is adopted to make the redundant degrees of freedom of the heavy-duty robot arm effectively compensate after the fault occurs, maintain the stability and control performance of the system, and set fault-tolerant priorities for key tasks to ensure efficient completion of the system tasks.

7. The trajectory tracking and control method of a heavy-duty robotic arm under dynamic conditions according to claim 6, wherein, The dynamic adjustment method of distributed redundancy control in step S32 is as follows: Assuming that the robot arm has m redundant joints, the redundant degree of freedom control optimization formula is: wherein, is a redundant control input; is a current joint state of the robot arm at time k; is a reference trajectory; N is a control time window length; Suppose the fault occurs in the jth joint, and the control input of the joint is set to zero. Then, the torque balance equation of the entire system is rewritten as: wherein represents an additional error term due to the fault; To maintain the stable operation of the system, the control inputs of the remaining joints need to be adjusted so that they can compensate i.e. to satisfy the following optimization objective: wherein, is a reference trajectory, is a regularization parameter for controlling the smoothness of the input, is a compensation for the joint; The constraint condition is introduced, i.e. the Jacobian matrix and the pseudo-inverse control method: wherein, J is the Jacobian matrix of the heavy manipulator, representing the mapping relationship from joint space to task space; After the fault occurs, the control input of part of the joints is limited, and the optimization problem is rewritten as: wherein, represents a generalized inverse matrix, is the Jacobian matrix corresponding to the faulty joint; By redistributing the control input using the remaining redundant degrees of freedom, the trajectory tracking accuracy is maintained as much as possible. During the redundancy control process, the saturation constraint of the actuator needs to be further considered to avoid new faults caused by overloading of the remaining joints, and a torque constraint condition is introduced: wherein, and Mmin and Mmax are the minimum and maximum allowed torque of the actuator, respectively; When the computed Beyond this range, a correction by a quadratic programming method is required to minimize the impact on the overall motion of the system: By using the Lagrange multiplier method, the optimal control input that satisfies the constraint condition is obtained.

8. The trajectory tracking and control method of a heavy-duty robotic arm under dynamic working conditions according to claim 7, characterized in that, The fault-tolerant priority adjustment process in step S32 is as follows: a task error function is defined as: wherein, represents the number of tasks currently required to be performed, represents the priority weight of the th task, represents the target trajectory of the task current joint state, represents its target trajectory; Introducing a fault severity weight to characterize the degree of fault of each joint: wherein, represents the degree of influence of a fault on a joint is a threshold value of the influence of a fault, is a parameter controlling the rate of change of the fault weight.​ task error weights and fault severity weights the adjusted control input optimization problem is represented as: The first term ensures that the task error is minimized, and the second term is used to regularize the influence of the fault on the control input to prevent the damaged joint from being overused. In the optimization process, the task priority weight According to the influence of the fault, dynamic adjustment is needed, and the weight of the task The weight update amount of the task is Δ The update formula is: wherein, is the task current error, is the maximum error allowed for the task, is the weight adjustment step size; When the task error is large, the system automatically increases the weight of the task, so that the task gets more redundant control resources in the optimization process. When the task error is small, the weight of the task is reduced to release control resources for other tasks. Further, in order to make the weight adjustment more smooth and avoid instability caused by sudden changes, an exponentially weighted moving average is used for smooth updating: wherein is a smoothing factor, controlling the dynamic response speed of the weight adjustment; After the fault occurs, the allocation of the redundant control resources is further optimized, and the final optimization problem is represented as: wherein the third term is used to regularize the control input for the redundant degrees of freedom, is a regularization parameter; By using the quadratic programming method, the optimal redundant control input is obtained. 9.A trajectory tracking and control system for heavy-duty manipulator under dynamic working condition, characterized in that, It includes: A dynamics parameter identification module including a plurality of sensors arranged on the robot arm, the plurality of sensors collect real-time motion state data of the robot arm, and the recursive least squares method and the particle swarm optimization algorithm are combined to update and optimize the dynamics parameters of the robot arm in real time, providing high-precision parameter support for subsequent control; A multi-model predictive control module, which dynamically selects the optimal model for trajectory tracking control by constructing a set of dynamics prediction models based on different dynamic load conditions and motion states, and rolls the control input in real time to minimize the trajectory tracking error and improve the stability and efficiency of trajectory execution; A fault-tolerant control module, which includes a fault detection unit and a redundancy control unit. The fault detection unit monitors the state of the robot arm actuator and sensor in real time based on multi-sensor fusion technology, and performs fault detection, positioning and diagnosis through time series analysis and Bayesian inference algorithm; The redundancy control unit dynamically allocates control resources based on the redundant degrees of freedom and distributed control architecture of the robot arm, adjusts the control strategy after a local fault occurs, and maintains normal operation of the system. Meanwhile, fault-tolerant priorities are set for key tasks to ensure efficient completion of the tasks.

10. The trajectory tracking and control system for heavy-duty robotic arms under dynamic conditions of claim 9, wherein, The kinetics parameter identification module further comprises an adaptive filter which processes the sensor data to reduce the influence of dynamic load variations and measurement noise on the accuracy of the parameter identification.

Images