A multi-target force position control method for a serial robotic arm

Abstract

This invention proposes a multi-objective force-position control method for serial robotic arms, applied to solve the interactive force control and path tracking problems of constrained collaborative robots. For elastic interaction force models, there is always a mechanical trade-off between interaction force and position; that is, neither force nor path tracking can fully meet the expected requirements. Based on this, two irreconcilable control specifications are proposed: a force objective function and a position tracking objective function. At each sampling time, the scheme automatically selects the control action from the Pareto optimal solution set based on the Nash bargaining solution in cooperative game theory. Furthermore, process constraints and control constraints are set, allowing the interaction force to be limited by user-defined values, and simultaneously satisfying actuator saturation within the control scheme. The control strategy proposed in this method is mainly applied in the grinding and machining field, capable of accurately tracking a given trajectory while maintaining constant relative forces.

Description

Technical Field

[0001] This invention relates to the field of multi-objective model predictive control, specifically to a multi-objective force-position control method for a serial robotic arm. Background Technology

[0002] In complex dynamic environments, robotic arms need to react to the external environment in real time and maintain the required position and force. This requires the robotic arm to have rapid sensing and control capabilities to cope with various emergencies, such as external interference and object movement; the accuracy and reliability of force sensors are crucial for force-position control. However, sensors may be affected by factors such as noise, drift, and non-serialization, leading to inaccurate or unstable measurement results, thus affecting the performance of the control system; the structural stiffness and compliance of the robotic arm have a significant impact on force-position control. Higher stiffness can improve the positioning accuracy of the robotic arm, but may increase sensitivity to external forces; conversely, higher compliance can reduce sensitivity to external forces, but may affect positioning accuracy; force-position control involves multiple control variables (position and force), requiring the design of complex control algorithms to achieve the desired control objectives. These algorithms need to consider factors such as the dynamic characteristics of the robotic arm, changes in the external environment, and uncertainties in sensor data, making algorithm design more complex and difficult; force-position control requires real-time adjustment of the robotic arm's motion trajectory and force to achieve the desired control objectives. Therefore, the control system needs to have high real-time performance and stability to ensure that the robotic arm can operate stably in various working scenarios. Robots are capable of performing a wide range of interactive tasks, such as picking and placing objects, deburring, polishing, painting, and high-precision positioning and assembly. All of these tasks require collaborative robots to simultaneously achieve precise, compliant force control and accurate path following to avoid generating forces that could harm the robot or the environment (and sometimes humans). Performing constrained motion through a pure position control system requires a very precise manipulator model and a very accurate understanding of the geometry and mechanical properties of the environment. As long as each mathematical model is only an approximation of reality, modeling errors will occur, leading to deviations from planned contact conditions. Since contact with the environment occurs in the form of interaction forces, large contact forces can be generated. Kazerooni and Houpt proposed that typical position control systems are considered stationary because they attempt to follow the commanded position trajectory while rejecting external forces considered disturbances. In constrained motion, contact forces are also treated as disturbances; the system attempts to reject them, resulting in even larger interaction forces. This behavior can lead to saturation, instability, and even physical failure. Correct execution of constrained motion tasks can be achieved by attempting to adjust the control system to unplanned external forces. Furthermore, the additional information provided by force sensors, if direct measurements of contact forces are used in the control strategy, can help compensate for a lack of real-world knowledge. Since contact force is a crucial representation of interaction with the environment, force control technology has been developed. Chiaverini proposed a dynamic force / position parallel control law and analyzed its performance in an elastic environment, illustrating the fundamental properties of the method. The implementation problem is discussed, and a PID-type parallel controller is derived by simplifying the aforementioned dynamic control law.

[0003] The main problems with existing control strategies are:

[0004] 1) High control algorithm complexity: Existing force-position control algorithms are often highly complex, requiring significant computational resources and time to implement. This limits their feasibility in real-time applications and increases the difficulty of system design and debugging.

[0005] 2) Insufficient adaptability to environmental changes: Existing force-position control technology often exhibits insufficient adaptability when facing complex and dynamic environments. For example, when the working environment changes, such as changes in the position, shape, or mass of an object, the control system may not be able to adjust in time to adapt to the new working conditions.

