Clamping control method of mobile mechanical arm

Abstract

The invention relates to a clamping control method of a movable mechanical arm. The clamping control method is used for solving the problem of collaborative optimization of stability and precision in clamping operation. According to the method, a motion coupling factor and a contact force gradient mode length are calculated in real time through a dynamic coordination module, base-world coordinate system conversion is combined to realize bidirectional constraint of chassis speed and mechanical arm torque, instability caused by sudden load change is inhibited, a time-varying sliding mode surface and an anti-buffeting control law are adopted for sliding mode control, and the stability of a mechanical arm is improved. Dynamic boundary layer thickness and adaptive gain are introduced, control parameters are further optimized through a double-delay depth deterministic strategy gradient, and a multi-target reward function is designed. According to the method, the robustness of the system is remarkably improved through closed-loop optimization, and the stability and precision of the movable mechanical arm in the clamping operation are improved.

Description

Technical Field

[0001] The present invention belongs to the field of robots, and particularly relates to a clamping control method for a mobile manipulator. Background Art

[0002] As a composite robot system integrating a mobile platform and a multi-degree-of-freedom manipulator, the mobile manipulator demonstrates important application value in the fields of industrial automation, logistics sorting, emergency rescue, etc. Its core challenge lies in how to coordinate the motion relationship between the mobile chassis and the manipulator. Especially when performing high-precision clamping tasks in a dynamic and unstructured environment, factors such as sudden load changes, external disturbances, and uneven ground are likely to cause system instability, resulting in clamping failure or increased energy consumption.

[0003] Traditional control methods, such as PID and fixed-gain sliding mode control, can partially solve the above problems, but have the following limitations: insufficient dynamic coordination: the dynamic coupling between the manipulator and the chassis is strong, and the control strategy with fixed parameters is difficult to adapt to the real-time change of the load, resulting in the risk of chassis overturning or the end of the manipulator shaking; contradiction between chattering and precision: traditional sliding mode control relies on high-frequency switching control signals to suppress disturbances, but it will introduce significant chattering, affecting the clamping precision and mechanical life; difficulty in multi-objective optimization: the clamping task needs to consider stability, energy saving and precision, and traditional methods are difficult to balance the conflicts between multiple objectives.

[0004] In recent years, reinforcement learning has received attention due to its adaptive optimization ability. However, applying it to mobile manipulators still faces challenges: efficient exploration of high-dimensional continuous state-action spaces, algorithm delays in real-time control, and reward function design under complex physical interactions. Summary of the Invention

[0005] The present invention proposes a clamping control method for a mobile manipulator, which includes the following steps: Real-time collect the contact force information at the end of the manipulator, the linear velocity of the chassis, and the joint torque of the manipulator, and dynamically calculate the motion coupling factor ω c , ω c =k1*(v base ×τ arm )+k2*||ΔF||, where k1 is the motion coupling coefficient, k2 is the force field sensitivity coefficient, and ΔF is the contact force gradient field, , where R b w is the rotation matrix from the chassis to the world coordinate system, is the tensor product; Design a time-varying sliding mode control, and its sliding mode surface is , where E is the joint angle and velocity error, K(t) is the gain matrix adaptive to the path curvature, and γ is the dynamic integral gain; The double-delay deep deterministic policy gradient algorithm is used to optimize the control parameters, and the parameter increment is dynamically adjusted through a multi-objective reward function; Output joint torque τ and chassis speed command v base ′ = v base ·sat(ω c )。

[0006] Specifically, the gain matrix K(t) satisfies the equation , where R(x) is the environmental curvature tensor and Q(x) is the kinetic energy conservation constraint matrix.

[0007] Specifically, the sliding mode control law is , where J c + is the pseudo-inverse of the Jacobian matrix, ρ is the adaptive gain, Λ is the dynamic coupling constraint matrix, M is the inertia matrix, C is the Coriolis force estimation matrix, G is the gravity compensation term, and δ is the sliding mode boundary layer thickness.

[0008] Specifically, the sliding mode boundary layer thickness δ satisfies δ = 0.1||ΔF|| + 0.05*ω c 。

[0009] Specifically, the saturation function of the chassis speed command is 。

[0010] Specifically, the state space of the double-delay deep deterministic policy gradient algorithm is , the action space is the control parameter increment, and the target Q value is calculated through a double Critic network. The reward function is , where σ F is the force volatility, θ align is the alignment angle between the tool and the target, E consumption is the energy consumption, Vibration amp is the vibration amplitude, and A, B, C, and D are the weights of each item.

