A robot arm self-protection control method and system

Abstract

This invention discloses a self-protection control method and system for a robotic arm, relating to the field of robotic arm control technology. The method includes the following steps: obtaining corresponding health factors based on the torque output capability of each joint; inputting multiple health factors into an objective function and solving it to obtain the first velocity of each joint; obtaining the second velocity of each joint based on the safe configuration of the robotic arm; combining the first and second velocities of each joint to obtain the reference velocity and reference position of each joint; inputting the real-time position and real-time velocity of each joint into an optimization model and solving it to obtain the optimal acceleration sequence of each joint; and generating joint torque commands through the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm. This invention, through hierarchical coordination of kinematics and dynamics, strictly limits the torque output of faulty joints while ensuring the accuracy of end-effector trajectory tracking, thereby achieving self-protection and safe operation of the robotic arm under complex working conditions.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control technology, and in particular to a self-protection control method and system for robotic arms. Background Technology

[0002] With the rapid development of human-machine collaboration technology, seven-DOF redundant robotic arms, due to their flexible motion capabilities and obstacle avoidance advantages, are widely used in high-value fields such as spacecraft on-orbit servicing, minimally invasive surgical robots, and precision industrial assembly. However, joint actuators, as core power components, must withstand nonlinear time-varying loads for extended periods, making them highly susceptible to wear and aging, leading to partial loss of effective output (PLOE). Unlike sudden, complete failure, PLOE is often a gradual process. If the control system fails to detect and intervene in a timely manner, blindly increasing the control input solely based on the error accumulation effect of the integrator will cause the already damaged joints to remain under overload for a prolonged period. This will accelerate their evolution from "partial performance degradation" to "irreversible permanent damage," ultimately leading to mission failure or even safety accidents.

[0003] Traditional fault-tolerant control (FTC) methods primarily focus on maintaining stability and restoring trajectory tracking accuracy after a fault, encompassing adaptive control, sliding mode control, and robust control. However, these methods often emphasize passive compensation, neglecting the secondary damage to the faulty joint that may result from high-gain control.

[0004] To overcome the limitations of passive compensation in traditional fault-tolerant control, existing research has proposed the concept of self-protective control, advocating a shift from "passive repair" to "active defense." Existing self-protective control schemes mainly include: first, control strategies based on residual lifetime (RUL), which reduce the degradation rate of the system by actively adjusting control variables (such as limiting dynamic response bandwidth) to reduce the load on critical components; second, collaborative protection strategies based on lifetime balancing, which use frameworks such as model predictive control to constrain the state imbalance of multi-joint systems and avoid premature system failure due to local bottleneck effects; and third, configuration optimization strategies based on zero-space projection, which use redundant degrees of freedom to reconstruct torque distribution for load reduction. However, most existing self-protective control schemes discuss kinematic avoidance or dynamic constraints in isolation, lacking a unified and integrated framework. This makes it difficult to achieve full-link active defense from motion planning to torque output while ensuring high-frequency real-time control, and fails to meet the needs of robotic arms to "work with injuries" and "prevent the worsening of injuries" under complex working conditions. Summary of the Invention

[0005] In view of the defects of the existing technology, the present invention provides a self-protection control method and system for robotic arms, which solves the existing problems.

[0006] The present invention adopts the following technical solution: In a first aspect, the present invention provides a self-protection control method for a robotic arm, comprising the following steps: Collect the real-time position and speed of each joint of the robotic arm during the current control cycle; Based on the torque output capability of each joint, corresponding health factors are obtained. Multiple health factors are input into the objective function and solved to obtain the first velocity of each joint. Based on the safety configuration of the robotic arm, the second velocity of each joint is obtained. The first and second velocities of each joint are combined to obtain the reference velocity and reference position of each joint. The objective function aims to minimize the joint velocity energy and uses the end-effector velocity as a constraint. The real-time position and velocity of each joint are input into the optimization model and solved to obtain the optimal acceleration sequence of each joint. The optimization model aims to minimize the difference between the real-time position and velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. The constraints are state-space equations, torque constraints, position constraints, and velocity constraints. Joint torque commands are generated by using the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm.

[0007] Preferably, the health factor is obtained by the ratio of the maximum sustainable output torque after damage to the rated output torque in the healthy state.