[0006] 3) Lack of versatility and generalization: Existing force and position control technologies are often limited to specific application scenarios or mechanical structures, lacking versatility and generalization capabilities. This means that control systems need to be redesigned and optimized for different application scenarios, increasing the cost and time of development and deployment.

[0007] 4) Sensitivity to external disturbances: Existing force position control technology is too sensitive to external disturbances (such as wind, vibration, etc.), which causes the movement trajectory and force of the robotic arm to be disturbed, thereby affecting the quality of task completion.

[0008] 5) Error accumulation in complex environments: In complex working environments, the robotic arm may be affected by various factors when performing tasks, such as friction, gravity, inertia, etc. These factors may lead to the accumulation of errors in the control system, ultimately affecting the accuracy and stability of the robotic arm. Summary of the Invention

[0009] To address the aforementioned technical problems, this invention proposes a multi-objective force-position control method for a serial robotic arm, outlining two irreconcilable control parameters: a force objective function and a position tracking objective function. Based on this, the scheme automatically selects the control action from the Pareto optimal solution set at each sampling time, according to the Nash bargaining solution in cooperative game theory.

[0010] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0011] A multi-target force-position control method for a serial robotic arm, using the robotic arm as the controlled object, tracks the trajectory of the robotic arm based on a given reference trajectory, and controls the end effector force of the robotic arm:

[0012] S01: Perform dynamic modeling of the robotic arm;

[0013]

[0014] Where τ is the joint torque and D(q) is the inertia matrix. G(q) is the Coriolis force, and G(q) is gravity. After obtaining the joint torque, the joint angle, velocity, and acceleration are obtained through the positive dynamic equation, and then the state update equation is performed.

[0015] x k+1 =A s x k +B s u k

[0016]

[0017] S02: Establish the cost function for position and force;

[0018]

[0019] Where A and B are the weight moments. This is the difference between the target state and the current state.

[0020] S03: The minimum value of the position and force cost function is obtained by applying the NSGAII algorithm, and the most suitable joint torque is derived from it.

[0021] The two cost functions mentioned in S02

[0022]

[0023]

[0024] We treat J1 and J2 as two functions of u in a multi-objective optimization process. The optimal u, i.e., the joint torque, is obtained through crossover mutation, non-dominated sorting, and crowding calculation. Then, we return to S01 and update the state equation.

[0025] Furthermore, step S01 defines the system state of the robotic arm. q is the joint angle. Let denot be the joint angular velocity, p be the position of the robotic arm's end effector, and F be the force applied to the end effector. For the prediction model, consider a second-order Taylor series expansion.

[0026]

[0027] Therefore, the state update equation is obtained:

[0028]

[0029]

[0030] Furthermore, in step 03, e(p) represents the path tracking error, which is defined as follows: The corresponding e(f) represents the interaction force error, which is defined as follows: To avoid collisions with the robot, the joint coordinates have a bounded constraint:

[0031] q min ≤q(k)≤q max k = 1, ..., n p

[0032] Furthermore, considering the potential overshoot of force control that could damage the robot or the interactive environment, the desired interaction forces are set as state constraints:

[0033] F ext (k)≤F ext (k) * +δ f k = 1, ..., n p

[0034] It is the target force at step k, δ f This is the allowable error for interactive force control.

[0035] Furthermore, in step 03, the interactive environment is a planar and flexible frame with a rotation matrix R. c =[t 1c t 2c n c ], where n c It is the plane normal, t 1c t 2c Since the contact is planar tangent, from a statics perspective, assuming the contact model is a simplified spring model, the contact force is calculated as follows:

[0036] F d =K f (P e -P o )

[0037] P o P represents the position of any point on the undeformed plane. e K represents the equilibrium position. f The contact rigidity matrix is ​​defined as follows:

[0038]

[0039] k f,n As a positive coefficient, the elastic spring model shows that the contact force is perpendicular to the plane, resulting in F. d With n c Zero error in force during alignment, or only the contact plane (t) can be obtained. 1c ,t 2c Zero error in the position of P. e Along nc The directional component must be adapted to the required force F. d Therefore, a tradeoff position P is generated. e,∞ The description is as follows:

[0040]

[0041]

[0042] Furthermore, the multi-objective problem in step S3 is defined as follows:

[0043]

[0044] Constrained by:

[0045] g(x(t))≥0,

