Robot force state estimation and feasibility correction method, device and storage medium

Abstract

The application relates to the field of intelligent robot control, and provides a robot force state estimation and feasibility correction method, a device and a storage medium. The method comprises the following steps: acquiring candidate contact states and credibility information and parameters of a robot in a contact operation process; constructing a physical constraint set according to a contact mode corresponding to the candidate contact states, and assigning corresponding confidence weights and maximum relaxation amounts to each soft constraint according to the credibility information; taking the candidate contact states as initial values, outputting a corrected contact state, an active constraint identifier, a hard constraint residual error, a soft constraint relaxation amount and a feasibility level according to the confidence weights, and if the hard constraints of any candidate mode cannot be simultaneously satisfied or are not converged within a preset solving time length, only the hard constraints are retained and a safety degradation operation is performed; the corrected state that passes the verification and has a qualified feasibility level is converted into a robot control parameter and is issued and executed, and execution feedback information is collected and returned after model parameters are updated.

Description

Technical Field

[0001] This application relates to the field of intelligent robot control, and in particular to a method, apparatus and storage medium for robot force state estimation and feasibility correction. Background Technology

[0002] Robotic contact operations (e.g., constant force following on curved surfaces, two-finger grasping, human-robot collaborative assembly, etc.) are one of the core application scenarios in the field of industrial and service robots. In such operations, robots need to estimate their contact state with the environment in real time through multimodal perception (vision, force perception, proprioception), including the position of the contact point, normal direction, gap, normal / tangential contact force, and contact mode (separation, adhesion, slippage, etc.), and generate force / position hybrid control commands accordingly to achieve stable and safe interaction.

[0003] In existing technologies, the acquisition of contact states mainly relies on two approaches: one is an observer based on a physical model, and the other is a data-driven multimodal fusion model. The former relies on accurate dynamic and contact geometry modeling, which makes it difficult to guarantee the consistency and feasibility of the state in real-world environments due to model errors, calibration deviations, and sensor noise; while the latter can learn mapping relationships from a large amount of data, its output often lacks explicit guarantees of physical constraints.

[0004] Specifically, existing technologies have the following shortcomings: 1) Physically impossible situations may occur in the candidate contact states output by the perception model or filter, such as outputting a large normal contact force while determining that there is no contact, or the contact point is located outside the object surface and the contact normal is obviously contradictory to the actual geometry; 2) Geometric information and force information may conflict with each other. For example, visual depth error may cause the tool to penetrate the rigid body several millimeters in geometry, while the force sensor only shows slight contact. Directly fusing such contradictory information will produce incorrect stiffness estimation and control actions; 3) Friction states lack feasibility verification. The estimated tangential force may exceed the range of the friction cone but be marked as an adhesive state, causing the controller to continue to increase the tangential force instead of recognizing slippage; In addition, in multi-contact situations (e.g., two-finger grasping, multi-point support), multiple candidate contact forces can produce the same resultant force. If constraints such as torque balance and non-negative pressure are not introduced, the distribution of contact force will not be unique or will change abruptly; 4) When model error, calibration deviation, or sensor failure causes all constraints to be unsolvable at the same time, existing systems often directly output the previous value or interrupt control, lacking a structured degradation mechanism and conflict cause indication. Summary of the Invention

[0005] This application provides a method, apparatus, and storage medium for robot force state estimation and feasibility correction, which eliminates physical inconsistencies by weighted correction of candidate states and safely degrades when there is no solution.

[0006] On the one hand, this application provides a method for estimating the force state of a robot and correcting its feasibility, the method comprising: Step S1: Obtain candidate contact states and their reliability information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters; Step S2: Based on the contact modes corresponding to the candidate contact states, construct a set of physical constraints including non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. Assign corresponding confidence weights and maximum relaxations to each soft constraint according to the confidence information. The contact point kinematic consistency conditions are constructed based on the robot kinematic model and the contact geometry model. The actuator limit conditions and the energy passivity conditions are hard constraints, while the non-penetration and contact complementarity conditions, the friction cone conditions, the multi-contact resultant force and resultant torque balance conditions, and the contact point kinematic consistency conditions are soft constraints. Step S3: Using the candidate contact state as the initial value, according to the confidence weight, solve for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set, and output the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied at the same time or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed, and the conflict field and constraint residual are output at the same time. Step S4: Convert the corrected state that has passed the verification in Step S3 and has qualified the feasibility level into robot control parameters and send them out for execution. Collect execution feedback information to update the material parameters, the credibility information and the model parameters in the physical constraint set, and return to Step S1.

[0007] On the other hand, this application provides a robot force state estimation and feasibility correction device, the device comprising: The acquisition module is used to acquire candidate contact states and their reliability information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters. The allocation module is used to construct a set of physical constraints based on the contact modes corresponding to the candidate contact states. This set includes non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. The module also assigns corresponding confidence weights and maximum relaxation amounts to each soft constraint based on the confidence information. The contact point kinematic consistency conditions are constructed based on the robot kinematic model and the contact geometry model. The actuator limit conditions and the energy passivity conditions are hard constraints, while the non-penetration and contact complementarity conditions, the friction cone conditions, the multi-contact resultant force and resultant torque balance conditions, and the contact point kinematic consistency conditions are soft constraints. The solution module is used to take the candidate contact state as the initial value, and according to the confidence weight, solve for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set. It outputs the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied at the same time or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed. At the same time, the conflict field and constraint residual are output. The conversion module is used to convert the verified and feasible corrected state into robot control parameters and issue them for execution, collect execution feedback information, update the material parameters, the confidence information and the model parameters in the physical constraint set, and return to the acquisition module.

[0008] Thirdly, this application provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the robot force state estimation and feasibility correction method described above.

[0009] Fourthly, this application provides a storage medium storing a computer program that, when executed by a processor, implements the robot force state estimation and feasibility correction method described above.

[0010] As can be seen from the technical solution provided in this application, on the one hand, by constructing a set of physical constraints including non-penetration and contact complementary conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, and by setting the actuator limit conditions and energy passivity conditions as unrelaxable hard constraints, physical contradictions in candidate contact states can be eliminated. Specifically, the non-penetration and contact complementary conditions ensure the complementary relationship between the gap and the normal force, eliminating the logical contradiction of "force without contact" or "force without penetration". The friction cone condition ensures that the tangential force is always within the allowable range of friction, preventing the controller from continuously increasing the tangential force in the adhesive state and ignoring the risk of slippage. The balance condition of the resultant force and resultant torque of multiple contacts, combined with the kinematic consistency condition of the contact point, ensures that the distribution of multiple contact forces satisfies the overall force balance and motion compatibility, avoiding non-uniqueness or abrupt changes in force. Thus, the corrected state is physically self-consistent and executable. On the other hand, according to the confidence information, the corresponding confidence weight and maximum relaxation amount are assigned to each soft constraint, so that the correction process can adaptively reflect the reliability of each sensor mode. When the covariance of a sensor field is large, the data is outdated, or it is in a saturated / faulty state, the corresponding soft constraint weight is automatically reduced, meaning that the field is allowed to undergo significant changes during correction. Conversely, high-confidence fields have higher weights and are maintained as much as possible during correction. This mechanism avoids the problem that simple pruning or equal-weight correction may destroy high-reliability observations, ensuring that the correction results retain the credible components of the original observations to the maximum extent while satisfying physical constraints. Thirdly, a feasibility level output and safety degradation mechanism are introduced during the solution process. When the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within a preset solution time, the system only retains the hard constraints and performs a safety degradation operation, while outputting conflict fields and constraint residuals. This ensures that when facing abnormal situations such as model errors, calibration deviations, or sensor failures, the system will not directly collapse or output an unknown state, but can report the cause of the conflict in a structured way and safely retreat, thereby significantly improving the robustness and safety of robot contact operations. In summary, the technical solution of this application eliminates physical contradictions by weighted correction of candidate states and safely degrades when there is no solution. Attached Figure Description

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

[0012] Figure 1This is a flowchart of the robot force state estimation and feasibility correction method provided in the embodiments of this application; Figure 2 This is a schematic diagram of the robot force state estimation and feasibility correction device provided in the embodiments of this application; Figure 3 This is a schematic diagram of the structure of the electronic device provided in the embodiments of this application. Detailed Implementation

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

[0014] In this specification, adjectives such as "first" and "second" are used only to distinguish one element or action from another, without necessarily requiring or implying any actual such relationship or order. Where circumstances permit, reference to an element or component or step (etc.) should not be construed as being limited to only one of the elements, components, or steps, but may be one or more of the elements, components, or steps, etc.

[0015] For ease of description, the dimensions of the various parts shown in the accompanying drawings are not drawn to actual scale.

[0016] In existing technologies, the acquisition of contact states mainly relies on two approaches: one is based on physical model-based observers (e.g., Kalman filters, perturbation observers), and the other is based on data-driven multimodal fusion models (e.g., neural networks, Gaussian process regression). The former relies on accurate dynamic and contact geometry modeling, which makes it difficult to guarantee the consistency and feasibility of the state in real-world environments due to model errors, calibration biases, and sensor noise; while the latter can learn mapping relationships from large amounts of data, its output often lacks explicit guarantees of physical constraints. Specifically, existing technologies have the following shortcomings: 1) Physically impossible situations may occur in the candidate contact states output by the perception model or filter, such as outputting a large normal contact force while determining that there is no contact, or the contact point is located outside the object surface and the contact normal is obviously contradictory to the actual geometry; 2) Geometric information and force information may conflict with each other. For example, visual depth error may cause the tool to penetrate the rigid body several millimeters in geometry, while the force sensor only shows slight contact. Directly fusing such contradictory information will produce incorrect stiffness estimation and control actions; 3) Friction states lack feasibility verification. The estimated tangential force may exceed the range of the friction cone but be marked as an adhesive state, causing the controller to continue to increase the tangential force instead of recognizing slippage; In addition, in multi-contact situations (e.g., two-finger grasping, multi-point support), multiple candidate contact forces can produce the same resultant force. If constraints such as torque balance and non-negative pressure are not introduced, the distribution of contact force will not be unique or will change abruptly; 4) When model error, calibration deviation, or sensor failure causes all constraints to be unsolvable at the same time, existing systems often directly output the previous value or interrupt control, lacking a structured degradation mechanism and conflict cause indication.

[0017] To address the aforementioned problems in the prior art, this application proposes a method for robot force state estimation and feasibility correction, the flowchart of which is attached. Figure 1 As shown, it mainly includes steps S1 to S4, which are detailed below: Step S1: Obtain candidate contact states and their reliability information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters.