[0008] Preferably, the construction of the objective function specifically includes the following steps: Weighting terms are constructed based on multiple health factors; Construct a joint space weighted matrix based on multiple weight terms; Weighted control energy is constructed based on the joint space weighting matrix and the velocity of each joint, and task tracking error is constructed based on the end-effector velocity constraint. By combining the weighted control energy and the task tracking error, the objective function is obtained.

[0009] Preferably, the weighting term is constructed using a penalty gain coefficient and a penalty nonlinearity exponent.

[0010] Preferably, the objective function is as follows: ; In the formula, Let be the objective function. For joint velocity, The damping factor, T It is a transpose operator. The joint space weighting matrix, For the desired terminal velocity, It is a Jacobian matrix.

[0011] Preferably, the step of combining the first and second velocities of each joint to obtain the reference velocity and reference position of each joint specifically includes the following steps: Multiplying the second velocity by the dynamic gain used to adjust the secondary task yields the null space component; Add the first velocity to the null space component to obtain the reference velocity; Integrating the reference velocity yields the reference position.

[0012] Preferably, the construction of the optimization model specifically includes the following steps: Discretize the robotic arm system to obtain The state-space equation at time t; Based on health factors, the feasible region of health constraint torque is determined. The dynamic parameters of the nonlinear dynamic equation are frozen and then linearized to transform the feasible region of health constraint torque into torque constraints. The optimization objectives are to minimize the difference between the real-time position and real-time velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. An optimization model is constructed with state-space equations, torque constraints, position and velocity constraints as constraints.

[0013] Preferably, the optimization model is as follows: ; ; ; ; ; In the formula, To optimize the objective, for k Joint acceleration at any moment for The state vector at time t, To predict the length of the time domain, In order to control the current moment Based on this, the future +1 prediction step system state vector In order to control the current moment Based on this, the future +1 prediction step reference velocity and reference position, This is the process state error weight matrix. To control the energy consumption weight matrix, For the terminal target state, This is the terminal cost weight matrix. and These are the state matrix and input matrix of the linearized time-varying system, respectively. The inertia matrix is ​​a locally linearized matrix. For locally linearized Coriolis force and centrifugal force terms, For locally linearized gravity terms, For Hadama accumulation, For joint health factor vectors, The rated output torque under healthy conditions. for Joint velocity at any given moment and These are the minimum and maximum values ​​of the state vector. and These represent the minimum and maximum values ​​of joint acceleration.

[0014] Preferably, the step of generating joint torque commands through the optimal acceleration sequence of each joint specifically includes the following steps: Substituting the first term of the optimal acceleration sequence of each joint into the linearized dynamic equation yields the feedforward term; Feedback terms are obtained based on position and velocity errors; The feedforward and feedback terms are added together to obtain the joint torque command.

[0015] In a second aspect, the present invention provides a self-protection control system for a robotic arm, comprising: The data acquisition module is used to acquire the real-time position and speed of each joint of the robotic arm during the current control cycle. The kinematics solution module is used to obtain the corresponding health factors based on the torque output capability of each joint, input multiple health factors into the objective function and solve it to obtain the first velocity of each joint; obtain the second velocity of each joint based on the safety configuration of the robotic arm; combine the first velocity and the second velocity of each joint to obtain the reference velocity and reference position of each joint; wherein, the objective function takes minimizing the joint velocity energy as the optimization objective and end-effector velocity as the constraint; The dynamics solution module is used to input the real-time position and real-time velocity of each joint into the optimization model and solve it to obtain the optimal acceleration sequence of each joint. The optimization model aims to minimize the difference between the real-time position and real-time velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. The constraints are state-space equations, torque constraints, position and velocity constraints. The control module is used to generate joint torque commands by using the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm.