[0046] h(x(t))=0,

[0047] Where x(t) is the decision variable, and J(U,x) is the cost function vector, defined as:

[0048] J(U,x)=[J1(U,x),J2(U,x),…,J l (U,x)] T g(x(t)) and h(x(t)) are equality constraints and inequality constraints, respectively. The result of multi-objective optimization is a series of Pareto solutions called the Pareto optimal boundary. Solutions belonging to the Pareto boundary have the following property: For any feasible solution U belonging to the Pareto boundary... p There will be no other solution U that satisfies:

[0049] And at least one of them satisfies J j (U,x) <J j (U p ,x)j∈{1,…,l}

[0050] The Pareto solution can then be found using the following method:

[0051] Let α i ∈(0,1), and If U p ∈U satisfies;

[0052]

[0053] So U p It is the Pareto solution.

[0054] Furthermore, the selection of the optimal Pareto solution in step S3 has the following characteristics:

[0055] The Nash solution N(S,d) guarantees that the product of utility benefits starting from the threat point d is maximized, which is a reasonable solution;

[0056]

[0057] In the formula, J∈S, J≤d, meaning J is superior to d. Since all cost functions Ji are convex, the Pareto solution is obtained by minimizing the cascade combination of these cost functions;

[0058]

[0059] In the formula, α = [α1, ..., α l ] T It is a positive weight vector, and Since the Nash negotiation solution also falls within the Pareto boundary, there must exist an α. N Satisfy U * Assume a convex function J i (U), i = 1, ..., l are neighbor-differentiable, therefore the relationship between the Nash negotiation solution and the cost function is obtained:

[0060]

[0061] Beneficial effects:

[0062] This invention discloses a multi-objective force-position control method for a serial robotic arm, which has the following advantages: Smaller error in complex environments: In complex working environments, the robotic arm may be affected by various factors when performing tasks, such as friction, gravity, and inertia. These factors may lead to the accumulation of errors in the control system, ultimately affecting the accuracy and stability of the robotic arm. This method can effectively reduce error accumulation. Good robustness: Under the influence of external disturbances such as wind and vibration, the movement trajectory and force of the robotic arm remain basically unchanged, resulting in high task completion quality. Applicable to almost any serial robotic arm: Simple to use and highly universal. Attached Figure Description

[0063] Figure 1 Flowchart for finding Nash negotiation points in this invention;

[0064] Figure 2 This is a flowchart illustrating how the multi-objective MPC of this invention transforms into a traditional MPC process.

[0065] Figure 3 This invention relates to a 4-DOF robot;

[0066] Figure 4 This is the position and force balance model of the present invention;

[0067] Figure 5 This is a schematic diagram of the Nash equilibrium point of the present invention. Detailed Implementation

[0068] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0069] This invention addresses the interactive force control and path tracking problems of cooperative robots under constraints, proposing a multi-objective model predictive control (MPC) scheme based on Nash negotiation. The flowchart for finding the Nash negotiation point is shown below. Figure 1 As shown. For elastic interaction force models, there is always a mechanical trade-off between the interaction force and the position, as illustrated in the position-force equilibrium model. Figure 4 As shown, neither force nor path following can fully meet the expected requirements. Based on this, two irreconcilable control specifications are proposed: a force objective function and a position tracking objective function. Furthermore, a novel multi-objective MPC scheme is proposed, transforming the multi-objective MPC into a traditional MPC flowchart as shown below. Figure 2 As shown, this scheme automatically selects the control action from the Pareto optimal solution set based on the Nash negotiation solution in cooperative game theory at each sampling time. Furthermore, we set process constraints and control constraints so that the interaction forces can be limited by user-defined values, and actuator saturation can be satisfied simultaneously within the control scheme. We illustrate the proposed controller on a 4-DOF robotic arm. The structure of the robot is as follows... Figure 3 As shown. We set the robot's base as the origin of the global coordinate system. The center of the interaction plane is Pi = (0.3, 0, 0.4) m, and its distance from the global coordinate system is wrt. In the simulation environment settings, the force model between the robot's manipulator and the interaction plane is an elastic model, F ext =K e δ p Considering the actual interaction environment, we set Ke = 10000 N / m. Thus, we obtain the robot's force contact model. Assuming the task has no singularities and is within the reachable workspace of the robotic arm, the task trajectory is predefined. It consists of two phases: the first phase is the approach phase, and the second phase is the interaction phase.