[0018] Specifically, the system first obtains the candidate contact states of the robot within the current control cycle from the upstream sensing module. These candidate contact states can be the output of a multimodal sensing fusion filter (e.g., an extended Kalman filter, particle filter), a learning-based observer (e.g., a neural network state estimator), or a traditional force / potential observer (e.g., an external force estimate based on a momentum observer). The candidate contact state includes at least the following fields: the three-dimensional position coordinates of the candidate contact point, the unit vector of the candidate contact normal, the signed gap (where a positive value indicates separation and a negative value indicates penetration), the magnitude of the normal contact force, the tangential contact force vector, the pose and velocity of the end effector, and the current discrete contact mode, such as separation, adhesion, slippage, flexible compression, etc.

[0019] Simultaneously, the system also acquires credibility information corresponding to each field in the candidate contact state. This credibility information is a multi-dimensional data structure, containing at least the covariance matrix of each field (reflecting the level of random measurement noise), data freshness (i.e., the time interval between the last update timestamp of the field and the current time, used to measure the timeliness of the information), sensor saturation status indicators (e.g., whether the force sensor is in the range saturation zone, whether the vision sensor has depth failure due to overexposure or underexposure), and sensor fault indicators (such as communication interruption, hardware self-test abnormality, etc.). The covariance matrix reflects the level of random measurement noise, and data freshness, i.e., the time interval between the last update timestamp of the field and the current time, is used to measure the timeliness of the information. The introduction of credibility information enables the system to distinguish the reliability of different sensor modes at the current time, providing a basis for subsequent weight allocation.

[0020] In addition, the system also needs to obtain the following offline or semi-offline calibration parameters: robot kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters. Among them, the robot kinematic model includes DH parameters, joint limits, velocity and acceleration limits; the contact geometry model is the geometric representation of the environmental surface, which can be a CAD mesh model, a point cloud implicit surface, a basic geometry such as a plane / sphere / cylindrical surface, or an online reconstructed local planar model; the material parameters include the prior values ​​or confidence intervals of contact stiffness, damping coefficient, and friction coefficient; the actuator limits include the maximum torque, maximum velocity, maximum acceleration of each joint, and the maximum allowable contact force of the end effector; the safety configuration parameters include the maximum allowable penetration depth, the lower limit of the energy tank safety limit, the emergency stop trigger threshold, etc.

[0021] In existing technologies, most methods focus only on acquiring state values ​​while neglecting the quantitative characterization of their reliability, resulting in an inability to make targeted responses when sensors degrade or malfunction. However, through the above step S1, not only are raw observation data for state estimation collected, but more importantly, meta-information for evaluating the reliability of these observation data is also collected, laying a data foundation for subsequent dynamic adjustment of correction intensity based on sensor quality.

[0022] Step S2: Based on the contact modes corresponding to the candidate contact states, construct a set of physical constraints including non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. Assign corresponding confidence weights and maximum relaxations to each soft constraint based on the confidence information. Among them, the contact point kinematic consistency conditions are constructed based on the robot kinematic model and contact geometry model, the actuator limit conditions and energy passivity conditions are hard constraints, and the non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, and contact point kinematic consistency conditions are soft constraints.

[0023] The purpose of step S2 is to establish a set of constraints that reflects the objective laws of the physical world, providing a basis for subsequent correction processes. This set of constraints is necessary because while the candidate contact states output by the upstream sensing module may be statistically optimal (e.g., minimizing observation residuals), they are not guaranteed to be physically feasible. For example, a candidate state might simultaneously satisfy both "positive contact force" and "positive gap," meaning no contact, which is physically impossible—because according to contact mechanics, normal pressure can only be transmitted when two objects are in contact. Similar contradictions include: tangential forces exceeding the friction cone being marked as an adhesive state, the sum of multiple contact forces failing to balance external loads, and mismatches between contact point velocities and contact patterns. Therefore, it is necessary to formalize these physical laws into mathematical constraints, forming a physically feasible set, and then project or correct the candidate states onto this set.

[0024] Specifically, the set of physical constraints constructed in this application includes six types of constraints: non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, which will be explained one by one below.

[0025] The first type: Complementary conditions between non-penetration and contact. This condition originates from the Signorini condition in contact mechanics, which specifies the complementary relationship between normal clearance and normal contact force. Specifically, for rigid or near-rigid environments, there is a signed clearance... Contact force with normal direction Should meet , (Normal force can only be pressure) and ,in, For robot joint angles, For the contact point location, a complementary relaxation condition applies: when the gap is positive, the normal force should be zero; when there is a normal force, the gap should be zero or negative. This condition manifests differently for different contact modes: in separation mode, forced... Approaching zero; in contact mode, the constraint gap is close to the material's compressibility. For flexible environments, it can be used... and Instead of the strict zero-gap condition, where, k and d These are contact stiffness and damping coefficient, respectively.

[0026] The second type: the friction cone condition. This condition specifies the tangential contact force. Contact force with normal direction The relationship between them. According to Coulomb's law of friction, the magnitude of the tangential force must not exceed the product of the normal force and the coefficient of friction, that is... ,in, The coefficient of friction, This represents the allowable relaxation amount. In adhesion mode, the tangential relative velocity is further constrained to near zero; in slip mode, the direction of the constrained tangential force is opposite to the direction of the relative sliding velocity. To reduce solution complexity, in practical applications, the conical friction cone can be approximated by a polyhedron with 4 to 16 faces.

[0027] The third type: Balance conditions of resultant force and torque in multi-contact applications. When a robot has multiple contact points (e.g., two-finger grasping, multi-point support), the contact forces at each point must satisfy the overall force and torque balance equations: ,in, The grasping matrix is ​​constructed from the positions and normals of each contact point. The stacked vector of all contact forces. A wrench is an external force / torque acting on an object, including gravity, inertial forces, etc. To allow for a certain level of balance residuals, it is also necessary to limit the non-negativity of each normal force and ensure that the center of pressure is located within the effective range of the contact area.

[0028] Category 4: Contact Point Kinematic Consistency Conditions. These conditions, based on the robot's kinematic model and contact geometry model, constrain the relationship between the velocity of the contact point and the robot's joint velocities. ,in, Let be the Jacobian matrix of the contact point. This condition ensures that the motion velocity of the contact point is kinematically consistent with the motion velocity of the robot joint, and adds corresponding constraints according to different contact modes. For example, in the adhesive mode, the velocity of the contact point relative to the environment should be zero.

[0029] Category 5: Actuator Limits. This category includes joint position limits, velocity limits, acceleration limits, joint torque limits, and the maximum permissible contact force of the end effector. These limits are derived from the physical limits and safety specifications of the robot hardware and must not be violated under any circumstances.

[0030] The sixth category: Energy-free conditions. These conditions ensure that the robot does not inject energy into the environment beyond safe limits during interaction. This can be achieved using a discrete energy tank model. ,in, This represents the current energy tank inventory. To control the command force, For the terminal velocity, For damping dissipation estimation, This is the lower limit of safe energy. This condition ensures the passivity and safety of the interaction from an energy perspective.

[0031] Of the six types of constraints mentioned above, the actuator limit condition and the energy passivity condition are set as hard constraints, meaning they must be strictly satisfied under all circumstances, with no relaxation allowed. This is because violating the actuator limit condition may lead to hardware damage, while violating the energy passivity condition may lead to system instability or even safety accidents. The remaining four types of conditions—non-penetration and contact complementarity condition, friction cone condition, multi-contact resultant force and resultant torque balance condition, and contact point kinematic consistency condition—are set as soft constraints, meaning they can be relaxed within certain limits. The reason for setting these conditions as soft constraints is that in real-world systems, there are model errors, calibration deviations, and sensor noise; strictly satisfying all physical conditions is sometimes impossible, requiring trade-offs within a certain range.

[0032] For these soft constraints, the system assigns corresponding confidence weights and maximum relaxation amounts based on the confidence information obtained in step S1. The confidence weight determines the priority of the constraint in the subsequent weighted projection solution—a higher weight means a greater cost for the corresponding observation field to be changed during correction, i.e., the system tends to maintain the original observation value of that field; a lower weight allows the field to be modified to a greater extent to satisfy other constraints. The maximum relaxation amount sets a tolerable maximum deviation limit for each soft constraint to prevent the relaxation amount from increasing indefinitely and causing the constraint to lose its meaning. This weight allocation mechanism enables the correction process to adaptively reflect the real-time reliability of each sensor mode: soft constraints corresponding to high-confidence sensor fields have high weights and are maintained as much as possible during correction; soft constraints corresponding to low-confidence sensor fields have low weights and are allowed to be modified to a greater extent to satisfy physical constraints.

[0033] The technical solution in step S2 above, on the one hand, explicitly constructs a set of physical constraints covering the six basic conditions of contact mechanics and distinguishes between hard constraints (which must be met) and soft constraints (which allow for a certain degree of violation). This ensures, without relying on large-scale training data, that the corrected state satisfies the basic physical laws and robot safety boundaries from a fundamental perspective, thereby eliminating physical contradictions in candidate states and improving the reliability and executability of state estimation. On the other hand, by introducing multi-dimensional credibility information including covariance matrix, data freshness, sensor saturation state, and fault flags, credibility weights and maximum relaxations are dynamically assigned to each soft constraint. This allows the correction process to adaptively "trust" high-reliability sensors and "forgive" low-reliability sensors—high-reliability fields are preserved as much as possible during correction, while low-reliability fields are allowed to be modified to a greater extent to meet physical constraints. This mechanism avoids both equal-weight correction that destroys high-reliability observations and low-reliability observations that solidify erroneous information into the correction results, thus establishing a reasonable trade-off between physical constraints and sensor credibility.

[0034] When contact modes are not directly observable, efficiently selecting the most likely mode from a large number of candidate mode assumptions for accurate solution, thus avoiding waste of computational resources, is an important issue. In existing technologies, a common approach is to select a fixed mode based solely on experience; for example, always assuming adhesion, or solving equally on all modes. The former is prone to correction failures due to mode misjudgment, while the latter is computationally too large to meet real-time requirements. Therefore, as an embodiment of this application, constructing a set of physical constraints based on the contact modes corresponding to candidate contact states, including non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, can be achieved through steps S21 to S23, as detailed below: Step S21: Generate a finite number of candidate mode hypotheses based on the candidate contact modes, and construct a set of sub-constraints corresponding to each candidate mode hypothesis.

