Shaft hole assembly method, system, electronic device, and storage medium

Abstract

This disclosure provides a shaft-hole assembly method, system, electronic device, and storage medium. The shaft-hole assembly method includes: receiving shaft-hole assembly task parameters, including position data and a prediction time domain for the assembly hole; generating a reference trajectory based on the position data, and acquiring the state data and contact force data of the robot's end effector at the current sampling moment; using model predictive control with the goal of minimizing an objective function, generating a control input sequence in the prediction time domain based on the reference trajectory, the state data, and the contact force data at the current sampling moment; wherein the objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function; and controlling the end effector to move towards the assembly hole according to the control input sequence to complete the shaft-hole assembly task. This disclosure enables active and smooth force-position hybrid control, significantly improving the efficiency, accuracy, and reliability of assembly.

Description

Technical Field

[0001] This disclosure relates to the field of robot assembly technology, and in particular to a shaft hole assembly method, system, electronic device and storage medium. Background Technology

[0002] In the field of industrial robots and automation, existing contact-based operation technologies generally have significant limitations: traditional position control, although relying on high-precision positioning and vision guidance, is extremely sensitive to disturbances such as minute errors and workpiece deformation, and is prone to jamming or collision damage due to trajectory deviation; passive compliant control, such as spring-compliant wrist RCC, although simple in structure, has fixed compliance characteristics and is difficult to adapt to the dynamic adjustment needs of different working conditions, resulting in a serious lack of versatility; while classical force / position hybrid control, although able to achieve force feedback adjustment, is mostly based on reactive response to the current state, lacking the ability to predict the contact process and proactively plan, and is difficult to handle complex search strategies and multi-constraint coupling problems in dynamic and uncertain environments, resulting in operational efficiency and robustness that cannot meet the requirements of high-precision and high-adaptability scenarios. Summary of the Invention

[0003] The technical problem to be solved by this disclosure is to overcome the shortcomings of existing technologies in which robots have difficulty in achieving precise control, dynamic adaptation and proactive decision-making capabilities in complex contact tasks, resulting in operational efficiency and robustness that cannot meet the requirements of high-precision and high-adaptability scenarios. This disclosure provides a shaft hole assembly method, system, electronic device and storage medium.

[0004] This disclosure solves the above-mentioned technical problems through the following technical solution:

[0005] A first aspect of this disclosure provides a shaft hole assembly method, the shaft hole assembly method comprising:

[0006] Receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain;

[0007] A reference trajectory is generated based on the location data, and the state data and contact force data of the robot's end effector at the current sampling time are obtained;

[0008] With minimizing the objective function as the optimization objective, model predictive control is used to generate the control input sequence in the prediction time domain based on the reference trajectory, the state data at the current sampling time, and the contact force data;

[0009] The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function.

[0010] The end effector is controlled to move toward the assembly hole according to the control input sequence to complete the shaft hole assembly task.

[0011] Optionally, the state data includes an environmental stiffness matrix;

[0012] The step of obtaining the state data of the robot's end effector at the current sampling time includes:

[0013] The contact force increment and position increment of the end effector in the current sampling period are obtained, and the contact force increment and position increment are obtained based on the difference between the contact force data and the spatial position data between the current sampling time and the previous sampling time.

[0014] The environmental stiffness matrix at the current sampling time is obtained by performing an exponentially smoothed online estimation based on the contact force increment and the position increment.

[0015] Optionally, the trajectory tracking cost function is constructed based on the predicted state data, expected state data, and preset trajectory tracking weights at each sampling time in the prediction time domain, wherein the expected state data is obtained based on the reference trajectory;

[0016] The control input smoothing cost function is constructed based on the predicted control input and the preset control input weights at each sampling time in the prediction time domain;

[0017] The force feedback compliance cost function is constructed based on the force feedback compliance weights at each sampling time in the prediction time domain, the prediction control input, and the acceleration correction. The force feedback compliance weights are obtained based on the environmental stiffness matrix at each sampling time, and the acceleration correction is obtained based on the contact force data at each sampling time.

[0018] Optionally, the shaft hole assembly task parameters include helical search parameters;

[0019] The step of generating a reference trajectory based on the location data includes:

[0020] Based on the location data and the spiral search parameters, a spiral reference trajectory is generated;

[0021] An assembly reference trajectory perpendicular to the plane containing the assembly hole is generated based on the position data.

[0022] Optionally, before the step of generating the control input sequence in the prediction time domain using model predictive control with the objective function as the optimization objective, based on the reference trajectory, the state data at the current sampling time, and the contact force data, the shaft hole assembly method further includes:

[0023] In response to the contact force data at the current sampling time being less than a first preset threshold, the model prediction control is controlled to enter the position mode, and the objective function is determined to be a first objective function, which is constructed based on the first trajectory tracking weight and the first control input weight.

[0024] or,

[0025] In response to the contact force data at the current sampling time being greater than or equal to the first preset threshold, the model prediction control is controlled to enter the planar spiral search mode, and the objective function is determined to be the second objective function, which is constructed based on the second trajectory tracking weight, the second control input weight, and the spiral reference trajectory.

[0026] or,

[0027] In response to the decrease in the contact force data at the current sampling time being greater than the second preset threshold, the model prediction control is controlled to enter the force control mode, and the objective function is determined to be the third objective function, which is constructed based on the third trajectory tracking weight, the third control input weight and the assembly reference trajectory.

[0028] Optionally, the shaft hole assembly method further includes:

[0029] With minimizing the exit optimization function as the optimization objective, model predictive control is used to generate the exit control sequence in the prediction time domain based on the contact force data at the current sampling time.

[0030] The exit optimization function is constructed based on the force feedback compliance cost function.

[0031] According to the exit control sequence, the end effector is controlled to exit the assembly hole in a direction perpendicular to the plane where the assembly hole is located;

[0032] And / or,

[0033] The objective function is as follows:

[0034]

[0035]

[0036] in, This represents the predicted state data at the t-th sampling time. This represents the expected state data at time t. This represents the predictive control input at the t-th sampling time. This represents the environmental stiffness matrix at the t-th sampling time. Indicates force feedback compliance weight. This indicates the acceleration correction amount. The conversion factor between force and acceleration. Let Q represent the contact force data at the t-th sampling time, Q represent the preset trajectory tracking weight, and R represent the preset control input weight. and This represents the discrete matrix obtained by discretizing the continuous-time state-space model of model predictive control using a zero-order hold. This indicates the maximum contact force value. Indicates the maximum position acceleration. This represents the velocity at the t-th sampling time. This indicates the maximum speed.

[0037] A second aspect of this disclosure provides a shaft hole assembly system, the shaft hole assembly system comprising:

[0038] The receiving module is used to receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain.

[0039] The acquisition module is used to generate a reference trajectory based on the position data, and to acquire the state data and contact force data of the robot's end effector at the current sampling time;

[0040] The generation module is used to generate the control input sequence in the prediction time domain based on the reference trajectory, the state data at the current sampling time, and the contact force data, with the goal of minimizing the objective function;

[0041] The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function.

[0042] The control module is used to control the end effector to move toward the assembly hole according to the control input sequence, so as to complete the shaft hole assembly task.

[0043] A third aspect of this disclosure provides an electronic device including a memory, a processor, and a computer program stored in the memory and for running on the processor, wherein the processor executes the computer program to implement the shaft hole assembly method described in the first aspect of this disclosure.

[0044] A fourth aspect of this disclosure provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the shaft-hole assembly method described in the first aspect of this disclosure.

[0045] A fifth aspect of this disclosure provides a computer program product, including a computer program that, when executed by a processor, implements the shaft-hole assembly method described in the first aspect of this disclosure.

[0046] Based on common knowledge in the field, the above-mentioned preferred conditions can be combined arbitrarily to obtain various preferred embodiments of this disclosure.

[0047] The positive advancements of this disclosure are as follows: By adding a force feedback compliance cost function to the objective function of model predictive control, the robot can achieve active and smooth force-position hybrid control, improving search speed and significantly reducing contact force, effectively avoiding workpiece damage. Furthermore, the objective function of MPC (Model Predictive Control) is adaptively adjusted according to different control stages, resolving the inherent contradiction between search speed and positioning accuracy, ensuring that the robot is always in the optimal working mode of the current stage throughout the entire task process, and maximizing the overall system performance. At the same time, by introducing the environmental stiffness matrix into the state space of MPC for online estimation, the weights of the force feedback compliance cost function can be dynamically reconstructed according to the real-time contact state, breaking through the force control bottleneck of traditional MPC in unknown stiffness environments, achieving high success rate and high compliance shaft-hole assembly, and significantly improving assembly efficiency, accuracy, and reliability. Attached Figure Description

[0048] Figure 1 This is a first flowchart of the shaft hole assembly method disclosed herein;

[0049] Figure 2 This is a schematic diagram of the planar spiral search disclosed herein;

[0050] Figure 3 This is a schematic diagram of the shaft hole assembly disclosed herein;

[0051] Figure 4 This is a second flowchart of the shaft and hole assembly method disclosed herein;

[0052] Figure 5 This is a schematic diagram of the module of the shaft hole assembly system disclosed herein;

[0053] Figure 6 This is a schematic diagram of the structure of the electronic device disclosed herein. Detailed Implementation

[0054] The present disclosure is further illustrated below by way of embodiments, but the present disclosure is not limited to the scope of the embodiments described herein.

[0055] The prefixes such as "first" and "second" used in this disclosure are merely for distinguishing different descriptive objects and do not limit the position, order, priority, quantity, or content of the described objects. The use of ordinal numbers and other prefixes used to distinguish descriptive objects in this disclosure does not constitute a limitation on the described objects. The description of the described objects is given in the claims or the context of the embodiments, and should not be construed as an unnecessary limitation. Furthermore, in the description of this embodiment, unless otherwise stated, "multiple" means two or more.

[0056] In the field of robot shaft and hole assembly technology, the nearest neighbor algorithm is usually used to control the robot's shaft and hole assembly by pre-sampling and constructing a mapping model from the current state (force / torque) to the next action direction. However, it can only find the local optimum based on the current state and cannot comprehensively consider multiple constraints such as trajectory tracking accuracy, control smoothness, contact force, speed and acceleration in a prediction time domain to achieve a globally optimal decision. This results in insufficient smoothness, efficiency and ability to meet constraints in the assembly process when facing complex assembly tasks.

[0057] There are also systems that rely on deep reinforcement learning to train a policy network to control the shaft and hole assembly of robots. While theoretically they can learn complex assembly techniques, the learned policy is a highly nonlinear black box model. Its decision-making logic is difficult to understand and explain. When faced with edge cases not present in the training set or sudden disturbances, the black box model may output unpredictable or even dangerous actions, lacking anti-interference capabilities.

[0058] In view of this, the present disclosure provides a shaft hole assembly method, system, electronic device and storage medium to solve the problems of lack of compliance, passive contact force control, easy jamming or damage to workpieces in existing high-precision shaft hole assembly for robots.