[0016] Compared with the prior art, the above-mentioned at least one technical solution adopted by the present invention can achieve the following beneficial effects: This invention quantifies the real-time torque output capability of joints through health factors. At the kinematic level, it aims to minimize weighted joint velocity energy. While satisfying end-effector velocity constraints, it actively transfers motion tasks from joints with low health factors to joints with high health factors, reducing the motion load on faulty joints at the source. Then, by superimposing a safety configuration based on the robotic arm, it obtains the second velocity of each joint and uses redundant degrees of freedom to guide damaged joints to preferentially return to a safe posture with less force. At the dynamics level, it uses minimizing state tracking error and joint acceleration as dual optimization objectives. It introduces state-space equations, torque constraints, and position and velocity constraints as conditions, transforming the complex robotic arm control problem into a real-time solvable optimization model. While ensuring real-time control, it ensures that the torque of faulty joints does not exceed the safe range, while maintaining end-effector trajectory tracking accuracy. The kinematic level of this invention is responsible for global posture optimization, playing a crucial role in overall control; the dynamics level is responsible for dynamic adjustment, playing a crucial role in fine-tuning details. This invention achieves self-protection and safe operation of the robotic arm under complex working conditions by using a hierarchical synergy of kinematics and dynamics to strictly limit the torque output of the faulty joint while ensuring the accuracy of end-effector trajectory tracking, thus preventing further deterioration due to overload. Attached Figure Description

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 This is a schematic diagram of a self-protection control method for a robotic arm according to the present invention; Figure 2 The change in joint velocity after motion compensation in an embodiment of the present invention; in, Figure 2 (a): Joint two, Figure 2 (b): Joint four, Figure 2 (c): Joint six; Figure 3 These are torque diagrams of the damaged joints after kinematic reprogramming and after applying the method of this invention, according to an embodiment of the present invention. Figure 4 This is an end trajectory diagram of an embodiment of the present invention; Figure 5 This is a trajectory error diagram according to an embodiment of the present invention; Figure 6 This is a flowchart of a self-protection control method for a robotic arm according to the present invention. Detailed Implementation

[0019] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0020] This invention provides a self-protective control method for active torque regulation of a robotic arm based on a hierarchical architecture. Focusing on the faulty joint as the primary protection target, it achieves "active load reduction" and "motion compensation" through a hierarchical architecture: At the kinematic level, weighted pseudo-inverse and null space reconstruction techniques are used to actively transfer the main motion task load to healthy joints, thus avoiding overuse of damaged joints from a structural perspective; at the dynamic level, time-varying torque constraint boundaries are constructed based on joint health factors, and a Linear Time-Varying Model Predictive Control (LTV-MPC) method based on a multi-shot strategy is proposed. This method locally linearizes the nonlinear dynamics of the robotic arm in each control cycle, transforming the complex nonlinear constraint optimization into a sparse quadratic programming (QP) problem.

[0021] Reference Figure 1 and Figure 6 This invention presents a self-protection control method for a robotic arm based on active torque regulation. It quantifies the residual torque output capability of joints through health factors, constructs time-varying torque constraint boundaries, and employs a three-layer control architecture of "motion compensation - torque regulation - closed-loop tracking" to achieve synergy between active unloading of faulty joints and stable system operation. Combined with real-time status feedback for dynamic iterative optimization, it prevents overload deterioration of faulty joints while ensuring the trajectory tracking accuracy of the end effector, ultimately improving the reliability and service life of the robotic arm under partial failure conditions. The specific implementation steps are as follows:

[0022] S1: Robotic arm dynamics modeling and joint health status quantitative modeling: considering The dynamic model of the multi-degree-of-freedom robotic arm is as follows: (1); in, These are joint position, velocity, and acceleration, respectively. It is a symmetric positive definite inertial matrix; These are the Coriolis force and centrifugal force terms; This is the term related to gravity. Joint torque; It is a real number.

[0023] To quantify the health status of actuators, health factors are defined. Quantification The real-time torque output capability of each joint is expressed by the following formula: (2); in, For the first The maximum sustainable output torque after a joint is damaged This is the rated output torque under healthy conditions. And it has a normal operating state: , Damaged condition: decline, The feasible region of the system's health constraint moments is:

[0024] (3); Define the torque constraint target for self-protection control.

[0025] S2: Kinematic Compensation Allocation: Constructing a Joint Spatial Weighted Matrix : (4); in, For the first The weight components of each joint are calculated using the following formula: (5); in, and These are the penalty gain coefficient and the penalty nonlinearity index, respectively.

[0026] In order to satisfy the end velocity constraint Under the premise of minimizing the overall kinetic energy consumption of the system, especially limiting the range of motion of the damaged joints and minimizing the weighted joint velocity energy, the following optimization objective function is constructed. : (6); The first term is the weighted control energy, and the second term is the task tracking error. For Jacobian matrices, The above is the fault-based weight matrix. The damping factor, T This is the transpose operator.