[0035] In practical robot contact operations, contact patterns are often not directly observable. For example, a contact point may be in one of several modes, such as separation, approach, adhesion, slippage, or rolling, and different modes correspond to different subsets of physical constraints. To address this issue, step S21 generates a finite number of candidate mode hypotheses based on the candidate contact mode information obtained in step S1. Specifically, the system enumerates all possible combinations of contact modes (e.g., for single-point contact, there are typically three modes: separation, adhesion, and slippage; for multi-point contact, the number of modes is the Cartesian product of the number of modes at each point) and constructs a corresponding set of sub-constraints for each candidate mode hypothesis. For example, for the "adhesion" mode, the sub-constraint set includes the condition that the tangential relative velocity is zero, while for the "slippage" mode, it includes the condition that the direction of the tangential force is opposite to the direction of the slippage velocity. By constructing constraints for different modes separately, the system can handle the uncertainty of contact patterns within a unified framework.

[0036] Considering that even after candidate mode hypotheses are generated, they may still contain a large number of geometrically unreasonable or obviously contradictory hypotheses with the measurement direction, directly incorporating these hypotheses into cost estimation or even complete solution would not only waste computing power but also potentially interfere with the accuracy of the ranking results. In existing technologies, a common practice is to send all hypotheses into subsequent processing indiscriminately, lacking an efficient pre-screening step. Therefore, the generation of a limited number of candidate mode hypotheses based on candidate contact modes in the above embodiment can be achieved through the following steps S211 to S213: Step S211: Extract candidate contact points, candidate contact normals, signed gaps, and measured external force directions from the candidate contact states.

[0037] Specifically, the system first extracts key geometric and mechanical information from the candidate contact states obtained in step S1: the three-dimensional position coordinates of the candidate contact points, the unit vector of the candidate contact normal, the signed gap value (where positive indicates separation and negative indicates penetration), and the direction of the external force measured by the force sensor. This information forms the basis for subsequent geometric rationality judgments.

[0038] Step S212: Based on the geometric distance between the candidate contact point and the contact geometry model, and the consistency relationship between the candidate contact normal and the measured external force direction, eliminate candidate mode assumptions whose geometric distance exceeds a preset geometric threshold or whose direction consistency is lower than a preset direction threshold.

[0039] After extracting information such as candidate contact points, candidate contact normals, signed gaps, and the direction of measured external forces from the candidate contact states, the system uses a contact geometry model to quickly verify the rationality of candidate mode hypotheses. Specifically, it includes two dimensions of verification: (1) Geometric distance verification, that is, calculating the shortest geometric distance from the candidate contact point to the environmental surface described by the contact geometry model. If this distance exceeds a preset geometric threshold (e.g., 5 mm), it is considered that the candidate contact point deviates too much from the actual physical contact position, and the corresponding candidate mode hypothesis should be eliminated; (2) Directional consistency verification, that is, calculating the angle between the candidate contact normal and the direction of measured external forces. If this angle exceeds a preset directional threshold (e.g., 60 degrees), it is considered that the candidate normal is seriously inconsistent with the actual contact force direction, and the corresponding candidate mode hypothesis should also be eliminated. The computational cost of these two verifications is extremely low, involving only basic geometric and vector operations.

[0040] In another embodiment of this application, the aforementioned preset geometric threshold and preset direction threshold are not fixed values, but are dynamically generated by a pre-trained multilayer perceptron network based on the confidence information of candidate contact states, robot end-effector velocity, and local curvature radius of the contact geometry model in the current control cycle. The input of the multilayer perceptron network also includes the proportion of candidate pattern assumptions that successfully passed the pre-rejection in the previous control cycle, so as to adjust the conservatism of the rejection online.

[0041] Step S213: Construct the remaining candidate mode hypotheses into a finite set of hypotheses to be solved, and proceed to step S22.

[0042] After the aforementioned double pre-elimination, a large number of geometrically unreasonable or directionally contradictory candidate model assumptions are eliminated, and the remaining candidate model assumptions constitute a significantly reduced set of assumptions to be solved. This set is then sent to step S22 for subsequent cost estimation and ranking.

[0043] The technical solutions described in steps S211 to S213 above extract candidate contact points, contact normals, signed gaps, and measured external force directions. They then perform dual pre-screening based on geometric distance thresholds and direction consistency thresholds. This allows for the early filtering of a large number of invalid assumptions at extremely low computational cost. It fully utilizes the spatial constraint information of the contact geometry model and the directional information provided by the force sensor to achieve joint pre-screening in the spatial and force domains. This further reduces the search space for subsequent solutions and improves the real-time performance and robustness of the overall method.

[0044] Step S22: For each candidate mode hypothesis, combine the confidence weight, credibility information and the preset candidate cost function to estimate the lower bound of the weighted projection distance under the candidate mode hypothesis, and use it as the solution cost corresponding to the candidate mode hypothesis.

[0045] If a full weighted projection solution is performed on all candidate pattern hypotheses, the computational cost will increase exponentially with the number of patterns, failing to meet the real-time requirements of robot control. Therefore, step S22 introduces a lightweight candidate cost function to quickly estimate the solution cost of each candidate pattern hypothesis before proceeding to the full solution. This candidate cost function utilizes the confidence weights and credibility information from step S1, as well as the geometric relationships between candidate states and constraints, to estimate the lower bound of the minimum weighted distance required to project the candidate states onto the physically feasible set under that candidate pattern hypothesis. Here, "lower bound" means that the actual weighted projection distance during the solution process will only be greater than or equal to this estimated value. In this way, the system can obtain an approximate measure of the "solution difficulty" of each candidate pattern hypothesis with extremely low computational cost.

[0046] Step S23: Sort the solution costs and prior probabilities of each candidate mode hypothesis, and send at least one candidate mode hypothesis and its sub-constraint set that are ranked first to step S3 for solution. The solution that satisfies the hard constraints and has the smallest weighted projection distance output by step S3 is determined as the final corrected state.

[0047] After obtaining the solution cost of each candidate mode hypothesis, the system further combines the prior probabilities of each mode (obtainable from historical statistical data, such as the frequency of a certain mode occurring under similar working conditions) to comprehensively rank the candidate mode hypotheses. The top-ranked candidate mode hypotheses are considered the "most promising" modes—they either have low solution costs (easy to satisfy) or high prior probabilities (more common historically). The system sends at least one top-ranked candidate mode hypothesis and its corresponding set of sub-constraints to step S3 for complete weighted projection solving. After step S3 is completed, the solution that satisfies the hard constraints and has the smallest weighted projection distance is selected from all the candidate mode hypotheses submitted for solving as the final corrected state. This mechanism significantly reduces the number of candidate modes requiring complete solving while ensuring solution quality, thus meeting real-time requirements.

[0048] It should be noted that when ranking the candidate mode hypotheses based on the solution cost and prior probability of each hypothesis, an exploratory-utilization balancing strategy based on Thompson sampling can be further introduced to randomly inject an exploratory hypothesis into the top-ranked candidate mode hypotheses, so as to ensure that low-frequency modes that have not been fully evaluated are not permanently ignored.

[0049] As can be seen from the technical solutions exemplified in steps S21 to S23 above, by introducing a candidate cost function to predict the lower bound of the weighted projection distance, a large number of candidate mode hypotheses can be sorted and screened at low cost before entering the complete solution stage. Only a few hypotheses with the highest ranking are sent to step S3 for accurate solution. This mechanism significantly reduces computational overhead without sacrificing solution accuracy, enabling multi-mode contact state correction to be completed within the real-time control cycle. Simultaneously, returning solution sovereignty to step S3 maintains the temporal consistency of the main process and makes the logical connection between dependent and independent claims more rigorous.

[0050] In multimodal sensing fusion, the reliability of different sensors varies greatly under different operating conditions. However, existing methods typically treat each sensor equally or fix the weights based on experience, leading to high-reliability observations being "contaminated" by low-reliability observations. Therefore, in order for the correction process to adaptively "trust" high-reliability sensors and "forgive" low-reliability sensors, a reasonable trade-off must be established between physical constraints and sensor reliability. This avoids equal-weight correction from damaging high-reliability observations and prevents low-reliability observations from solidifying erroneous information into the correction results. The above embodiment can assign corresponding confidence weights and maximum relaxation amounts to each soft constraint based on reliability information through the following steps S21a and S21b: Step S21a: Determine the measurement reliability of each state field based on the covariance matrix, data freshness, sensor saturation status, and sensor fault flags contained in the reliability information.

[0051] In specific implementation, the system extracts four dimensions of indicators from the credibility information to comprehensively evaluate the measurement reliability of each state field: (1) The diagonal elements of the covariance matrix reflect the random measurement noise level of the field. The larger the covariance, the more unstable the measurement; (2) Data freshness is measured by comparing the difference between the current timestamp and the field update timestamp. The larger the difference, the more outdated the information and the lower the reliability; (3) The sensor saturation state flag indicates whether the sensor is working outside its linear range. Measurements under saturation are usually unreliable; (4) The sensor fault flag indicates whether there are hardware faults such as communication interruption or self-test abnormality. Measurements under fault conditions should be considered completely unreliable. The system integrates the information of these four dimensions into a quantitative measurement reliability index through a preset fusion rule (e.g., weighted product or minimum value selection).

[0052] Step S21b: Set the confidence weight of the soft constraint corresponding to the state field with high measurement reliability to a higher value, and set the confidence weight of the soft constraint corresponding to the state field with low measurement reliability to a lower value. At the same time, set an upper limit value for each soft constraint that is appropriate to its physical meaning as the maximum relaxation amount.

[0053] After obtaining the measurement reliability of each state field, the system sets the confidence weight of each soft constraint accordingly. Specifically, soft constraints corresponding to fields with high measurement reliability (e.g., high-precision readings of force sensors within their normal operating range) are assigned higher confidence weights, meaning that in subsequent weighted projection solutions, the system tends to maintain the original observation value of that field without making significant modifications. Conversely, soft constraints corresponding to fields with low measurement reliability (e.g., depth estimation by a vision sensor under occlusion or overexposure conditions) are assigned lower confidence weights, allowing for greater adjustment during corrections. Furthermore, the system sets a maximum relaxation amount for each soft constraint that is appropriate to its physical meaning. For example, for non-penetration conditions, the maximum relaxation amount can be set as the upper limit of the material's elastic deformation; for friction cone conditions, the maximum relaxation amount can be set as the upper limit of the uncertainty interval of the friction coefficient. This upper limit ensures that the relaxation amount does not increase indefinitely, thus preserving the basic physical meaning of the constraint.

[0054] Step S3: Using the candidate contact state as the initial value, according to the confidence weight, solve for the corrected state that minimizes the weighted projection distance from the candidate state to the physical constraint set for each candidate mode. Output the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed. At the same time, the conflict field and constraint residual are output.