[0059] Example 1

[0060] In a specific embodiment of this disclosure, a shaft hole assembly method is provided, such as... Figure 1 As shown, the shaft hole assembly method includes:

[0061] S1. Receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain.

[0062] S2. Generate a reference trajectory based on the position data, and obtain the state data and contact force data of the robot's end effector at the current sampling moment;

[0063] S3. With minimizing the objective function as the optimization objective, model predictive control is used to generate a control input sequence in the prediction time domain based on the reference trajectory, the state data and contact force data at the current sampling time.

[0064] The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function.

[0065] S4. Control the end effector to move toward the assembly hole according to the control input sequence to complete the shaft hole assembly task.

[0066] Specifically, when it is necessary to control the robot to perform a shaft and hole assembly task, the shaft and hole assembly task parameters are first received through step S1, including the prediction time domain N and the position data of the assembly hole. The state data s is initialized. The prediction time domain N can be adjusted according to actual needs; for example, the prediction time domain N can be 1 second.

[0067] Step S2 generates a reference trajectory based on the position data of the assembly holes. In the plane search stage, an involute spiral can be used as the reference trajectory to ensure that the entire circular area is covered by the shortest path.

[0068] By acquiring the current position p and current velocity v of the robot's end effector (e.g., end effector tooling), the state data at the current sampling time t is constructed. Where the current position p can be the robot's position information in the Cartesian coordinate system. The current velocity v can be the robot's velocity information in the Cartesian coordinate system. The contact force data at the current sampling time t is obtained by using the real-time measurement value from the torque sensor on the end effector. This allows MPC to solve for an optimal control input sequence in the prediction time domain based on the state data and contact force data at each sampling time.

[0069] To enable robots to achieve smooth force-position hybrid control, significantly reducing contact forces and effectively preventing workpiece damage, the cost of trajectory tracking can be reduced. Control input smoothing cost And the cost of force feedback compliance As a consideration, an objective function is constructed that includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function as the optimization objective of MPC.

[0070] Among them, the trajectory tracking cost function is used to ensure that the robot accurately follows the pre-planned path (reference path); the control input smoothing cost function is used to suppress excessive control input, making the robot's movement smoother; and the force feedback compliance cost function is used to process the torque sensor's measurements (contact force data). This is converted into the desired acceleration correction amount available to the MPC controller, so that the contact force data... When the size increases, MPC can prioritize ensuring smooth contact force by sacrificing some trajectory tracking accuracy, thereby achieving "soft contact" on unknown and complex morphological surfaces and effectively preventing rigid collisions.

[0071] Step S3 uses MPC to determine the state data based on the reference trajectory and the current sampling time t. and contact force data The goal is to minimize the objective function and generate the optimal control input sequence within the prediction time domain N. , This represents the positional acceleration in the x, y, and z directions of the robot in the Cartesian coordinate system. For example, if each 10ms (millisecond) is a control instant, then the MPC generates a sequence of control inputs corresponding to 100 control instants within a 1s prediction time domain.

[0072] The continuous-time state-space model of MPC is shown in Equation (1):

[0073] (1)

[0074] in, , Then the discrete-time model of MPC is shown in formula (2):

[0075] (2)

[0076] in, , , and It is a matrix obtained by discretizing the continuous-time state model using a zero-order hold. The sampling period is This represents the integral variable.

[0077] By concatenating all states and control inputs within the entire prediction time domain N steps, the system dynamic constraints are obtained as shown in equation (3):

[0078] (3)

[0079] Combining contact force constraints, acceleration constraints, and velocity constraints, the objective function can be obtained as shown in formula (4):

[0080] (4)

[0081]

[0082] in, This indicates the maximum contact force, used to limit the contact force between the robot and the environment, preventing damage to the workpiece due to excessive contact force. This indicates the maximum position acceleration, used to measure the position acceleration output by the controller, to prevent excessive current from being drawn during robot control. This indicates the maximum speed, used to limit the robot's movement speed to ensure operational safety and positioning accuracy, and also to avoid increasing positioning errors and collision risks due to excessive speed.

[0083] In step S4, at each control instant, the end effector is controlled to gradually approach the assembly hole according to the position acceleration corresponding to the control input sequence generated by MPC, so as to complete the shaft hole assembly task.

[0084] This specific implementation adds a force feedback compliance cost function to the objective function of model predictive control, enabling the robot to achieve active and smooth force-position hybrid control, improving search speed, significantly reducing contact force, and effectively avoiding workpiece damage.

[0085] In one specific embodiment, the shaft hole assembly task parameters include spiral search parameters;

[0086] Step S2 includes:

[0087] S211. Generate a spiral reference trajectory based on the location data and spiral search parameters;

[0088] S212. Generate an assembly reference trajectory perpendicular to the plane where the assembly hole is located based on the position data.

[0089] Specifically, an involute spiral can be used as the spiral reference trajectory in the planar search stage to ensure that the entire circular area is covered by the shortest path. The equation of the involute spiral is shown in formula (5):

[0090] (5)

[0091] Among them, location data The coordinates of the spiral center are... Indicates the base circle radius. This indicates the radius growth rate, used to control the density of the helix. Represents the initial radius growth rate, k and Indicates the radius growth factor. This represents angular velocity, used to control the search speed. Indicates the pitch.

[0092] In the early stages of the search, t is relatively small. Larger size allows for fast coarse search, but as time progresses, It exhibits exponential decay, with the spiral lines becoming denser, enabling precise searching. Among these, , , , The helical search parameters can be obtained from the pre-entered shaft and hole assembly task parameters.