[0011] Specifically, the double-delay deep deterministic policy gradient algorithm adopts a delayed Actor network update strategy, and the Actor update is performed once every 2 Critic updates.

[0012] Specifically, the environmental curvature tensor R(x) is , where κ i is the principal curvature of the trajectory.

[0013] Beneficial technical effects: By dynamically coordinating the module to adjust the coupling relationship between the chassis speed and the manipulator torque in real time, the risk of chassis overturning caused by sudden load changes or external force disturbances is suppressed; the improved sliding mode control law combines adaptive gain and dynamic boundary layer thickness to reduce the amplitude of chattering, avoid mechanical wear caused by high-frequency switching, reduce the steady-state tracking error of the sliding mode control, and improve the clamping reliability; in an unstructured environment (such as uneven ground, dynamic obstacles), by adaptively adjusting the sliding mode gain according to the environmental curvature, the path tracking accuracy is improved and the system robustness is significantly enhanced. Description of the Drawings

[0014] Appendix Figure 1 It is a schematic diagram of the clamping operation of the mobile manipulator; Appendix Figure 2 It is a flowchart of the clamping control method of the mobile manipulator according to the present invention. Detailed Embodiments

[0015] As Figure 1 shown, the mobile manipulator generally includes a mobile base, which is located at the bottom of the manipulator and is used to provide stable support for the manipulator and a spatial movement carrier; a manipulator, the most common form of which is a joint-type manipulator, and one or more actuators are usually installed at the end of the manipulator to complete specific tasks, such as clamping. The end effector can be provided with a six-axis force sensor as needed, and the position and posture of the end effector are adjusted through joint movement, so as to perform clamping.