[0027] Solve for the weighted pseudo-inverse: (7); make: Right now: (8); in, This refers to the speed of the primary task joint, i.e., the first speed.

[0028] Introducing null space components : (9); in, Must meet .

[0029] Introducing secondary task speed (second speed) : (10); in, It is the gain matrix. It is a safe configuration for robotic arms that experiences less stress.

[0030] Introducing dynamic gain Let the Cartesian tracking error be... :

[0031] (11); in, The threshold for full activation, This is the complete cutoff threshold. This refers to the current terminal trajectory (or the actual trajectory).

[0032] Final reference speed The expression is as follows: (12); In the formula This is the optimized desired joint reference velocity output from the first-layer motion planning. The core purpose of constructing this reference velocity is to achieve active compensation and configuration optimization at the kinematic level, and to provide a safe tracking benchmark for the underlying dynamic control. By solving for the main task velocity with a health factor penalty weight, the velocity command allocated to the faulty joint is mathematically actively reduced, safely transferring the motion task to the healthy joint, thus achieving motion compensation. Then, by superimposing the null-space secondary task velocity, redundant degrees of freedom are used to guide the damaged joint to preferentially return to a safe posture with less force. Finally, this reference velocity and the reference position obtained by its integration together constitute the reference tracking target in the state space equation of the second-layer linear time-varying model predictive control, thereby guiding the underlying controller to safely complete the predetermined end-effector trajectory task under the premise of strictly satisfying the torque safety tolerance. This achieves the compensatory allocation of the motion task to the healthy joint.

[0033] S3: Dynamic Torque Control: Selecting Joint Positions and joint velocity For state variables, i.e. Joint acceleration To control the input.

[0034] The continuous system is discretized using the explicit Euler method, with the sampling time set to... Then at the prediction time The state-space equation can be expressed as: (13); Wherein, the state transition matrix With input matrix Defined as: , in, Sampling time, Let be an n-order identity matrix. In order to control the current moment Based on this, the future The system state vector for each prediction step In order to control the current moment Based on this, the future The control input vector for each prediction step.

[0035] Freeze dynamic parameters based on the current state, including inertial terms. Coriolis force and centrifugal force terms C Gravity term G For nonlinear dynamic equations Perform sequence linearization to transform the torque constraint into:

[0036] (14); in, The inertia matrix is ​​a locally linearized matrix. For nonlinear term vectors, For locally linearized Coriolis force and centrifugal force terms, For locally linearized gravity terms, This represents the Hadamard product between two matrices of the same order. This represents a vector of joint health factors.

[0037] Establish a quadratic objective function in the finite time domain : (15a); (15b); (15c); (15d); (15e); in, To predict the length of the time domain, Indicates that the system is in The state vector at time t, i.e. ; Let be the control input vector to be optimized. ; This serves as the reference trajectory for the first layer's output. The target state of the terminal; and These are the process state error weight matrix and the control energy consumption weight matrix, respectively, both of which are positive semidefinite symmetric matrices. This is the terminal cost weight matrix, used to ensure the stability of the closed-loop system; and These are the state matrix and input matrix of the linearized time-varying system, respectively.

[0038] Combining position, velocity, and acceleration limit constraints, the problem is transformed into a sparse quadratic programming problem to find the optimal control sequence, taking the first term. As a feedforward input for the lower-level controller.

[0039] S4: Feedback Control Design: To eliminate model uncertainties and accurately execute MPC commands, a control law based on inverse dynamics feedforward is designed. (16); in, For positional error, Accelerating the expected pace of MPC planning , The nominal model estimate of the robot's dynamic parameters; The positive definite diagonal gain matrix is ​​used to adjust the stiffness and damping characteristics of the system, respectively.

[0040] Example 1) Quantitative modeling of joint health status: Simulation verification was performed using the ROKAE xMate ER7 Pro7 articulated robotic arm as the object. Working conditions were set. At that time, joint 2 underwent severe degeneration, and the health factor suddenly dropped to 0.3, which means that the maximum output torque decreased by 70%.