[0093] Since the shaft-hole assembly task requires inserting the assembly shaft into the assembly hole after searching for it, an assembly reference trajectory perpendicular to the plane of the assembly hole can be generated based on the position data of the assembly hole and the plane where the assembly hole is located. During the assembly stage, the corresponding control input sequence is generated by using the assembly reference trajectory as the trajectory tracking reference, so as to insert the assembly shaft into the assembly hole and complete the shaft-hole assembly task.

[0094] In one specific implementation, the state data includes an environmental stiffness matrix;

[0095] Step S2 includes:

[0096] S221. Obtain the contact force increment and position increment of the end effector in the current sampling period. The contact force increment and position increment are obtained based on the difference between the contact force data and the spatial position data between the current sampling time and the previous sampling time.

[0097] S222. Based on the contact force increment and position increment, perform exponential smoothing online estimation to obtain the environmental stiffness matrix at the current sampling time.

[0098] Specifically, in each sampling cycle, the contact force increment is collected from the six-dimensional force sensor of the robot's end effector. and end position increment The environmental stiffness matrix is ​​updated in real time using an exponential smoothing online estimation method, as shown in formula (6):

[0099] (6)

[0100] in, , , This represents the forgetting factor, used to smooth sensor noise.

[0101] From this, state data can be obtained. By introducing an online estimated environmental stiffness matrix into the state data, the weights of the force feedback compliance cost function can be dynamically reconstructed based on the real-time contact state. This overcomes the force control bottleneck of traditional MPC in unknown stiffness environments and avoids the risk of rigid collisions that are prone to occur when contacting areas with high stiffness.

[0102] In one specific implementation, the trajectory tracking cost function is constructed based on the predicted state data, expected state data, and preset trajectory tracking weights at each sampling time in the prediction time domain, wherein the expected state data is obtained based on the reference trajectory;

[0103] The control input smoothing cost function is constructed based on the predicted control input and the preset control input weights at each sampling time in the prediction time domain;

[0104] The force feedback compliance cost function is constructed based on the force feedback compliance weight, predictive control input, and acceleration correction at each sampling time in the prediction time domain. The force feedback compliance weight is obtained based on the environmental stiffness matrix at each sampling time, and the acceleration correction is obtained based on the contact force data at each sampling time.

[0105] Specifically, the trajectory tracking cost function As shown in formula (7):

[0106] (7)

[0107] in, This represents the predicted state data at sampling time t. Let Q represent the desired state data at sampling time t, where Q is the preset trajectory tracking weight matrix. It varies depending on the different search stages. For example, in the planar search stage, the spiral reference trajectory is used as the desired state data. This allows MPC to generate a control input sequence using a spiral reference trajectory as the trajectory tracking benchmark, while in the assembly stage, the assembly reference trajectory is used as the desired state data. This enables MPC to generate control input sequences based on the assembly reference trajectory as the trajectory tracking reference.

[0108] Control input cost function As shown in formula (8):

[0109] (8)

[0110] in, R represents the predictive control input at sampling time t, and R represents the preset control input weight matrix.

[0111] Force feedback compliance cost for:

[0112] (9)

[0113] in, This indicates the acceleration correction amount. It is the conversion coefficient between force and acceleration, and is the force-acceleration conversion gain matrix. Its physical meaning is the reciprocal of the equivalent mass, used to convert the measured value of the torque sensor. Convert to the expected acceleration correction available for MPC. This represents the force feedback compliance weight.

[0114] It should be noted that the force feedback compliance weight It concerns the environmental stiffness matrix. The dynamic adaptive function. When the environmental stiffness matrix is ​​estimated online. As the contact surface hardens, the force feedback compliance weight increases exponentially. MPC will automatically sacrifice some trajectory tracking accuracy to prioritize ensuring smooth contact force, thereby achieving "soft contact" on unknown and complex morphological surfaces and effectively preventing rigid collisions.

[0115] Therefore, the objective function can be obtained as shown in formula (10):

[0116] (10)

[0117]

[0118] in, This represents the predicted state data at the t-th sampling time. This represents the expected state data at time t. This represents the predictive control input at the t-th sampling time. This represents the environmental stiffness matrix at the t-th sampling time. Indicates force feedback compliance weight. This indicates the acceleration correction amount. The conversion factor between force and acceleration. Let Q represent the contact force data at the t-th sampling time, Q represent the preset trajectory tracking weight, and R represent the preset control input weight. and This represents the discrete matrix obtained by discretizing the continuous-time state-space model of model predictive control using a zero-order hold. This indicates the maximum contact force value. Indicates the maximum position acceleration. This represents the velocity at the t-th sampling time. This indicates the maximum speed.

[0119] This specific implementation incorporates a force feedback compliance cost function into the objective function of MPC, and dynamically determines the weight of the force feedback compliance cost function based on the online estimated environmental stiffness matrix. This enables the robot to adjust the force feedback weights according to different stages, thereby improving the search speed. Furthermore, it achieves active and smooth force-position hybrid control, significantly reducing contact forces and effectively avoiding workpiece damage.

[0120] In one specific embodiment, prior to step S3, the shaft hole assembly method further includes:

[0121] In response to the contact force data at the current sampling time being less than the first preset threshold, the control model predicts and controls to enter the position mode, and determines the objective function as the first objective function, which is constructed based on the first trajectory tracking weight and the first control input weight.

[0122] or,