[0055] After constructing and weighting the set of physical constraints in step S2, the core task of step S3 is to project the candidate contact states onto the physically feasible set defined by these constraints and output the corrected, physically self-consistent states and their associated diagnostic information. This projection operation is necessary because while the candidate contact states obtained in step S1 may be statistically optimal (e.g., minimizing sensor observation residuals), they do not inherently satisfy the physical constraints constructed in step S2. For example, a candidate state may simultaneously contain "positive gap" (i.e., no contact) and "positive contact force"—which physically violates the principle of contact complementarity; or the tangential force in the candidate state may exceed the range of the friction cone—leading the controller to misjudge the contact mode. Therefore, it is necessary to adjust the candidate states into the physically feasible set with minimal weighted changes while maintaining a high degree of confidence in the candidate states.

[0056] Specifically, for each candidate mode hypothesis (input from step S23), the system solves a weighted projection optimization problem of the following form: Under the premise of satisfying all hard constraints (actuator limit conditions and energy passivity conditions), find a modified state that minimizes the weighted distance to the original candidate state, while allowing soft constraints to be moderately violated within their respective maximum relaxation ranges. Here, "weighted distance" refers to the Mahalanobis distance or weighted Euclidean distance measured by the confidence weight matrix W assigned in step S2—the larger the weight, the higher the cost of changing the corresponding field, and the more the system tends to maintain the original value of that field. This optimization problem can be implemented using various numerical solution methods, including but not limited to: quadratic programming (when both constraints and the objective function are quadratic), second-order cone programming (when the friction cone is in conical form), sequential quadratic programming (when there are nonlinear terms in the constraints), or a complementarity problem solver (when precise handling of complementarity conditions is required). For real-time control systems with limited computational resources, the complete friction cone can also be approximated by a polyhedron with 4 to 16 faces to reduce the solution complexity.

[0057] After the solution is completed, the system outputs the following information: (1) Corrected contact state: including corrected contact point position, contact normal, normal contact force, tangential contact force, gap value and determined contact mode; (2) Activity constraint identifier: indicates which constraints are active at the optimal solution (i.e., the constraint boundary is touched), which is of great significance for diagnosing which physical conditions are the key factors that restrict the current state; (3) Hard constraint residuals: the degree of violation of each hard constraint (ideally it should be zero, but there may be small residuals in numerical solution). (4) Soft constraint relaxation amount: The actual relaxation amount of each soft constraint reflects the gap between the candidate state and the physical feasible set; (5) Feasibility level: The feasibility of the current modified state is classified according to the solution results. It can usually be divided into "fully feasible" (all constraints are satisfied and the relaxation amount is within the allowable range), "relaxed feasible" (some soft constraints are relaxed but still do not exceed the maximum relaxation amount), "infeasible" (hard constraints cannot be satisfied at the same time) or "solution timeout" (failed to converge within the preset solution time).

[0058] When the system determines that the hard constraints of any candidate mode cannot be satisfied simultaneously, or fails to converge within the preset solution time, it indicates that the current working condition has exceeded the normal correction boundary—possibly due to excessive model error, severe calibration deviation, simultaneous failure of multiple sensor channels, or unexpected drastic changes in the environment. In this situation, continuing to pursue finding a feasible solution under all constraints is neither realistic nor safe. Therefore, the system performs a safety degradation operation: retaining only the hard constraints (i.e., actuator limit conditions and energy passivity conditions), abandoning the satisfaction requirements of all soft constraints, and outputting conflict fields and constraint residuals. This means that the system no longer attempts to maintain the consistency of the physical model, but instead focuses all its efforts on ensuring the hardware and energy safety of the robot. The conflict fields identify the most likely causes of no solution (e.g., "joint 2 torque exceeds the upper limit," "energy tank depleted," etc.), providing clear clues for subsequent fault diagnosis and recovery.

[0059] Clearly, unlike existing technologies that directly use candidate states for control (ignoring physical contradictions) leading to robots executing instructions that violate physical laws, or simply reporting errors and stopping or outputting the previous value when no solution is found, causing robots to execute instructions that violate physical laws, step S3 addresses the problem of how to systematically correct candidate states with physical contradictions to the physically feasible set, and provides a structured safety fallback mechanism in extreme abnormal situations. That is, by retaining only the two most basic safety bottom lines—actuator limit conditions and energy passivity conditions—when hard constraints cannot be satisfied, and performing safety degradation operations, while outputting conflict fields and constraint residuals, the system can still safely retreat in a controlled manner in extreme abnormal situations, rather than going out of control or collapsing. Specifically: retaining actuator limit conditions ensures that joint torque and speed do not exceed hardware limits, avoiding motor burnout or mechanical damage; retaining energy passivity conditions ensures that the system does not inject energy into the environment, avoiding oscillation divergence; and outputting conflict fields provides clear causal clues for subsequent diagnosis and recovery. This mechanism transforms abnormal situations from system weaknesses into a predictable, reportable, and recoverable controlled state, significantly improving the robustness and safety of robot-assisted operations.

[0060] Specifically, the above-mentioned corrected state, which uses the candidate contact state as the initial value to minimize the weighted projection distance from the candidate state to the physical constraint set, can be achieved through steps S31 to S33, as detailed below: Step S31: Use the corrected state of the previous control cycle to start the solution process of the current control cycle in a hot start, and find a feasible solution that satisfies all hard constraints through iterative optimization within the preset solution time.

[0061] In robot contact operations, the contact states of adjacent control cycles typically exhibit continuity and gradual change—the corrected state of the previous cycle is often very close to the true state of the current cycle. Utilizing this temporal correlation, step S31 uses the corrected state successfully solved in the previous control cycle as the initial guess value for the current control cycle; this technique is called "warm start." The advantage of warm start is that, compared to cold start from an arbitrary initial point, warm start significantly reduces the number of iterations and accelerates convergence, thus making it more likely to find a high-quality feasible solution within a limited computational budget. Within a preset solution duration (e.g., 1ms or 2ms, depending on the length of the control cycle and the computational budget allocated to the solver), the system uses an iterative optimization algorithm to find a feasible solution that satisfies all hard constraints. If a feasible solution is found within the preset duration, optimization continues to approximate the optimal solution with the minimum weighted projective distance; if no feasible solution is found within the preset duration, a subsequent timeout handling mechanism is triggered.

[0062] Regarding how to provide a safe rollback scheme when a solution times out, existing technologies typically treat timeout as a failure state, where the system may output an unconverged intermediate solution (potentially violating hard constraints) or directly report an error. Therefore, as an embodiment of this application, the solution process of the current control cycle is warm-started using the corrected state of the previous control cycle, and a feasible solution satisfying all hard constraints is found through iterative optimization within a preset solution duration. This can be achieved through the following steps S311 to S313: Step S311: Determine whether the previous control cycle was successfully solved and output the corrected state that satisfies the hard constraints.

[0063] Before performing a warm start, the system first needs to determine whether the solution results from the previous control cycle are available. Specifically, the system queries the solution record of the previous control cycle to check whether a corrected state satisfying all hard constraints was successfully output. If the previous cycle failed to output a valid corrected state due to unsolvable hard constraints or a timeout, then the solution of that cycle cannot be used as the initial value for the warm start of this cycle.

[0064] Step S312: If the judgment result is yes, then the corrected state of the previous control cycle is used as the initial guess value for solving the current control cycle. If the judgment result is no, then the candidate contact state obtained in step S1 is used as the initial guess value for solving the current control cycle.

[0065] Based on the judgment result of step S311, the system selects different initial guessing value strategies: if the previous cycle is successfully solved, the corrected state of the previous cycle is used as the initial guessing value - this strategy utilizes the temporal continuity of the contact state, which can accelerate convergence; if the previous cycle fails to solve, the candidate contact state obtained in step S1 is used as the initial guessing value - although the candidate contact state may not fully satisfy the physical constraints, it at least contains the latest observation information of the current cycle, which is more reasonable than a random initial point or a zero initial point.

[0066] In another embodiment of this application, if the previous cycle is solved successfully, the Lagrange multipliers of each constraint in the previous cycle correction state are further extracted, and the Lagrange multipliers are used as the initial dual variables for the current control cycle solution, together with the correction state to form the initial guess value; accordingly, if the preset convergence threshold is not reached in the subsequent step S313, the alternating direction multiplier method is used to decompose the original problem into several subproblems for parallel solution, and an adaptive penalty parameter adjustment strategy is introduced in the solution process of each subproblem to accelerate the convergence speed in the case of timeout.

[0067] Step S313: If the preset convergence threshold has been reached within the preset solution time, the state corresponding to the current iteration point is used as the initial value for subsequent iteration optimization; otherwise, the state that satisfies the hard constraints obtained in the most recent iteration is used as the corrected state and the process returns to step S32.

[0068] Step S313 further specifies two branch paths in the solution process: if the change in the objective function value has reached the preset convergence threshold within the preset solution time, it indicates that the solution has converged, and the system uses the state corresponding to the current iteration point as the initial value for subsequent iterations (i.e., preparing to enter the next stage of fine optimization or directly outputting); if the convergence threshold has not been reached by the end of the preset solution time, it indicates that the solution has timed out, and the system no longer waits for convergence, but outputs the state that satisfies all hard constraints obtained in the most recent iteration as the corrected state, and returns to step S32 to continue the next iteration cycle. This mechanism ensures that even in the case of solution timeout, the system can still output a "safe solution" that at least satisfies the hard constraints, rather than outputting an unreliable solution that may violate the hard constraints.

[0069] The technical solution described in steps S311 to S313 above achieves an intelligent hot-start strategy by determining whether the previous control cycle was successfully solved and using either the corrected state from the previous cycle or the candidate contact state from the current cycle as the initial guess value. Furthermore, a dual guarantee mechanism is defined: the converged state is used as the initial value for subsequent iterations, and in case of timeout, the most recently feasible state is used as the corrected state and the iteration is returned to. This design fully utilizes the temporal correlation of the contact process, accelerating the solution convergence speed, and provides a structured fallback path for solution timeouts, avoiding a chain reaction of failures caused by single-cycle solution failures.

[0070] Step S32: In each iteration, calculate the constraint function value of each soft constraint at the current iteration point, and sum the weighted values ​​of the degree of violation of the soft constraints according to the confidence weights to form the objective function value of the current iteration point.

[0071] In each iteration, the system first calculates the constraint function values ​​of each soft constraint at the current iteration point, that is, it determines the degree to which the current point satisfies each soft constraint—if the constraint is satisfied, the function value is negative or zero; if the constraint is violated, the function value is positive, indicating the magnitude of the violation. Then, the system performs a weighted sum of these violation degrees according to the confidence weights assigned in step S2 to form the objective function value of the current iteration point. Specifically, the violation terms corresponding to soft constraints with high confidence weights occupy a larger proportion in the objective function, meaning that the optimization algorithm will prioritize satisfying these constraints; the violation terms corresponding to soft constraints with low confidence weights occupy a smaller proportion in the objective function, meaning that these constraints can be sacrificed to a greater extent. This weighting mechanism enables the optimization process to automatically reflect the confidence differences of each sensor mode.