[0041] 2) Kinematic compensation allocation: From the simulation results ( Figure 2 As shown in the diagram, after the fault occurred at t=5.0s, the first layer was able to react and quickly adjust the distribution of joint velocities. The velocity amplitude of the damaged joint 2 decreased and tended to stabilize; while the velocity amplitudes of healthy joints such as joints 4 and 6 increased accordingly, actively sharing the motion tasks originally handled by joint 2. This kinematic "mutual assistance" can effectively reduce the dynamic load on the damaged joints and achieve load reduction protection for the system.

[0042] 3) Dynamic torque control: from the torque response curve ( Figure 3 As shown in the diagram, when using only the kinematic reprogramming control method, the torque of the damaged joint always exceeds the safety tolerance. However, after introducing the control strategy proposed in this invention, the output torque of joint 2 is strictly limited to the safe range after contraction. By optimizing the acceleration command, a smooth torque limitation is achieved. This strategy successfully avoids the overload risk of the damaged joint and realizes the self-protection purpose of the damaged joint of the robotic arm.

[0043] 4) Feedback closed-loop control design: In the simulation, the robotic arm draws a circle with its center at (0.45, 0.0, 0.45) and a radius of 0.12 m. The trajectory of the robotic arm's end effector is shown in the diagram. Figure 4 and Figure 5 As shown in the figure, after the 5s joint was damaged, although the operating load of the robotic arm was readjusted, the position tracking error of the end effector remained below 0.015m, maintaining relatively stable operating accuracy. This indicates that the hierarchical strategy proposed in this invention, while achieving self-protection of the faulty joint, maximizes the preservation of global task execution capability by tapping into the redundancy potential of the system.

[0044] This invention establishes a weighted virtual stiffness model based on health factors, and kinematic compensation allocation successfully achieves soft isolation of faulty joints, unloading the main task from the faulty joints and distributing it to other healthy joints. Simulations show that the motion speed and mechanical load of the faulty joints are significantly reduced, thus slowing down the performance degradation trend of the faulty joints.

[0045] To address the real-time control challenges posed by nonlinear dynamics, this invention proposes an LTV-MPC framework based on sequence linearization, which successfully transforms complex nonlinear torques into an efficient quadratic programming (QP) problem. This method guarantees a control frequency of 100-500Hz and physically prevents any overload commands that could worsen the fault, effectively protecting the system's safety.

[0046] The three-layer architecture used in this invention works collaboratively. The kinematic layer is responsible for global attitude optimization, playing a role in overall control; the dynamic layer is responsible for dynamic adjustment, playing a role in fine-tuning details; and the feedforward torque control layer plays a role in implementation, ensuring that the system can still maintain millimeter-level trajectory tracking accuracy even in extreme cases where 70% of the joint torque output capability is lost, thus achieving the goal of preventing "injury deterioration" in the case of "working with injury".

[0047] Based on the same concept, the present invention also provides a self-protection control system for a robotic arm, including a data acquisition module, a kinematics solution module, a dynamics solution module, and a control module.

[0048] The data acquisition module is used to collect the real-time position and speed of each joint of the robotic arm during the current control cycle.

[0049] The kinematics solution module is used to obtain the corresponding health factors based on the torque output capability of each joint, input multiple health factors into the objective function and solve it to obtain the first velocity of each joint; obtain the second velocity of each joint based on the safety configuration of the robotic arm; combine the first velocity and the second velocity of each joint to obtain the reference velocity and reference position of each joint; wherein, the objective function takes minimizing the joint velocity energy as the optimization objective and end-effector velocity as the constraint.

[0050] The dynamics solution module is used to input the real-time position and velocity of each joint into the optimization model and solve it to obtain the optimal acceleration sequence of each joint. The optimization model aims to minimize the difference between the real-time position and velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. The constraints are state-space equations, torque constraints, position and velocity constraints.

[0051] The control module is used to generate joint torque commands by using the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm.

[0052] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.

[0053] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A self-protection control method for a robotic arm, characterized in that, Includes the following steps: Collect the real-time position and speed of each joint of the robotic arm during the current control cycle; Based on the torque output capability of each joint, the corresponding health factors are obtained. Multiple health factors are input into the objective function and solved to obtain the first velocity of each joint. The second velocity of each joint is obtained based on the safety configuration of the robotic arm; the first and second velocities of each joint are combined to obtain the reference velocity and reference position of each joint; wherein, the objective function takes minimizing the joint velocity energy as the optimization objective and the end-effector velocity as the constraint; The real-time position and velocity of each joint are input into the optimization model and solved to obtain the optimal acceleration sequence of each joint. The optimization model aims to minimize the difference between the real-time position and velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. The constraints are state-space equations, torque constraints, position constraints, and velocity constraints. Joint torque commands are generated by using the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm.

2. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The health factor is obtained by the ratio of the maximum sustainable output torque after damage to the rated output torque under healthy conditions.

3. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The construction of the objective function specifically includes the following steps: Weighting terms are constructed based on multiple health factors; Construct a joint space weighted matrix based on multiple weight terms; Weighted control energy is constructed based on the joint space weighting matrix and the velocity of each joint, and task tracking error is constructed based on the end-effector velocity constraint. By combining the weighted control energy and the task tracking error, the objective function is obtained.

4. The self-protection control method for a robotic arm as described in claim 3, characterized in that, The weighting term is constructed using a penalty gain coefficient and a penalty nonlinearity exponent.

5. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The specific objective function is as follows: ； In the formula, Let be the objective function. For joint velocity, The damping factor, T It is a transpose operator. The joint space weighting matrix, For the desired terminal velocity, It is a Jacobian matrix.

6. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The process of combining the first and second velocities of each joint to obtain the reference velocity and reference position of each joint specifically includes the following steps: Multiplying the second velocity by the dynamic gain used to adjust the secondary task yields the null space component; Add the first velocity to the null space component to obtain the reference velocity; Integrating the reference velocity yields the reference position.

7. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The construction of the optimization model specifically includes the following steps: Discretize the robotic arm system to obtain The state-space equation at time t; Based on health factors, the feasible region of health constraint torque is determined. The dynamic parameters of the nonlinear dynamic equation are frozen and then linearized to transform the feasible region of health constraint torque into torque constraints. The optimization objectives are to minimize the difference between the real-time position and real-time velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. An optimization model is constructed with state-space equations, torque constraints, position and velocity constraints as constraints.

8. The self-protection control method for a robotic arm as described in claim 7, characterized in that, The optimization model is as follows: ； ； ； ； ； In the formula, To optimize the objective, for k Joint acceleration at any moment for The state vector at time t, To predict the length of the time domain, In order to control the current moment Based on this, the future +1 prediction step system state vector In order to control the current moment Based on this, the future +1 prediction step reference velocity and reference position, This is the process state error weight matrix. To control the energy consumption weight matrix, For the terminal target state, This is the terminal cost weight matrix. and These are the state matrix and input matrix of the linearized time-varying system, respectively. The inertia matrix is ​​a locally linearized matrix. For locally linearized Coriolis force and centrifugal force terms, For locally linearized gravity terms, For Hadama accumulation, For joint health factor vectors, The rated output torque under healthy conditions. for Joint velocity at any given moment and These are the minimum and maximum values ​​of the state vector. and These represent the minimum and maximum values ​​of joint acceleration.

9. The self-protection control method for a robotic arm as described in claim 1, characterized in that, The process of generating joint torque commands through the optimal acceleration sequence of each joint specifically includes the following steps: Substituting the first term of the optimal acceleration sequence of each joint into the linearized dynamic equation yields the feedforward term; Feedback terms are obtained based on position and velocity errors; The feedforward and feedback terms are added together to obtain the joint torque command.

10. A self-protection control system for a robotic arm, characterized in that, include: The data acquisition module is used to acquire the real-time position and speed of each joint of the robotic arm during the current control cycle. The kinematics solution module is used to obtain the corresponding health factors based on the torque output capability of each joint, input multiple health factors into the objective function and solve it to obtain the first velocity of each joint; The second velocity of each joint is obtained based on the safety configuration of the robotic arm; the first and second velocities of each joint are combined to obtain the reference velocity and reference position of each joint; wherein, the objective function takes minimizing the joint velocity energy as the optimization objective and the end-effector velocity as the constraint; The dynamics solution module is used to input the real-time position and real-time velocity of each joint into the optimization model and solve it to obtain the optimal acceleration sequence of each joint. The optimization model aims to minimize the difference between the real-time position and real-time velocity of each joint and the reference velocity and reference position, and to minimize the joint acceleration. The constraints are state-space equations, torque constraints, position and velocity constraints. The control module is used to generate joint torque commands by using the optimal acceleration sequence of each joint to control the movement of each joint of the robotic arm.

Images