[0123] In response to the contact force data at the current sampling time being greater than or equal to the first preset threshold, the control model predictive control enters the planar spiral search mode, and the objective function is determined to be the second objective function. The second objective function is constructed based on the second trajectory tracking weight, the second control input weight, and the spiral reference trajectory.

[0124] or,

[0125] In response to the decrease in contact force data at the current sampling time being greater than the second preset threshold, the control model predictive control enters the force control mode, and the objective function is determined to be the third objective function. The third objective function is constructed based on the third trajectory tracking weight, the third control input weight, and the assembly reference trajectory.

[0126] Specifically, since the assembly hole positioning task includes multiple stages such as coarse search, fine positioning (plane search), and precise alignment (assembly), a single control strategy is difficult to meet the needs of each stage at the same time. Therefore, different weight matrices can be set for different search stages, and a state machine can be introduced to manage the entire task process at a high level, so that the objective function of MPC can be adaptively adjusted as the state machine switches.

[0127] When the contact force data collected by the torque sensor of the robot's end effector is less than a first preset threshold, for example, the contact force data... This indicates that the end effector is in the free spatial search stage and has not yet contacted the workpiece surface. Therefore, the state machine control MPC enters the position mode, with the position data error as the control target and the position closed loop as the main control. The preset trajectory tracking weight in the objective function is set to a smaller first trajectory tracking weight Q1, and the preset control input weight in the objective function is set to a larger first control input weight R1 to obtain the first objective function. This allows the MPC to generate the optimal control input sequence in the prediction time domain with the position data of the assembly hole as the target, so that the end effector moves towards the center point of the assembly hole.

[0128] When the contact force data collected by the torque sensor of the end effector is greater than or equal to a first preset threshold, for example, the contact force data... (Newton) indicates that when the end effector contacts the workpiece surface, the state machine determines that it has reached the workpiece surface and controls the MPC to enter the planar spiral search mode, such as... Figure 2 As shown, MPC uses the spiral reference trajectory as the trajectory tracking benchmark, sets the preset trajectory tracking weight in the objective function to a larger second trajectory tracking weight Q2, and sets the preset control input weight in the objective function to a smaller second control input weight R2 to obtain the second objective function. This allows MPC to generate the optimal control input sequence in the prediction time domain with the accurate spiral reference trajectory as the target, thereby controlling the end effector to move along the spiral trajectory in the plane where the assembly hole is located, and searching for the position of the assembly hole through the planar spiral.

[0129] When the contact force data (height in the z-axis direction) collected by the end effector during helical motion decreases significantly, for example, the contact force data... If the threshold value exceeds the second preset threshold, it indicates that an assembly hole may have been found. In this case, the state machine controls the MPC to enter force control mode. Figure 3 As shown, the trajectory tracking reference is changed to the assembly reference trajectory, and the preset trajectory tracking weight in the objective function is set to a suitable third trajectory tracking weight Q3. The preset control input weight in the objective function is set to a suitable second control input weight R3 to obtain the second objective function. This allows MPC to sacrifice some trajectory tracking accuracy in order to prioritize ensuring smooth contact force and generate the optimal control input sequence in the prediction time domain. This drives the end effector to move towards the center of the assembly hole and slowly descend. When the distance the end effector moves in the vertical direction of the plane where the assembly hole is located meets the third preset threshold, it indicates that the end effector has reached the specified depth, and the shaft hole assembly task is completed.

[0130] Among them, Q1, R1, Q2, R2, Q3, and R3 can be preset through the shaft and hole assembly task parameters.

[0131] This specific implementation introduces a state machine for high-level management of the entire task process and enables the parameters of the MPC controller to adaptively adjust as the state machine switches. This resolves the inherent contradiction between search speed and positioning accuracy, ensuring that the robot is always in the optimal working mode of the current stage throughout the entire task process, thereby maximizing the overall system performance.

[0132] In one specific embodiment, the shaft hole assembly method further includes:

[0133] S5. With minimizing the exit optimization function as the optimization objective, model predictive control is used to generate an exit control sequence in the prediction time domain based on the contact force data at the current sampling time.

[0134] The exit optimization function is constructed based on the force feedback compliance cost function.

[0135] S6. According to the exit control sequence, control the end effector to exit the assembly hole in the direction perpendicular to the plane where the assembly hole is located.

[0136] Specifically, after the shaft and hole assembly task is completed, the end effector enters the exit phase, and the MPC will transfer the expected state data of the trajectory tracking reference. The exit path is changed to an upward exit path along the Z-axis. During the upward exit process, if radial jamming occurs due to slight deflection, residual stress, or fit tolerances, the contact force data measured in real time by the torque sensor will be monitored. It will surge, therefore, it can be based solely on the force feedback compliance cost function. Construct an exit optimization function so that MPC can adapt to force feedback compliance costs. Generate an exit control sequence that includes radial fine-tuning or attitude compliance, enabling the robot to exit upwards based on real-time contact force data. Adjust the posture to smoothly resolve the risk of jamming, and continue until a safe height is reached to complete the shaft and hole assembly task.

[0137] The entire assembly process in this embodiment is completed in a closed loop within a single MPC optimization framework. Through the predictive capabilities of MPC, contact input information can be planned in advance, significantly reducing contact force peaks and the risk of jamming. It can simultaneously handle multiple complex requirements such as pose control, force control, safety constraints, and search strategies. Furthermore, through the mapping between the state machine and cost function weights, it can adaptively switch stages based on force feedback, achieving a high degree of automation. This avoids system oscillations caused by force / position control mode switching in traditional methods and eliminates the need for complex nonlinear frictional hard constraints, ensuring the high-frequency real-time solution capability of the underlying controller. In addition, the assembly process in this embodiment is insensitive to initial positioning errors and workpiece dimensional tolerances, and can effectively correct deviations through force feedback and active search strategies.