[0016] As Figure 2 shown, the present invention proposes a clamping control method for a mobile manipulator, which includes the following steps: Contact force perception and dynamic coordination, specifically: Real-time acquisition of the contact force F contact =[f x , f y , f z , τ x , τ y , τ z T , the chassis linear velocity v base and the manipulator joint torque τ arm ; Dynamically coordinate and calculate the motion coupling factor: ω c =k1*(v base ×τ arm )+k2*||ΔF||, where k1 is the motion coupling coefficient, the initial value can be 0.8, k2 is the force field sensitivity coefficient, the initial value can be 1.2, and ΔF is the contact force gradient field, specifically , where R b w is the rotation matrix from the chassis to the world coordinate system, ​is the tensor product; Output the motion coupling factor ω c and the magnitude of the contact force gradient ||ΔF||, where the motion coupling factor ω c is used to limit the chassis speed, and the magnitude of the contact force gradient ||ΔF|| is used for the gain adjustment of the sliding mode controller.

[0017] Sliding mode control: Collect the desired trajectory error E in real time, , that is, the joint angle and velocity error, and the motion coupling factor ω c , and the magnitude of the contact force gradient ||ΔF||.

[0018] The time-varying sliding mode surface is designed as: , where γ is the dynamic integral gain. When the error is large, γ can increase the integral gain to accelerate the system to converge to the sliding mode surface. When approaching the steady state, the gain is reduced to avoid overshoot and balance the response speed and stability. By dynamically adjusting the integral action, it can effectively cope with uncertainties such as load mutations and external disturbances, and reduce the risk of oscillation or instability that may be caused by traditional fixed-gain integration. The gain matrix K(t) satisfies: , where R(x) is the environmental curvature tensor, a tensor describing the geometric characteristics of the robot's motion environment, which is jointly determined by the curvature of the chassis motion trajectory and the manipulator configuration. In an unstructured environment (such as an uneven ground), the curvature tensor reflects the degree of curvature of the motion path. , where κ i is the principal curvature of the trajectory. By affecting the adjustment of the sliding mode gain matrix K(t), when the path curvature is larger, the control gain is reduced to avoid overshoot. The energy constraint matrix Q(x) is determined by the conservation of kinetic energy, a symmetric positive definite matrix derived from the kinetic energy conservation equation of the robot, which is used to constrain the change range of the sliding mode gain K(t) and prevent instability caused by sudden energy changes. The control law is:

[0019] where the pseudo-inverse J of the Jacobian matrix c + is the pseudo-inverse of the Jacobian matrix J of the end effector of the manipulator c , calculated as =J c T (J c J c T ) -1 . The adaptive gain ρ = ρ0 + α||ΔF||, α is the reinforcement learning optimization parameter, and the dynamic coupling constraint matrix Λi = diag(ω c *λ i) where M is the inertia matrix, C is the Coriolis force estimation matrix, and G is the gravity compensation term; δ is the thickness of the sliding mode boundary layer. The smaller the value, the higher the accuracy of approaching the ideal sliding mode surface, but it may cause chattering. The larger the value, the smoother the control signal, but it will introduce a steady-state error. Therefore, δ can be set to 0.1||ΔF|| + 0.05*ω c , balancing accuracy and smoothness, and can be dynamically adjusted through parameter optimization later; σ is the sliding mode surface.

[0020] Optimization of double-delay deep deterministic policy gradient parameters, specifically: 1. State-action space: Collect q, joint angular velocity, chassis attitude angle θ base , chassis speed v base , F contact , and set the state as , action a t = [Δk1, Δk2, Δρ, Δδ], that is, the parameter increment 2. Reward function design:

[0021] Among them, σ F is the force volatility, σ F = std(F contact ), that is, the contact force standard deviation, θ align is the alignment angle between the tool and the target, specifically the angle between the end effector coordinate system and the target direction, energy consumption , Vibration amp is the vibration amplitude, which is the maximum vibration amplitude at the end of the robotic arm in the frequency domain analysis, usually taking the peak value in the 0-100Hz frequency band. The weights A, B, C, D of each item can be 0.4, 0.2, 0.3, 0.1 respectively.

[0022] 3. Network update strategy: (1) Double Critic to suppress overestimation: Q = min(Q θ1′ , Q θ2′ ), where Q θ1′ , Q θ2′ is the target Critic network, a neural network used to calculate the target Q value. The minimum value is taken through the double Critic network to suppress the overestimation of the Q value.

[0023] (2) Delayed Actor update: φ′ ← 0.995φ′ + 0.005φ; (3) Perform 1 Actor update after every 2 Critic updates.

[0024] Output joint torque τ, chassis speed command v base ′ = v base· sat(ωc), speed limit through the saturation function sat. sat can be: .

[0025] Through ω c Real-time constraint on the chassis speed, while ||ΔF|| adjusts the sliding mode control gain to form a bidirectional coupling. In the sliding mode control, δ is dynamically adjusted by combining the double-delay deep deterministic strategy to suppress chattering while maintaining robustness. The double-delay deep deterministic strategy realizes multi-objective optimization through the composite index of the reward function to ensure the overall optimality of the system.

[0026] This scheme deeply integrates dynamic coordination, sliding mode control and reinforcement learning, realizes a closed loop from force perception to motion control and then to parameter optimization, and significantly improves the stability and accuracy of the mobile manipulator in the gripping operation.

[0027] The present invention also proposes a dynamic obstacle avoidance control method for a mobile manipulator, which includes the following steps: Allocate dynamic priorities: According to the environmental threat and the motion state of the manipulator, the priority weights of the chassis and the manipulator are adjusted in real time to achieve a balance between obstacle avoidance and motion planning.

[0028] Obtain environmental perception data and joint states through a depth camera and sensors, and calculate the reachability index Ri(q) and the dynamic weight matrix W(t). The dynamic weight matrix W(t) serves as the weight matrix of the subsequent control objective function, and the value of the reachability index Ri(q) is used as the input of the parameter optimization module.

[0029] Among them, the dynamic weight matrix W(t) is:

[0030] Among them, ζ is the forgetting factor, ζ ∈ [0.05, 0.2], which is used to control the attenuation speed of historical data. This parameter can be set and adjusted according to experience or optimized in real time through the parameter optimization module. σ base , σ arm Are respectively the chassis movement priority and the manipulator joint movement priority, which can be calculated by fuzzy logic or set and adjusted according to experience. σ base , σ arm ∈ (0, 1), R base Is the chassis motion reachability matrix, which represents the speed limit matrix of the movable directions of the chassis at the current position. For example, it can be diag(v x,max , v y,max , ω max ), R i (q) is the manipulator joint reachability index, R i (q) = det(JJ T ) + h, h = 10 -6 , which is used to prevent singularity. J is the Jacobian matrix.

[0031] When the detected distance d to the obstacle ahead is less than the safety threshold d0, the chassis movement priority is set to σ base ←σ base +0.7(1 - d / d0). The closer the obstacle is, the more the chassis obstacle avoidance action takes precedence over the manipulator movement; when the joint approaches the singular configuration, that is, when R i (q) tends to 0, the manipulator joint movement priority is set to σ arm ←σ arm +0.5*(1 - ||Ri(q)||), so as to avoid the manipulator getting out of control due to the singular configuration. By allocating dynamic priorities, the success rate of obstacle avoidance can be improved, the movement flexibility of the mobile manipulator can be monitored in real time, and the risk of singular configurations can be reduced.

[0032] Dynamic obstacle prediction and repulsive potential field planning: By fusing visual perception and motion prediction, a dynamic repulsive force field is generated to guide obstacle avoidance, thereby improving the prediction accuracy of dynamic obstacle trajectories.

[0033] Among them, the improved potential field function U rep is:

[0034] Among them, η is the potential field strength coefficient, the initial value of η can be 2.5, and it can be adjusted through the parameter optimization module or empirical setting; λ is the velocity decay factor, and λ can be 0.1; v obs is the predicted vector of the relative velocity of the obstacle, which is predicted and output by the Vision Transformer, d is the current distance to the obstacle, and d0 is the safety threshold. When the obstacle is stationary, that is, v obs =0, it is transformed into a traditional potential field; when the obstacle approaches at high speed, the short-term repulsive force is enhanced through the exponential term.

[0035] Among them, the specific prediction of the Vision Transformer is as follows: (1) Input the environmental point cloud data. The point cloud can be divided into sub-regions of 16×16×16 according to the spatial grid, and the geometric center coordinates, point cloud density, average color, and normal vector direction features of each sub-region are extracted.

[0036] (2) The encoding layer calculates the motion correlation degree of the obstacle through the multi-head attention mechanism:

[0037] Among them, Q, K, and V represent the query, key, and value matrices respectively. The query matrix Q focuses on the motion correlation requirements of the current block. If a moving obstacle is detected in the block, Q will strengthen its time dimension features. The key matrix K describes the spatio-temporal features of other blocks and calculates the correlation weights between other blocks and the current block. The value matrix V is the actual transmitted feature value, such as the velocity change amount, d kFor the feature dimension, the softmax function is used to convert the attention scores into a probability distribution. , A high probability value indicates that the block has a significant impact on the current motion prediction, such as an obstacle approaching rapidly. Unrelated noise, such as a static object in the distance, is filtered out by a low probability value.

[0038] (3) Output the predicted obstacle motion vector.

[0039] Through the calculation of the obstacle motion correlation, the motion trend of the obstacle can be identified, such as lateral movement or accelerating approach, distinguish static obstacles from dynamic obstacles, and determine the motion coupling relationship between multiple obstacles.

[0040] Spatio-temporal constraint model predictive control: On the premise of meeting the set constraints, through model predictive control, continuous and smooth motion trajectory planning is achieved. The objective function of model predictive control is:

[0041] Where, Q des , Q act are the desired and actual attitude quaternions respectively, τ is the joint torque, W τ is the torque weight matrix, W τ =diag(1 / τ max 2 ), τ max is the maximum joint torque, θ i is the joint angle, θ safe is the joint safety threshold, k is the steepness coefficient, u is the control input vector, which includes joint torque and chassis speed, t0, t f are the start and end times of the prediction horizon respectively, α, β, γ are the weights of the kinematic term, dynamic term, and ergonomic term respectively, which are used to adjust the balance of attitude tracking accuracy and obstacle avoidance sensitivity, suppress the sudden change of joint torque, and avoid the joint exceeding the safe angle range, and can be set to 0.6, 0.3, 0.1 respectively.

[0042] The constraint conditions of model predictive control are:

[0043] The constraint conditions include quaternion differential constraint, maximum torque constraint, contact force constraint, and dynamic priority constraint. By embedding the dynamic weight matrix W(t) into the constraint conditions, the motion bandwidth of high-priority tasks is guaranteed, and the Euler angle singularity problem is avoided through the quaternion differential constraint.

[0044] Parameter co-optimization: Dynamically adjust the parameters of each module through the improved double-delayed deep deterministic policy gradient algorithm to achieve the optimal global performance. Specifically: (1) State-action space definition The state s includes joint state, dynamic weight, reachability index, and obstacle speed prediction.

[0045] The action a = [ζ, η, α, β, γ], which means optimizing the dynamic priority, potential field strength, and model prediction weight coefficient simultaneously.

[0046] (2)Reward function design

[0047] Among them, Δt collision is the collision time interval, which is set to ∞ or a fixed value such as 10 s when there is no collision, E consumption is the energy consumption calculated by integrating the motor current, θ tool is the angle between the tool coordinate system and the target direction, σ τ is the standard deviation of the joint torque, which is used to reflect the load balance. The weights A, B, C, and D of each item can be 0.4, 0.2, 0.3, and 0.1 respectively.

[0048] (3)Network update strategy, which is specifically as follows: 3.1 Double Critic to suppress overestimation: , where Q θi′ is the target Critic network, which provides a stable Q-value estimate to prevent policy oscillation caused by overfitting of a single network. π(s′) is the target Actor network, which generates suboptimal action exploration, delays the update of the policy network to improve stability, and j is the exploration noise, which enhances environmental exploration at the beginning of training to avoid falling into local optima.

[0049] 3.2 Delayed policy update: Perform 1 Actor update after every 2 Critic updates; 3.3 Soft update the target network as φ′←0.995φ′ + 0.005φ.

[0050] Through the improved double-delayed deep deterministic policy gradient algorithm, the output parameter compensation amount is obtained. By adjusting the model predictive control weight parameters α, β, γ, the dynamic priority parameter ζ, and the potential field strength η, the final command is affected, and dynamic response adjustment is achieved. For example, when an emergency obstacle avoidance is detected, the algorithm increases η, resulting in an enhanced potential field repulsive force, and the v base generated by the model predictive control increases accordingly.

[0051] Control command synthesis: Generate joint angular velocity and chassis movement speed commands.

[0052] This method directly embeds dynamic priority weights into the objective function of model predictive control to achieve a decision-making and planning closed-loop; predicts the speed of obstacles to correct the potential field and motion constraints in real time; and achieves global optimization through an improved double-delayed deep deterministic policy gradient algorithm, which runs through all adjustable parameters and breaks through the local optimum limit.

[0053] In several embodiments provided by the present invention, it should be understood that the disclosed method can be implemented in other ways. For example, the above-described invention embodiments are merely illustrative. For example, the division of method modules and steps is only a logical function division, and there may be other division methods in actual implementation. Modules and steps described as separate components or separate steps may or may not be physically separated. Components shown as modules may or may not be physical modules, and may be located in one place or distributed to multiple network modules. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0054] In addition, each functional module and step in various embodiments of the present invention can be integrated into one processing module or method, or each module and step can exist physically alone, or two or more modules and steps can be integrated into one module or method. The above integrated modules can be implemented in the form of hardware or in the form of hardware plus software functional modules.