[0072] Step S33: When the change in the objective function value between two consecutive iterations is less than the preset convergence threshold, convergence is determined, and the state corresponding to the convergence is determined as the correction state. The preset convergence threshold is determined based on the control period and the preset solution time.

[0073] The termination condition for iterative optimization is determined by the change in the objective function value. When the change in the objective function value between two consecutive iterations is less than a preset convergence threshold, it indicates that further iterations offer little help in improving the solution quality. The system then determines convergence and identifies the state corresponding to the current iteration point as the corrected state. The key point here is that the preset convergence threshold is not fixed but dynamically determined based on the control cycle and the preset solution duration. For example, in a short control cycle (e.g., 1 ms), the convergence threshold can be appropriately relaxed to ensure a usable solution is output within a finite time; while in a long control cycle (e.g., 10 ms), the convergence threshold can be tightened to obtain a more accurate solution. This dynamic adjustment mechanism allows the solver to flexibly balance solution speed and accuracy under different real-time requirements.

[0074] The technical solutions described in steps S31 to S33 above effectively solve the problem of efficiently solving the weighted projection optimization problem within a limited real-time computing budget. They significantly accelerate the convergence speed, improve the utilization efficiency of computing resources, and enable complex constrained optimization problems to be reliably solved within the real-time control cycle.

[0075] As mentioned earlier, if the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within a preset solution time, only the hard constraints are retained and a safety degradation operation is performed. However, when the hard constraints cannot be satisfied simultaneously, how should the system degrade in a controlled manner, rather than suddenly crashing or outputting an unknown state? Existing solutions either directly report an error and stop the system or output the value of the previous cycle. The former is unacceptable on a production line, and the latter may exacerbate the danger when the state changes rapidly. In view of this, this application introduces a specific implementation of the safety degradation operation, as described in steps S341 to S343: Step S341: When it is determined that hard constraints cannot be satisfied simultaneously, release the lower priority hard constraints in sequence according to the preset priority order until the remaining constraints have a feasible solution or only the actuator limit condition and the energy passivity condition are retained.

[0076] When the system determines that all hard constraints cannot be satisfied simultaneously (for example, under extreme conditions, the actuator limit condition requires the joint torque to not exceed 100 Nm, while the energy passivity condition requires maintaining contact force without consuming more energy, and the two may conflict), this step initiates a controlled constraint release process. The system releases the hard constraints one by one, starting with the lowest priority constraint, according to a preset priority order (i.e., temporarily ignoring the constraint), and then re-checks whether there is a feasible solution for the remaining constraints. This process continues until there is a feasible solution for the remaining constraints, or until only the two most basic hard constraints remain—the actuator limit condition and the energy passivity condition. These two conditions are reserved for last because they represent hardware safety (preventing motor overload and mechanical damage) and energy safety (preventing the system from injecting dangerous energy into the environment), respectively; they are the bottom line of safety and cannot be breached under any circumstances.

[0077] Step S342: Generate a conflict field identifier to indicate the cause of the conflict based on the release order of each hard constraint and the corresponding constraint residual.

[0078] During constraint release, the system records the name of each released hard constraint, the release order, and the residual size of that constraint before release. This information is integrated into a structured conflict field identifier to indicate the root cause of the unsolvable hard constraint. For example, if the system finds a feasible solution after releasing the "Joint 3 Torque Limit" constraint first, the conflict field identifier will indicate "Joint 3 Torque Exceedance is the main conflict source"; if a feasible solution is found only after releasing the actuator limit and energy passivity condition, the conflict field identifier will indicate "Multiple constraints in a comprehensive conflict, reduced to the minimum safe configuration." This identifier provides a clear causal clue for subsequent fault diagnosis, maintenance decisions, and system recovery.

[0079] Step S343: Reduce or zero the robot running speed corresponding to the conflict field identifier, and control the robot to retreat from the contact area in a safe direction.

[0080] After determining the cause of the conflict, the system executes corresponding safety actions based on the severity indicated by the conflict field. For minor conflicts (e.g., a single joint briefly exceeding its limit), the system can appropriately reduce the robot's operating speed to decrease dynamic loads and make the constraints feasible again. For severe conflicts (e.g., multiple constraints remaining unresolved simultaneously, or multiple sensor channels malfunctioning), the system directly sets the operating speed to zero and controls the robot to retreat from the contact area along a pre-defined safe direction (e.g., a clearance direction away from humans or precision equipment). This retreat action is also supervised by hard constraints—even during the retreat process, actuator limit conditions and energy passivity conditions are strictly monitored to ensure that the retreat action itself does not cause new safety problems.

[0081] The technical solutions described in steps S341 to S343 above enable the system to safely degrade in a predictable, reportable, and recoverable manner under extreme abnormal conditions. This avoids sudden shutdowns that could lead to production interruptions, as well as blind maintenance that could result in safety incidents. At the same time, the output conflict field identifiers provide valuable diagnostic information for subsequent troubleshooting and system recovery.

[0082] In the extreme combined failure scenario of simultaneous failure of force and vision, how to provide a targeted and hierarchical safety degradation strategy? Existing solutions involve a one-size-fits-all emergency stop, which, while safe, is overly conservative and can lead to unnecessary production interruptions. Therefore, in the above embodiment, reducing or zeroing the robot's running speed corresponding to the conflict field identifier and controlling the robot to retreat from the contact area in a safe direction can be achieved through steps S343a and S343b, described in detail below: Step S343a: If the conflict field indicator indicates that the force sensor is in a saturated state and the vision sensor fails at the same time, then ignore the remaining constraints except for the actuator limit condition and the energy passivity condition, and reduce the target contact force and approach speed in the robot control parameters in sync.

[0083] Step S343a provides a refined degradation strategy for a specific, high-risk compound failure scenario—force sensor saturation and simultaneous visual sensor failure. In this scenario, the force sensor is unable to provide accurate contact force readings due to exceeding its range, and the visual sensor is unable to provide environmental geometry information due to failure. This means the system loses the two most important sources of observation of the contact state. In this situation, any correction based on model constraints is unreliable because the inputs required by the model (contact force, contact position) are distorted. Therefore, the system decisively ignores all other constraints except for actuator limit conditions and energy passivity conditions, focusing all attention on maintaining the safety baseline. Simultaneously, the system synchronously reduces the target contact force and approach speed in the robot control parameters—reducing the target contact force reduces the interaction intensity, and reducing the approach speed reduces the collision kinetic energy. The synergistic effect of these two factors allows the robot to interact with the environment and people in a gentler and safer manner even in the event of sensor failure.

[0084] As another embodiment of this application, if it is determined that the force sensor is in a saturated state and the vision sensor fails at the same time, an inertial measurement unit (IMU) and a joint torque sensor can be introduced as redundant observation sources. By fusing the IMU acceleration, angular velocity and joint torque information through an extended Kalman filter, the missing virtual measurement values ​​of contact force and contact position are reconstructed. The virtual measurement values ​​are temporarily injected into the candidate contact state in the aforementioned step S1 to replace the failed sensor data for subsequent correction. The operation of reducing the target contact force and approach speed in step S343a is only executed when the covariance of the virtual measurement values ​​exceeds a preset threshold.

[0085] Step S343b: After reducing the target contact force and approach speed, if the hard constraint residual still does not decrease to a safe range, the approach speed is directly set to zero, and the robot end effector is driven to retreat in the pre-calibrated human-machine or environmental avoidance direction.

[0086] If the hard constraint residuals still do not decrease to a safe range after the first-level degradation strategy (simultaneously reducing the target contact force and approach velocity) is implemented, it indicates that the system still faces safety risks even with reduced interaction intensity. In this case, the system executes the second-level, more thorough degradation strategy: setting the approach velocity to zero, i.e., completely stopping movement in the contact direction, and driving the robot end effector to retreat according to a pre-defined human-machine or environmental avoidance direction. This retreat direction is pre-defined before system deployment based on the specific application scenario and safety risk assessment results—for example, in human-machine collaboration scenarios, the retreat direction typically points away from the operator; in precision assembly scenarios, the retreat direction typically points away from the precision workpiece.

[0087] As can be seen from steps S343a and S343b above, a refined two-level degradation strategy is provided for the specific and dangerous combined fault scenario of force saturation and visual failure: the first level simultaneously reduces the target contact force and approach speed, attempting to reduce risk while retaining some operational capability; the second level, when the first level is ineffective, directly sets the approach speed to zero and retreats in a predetermined avoidance direction. This layered and progressive, scenario-matched degradation strategy achieves a better balance between safety requirements and operational continuity, reflecting a deep understanding of sensor failure modes in actual industrial scenarios.

[0088] Step S4: Convert the corrected state that has passed the verification in Step S3 and has qualified the feasibility level into robot control parameters and send them out for execution. Collect execution feedback information to update material parameters, credibility information and model parameters in the physical constraint set, and return to Step S1.

[0089] After the weighted projection solution and safety degradation processing in step S3, the system obtains a corrected state and its associated feasibility level that has been verified by physical feasibility. However, the corrected state itself is not an instruction that the robot controller can directly execute—the controller requires specific control parameters such as the force control target direction, force / torque limit, desired pose, or velocity. Therefore, the primary task of this step is to convert the corrected state into control parameters that conform to the controller interface specification and send them to the underlying robot controller for execution.

[0090] This conversion is necessary because the corrected state is described from the perspective of a "physical constraint manifold"—it includes information such as contact point location, normal, normal force, tangential force, and contact pattern. However, the robot controller requires information from the perspective of "control commands"—such as the desired end effector force, desired position, impedance parameters, and velocity limits. A semantic gap exists between these two, which needs to be bridged through a conversion bridge. For example, the corrected contact normal and normal contact force can be used to update the force control target direction—causing the robot to apply the desired contact force along the corrected normal; the corrected tangential force can be used to adjust the damping term in impedance control to simulate appropriate frictional behavior.

[0091] After the control parameters are issued and executed, the system does not end there, but enters a closed-loop feedback and online update phase. The system collects execution feedback information—including the actual measured contact force, contact position, joint torque, end velocity, etc.—and uses this information to update material parameters (e.g., online calibration of contact stiffness and friction coefficient), reliability information (e.g., adjusting the covariance matrix based on the deviation between actual and predicted values), and model parameters in the physical constraint set (e.g., updating the geometric model of the environmental surface and online calibration of sensor extrinsic parameters). The updated parameters are fed back to step S1 for candidate contact state acquisition and reliability reconstruction in the next control cycle, thus forming a complete "sensing-correction-control-feedback" closed loop.