[0138] In a specific example, such as Figure 4 As shown, when a shaft-hole assembly task is received, the system first enters the initialization phase, loading preset shaft-hole assembly task parameters, including prediction time domain, control cycle, spiral search parameters, MPC weight matrix, and physical constraints. Simultaneously, the state machine is started, with its initial state set to surface position search.

[0139] The state machine controls the MPC to enter position mode, causing the end effector to move towards the target hole in position mode. When the end effector contacts the workpiece surface, the force / torque sensor reading in the z-axis direction will exceed a first preset threshold.

[0140] Once contact is detected, the surface is determined to be found. The state machine controls the MPC to enter a planar spiral search mode, generating an Archimedean spiral reference trajectory with the theoretical hole center point as the origin. The initial radius, radius growth rate, angular velocity, and other parameters of this spiral reference trajectory can be set according to the hole size and assembly tolerances. The MPC uses the spiral reference trajectory as the trajectory tracking benchmark and sets the first trajectory tracking weight and the first control input weight, as well as constraints (such as maintaining a safe height), to generate the optimal control input sequence for each prediction time domain, causing the end effector to move along the spiral trajectory in the plane where the target hole is located.

[0141] When the end effector moves along a helical trajectory and detects a significant decrease in contact force data, indicating that the target hole may have been found, the state machine controls the MPC to enter force control mode. The trajectory tracking reference is changed to a direction perpendicular to the plane of the target hole, and the weights are adjusted to the second trajectory tracking weight and the second control input weight to improve position tracking accuracy and force feedback compliance. This generates the optimal control input sequence for each prediction time domain, driving the end effector to move towards the center of the hole and slowly descend into the hole or to a specified depth. When the end effector reaches the specified depth, the shaft-hole assembly task is completed, and the state machine switches to task completion.

[0142] The state machine controls the MPC to enter and exit mode, and the MPC will transfer the desired state data of the trajectory tracking reference. Change the exit path to the Z-axis upwards, based on the force feedback compliance cost function. An exit control sequence is generated to drive the end effector to exit the target hole. The task ends when the end effector reaches a safe height.

[0143] This embodiment adds a force feedback compliance cost function to the objective function of model predictive control, enabling the robot to achieve active and smooth force-position hybrid control. This improves search speed, significantly reduces contact force, and effectively avoids workpiece damage. Furthermore, the objective function of MPC adaptively adjusts with different control stages, resolving the inherent contradiction between search speed and positioning accuracy. This ensures that the robot remains in the optimal working mode of the current stage throughout the entire task process, maximizing overall system performance. Simultaneously, introducing the environmental stiffness matrix into the MPC state space for online estimation allows for dynamic reconstruction of the weights of the force feedback compliance cost function based on real-time contact states. This overcomes the force control bottleneck of traditional MPC in unknown stiffness environments, achieving high success rate and high compliance in shaft-hole assembly, significantly improving assembly efficiency, accuracy, and reliability.

[0144] Example 2

[0145] In one specific embodiment of this disclosure, a shaft hole assembly system is provided, such as... Figure 5 As shown, the shaft hole assembly system includes:

[0146] The receiving module 100 is used to receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain.

[0147] The acquisition module 200 is used to generate a reference trajectory based on the position data and acquire the state data and contact force data of the robot's end effector at the current sampling time.

[0148] The generation module 300 is used to generate a control input sequence in the prediction time domain based on the reference trajectory, the state data and contact force data at the current sampling time, with the goal of minimizing the objective function.

[0149] The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function.

[0150] The control module 400 is used to control the end effector to move toward the assembly hole according to the control input sequence to complete the shaft hole assembly task.

[0151] In one specific implementation, the state data includes an environmental stiffness matrix;

[0152] The acquisition module 200 is specifically used to acquire the contact force increment and position increment of the end effector in the current sampling period. The contact force increment and position increment are obtained based on the difference between the contact force data and the spatial position data between the current sampling time and the previous sampling time. Based on the contact force increment and position increment, an exponential smoothing online estimation is performed to obtain the environmental stiffness matrix at the current sampling time.

[0153] In one specific implementation, the trajectory tracking cost function is constructed based on the predicted state data, expected state data, and preset trajectory tracking weights at each sampling time in the prediction time domain, wherein the expected state data is obtained based on the reference trajectory;

[0154] The control input smoothing cost function is constructed based on the predicted control input and the preset control input weights at each sampling time in the prediction time domain;

[0155] The force feedback compliance cost function is constructed based on the force feedback compliance weight, predictive control input, and acceleration correction at each sampling time in the prediction time domain. The force feedback compliance weight is obtained based on the environmental stiffness matrix at each sampling time, and the acceleration correction is obtained based on the contact force data at each sampling time.

[0156] In one specific embodiment, the shaft hole assembly task parameters include spiral search parameters;

[0157] The acquisition module 200 is also used to generate a spiral reference trajectory based on the position data and spiral search parameters; and to generate an assembly reference trajectory perpendicular to the plane where the assembly hole is located based on the position data.

[0158] In one specific embodiment, the shaft hole assembly system further includes a determining module;

[0159] The determination module is used to respond to the contact force data at the current sampling time being less than a first preset threshold, and the control model predicts and controls to enter the position mode. The objective function is determined to be the first objective function, which is constructed based on the first trajectory tracking weight and the first control input weight.