[0055] For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and can be implemented in other specific forms without departing from the basic characteristics of the present invention.

[0056] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A clamping control method for a mobile manipulator, characterized in that, It includes the following steps: Real-time collect the contact force F at the end of the robotic arm, the linear velocity v of the chassis base and the joint torque τ of the robotic arm arm , dynamically calculate the motion coupling factor ω c , ω c =k1*(v base ×τ arm ), where k1 is the motion coupling coefficient, k2 is the force field sensitivity coefficient, and ΔF is the contact force gradient field, , where R b w is the rotation matrix from the chassis to the world coordinate system, is the tensor product; Design a time-varying sliding mode control, and its sliding mode surface is , where E is the joint angle and velocity error, K(t) is the gain matrix adapted to the path curvature, and γ is the dynamic integral gain; Adopt the double-delayed deep deterministic policy gradient algorithm to optimize the control parameters, and dynamically adjust the parameter increment through a multi-objective reward function; Output joint torque τ and chassis speed command v base ′ = v base ·sat(ω c )。 2. The method according to claim 1, wherein The gain matrix K(t) satisfies the equation , where R(x) is the environmental curvature tensor and Q(x) is the kinetic energy conservation constraint matrix.

3. The method according to claim 1, wherein The sliding mode control law is , where J c + is the pseudo-inverse of the Jacobian matrix, ρ is the adaptive gain, Λ is the dynamic coupling constraint matrix, M is the inertia matrix, C is the estimated Coriolis force matrix, G is the gravity compensation term, and δ is the sliding mode boundary layer thickness.

4. The method according to claim 3, characterized in that, The thickness δ of the sliding mode boundary layer satisfies δ = 0.1||ΔF|| + 0.05*ω c .

5. The method according to claim 1, characterized in that The saturation function of the chassis speed command is .

6. The method according to claim 1, characterized in that, The state space of the Double Delayed Deep Deterministic Policy Gradient algorithm is , the action space is the control parameter increment, and the target Q value is calculated through the Double Critic network. The reward function is , where σ F is the force volatility, θ align is the alignment angle between the tool and the target, E consumption is the energy consumption, Vibration amp is the vibration amplitude, and A, B, C, and D are the weights of each item.

7. The method according to claim 6, characterized in that, The double-delayed deep deterministic policy gradient algorithm adopts a delayed Actor network to update the policy φ′←0.995φ′+0.005φ, and performs 1 Actor update after every 2 Critic updates.

8. The method according to claim 2, wherein The environmental curvature tensor R(x) is , where κ i is the principal curvature of the trajectory.

9. A mobile manipulator system for performing the clamping control method according to any one of claims 1-8.

Images