[0092] The above step S4, by introducing a closed-loop feedback and online update mechanism, forms an adaptive and continuously learning system that can automatically adjust model parameters and reliability based on actual execution results, maintaining high state estimation accuracy and control performance in long-term operation.

[0093] Specifically, the modified state, which has passed the verification in step S3 and has a qualified feasibility level, can be converted into robot control parameters and issued for execution through the following steps S41 to S43: Step S41: Extract the corrected contact normal, normal contact force, tangential contact force, and contact mode from the corrected state.

[0094] Step S41 first extracts four key pieces of information from the corrected state output in step S3: the corrected contact normal (a unit vector indicating the direction of the normal to the contact surface), the normal contact force (a scalar representing the magnitude of the pressure along the contact normal), the tangential contact force (a two-dimensional or three-dimensional vector representing the frictional force parallel to the contact surface), and the contact mode (discrete values, such as separation, adhesion, slippage, etc.). This information forms the basis for the subsequent generation of force control commands.

[0095] Step S42: Update the force control target direction based on the corrected contact normal and normal contact force, and reduce the upper limit of the end force and the joint moment limit according to the feasibility level.

[0096] After extracting the above information, the system first updates the force control target direction based on the corrected contact normal—meaning the robot will be controlled to apply the desired contact force along the corrected normal, rather than along the normal in the original candidate state. This update is crucial because if there is a large error in the candidate normal (e.g., normal deviation due to visual depth noise), and the controller still applies force along the incorrect normal, the actual contact force direction will deviate from the expected direction, potentially causing tool slippage or workpiece damage.

[0097] Simultaneously, the system reduces the upper limit of the end effector force and the joint torque limit based on the feasibility level output in step S3. This reduction operation aims to correlate the reliability of the state estimation with the aggressiveness of the control: when the state is determined to be "fully feasible," it indicates that the system has sufficient confidence in correcting the state and can maintain the original actuator limits; when the state is determined to be "relaxed feasible," it indicates that some soft constraints have been relaxed, and there is some uncertainty in the corrected state. In this case, the actuator limits should be appropriately reduced to operate in a more conservative manner; when the state is determined to be "infeasible," it indicates that the system can no longer provide a reliable corrected state, and the actuator limits should be reduced to zero or close to zero, forcing the robot to stop or enter a safe mode. This hierarchical and interconnected reduction mechanism ensures that the safety of the control commands always matches the reliability of the state estimation.

[0098] In the prior art, state estimation and controller are usually designed independently—even if the state estimator outputs an "unreliable" flag, the controller still outputs commands according to the rated limits, which may lead to overly aggressive actions when the state is unreliable. To quantitatively correlate the reliability of the state estimation with the execution boundary of the controller, so that the safety margin of the control commands can be dynamically adjusted as the state reliability changes, this application introduces a hierarchical linkage limit contraction mechanism. The above embodiment updates the force control target direction based on the modified contact normal and normal contact force, and contracts the end-effector force upper limit and joint moment limit according to the feasibility level, which can be achieved through the following steps S421 and S422: Step S421: Determine whether the feasibility level is fully feasible, relaxed feasible, or infeasible.

[0099] This step first determines the feasibility level output from step S3. As before, there are three possible feasibility levels: fully feasible (all hard and soft constraints are satisfied, and the relaxation amount is within the allowable range), relaxed feasible (some soft constraints are relaxed but still do not exceed the maximum relaxation amount, and all hard constraints are satisfied), and infeasible (hard constraints cannot be satisfied simultaneously, and a safety degradation has been performed). The system selects different limit contraction strategies based on this determination result.

[0100] Step S422: If the feasibility level is fully feasible, keep the upper limit of the end force and the joint moment limit unchanged. If the feasibility level is relaxed feasible, linearly shrink the upper limit of the end force and the joint moment limit according to the amount of soft constraint relaxation. If the feasibility level is infeasible, shrink the upper limit of the end force and the joint moment limit to zero.

[0101] Depending on the feasibility level, the system implements a three-tiered differentiated limit reduction strategy: (1) Fully feasible: The system has the highest confidence in the corrected state and considers the current state to be physically consistent and reliable. In this case, the upper limit of the end effector force and the joint torque limit remain unchanged at the system's preset values, allowing the robot to operate at full performance.

[0102] (2) Relaxation Feasibility: Although the system has found a solution that satisfies the hard constraints, some soft constraints have been relaxed to a certain extent—this means that there is a deviation between the corrected state and the ideal physical model, which may be due to model parameter drift, increased sensor noise, or environmental changes. In this case, the system linearly shrinks the end force limit and joint torque limit according to the magnitude of the soft constraint relaxation. Specifically, the larger the relaxation, the larger the shrinkage ratio—for example, if the relaxation of a certain soft constraint reaches 50% of its maximum allowable relaxation, then the end force limit and joint torque limit are correspondingly shrunk to 50% of their nominal values. This linear mapping relationship makes the aggressiveness of the controller proportional to the uncertainty of the state; the more uncertain the state, the more conservative the control.

[0103] (3) Infeasible: The system can no longer find a solution that satisfies all hard constraints and performs a safety degradation operation. At this point, the corrected state is unreliable, and any control commands generated based on this state may pose a safety risk. Therefore, the system directly reduces the upper limit of the end effector force and the joint torque limit to zero—meaning that the robot will no longer apply any active force and will only rely on passive compliance or gravity compensation to maintain its current position, or perform a retreating action according to the safety degradation strategy.

[0104] In another embodiment of this application, when the upper limit of the end force and the joint moment limit are linearly contracted according to the magnitude of the soft constraint relaxation, a safety constraint based on a control obstacle function can be further introduced. The contracted upper limit of the end force and the joint moment limit are used as input parameters of the control obstacle function to construct a quadratic programming problem. The actual control command that satisfies the safety constraint and has the smallest deviation from the desired control command is solved in real time. The form of the control obstacle function is switched according to the different feasibility levels: when fully feasible, a high-order control obstacle function is used to ensure smooth tracking; when relaxed and feasible, an exponentially convergent control obstacle function is used to force a rapid departure from the unsafe region; when infeasible, a zero-order control obstacle function is used to directly map the control command to zero.

[0105] The technical solutions exemplified in steps S421 and S422 above achieve hierarchical correlation and contraction between the feasibility level (fully feasible, relaxed feasible, infeasible) and the upper limit of the end force and the joint torque limit: the limit remains unchanged when fully feasible, it contracts linearly according to the soft constraint relaxation amount when relaxed feasible, and it contracts to zero when infeasible. This mechanism enables the controller's execution boundary to automatically and dynamically adjust according to the reliability of the state—the more unreliable the state, the more conservative the execution boundary, thereby preventing dangerous control commands generated based on unreliable states from the source and forming a full-link safety guarantee of "estimation-correction-control".

[0106] Step S43: Encapsulate the updated force control target direction, the upper limit of the retracted end force, and the joint torque limit after retraction into robot control parameters, and send them to the robot controller.

[0107] Finally, the system encapsulates the updated force control target direction, the upper limit of the retracted end effector force, and the joint torque limit after retraction into a standardized control command frame according to the format required by the controller, and sends it to the robot controller via a real-time communication bus (e.g., EtherCAT, CANopen, or shared memory). After receiving these parameters, the controller applies them to the force / position hybrid control or impedance control algorithm in the next control cycle to drive the robot to perform the corresponding actions.

[0108] Unlike many advanced state estimation algorithms in the prior art, which are theoretically superior to traditional methods but difficult to implement in practical systems due to incompatibility between the output format and the controller interface, the technical solution exemplified in steps S41 to S43 above seamlessly integrates the results of physical constraint correction into the existing robot control framework without requiring large-scale modifications to the controller itself. By defining a clear control parameter conversion interface—extracting key information from the corrected state, updating the force control direction, shrinking the limit, and encapsulating and distributing it—this method can be easily integrated with various mainstream robot controllers (e.g., ROS-based controllers, commercial robot controllers, etc.), lowering the threshold for engineering deployment.

[0109] In order to automatically detect and compensate for the gradual degradation of system parameters without interrupting the operation, as an embodiment of this application, the above-mentioned collection of execution feedback information for updating material parameters, confidence information and model parameters in the physical constraint set can be achieved through steps S44 to S46, as described in detail below.

[0110] Step S44: Record the activity status of each constraint, hard constraint residual, soft constraint relaxation amount, and the actual contact force and contact position collected in each control cycle.

[0111] After each control cycle, the system records the following information: (1) the activity status of each constraint—that is, which constraints were activated (touched the constraint boundary) during the solution process, and which constraints were inactive; (2) the hard constraint residuals—the degree to which each hard constraint was satisfied, ideally zero or a minimum value; (3) the soft constraint relaxation amount—the actual amount of relaxation of each soft constraint; and (4) the actual contact force and contact position collected—that is, the real interaction data obtained by force sensors and position sensors. These records constitute a historical database that accumulates over time, providing a data basis for subsequent anomaly detection and parameter calibration.

[0112] Step S45: When the same soft constraint remains active for multiple consecutive control cycles and its relaxation amount continues to increase, online recalibration of the corresponding material parameters or sensor extrinsic parameters is triggered.

[0113] Step S45 is the core diagnostic and triggering mechanism for closed-loop feedback updates. During long-term robot contact operations, material parameters (e.g., contact stiffness, coefficient of friction) may slowly drift due to wear and temperature changes, and sensor extrinsic parameters (e.g., the mounting position of force sensors, intrinsic and extrinsic parameters of vision cameras) may also shift due to collisions or vibrations. These changes may be negligible within a single control cycle. However, if they exhibit the same trend over multiple consecutive control cycles—that is, the same soft constraint remains active (indicating that the constraint is constantly limiting the solution) and its relaxation amount continuously increases (indicating that the constraint is becoming increasingly difficult to satisfy)—it indicates a gradual parameter degradation in the system. At this point, the system triggers online recalibration of the corresponding material parameters or sensor extrinsic parameters. For example, if the relaxation amount of the "non-penetration condition" continuously increases, it may mean that the geometric model of the environmental surface has shifted, requiring recalibration of the vision sensor extrinsic parameters or updating of the environmental map; if the relaxation amount of the "friction cone condition" continuously increases, it may mean that the coefficient of friction has changed, requiring recalibration of the coefficient of friction.