[0070] Next, we perform dynamic modeling of the robot. For multi-joint serial robotic arms, we use the Lagrangian method. First, we calculate the inertia matrix of the robotic arm, which can generally be obtained directly in modeling software such as SolidWorks. The formula is as follows. The value of the inertia tensor is different in different coordinate systems. We can connect the inertia tensor matrices in different coordinate systems through coordinate rotation and the parallel axis shift theorem.

[0071]

[0072] Where Ixx=∫∫∫v (v 2 +z 2 )ρdv Iyy=∫∫∫ v (x 2 +z 2 )ρdv Izz=∫∫∫v(y 2 +x 2 )ρdv

[0073] Ixy=∫∫∫ v (xy)ρdv I xz =∫∫∫ v (xz)ρdv Iyz=∫∫∫ v (yz)ρdv

[0074] The Lagrange quantity L is the difference between the kinetic and potential energy of a system, i.e., L = TU, where T represents the kinetic energy of the system and U represents the potential energy. The kinetic energy of a robot can be expressed as the kinetic energy T of each joint. i The sum, that is Where n is the number of degrees of freedom of the robot. The kinetic energy of each joint can be calculated based on the joint's mass, velocity, and inertia matrix (moment of inertia). Typically, the joint kinetic energy T... i It can be represented as Where m i It is joint mass, v i It is the joint linear velocity, w i It is the joint angular velocity, I i It is the rotational inertia matrix of the joint.

[0075]

[0076]

[0077]

[0078] L1, L2, L3, and L4 are the lengths of the connecting rods.

[0079] r1 = 0.5L1; r2 = 0.5L2

[0080] x = L1 + L2c2 + L3c 23

[0081]

[0082] y = L2s² + L3s 23

[0083]

[0084]

[0085]

[0086] Calculation of potential energy U: The robot's potential energy can be expressed as the potential energy U of each joint. i The sum, that is The potential energy of each joint can include gravitational potential energy and elastic potential energy, among others. Gravitational potential energy can be expressed as U. i =m i gh i Where m i It is the joint mass, g is the acceleration due to gravity, h i This is the height of the joint (the perpendicular distance from the reference plane). Applying the Lagrange equation: The Lagrange equation can be written as... Q i This represents the external force or torque acting on the system. For each generalized coordinate q... i The Lagrange equations can be applied to the robot system to obtain the equations of motion for these coordinates. Solving the equations of motion reveals how the robot system's motion state changes over time. These equations are typically non-sequential differential equations, requiring numerical methods (such as numerical integration and numerical differential equation solving) to solve. Solving the equations of motion yields the robot system's trajectory, velocity, and acceleration under a given external force. Through these steps, a dynamic model of the robot can be established using the Lagrange method, and the system's equations of motion can be solved. Finally, we obtain the robot arm's moment of inertia, Coriolis matrix, and mass matrix.

[0087] Next, we calculate the cost function, and we define the system of the robotic arm. q is the joint angle. denoted as angular velocity of the joint, p as the position of the robotic arm's end effector, and F as the force applied to the end effector.

[0088]

[0089] Therefore, we obtain the state update equation:

[0090]

[0091]

[0092] e(p) represents the path tracking error, defined by the corresponding e(f) representing the interaction.

[0093] Therefore, we obtain two cost functions:

[0094]

[0095]

[0096] The optimal Pareto solution set for each step of the MPC is obtained using a multi-objective genetic optimization algorithm. The two cost functions J1 and J2 are used as the fitness functions for the genetic algorithm, and selection, crossover, and mutation are performed. Then, a Nash negotiation solution is searched from the Pareto solution set, as follows:

[0097]

[0098] In the formula, J∈S, J≤d, meaning J is superior to d. Since all cost functions Ji are convex functions, the Pareto solution can be obtained by minimizing the cascade combination of these cost functions.

[0099]

[0100] In the formula, α = [α1, ..., α l ] T It is a positive weight vector, and Since the Nash negotiation solution also falls within the Pareto boundary, there must exist an α. N Satisfy U * We assume that the function J is convex. i (U), i = 1, ..., l are neighbor-differentiable, therefore we can obtain the relationship between the Nash negotiation solution and the cost function.