[0160] or,

[0161] The module determines that in response to the contact force data at the current sampling time being greater than or equal to a first preset threshold, the control model predicts and controls the entry into a planar spiral search mode, and the objective function is determined to be a second objective function, which is constructed based on the second trajectory tracking weight, the second control input weight, and the spiral reference trajectory.

[0162] or,

[0163] The module determines that when the decrease in contact force data at the current sampling time exceeds the second preset threshold, the control model predicts and controls the force control mode. The objective function is determined to be the third objective function, which is constructed based on the third trajectory tracking weight, the third control input weight, and the assembly reference trajectory.

[0164] In one specific implementation, the generation module 300 is further configured to generate an exit control sequence in the prediction time domain based on the contact force data at the current sampling time, with the goal of minimizing the exit optimization function;

[0165] The exit optimization function is constructed based on the force feedback compliance cost function.

[0166] The control module 400 is also used to control the end effector to exit the assembly hole in a direction perpendicular to the plane where the assembly hole is located, according to the exit control sequence.

[0167] And / or,

[0168] The objective function is as follows:

[0169]

[0170]

[0171] in, This represents the predicted state data at the t-th sampling time. This represents the expected state data at time t. This represents the predictive control input at the t-th sampling time. This represents the environmental stiffness matrix at the t-th sampling time. Indicates force feedback compliance weight. This indicates the acceleration correction amount. The conversion factor between force and acceleration. Let Q represent the contact force data at the t-th sampling time, Q represent the preset trajectory tracking weight, and R represent the preset control input weight. and This represents the discrete matrix obtained by discretizing the continuous-time state-space model of model predictive control using a zero-order hold. This indicates the maximum contact force value. Indicates the maximum position acceleration. This represents the velocity at the t-th sampling time. This indicates the maximum speed.

[0172] For the system embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to in the description of the method embodiments. The system embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components 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 modules can be selected to achieve the purpose of this disclosure according to actual needs.

[0173] This embodiment adds a force feedback compliance cost function to the objective function of model predictive control, enabling the robot to achieve active and smooth force-position hybrid control. This improves search speed, significantly reduces contact force, and effectively avoids workpiece damage. Furthermore, the objective function of MPC adaptively adjusts with different control stages, resolving the inherent contradiction between search speed and positioning accuracy. This ensures that the robot remains in the optimal working mode of the current stage throughout the entire task process, maximizing overall system performance. Simultaneously, introducing the environmental stiffness matrix into the MPC state space for online estimation allows for dynamic reconstruction of the weights of the force feedback compliance cost function based on real-time contact states. This overcomes the force control bottleneck of traditional MPC in unknown stiffness environments, achieving high success rate and high compliance in shaft-hole assembly, significantly improving assembly efficiency, accuracy, and reliability.

[0174] Example 3

[0175] Figure 6 This is a schematic diagram of the structure of an electronic device according to an example embodiment of the present disclosure. The electronic device includes a memory, a processor, and a computer program stored in the memory and used to run on the processor. When the processor executes the computer program, it implements the shaft hole assembly method described in any of the above embodiments. Figure 6 The electronic device 30 shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments disclosed herein.

[0176] like Figure 6 As shown, the electronic device 30 can be manifested as a general-purpose computing device, such as a server device. The components of the electronic device 30 may include, but are not limited to: at least one processor 31, at least one memory 32, and a bus 33 connecting different system components (including memory 32 and processor 31).

[0177] Bus 33 includes a data bus, an address bus, and a control bus.

[0178] The memory 32 may include volatile memory, such as random access memory (RAM) 321 and / or cache memory 322, and may further include read-only memory (ROM) 323.

[0179] The memory 32 may also include a program tool 325 (or utility) having a set (at least one) program module 324, such program module 324 including but not limited to: an operating system, one or more application programs, other program modules and program data, each or some combination of these examples may include an implementation of a network environment.

[0180] The processor 31 executes various functional applications and data processing by running computer programs stored in the memory 32, such as the shaft hole assembly method provided in any of the above embodiments.

[0181] Electronic device 30 can also communicate with one or more external devices 34 (e.g., keyboard, pointing device, etc.). This communication can be performed through input / output (I / O) interface 35. Furthermore, electronic device 30 can also communicate with one or more networks (e.g., local area network (LAN), wide area network (WAN), and / or public network, such as the Internet) via network adapter 36. As shown, network adapter 36 communicates with other modules of electronic device 30 via bus 33. It should be understood that, although not shown in the figure, other hardware and / or software modules can be used in conjunction with electronic device 30, including but not limited to: microcode, device drivers, redundant processors, external disk drive arrays, RAID (disk array) systems, tape drives, and data backup storage systems.

[0182] It should be noted that although several units / modules or sub-units / modules of the electronic device have been mentioned in the detailed description above, this division is merely exemplary and not mandatory. In fact, according to embodiments of this disclosure, the features and functions of two or more units / modules described above can be embodied in one unit / module. Conversely, the features and functions of one unit / module described above can be further divided and embodied by multiple units / modules.

[0183] Example 4

[0184] This disclosure also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the shaft-hole assembly method provided in any of the above embodiments.

[0185] The readable storage medium may be more specifically adopted, including but not limited to: portable disk, hard disk, random access memory, read-only memory, erasable programmable read-only memory, optical storage device, magnetic storage device, or any suitable combination thereof.

[0186] Example 5

[0187] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, implements the shaft-hole assembly method described in any of the above embodiments.