[0114] It should be noted that in step S45 above, when the same soft constraint remains active for multiple consecutive control cycles and its relaxation amount continues to increase, another approach is not to directly trigger recalibration, but to first use an anomaly detection model based on a variational autoencoder to determine whether the current parameter drift belongs to a previously seen historical pattern; if it belongs to a historical pattern, the corresponding optimal calibration parameters are retrieved directly from the memory for rapid compensation; if it does not belong to a historical pattern, an online calibration process based on Bayesian optimization is initiated to efficiently search for the optimal calibration value within the parameter space that does not exceed the preset safety boundary, and the newly discovered drift pattern and its calibration results are stored in the memory for future reuse.

[0115] Step S46: Update the material parameters and correct the covariance matrix in the confidence information based on the recalibration results, and feed the updated parameters back to step S1 for candidate contact state acquisition and confidence reconstruction in the next control cycle.

[0116] After completing the online recalibration, the system writes the new calibration results into the corresponding parameter storage area: updating material parameters (e.g., new friction coefficient, new contact stiffness) and correcting the covariance matrix in the reliability information (e.g., if the sensor's deviation is corrected after recalibration, the covariance can be appropriately reduced to improve the sensor's reliability). These updated parameters are fed back to step S1. In the next control cycle, step S1 will use the updated material parameters and reliability information to obtain candidate contact states, thereby enabling the entire system to adaptively compensate for parameter drift.

[0117] The technical solution described in steps S44 to S46 above achieves autonomous perception and adaptive correction of model parameter drift by recording the activity state and relaxation amount of each constraint in each control cycle and triggering online recalibration when the same soft constraint remains active and the relaxation amount continues to increase. The recalibrated parameters are fed back to step S1 for candidate contact state acquisition and credibility reconstruction in the next cycle, forming a self-healing closed loop of monitoring-diagnosis-calibration-feedback. This mechanism enables the system to maintain high state estimation accuracy for a long period without human intervention, significantly reducing maintenance costs and downtime.

[0118] From the above Figure 1As can be seen from the example of the robot force state estimation and feasibility correction method, on the one hand, by constructing a set of physical constraints including non-penetration and contact complementary conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, and setting the actuator limit conditions and energy passivity conditions as unrelaxable hard constraints, it is possible to eliminate physical contradictions in candidate contact states. Specifically, the non-penetration and contact complementary conditions ensure the complementary relationship between the gap and the normal force, eliminating the logical contradictions of "force without contact" or "force without penetration". The system resolves contradictions; the friction cone condition ensures that the tangential force is always within the allowable range of friction, preventing the controller from continuously increasing the tangential force in the adhesive state and ignoring the risk of slippage; the balance condition of the resultant force and resultant torque of multiple contacts, combined with the kinematic consistency condition of the contact point, ensures that the distribution of multiple contact forces satisfies the overall force balance and motion compatibility, avoiding non-uniqueness or abrupt changes in force. Thus, the corrected state is physically self-consistent and executable; on the other hand, according to the confidence information, corresponding confidence weights and maximum relaxation amounts are assigned to each soft constraint, enabling the correction process to adaptively reflect the reliability of each sensor mode. When the covariance of a sensor field is large, the data is outdated, or it is in a saturated / faulty state, the corresponding soft constraint weight is automatically reduced, meaning that the field is allowed to undergo significant changes during correction. Conversely, high-confidence fields have higher weights and are maintained as much as possible during correction. This mechanism avoids the problem that simple pruning or equal-weight correction may destroy high-reliability observations, ensuring that the correction results retain the credible components of the original observations to the maximum extent while satisfying physical constraints. Thirdly, a feasibility level output and safety degradation mechanism are introduced during the solution process. When the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within a preset solution time, the system only retains the hard constraints and performs a safety degradation operation, while outputting conflict fields and constraint residuals. This ensures that when facing abnormal situations such as model errors, calibration deviations, or sensor failures, the system will not directly collapse or output an unknown state, but can report the cause of the conflict in a structured way and safely retreat, thereby significantly improving the robustness and safety of robot contact operations. In summary, the technical solution of this application eliminates physical contradictions by weighted correction of candidate states and safely degrades when there is no solution.

[0119] Please see the appendix Figure 2 This application provides a robot force state estimation and feasibility correction device, which includes an acquisition module 201, an allocation module 202, a solution module 203, and a conversion module 204, as detailed below: The acquisition module 201 is used to acquire candidate contact states and their reliability information of the robot during the contact operation process, as well as the robot kinematic model, contact geometry model, material parameters, actuator limits and safety configuration parameters; The allocation module 202 is used to construct a set of physical constraints based on the contact modes corresponding to the candidate contact states. This set includes non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. The module also assigns corresponding confidence weights and maximum relaxation amounts to each soft constraint based on the confidence information. The contact point kinematic consistency conditions are constructed based on the robot kinematic model and contact geometry model. The actuator limit conditions and energy passivity conditions are hard constraints. The non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, and contact point kinematic consistency conditions are soft constraints. The solver module 203 is used to solve for each candidate mode with the candidate contact state as the initial value and according to the confidence weight, the corrected state that minimizes the weighted projection distance from the candidate state to the physical constraint set. It outputs the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied at the same time or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed. At the same time, the conflict field and constraint residual are output. The conversion module 204 is used to convert the verified and feasible corrected state into robot control parameters and send them for execution, collect execution feedback information, update material parameters, confidence information and model parameters in the physical constraint set, and return to the acquisition module.

[0120] From the above Figure 2As can be seen from the example of the robot force state estimation and feasibility correction device, on the one hand, by constructing a set of physical constraints including non-penetration and contact complementary conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, and setting the actuator limit conditions and energy passivity conditions as non-relaxable hard constraints, physical contradictions in candidate contact states can be eliminated. Specifically, the non-penetration and contact complementary conditions ensure the complementary relationship between the gap and the normal force, eliminating the logical contradiction of "no contact but force" or "penetration but no force"; the friction cone condition ensures that the tangential force is always within the allowable range of friction, preventing the controller from continuously increasing the tangential force in the adhesive state and ignoring the risk of slippage; the multi-contact resultant force and resultant torque balance conditions combined with the contact point kinematic consistency conditions ensure that the distribution of multi-contact forces satisfies the overall force balance and motion compatibility, avoiding non-uniqueness or abrupt changes in force. Thus, the corrected state is physically self-consistent and executable. On the other hand, according to the confidence information, corresponding confidence weights and maximum values ​​are assigned to each soft constraint. The relaxation factor allows the correction process to adaptively reflect the reliability of each sensor mode. When the covariance of a sensor field is large, the data is outdated, or it is in a saturated / faulty state, the corresponding soft constraint weight is automatically reduced, meaning that the field is allowed to undergo significant changes during correction. Conversely, high-confidence fields have higher weights and are maintained as much as possible during correction. This mechanism avoids the problem that simple pruning or equal-weight correction may destroy high-reliability observations, so that the correction results retain the credible components in the original observations to the maximum extent while satisfying physical constraints. Thirdly, a feasibility level output and safety degradation mechanism are introduced during the solution process. When the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within the preset solution time, the system only retains the hard constraints and performs a safety degradation operation, while outputting conflict fields and constraint residuals. This ensures that when the system faces abnormal situations such as model errors, calibration deviations, or sensor failures, it will not directly collapse or output an unknown state, but can report the cause of the conflict in a structured way and safely retreat, thereby greatly improving the robustness and safety of robot contact operations. In summary, the technical solution of this application eliminates physical contradictions by weighted modification of candidate states and safely downgrades when there is no solution.

[0121] Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. For example... Figure 3 As shown, the electronic device 3 in this embodiment mainly includes: a processor 30, a memory 31, and a computer program 32 stored in the memory 31 and executable on the processor 30, such as a program for a robot force state estimation and feasibility correction method. When the processor 30 executes the computer program 32, it implements the steps in the above-described robot force state estimation and feasibility correction method embodiment, for example... Figure 1Steps S1 to S4 are shown. Alternatively, when processor 30 executes computer program 32, it implements the functions of each module / unit in the above-described device embodiments, for example... Figure 2 The functions of the acquisition module 201, allocation module 202, solution module 203, and conversion module 204 are shown.

[0122] For example, the computer program 32 for the robot force state estimation and feasibility correction method mainly includes: acquiring candidate contact states and their confidence information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters; constructing a set of physical constraints based on the contact modes corresponding to the candidate contact states, including non-penetration and contact complementary conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions; and assigning corresponding confidence weights and maximum relaxations to each soft constraint based on the confidence information. The contact point kinematic consistency conditions are constructed based on the robot's kinematic model and contact geometry model, while the actuator limit conditions and energy passivity conditions are hard constraints. The conditions for complementary contact, friction cone, balance of resultant force and moment in multiple contacts, and kinematic consistency of contact points are considered soft constraints. Using candidate contact states as initial values, and according to confidence weights, a corrected state is calculated for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set. The corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount, and feasibility level are output. If the hard constraints of any candidate mode cannot be simultaneously satisfied or convergence is not achieved within a preset solution time, only the hard constraints are retained, and a safety degradation operation is performed, while the conflict field and constraint residual are output. The verified corrected states with acceptable feasibility levels are converted into robot control parameters and executed. Execution feedback information is collected to update material parameters, confidence information, and model parameters in the physical constraint set, and then returned. The computer program 32 can be divided into one or more modules / units, which are stored in memory 31 and executed by processor 30 to complete this application. One or more modules / units can be a series of computer program instruction segments capable of performing specific functions, which describe the execution process of the computer program 32 in the electronic device 3.For example, computer program 32 can be divided into the functions of acquisition module 201, allocation module 202, solution module 203, and conversion module 204 (modules in the virtual device). The specific functions of each module are as follows: Acquisition module 201 is used to acquire candidate contact states and their confidence information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters; Allocation module 202 is used to construct a set of physical constraints based on the contact modes corresponding to the candidate contact states, including non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions, and to assign corresponding confidence weights and maximum relaxations to each soft constraint based on the confidence information. Among them, the contact point kinematic consistency conditions are constructed based on the robot's kinematic model and contact geometry model, and the actuator limit conditions and energy passivity conditions are constructed based on the confidence information. The passive condition is a hard constraint, while the non-penetration and contact complementarity condition, friction cone condition, multi-contact resultant force and resultant moment balance condition, and contact point kinematic consistency condition are soft constraints. The solution module 203 is used to solve for each candidate mode with the candidate contact state as the initial value, according to the confidence weight, to minimize the weighted projection distance from the candidate state to the physical constraint set. It outputs the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount, and feasibility level. If the hard constraints of any candidate mode cannot be satisfied simultaneously or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed. At the same time, the conflict field and constraint residual are output. The conversion module 204 is used to convert the verified and feasible corrected state into robot control parameters and send them for execution. It collects execution feedback information to update material parameters, confidence information, and model parameters in the physical constraint set, and returns to the acquisition module.