[0101]

[0102] Figure 5 The N points shown are the obtained Nash negotiation points. Finally, the optimal input sequence for MPC is obtained, and the first value of the sequence is used as the joint torque to achieve optimal force-position control.

[0103]

[0104] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A multi-target force-position control method for a serial robotic arm, wherein the robotic arm is the controlled object, the trajectory of the robotic arm is tracked based on a given reference trajectory, and the end effector force of the robotic arm is controlled, characterized in that: S01: Perform dynamic modeling of the robotic arm; ； Where τ is the joint torque and D(q) is the inertia matrix. For Cocteau force, For gravity, after obtaining the joint torque, the joint angle, velocity, and acceleration are obtained through the positive dynamic equation, and then the state update equation is performed; ； ； S02: Establish the cost function for position and force; ； Where A and B are the weight matrices. This is the difference between the target state and the current state. S03: The minimum value of the position and force cost function is obtained by applying the NSGAII algorithm, and the most suitable joint torque is derived from it. The two cost functions mentioned in S02 ； These will be used as two functions for multi-objective optimization. Given two functions of u, the most suitable u, i.e., joint torque, is obtained by selecting crossover mutation, non-dominated sorting, and crowding degree calculation. Then, return to S01 and update the state equation.

2. The multi-target force position control method for a serial robotic arm according to claim 1, characterized in that: Step S01 defines the system state of the robotic arm. q is the joint angle. Let denot be the joint angular velocity, p be the position of the robotic arm's end effector, and F be the force applied to the end effector. For the prediction model, consider a second-order Taylor series expansion. ； Therefore, the state update equation is obtained: ； 。 3. The multi-target force position control method for a serial robotic arm according to claim 1, characterized in that: In step 03 The path tracking error is defined as follows: Corresponding The error in the interaction force is defined as follows: To avoid collisions with the robot, the joint coordinates have a bounded constraint: ； Furthermore, considering the potential overshoot of force control that could damage the robot or the interactive environment, the desired interaction forces are set as state constraints: ； It is the target force at step k. This is the allowable error for interactive force control.

4. The multi-target force position control method for a serial robotic arm according to claim 3, characterized in that: In step 03, the interactive environment is a planar and flexible frame with the following rotation matrix: ,in It is the plane normal. , Since the contact is planar tangent, from a statics perspective, assuming the contact model is a simplified spring model, the contact force is calculated as follows: ； Indicates the position of any point on the undeformed plane. Indicates the equilibrium position. The contact rigidity matrix is ​​defined as follows: ； As a positive coefficient, the elastic spring model shows that the contact force is perpendicular to the plane, thus yielding... and Zero error in force during alignment, or only the contact plane can be obtained. Zero error in position on, along The directional component must be adapted to the required force. Therefore, a trade-off position is created. The description is as follows: 。 5. The multi-target force position control method for a serial robotic arm according to claim 1, characterized in that: The multi-objective problem in step S3 is defined as follows: ； Constrained by: Where x(t) is the decision variable, and J(U,x) is the cost function vector, defined as: g(x(t)) and h(x(t)) are the equality constraint and inequality constraint, respectively; the result of multi-objective optimization is a series of Pareto solutions called the Pareto optimal boundary. Solutions belonging to the Pareto boundary have the following property: for any feasible solution belonging to the Pareto boundary... There will be no other solution U that satisfies: , And at least one of them satisfies ; The Pareto solution can then be found using the following method: set up , and , if satisfy; ； So It is the Pareto solution.

6. The multi-target force position control method for a serial robotic arm according to claim 5, characterized in that: The selection of the optimal Pareto solution in step S3 has the following characteristics: The Nash solution N(S, d) guarantees that the product of utility benefits starting from the threat point d is maximized, which is a reasonable solution; ； In the formula, J∈S, J≤d, that is, J is better than d, because all cost functions Ji are convex, so the Pareto function solution is obtained by minimizing the cascade combination of these cost functions; ； In the formula It is a positive weight vector, and Since the Nash negotiation solution also falls within the Pareto boundary, there must exist a... satisfy Assume a convex function Since i=1,…,l are neighbor-differentiable, we can obtain the relationship between the Nash negotiation solution and the cost function: .

Images