[0188] The program code for executing the computer program product of this disclosure can be written in any combination of one or more programming languages, and the program code can be executed entirely on a user device, partially on a user device, as a stand-alone software package, partially on a user device and partially on a remote device, or entirely on a remote device.

[0189] While specific embodiments of this disclosure have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of this disclosure is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of this disclosure, but all such changes and modifications fall within the scope of protection of this disclosure.

Claims

1. A shaft hole assembly method characterized by, The shaft hole assembly method includes: Receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain; A reference trajectory is generated based on the location data, and the state data and contact force data of the robot's end effector at the current sampling time are obtained; With minimizing the objective function as the optimization objective, model predictive control is used to generate the control input sequence in the prediction time domain based on the reference trajectory, the state data at the current sampling time, and the contact force data; The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function. The end effector is controlled to move toward the assembly hole according to the control input sequence to complete the shaft hole assembly task.

2. The shaft hole assembly method according to claim 1, characterized by, The state data includes the environmental stiffness matrix; The step of obtaining the state data of the robot's end effector at the current sampling time includes: The contact force increment and position increment of the end effector in the current sampling period are obtained, and the contact force increment and position increment are obtained based on the difference between the contact force data and the spatial position data between the current sampling time and the previous sampling time. The environmental stiffness matrix at the current sampling time is obtained by performing an exponentially smoothed online estimation based on the contact force increment and the position increment.

3. The shaft hole assembly method according to claim 2, wherein The trajectory tracking cost function is constructed based on the predicted state data, expected state data, and preset trajectory tracking weights at each sampling time in the prediction time domain, wherein the expected state data is obtained based on the reference trajectory; The control input smoothing cost function is constructed based on the predicted control input and the preset control input weights at each sampling time in the prediction time domain; The force feedback compliance cost function is constructed based on the force feedback compliance weights at each sampling time in the prediction time domain, the prediction control input, and the acceleration correction. The force feedback compliance weights are obtained based on the environmental stiffness matrix at each sampling time, and the acceleration correction is obtained based on the contact force data at each sampling time.

4. The shaft hole assembly method according to claim 1, characterized in that, The shaft hole assembly task parameters include spiral search parameters; The step of generating a reference trajectory based on the location data includes: Based on the location data and the spiral search parameters, a spiral reference trajectory is generated; An assembly reference trajectory perpendicular to the plane containing the assembly hole is generated based on the position data.

5. The shaft hole assembly method according to claim 4, characterized in that, Before the step of generating the control input sequence in the prediction time domain using model predictive control with the objective function minimization as the optimization objective, based on the reference trajectory, the state data at the current sampling time, and the contact force data, the shaft hole assembly method further includes: In response to the contact force data at the current sampling time being less than a first preset threshold, the model prediction control is controlled to enter the position mode, and the objective function is determined to be a first objective function, which is constructed based on the first trajectory tracking weight and the first control input weight. or, In response to the contact force data at the current sampling time being greater than or equal to the first preset threshold, the model prediction control is controlled to enter the planar spiral search mode, and the objective function is determined to be the second objective function, which is constructed based on the second trajectory tracking weight, the second control input weight, and the spiral reference trajectory. or, In response to the decrease in the contact force data at the current sampling time being greater than the second preset threshold, the model prediction control is controlled to enter the force control mode, and the objective function is determined to be the third objective function, which is constructed based on the third trajectory tracking weight, the third control input weight and the assembly reference trajectory.

6. The shaft hole assembly method according to any one of claims 1 to 5, characterized in that, The shaft hole assembly method further includes: With minimizing the exit optimization function as the optimization objective, model predictive control is used to generate the exit control sequence in the prediction time domain based on the contact force data at the current sampling time. The exit optimization function is constructed based on the force feedback compliance cost function. According to the exit control sequence, the end effector is controlled to exit the assembly hole in a direction perpendicular to the plane where the assembly hole is located; And / or, The objective function is as follows: in, This represents the predicted state data at the t-th sampling time. This represents the expected state data at time t. This represents the predictive control input at the t-th sampling time. This represents the environmental stiffness matrix at the t-th sampling time. Indicates force feedback compliance weight. This indicates the acceleration correction amount. The conversion factor between force and acceleration. Let Q represent the contact force data at the t-th sampling time, Q represent the preset trajectory tracking weight, and R represent the preset control input weight. and This represents the discrete matrix obtained by discretizing the continuous-time state-space model of model predictive control using a zero-order hold. This indicates the maximum contact force value. Indicates the maximum position acceleration. This represents the velocity at the t-th sampling time. This indicates the maximum speed.

7. A shaft hole assembly system, characterized in that, The shaft hole assembly system includes: The receiving module is used to receive shaft hole assembly task parameters, which include the position data of the assembly hole and the prediction time domain. The acquisition module is used to generate a reference trajectory based on the position data, and to acquire the state data and contact force data of the robot's end effector at the current sampling time; The generation module is used to generate the control input sequence in the prediction time domain based on the reference trajectory, the state data at the current sampling time, and the contact force data, with the goal of minimizing the objective function; The objective function is pre-constructed and includes a trajectory tracking cost function, a control input smoothing cost function, and a force feedback compliance cost function. The control module is used to control the end effector to move toward the assembly hole according to the control input sequence, so as to complete the shaft hole assembly task.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and for running on the processor, characterized in that, When the processor executes the computer program, it implements the shaft hole assembly method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the shaft-hole assembly method according to any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the shaft hole assembly method as described in any one of claims 1 to 6.

Images