[0123] Electronic device 3 may include, but is not limited to, processor 30 and memory 31. Those skilled in the art will understand that... Figure 3 This is merely an example of electronic device 3 and does not constitute a limitation on electronic device 3. It may include more or fewer components than shown, or combine certain components, or different components. For example, electronic device may also include input / output devices, network access devices, buses, etc.

[0124] The processor 30 may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0125] The memory 31 can be an internal storage unit of the electronic device 3, such as a hard disk or RAM. The memory 31 can also be an external storage device of the electronic device 3, such as a plug-in hard disk, Smart Media Card (SMC), Secure Digital (SD) card, or Flash Card. Furthermore, the memory 31 can include both internal and external storage units of the electronic device 3. The memory 31 is used to store computer programs and other programs and data required by the electronic device. The memory 31 can also be used to temporarily store data that has been output or will be output.

[0126] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed. That is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above-described device can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0127] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0128] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0129] In the embodiments provided in this application, it should be understood that the disclosed devices / electronic devices and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another device, or some features may be ignored or not executed. Furthermore, the mutual coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0130] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0131] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0132] If integrated modules / units are implemented as software functional units and sold or used as independent products, they can be stored in a storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments of this application can also be implemented by a computer program instructing related hardware. The computer program for the robot force state estimation and feasibility correction method can be stored in a storage medium. When the computer program is executed by a processor, it can implement the steps of the above method embodiments, namely, obtaining the candidate contact states and their confidence information of the robot in the contact operation process, as well as the robot kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters; constructing a set of physical constraints including non-penetration and contact complementary conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions according to the contact modes corresponding to the candidate contact states; and assigning corresponding confidence weights and maximum relaxation amounts to each soft constraint according to the confidence information. Among them, the contact point kinematic consistency conditions are based on the machine... The robot's kinematic and contact geometry models are constructed. Actuator limit conditions and energy passivity conditions are considered hard constraints, while non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, and contact point kinematic consistency conditions are considered soft constraints. Using candidate contact states as initial values, and according to confidence weights, a corrected state is calculated for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set. The corrected contact state, active constraint identifier, hard constraint residuals, soft constraint relaxation amounts, and feasibility level are output. If the hard constraints of any candidate mode cannot be simultaneously satisfied or convergence is not achieved within a preset solution time, only the hard constraints are retained, and a safety degradation operation is performed, while the conflict field and constraint residuals are output. The verified corrected states with acceptable feasibility levels are converted into robot control parameters and executed. Execution feedback information is collected to update material parameters, confidence information, and model parameters in the physical constraint set, and then returned. The computer program includes computer program code, which can be in source code form, object code form, executable file, or some intermediate form. Storage media can include: any entity or device capable of carrying computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the contents of storage media can be appropriately added to or removed according to the requirements of legislation and patent practice in a jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, storage media may not include electrical carrier signals and telecommunication signals.

[0133] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. These modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application. The specific embodiments described above further illustrate the purpose, technical solutions, and beneficial effects of this application. It should be understood that the above descriptions are merely specific embodiments of this application and are not intended to limit the protection scope of this application. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for estimating the force state and correcting the feasibility of a robot, characterized in that, The method includes: Step S1: Obtain candidate contact states and their reliability information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters; Step S2: Based on the contact modes corresponding to the candidate contact states, construct a set of physical constraints including non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. Assign corresponding confidence weights and maximum relaxations to each soft constraint according to the confidence information. The contact point kinematic consistency conditions are constructed based on the robot kinematic model and the contact geometry model. The actuator limit conditions and the energy passivity conditions are hard constraints, while the non-penetration and contact complementarity conditions, the friction cone conditions, the multi-contact resultant force and resultant torque balance conditions, and the contact point kinematic consistency conditions are soft constraints. Step S3: Using the candidate contact state as the initial value, according to the confidence weight, solve for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set, and output the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied at the same time or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed, and the conflict field and constraint residual are output at the same time. Step S4: Convert the corrected state that has passed the verification in Step S3 and has qualified the feasibility level into robot control parameters and send them out for execution. Collect execution feedback information to update the material parameters, the credibility information and the model parameters in the physical constraint set, and return to Step S1.

2. The robot force state estimation and feasibility correction method according to claim 1, characterized in that, The step of constructing a set of physical constraints based on the contact modes corresponding to the candidate contact states includes non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. Step S21: Generate a finite number of candidate mode hypotheses based on the candidate contact modes, and construct a set of sub-constraints corresponding to each candidate mode hypothesis; Step S22: For each candidate mode hypothesis, combine the confidence weight, the credibility information and the preset candidate cost function to estimate the lower bound of the weighted projection distance under the candidate mode hypothesis, which is used as the solution cost corresponding to the candidate mode hypothesis; Step S23: Sort the solution costs and prior probabilities of each candidate mode hypothesis, and send at least one candidate mode hypothesis and its sub-constraint set that are ranked first to step S3 for solution. The solution that satisfies the hard constraints and has the smallest weighted projection distance output by step S3 is determined as the final corrected state.

3. The robot force state estimation and feasibility correction method according to claim 2, characterized in that, The step of generating a finite number of candidate pattern hypotheses based on the candidate contact patterns includes: Step S211: Extract the candidate contact points, candidate contact normals, signed gaps, and measured external force directions from the candidate contact states; Step S212: Based on the geometric distance between the candidate contact point and the contact geometry model, and the consistency relationship between the candidate contact normal and the measured external force direction, eliminate candidate pattern assumptions where the geometric distance exceeds a preset geometric threshold or the direction consistency is lower than a preset direction threshold; Step S213: Construct the remaining candidate mode hypotheses into a finite set of hypotheses to be solved, and proceed to step S22.

4. The robot force state estimation and feasibility correction method according to claim 1, characterized in that, The step of finding the corrected state that minimizes the weighted projection distance from the candidate contact state to the physical constraint set, using the candidate contact state as the initial value, includes: Step S31: Use the correction state of the previous control cycle to start the solution process of the current control cycle in a hot start, and find a feasible solution that satisfies all hard constraints through iterative optimization within the preset solution time. Step S32: In each iteration, calculate the constraint function value of each soft constraint at the current iteration point, and perform a weighted summation of the degree of violation of the soft constraints according to the confidence weight to form the objective function value of the current iteration point; Step S33: When the change in the objective function value between two consecutive iterations is less than a preset convergence threshold, convergence is determined, and the state corresponding to the convergence is determined as the corrected state. The preset convergence threshold is determined based on the control period and the preset solution time.

5. The robot force state estimation and feasibility correction method according to claim 4, characterized in that, The process of using the corrected state of the previous control cycle to perform a hot start on the solution process of the current control cycle, and finding a feasible solution that satisfies all hard constraints through iterative optimization within the preset solution time, includes: Step S311: Determine whether the previous control cycle was successfully solved and output the corrected state that over-satisfied the hard constraints; Step S312: If the judgment result is yes, then the corrected state of the previous control cycle is used as the initial guess value for solving the current control cycle; if the judgment result is no, then the candidate contact state obtained in step S1 is used as the initial guess value for solving the current control cycle. Step S313: If the preset convergence threshold has been reached within the preset solution time, the state corresponding to the current iteration point is used as the initial value for subsequent iteration optimization; otherwise, the state that satisfies the hard constraints obtained in the most recent iteration is used as the corrected state and the process returns to step S32.

6. The robot force state estimation and feasibility correction method according to claim 1, characterized in that, The security degradation operation includes: Step S341: When it is determined that hard constraints cannot be satisfied simultaneously, release the lower priority hard constraints in sequence according to the preset priority order until there is a feasible solution for the remaining constraints or only the actuator limit condition and the energy passivity condition are retained. Step S342: Generate a conflict field identifier to indicate the cause of the conflict based on the release order of each hard constraint and the corresponding constraint residual; Step S343: Reduce or zero the robot running speed corresponding to the conflict field identifier, and control the robot to retreat from the contact area in a safe direction.

7. The robot force state estimation and feasibility correction method according to claim 6, characterized in that, The step of reducing or zeroing the robot's running speed corresponding to the conflict field identifier and controlling the robot to retreat from the contact area in a safe direction includes: Step S343a: If the conflict field identifier indicates that the force sensor is in a saturated state and the vision sensor fails at the same time, then ignore the remaining constraints except for the actuator limit condition and the energy passivity condition, and reduce the target contact force and approach speed in the robot control parameters in sync. Step S343b: After reducing the target contact force and approach speed, if the hard constraint residual still does not decrease to a safe range, the approach speed is directly set to zero, and the robot end effector is driven to retreat in the pre-calibrated human-machine or environmental avoidance direction.

8. A robot force state estimation and feasibility correction device, characterized in that, The device includes: The acquisition module is used to acquire candidate contact states and their reliability information during the robot's contact operation, as well as the robot's kinematic model, contact geometry model, material parameters, actuator limits, and safety configuration parameters. The allocation module is used to construct a set of physical constraints based on the contact modes corresponding to the candidate contact states. This set includes non-penetration and contact complementarity conditions, friction cone conditions, multi-contact resultant force and resultant torque balance conditions, contact point kinematic consistency conditions, actuator limit conditions, and energy passivity conditions. The module also assigns corresponding confidence weights and maximum relaxation amounts to each soft constraint based on the confidence information. The contact point kinematic consistency conditions are constructed based on the robot kinematic model and the contact geometry model. The actuator limit conditions and the energy passivity conditions are hard constraints, while the non-penetration and contact complementarity conditions, the friction cone conditions, the multi-contact resultant force and resultant torque balance conditions, and the contact point kinematic consistency conditions are soft constraints. The solution module is used to take the candidate contact state as the initial value, and according to the confidence weight, solve for each candidate mode to minimize the weighted projection distance from the candidate state to the physical constraint set. It outputs the corrected contact state, active constraint identifier, hard constraint residual, soft constraint relaxation amount and feasibility level. If the hard constraints of any candidate mode cannot be satisfied at the same time or fail to converge within the preset solution time, only the hard constraints are retained and a safety degradation operation is performed. At the same time, the conflict field and constraint residual are output. The conversion module is used to convert the verified and feasible corrected state into robot control parameters and issue them for execution, collect execution feedback information, update the material parameters, the confidence information and the model parameters in the physical constraint set, and return to the acquisition module.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the robot force state estimation and feasibility correction method as described in any one of claims 1 to 7.

10. A storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it implements the robot force state estimation and feasibility correction method as described in any one of claims 1 to 7.

Images