A dual-arm compound robot adaptive task planning control system

Abstract

The present application relates to the technical field of robot task planning, and particularly relates to a self-adaptive task planning control system for a dual-arm compound robot. The system comprises a plug-in space establishment module, a stable supporting state judgment module, an initial contact identification module, a plug-in state judgment module, an abnormal alignment and plug-in completion confirmation module. The plug-in entrance, plug-in direction and theoretical plug-in depth are established according to the actual position and posture of the main workpiece, and the actual axial pushing amount and axial plug-in force are calculated along the plug-in direction; after confirming that the main workpiece is stably supported, plug-in is performed, and suspected plug-in abnormalities are identified according to the axial pushing increment and average axial plug-in force. When an abnormality occurs, the plug-in mechanical arm is controlled to exit to a pre-plug-in safety position, the plug-in space reference is updated again and transverse realignment is performed, and the initial contact position is determined again; the final plug-in state is confirmed in combination with the plug-in completion position error and axial assembly gap, and the accuracy, stability and recovery capability of the robot task planning are improved.

Description

Technical Field

[0001] This invention relates to the field of robot task planning technology, and more specifically to an adaptive task planning and control system for a dual-arm composite robot. Background Technology

[0002] A dual-arm composite robot is a robotic system in which two robotic arms work together to complete the same assembly task. Typically, one arm is a support arm, primarily responsible for gripping, supporting, and stabilizing the main workpiece; the other is an insertion arm, primarily responsible for gripping the insertion parts and performing actions such as approaching, aligning, inserting, abnormal withdrawal, and realigning. In other words, it involves two robotic arms working collaboratively to complete the same assembly task.

[0003] Anomalies during the insertion process of a dual-arm robot are not merely manifested as simple positional deviations or increased insertion force. They are often simultaneously influenced by factors such as the stability of the supporting workpiece, the actual spatial orientation of the insertion inlet, lateral offset of the insertion component, and changes in contact resistance. When the workpiece experiences slight slippage or changes in orientation after clamping, the original insertion direction and insertion inlet position may change. Lateral offset of the insertion component can lead to localized collisions or friction, increasing the axial insertion force but resulting in insufficient actual propulsion. Therefore, relying solely on the robot arm's movement distance, instantaneous insertion force, or whether the preset trajectory has been completed is insufficient to distinguish between normal friction, changes in the workpiece's state, and genuine jamming, easily leading to misjudgments of anomalies.

[0004] Furthermore, current insertion control typically uses a fixed insertion reference established at the start of the task. When an anomaly occurs and the task is terminated, if the main workpiece has changed position or orientation, continuing to use the original insertion entry point, insertion direction, and insertion depth reference for re-insertion can easily lead to a mismatch between the new insertion path and the current actual state of the main workpiece. Simultaneously, if the previous contact position or cumulative movement distance is still used as the insertion depth basis, the displacement generated by the previous abnormal advance, exit, and lateral adjustment may be carried over into the new insertion process, resulting in abnormal actual advance. Summary of the Invention

[0005] To address the aforementioned shortcomings of existing technologies, this invention provides an adaptive task planning and control system for a dual-arm composite robot, which can effectively solve the problems of misjudgment of insertion status and abnormal alignment caused by deviations in the position and posture of the main workpiece, unstable support, and insertion jamming during the insertion process of dual-arm robots in existing technologies.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] This invention provides an adaptive task planning and control system for a dual-arm composite robot, comprising at least:

[0008] The insertion space establishment module determines the insertion entry center point, insertion completion position, insertion direction unit vector, and pre-insertion safety position based on the main workpiece.

[0009] The stable support state judgment module is used to continuously acquire the position, posture and force at the end of the support robot arm of the main workpiece, and allow the insertion robot arm to be inserted after the corresponding fluctuations meet the support conditions.

[0010] The initial contact recognition module controls the insertion robot arm to approach the main workpiece from the pre-insertion safety position along the insertion direction by a unit vector. It identifies the actual initial contact based on the axial insertion force obtained along the unit vector of the insertion direction and records the initial contact position.

[0011] The insertion status judgment module takes the initial contact position as the zero point of advancement, determines the actual axial advancement amount based on the displacement of the unit vector along the insertion direction according to the position of the insertion reference point, and identifies the continuous advancement status or suspected insertion abnormal status based on the axial advancement increment and average axial insertion force in the continuous status judgment window.

[0012] The abnormal alignment module controls the insertion robot arm to retract to the pre-insertion safety position when the insertion is suspected to be abnormal. It reconfirms the stable support status of the main workpiece, updates the insertion entry center point, insertion completion position, insertion direction unit vector and pre-insertion safety position, and performs re-alignment based on the spatial deviation between the front end center point of the insertion part and the insertion entry center point after removing the axial component, and re-identifies the initial contact position.

[0013] The insertion completion confirmation module determines the insertion completion position error based on the reference point position of the insertion part and the insertion completion position, and determines the axial assembly clearance based on the positioning reference positions of the main workpiece and the insertion part, and confirms the insertion task completion status based on both.

[0014] Furthermore, after the support robotic arm completes the initial support, the stable support state judgment module establishes a stable observation window, and the vision acquisition unit continuously acquires at least three fixed and non-collinear reference feature points of the main workpiece set on the main workpiece.

[0015] The geometric center is calculated based on the positions of all reference feature points of the main workpiece at each sampling time, and the spatial distance between the geometric center at each sampling time and the geometric center at the beginning sampling time of the stable observation window is calculated. The largest spatial distance within the stable observation window is selected as the maximum position fluctuation of the main workpiece.

[0016] Furthermore, the stable support status judgment module also includes:

[0017] The reference plane of the main workpiece is determined using three fixed and non-collinear reference feature points of the main workpiece.

[0018] The normal direction of the reference plane of the main workpiece is obtained by cross product of the spatial vectors formed by the lines connecting the reference feature points of different main workpieces.

[0019] Then normalize the normal direction to obtain the unit normal vector of the reference plane;

[0020] The in-plane reference direction unit vector is obtained by normalizing the direction from the first main workpiece reference feature point to the second main workpiece reference feature point.

[0021] Calculate the angle between the unit normal vector of the reference plane at each sampling time and the unit normal vector of the reference plane at the beginning of the stable observation window, as well as the angle between the unit vector of the in-plane reference direction at each sampling time and the unit vector of the in-plane reference direction at the beginning of the stable observation window.

[0022] The larger of the two included angles is taken as the change in the posture of the main workpiece at the corresponding sampling time, and the largest change in the posture of the main workpiece within the stable observation window is taken as the maximum posture fluctuation of the main workpiece.

[0023] Furthermore, a first torque sensor is disposed at the end of the supporting robotic arm, wherein:

[0024] The first torque sensor continuously collects the force vector at the end of the supporting robotic arm, and the average of all the force vectors at the end of the supporting robotic arm within the stable observation window is used to obtain the stable average supporting force vector.

[0025] Calculate the difference vector between the force vector at the end of the supporting robotic arm and the stable average supporting force vector at each sampling time, and calculate the Euclidean length of each difference vector. Select the maximum value from all Euclidean lengths as the maximum fluctuation of the force at the end of the supporting robotic arm.

[0026] Furthermore, when the maximum position fluctuation of the main workpiece is not higher than the upper limit of the allowable position fluctuation, the maximum attitude fluctuation of the main workpiece is not higher than the upper limit of the allowable attitude fluctuation, and the maximum fluctuation of the end force of the supporting robotic arm is not higher than the upper limit of the allowable supporting force fluctuation, the main workpiece is determined to be in a stable supporting state.

[0027] Furthermore, the initial contact recognition module controls the insertion robot arm to approach the insertion inlet center point at a low speed from the pre-insertion safety position along the insertion direction by a unit vector, wherein a second torque sensor is disposed at the end of the insertion robot arm;

[0028] The end-effector force of the insertion robot arm collected by the second torque sensor is converted to the robot base coordinate system, and the end-effector force vector in the robot base coordinate system is projected onto the unit vector of the insertion direction to obtain the axial insertion force at each sampling moment.

[0029] When the axial insertion force corresponding to a consecutive preset number of sampling points is not lower than the contact force threshold, it is determined that the insert and the main workpiece form a real initial contact.

[0030] The initial contact time is determined by the moment when the continuous contact condition is first met, and the reference point position of the insert corresponding to the initial contact time is saved as the initial contact position.

[0031] Furthermore, the method for determining the axial thrust increment is as follows:

[0032] Subtract the spatial position of the reference point of the insert at the current sampling moment from the initial contact position to obtain the spatial displacement vector since the formation of the actual initial contact.

[0033] The spatial displacement vector is projected onto the unit vector of the insertion direction, and the projection length is used as the actual axial advance at the current sampling moment.

[0034] Using the current sampling time as the end time of the state judgment window, the actual axial thrust corresponding to the start and end sampling times of the state judgment window is read, and the difference between the two actual axial thrusts is used as the axial thrust increment.

[0035] Furthermore, the arithmetic mean of all axial insertion forces within the state judgment window is used to obtain the average axial insertion force; when the axial advance increment is not lower than the advance lower limit and the average axial insertion force is not higher than the abnormal insertion force threshold, it is determined to be a continuous advance state;

[0036] When the average axial insertion force is not lower than the abnormal insertion force threshold and the axial advance increment is lower than the advance lower limit, and the corresponding combination state continuously maintains a preset number of state judgment windows, it is determined to be a suspected abnormal insertion state.

[0037] Furthermore, the method for realigning and re-identifying the initial contact position after removing the axial component and correcting the spatial deviation is as follows:

[0038] Obtain the current center point of the front end of the insert, and form a spatial error vector based on the positional difference between the center point of the insert inlet and the center point of the front end of the insert;

[0039] The projection of the spatial error vector onto the unit vector of the insertion direction is taken as the axial component. The axial component is removed from the spatial error vector to obtain the lateral alignment error vector.

[0040] The Euclidean length of the lateral alignment error vector is used as the magnitude of the lateral alignment error.

[0041] The insertion robot arm is controlled to perform lateral position correction along the direction indicated by the lateral alignment error vector until the magnitude of the lateral alignment error is not higher than the upper limit of the allowable lateral alignment error.

[0042] The actual initial contact identification is re-executed, and the zero point of the actual axial thrust is re-established based on the re-identified initial contact position.

[0043] Furthermore, the current reference point position of the insert is obtained, and the Euclidean distance between the reference point position of the insert and the insertion completion position is calculated as the insertion completion position error;

[0044] Obtain the positioning reference point of the main workpiece set on the main workpiece and the positioning reference point of the insert set on the insert, and obtain the positioning position difference vector based on the position difference between the main workpiece positioning reference point and the insert positioning reference point;

[0045] Project the position difference vector onto the unit vector in the insertion direction and take the absolute value of the projection length to obtain the current axial assembly clearance;

[0046] Compare the current axial assembly clearance with the design target axial assembly clearance, and use the absolute value of the difference between the two as the axial clearance error:

[0047] When the insertion completion position error is not higher than the upper limit of the allowable completion position error and the axial clearance error is not higher than the upper limit of the allowable axial clearance error, and a preset number of sampling points are continuously maintained, the insertion task completion status is output.

[0048] The technical solution provided by this invention has the following advantages compared with the known prior art:

[0049] By continuously monitoring the position and attitude of the main workpiece and the force at the end of the supporting robotic arm, insertion is only initiated after all three fluctuations meet the stability conditions, reducing false judgments caused by the unstable main workpiece. The actual initial contact position is used as the zero point of the actual axial advance, and the axial advance increment and average axial insertion force within the status judgment window are combined to identify abnormal states of sustained high force and low advance, reducing false judgments caused by instantaneous friction peaks. After an anomaly occurs, the insertion robotic arm retracts to the pre-insertion safety position, re-monitors the main workpiece status, updates the insertion space reference, removes the axial component, performs lateral re-alignment, and re-determines the initial contact position, avoiding the cumulative impact of the previous failed insertion process on the re-insertion. The insertion completion status is continuously confirmed by combining the insertion completion position error and the actual axial assembly clearance, improving the reliability of the final positioning judgment. Attached Figure Description

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

[0051] Figure 1 This is a schematic diagram of the overall system architecture of the present invention. Detailed Implementation

[0052] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings.

[0053] Example 1 (see Figure 1 (i) After the components, i.e., the main workpiece, arrive at the assembly station, the actual stopping position and posture of the main workpiece may deviate from the preset position, and the insertion direction may not necessarily coincide with the X-axis, Y-axis, or Z-axis of the robot's base coordinate system. If the displacement of the robot arm on a certain coordinate axis is used as the insertion depth, the lateral movement or posture correction action of the insertion robot arm to align with the insertion entrance may be incorrectly included in the insertion distance. Therefore, this adaptive task planning and control system for a dual-arm composite robot includes at least:

[0054] The insertion space establishment module is used to establish the insertion entry point, insertion direction, and theoretical insertion depth for this insertion task based on the actual position of the current main workpiece. Specifically:

[0055] The vision acquisition unit includes an RGB-D camera or a binocular camera to acquire images and depth information of the main workpiece surface. The rotation matrix and translation vector of the vision acquisition unit's coordinate system relative to the robot's base coordinate system are pre-determined. The spatial position obtained by the vision acquisition unit is then transformed to the robot's base coordinate system based on the rotation matrix and translation vector (a common calibration transformation method in existing technology), resulting in the position vector of the insertion inlet center point. is a three-dimensional vector. , , These are represented by three coordinate components, in turn. Combining the current spatial pose of the main workpiece with the pre-stored target position of the fully assembled insert in the coordinate system of this model's main workpiece, the target position is transformed to the robot's base coordinate system, yielding the insertion completion position vector corresponding to this insertion task. It is a three-dimensional vector, based on the position vectors of the two. The insertion direction vector V, which points from the insertion entry center point to the insertion completion position, is obtained and used to determine the actual spatial propulsion direction of the current insertion task based on the insertion entry center point and the insertion completion position.

[0056] Furthermore, the theoretical insertion depth is calculated based on the insertion direction vector. , The Euclidean norm of a vector is used to determine the theoretical axial advance distance that the insert should complete from the center point of the insert entry to the fully inserted position.

[0057] Establish the unit vector n for the insertion direction, that is: This is used to normalize the insertion direction vector, so that the subsequent displacement of the robotic arm and the end force can be uniformly projected onto the actual insertion direction, eliminating the influence of lateral movement on the calculation of axial propulsion and axial insertion force.

[0058] In this embodiment, to ensure a uniform exit and realignment position for the insertion robot arm in case of an anomaly, a pre-insertion safety position is provided outside the insertion inlet: , This indicates the preset safety distance from the pre-insertion safety position to the center point of the insertion inlet along the insertion direction. This represents the three-dimensional position vector of the pre-insertion safe position in the robot's base coordinate system. It is used to establish a unified approach and exit position outside the insertion inlet, ensuring the insertion robot arm has a clear safe starting position during initial insertion, stuck exit, and realignment, avoiding attitude adjustments during contact. It is defined by the insertion inlet center point, insertion completion position, theoretical insertion depth, insertion direction unit vector, and pre-insertion safe position. Subsequent actual axial advance is calculated only along the insertion direction unit vector; lateral alignment actions are not incorrectly included in the insertion depth. In the event of an anomaly, the insertion robot arm also uniformly exits to the pre-insertion safe position.

[0059] Before the insertion robot arm begins its advance, after the supporting robot arm completes clamping or supporting the main workpiece, the main workpiece may not immediately be in a stable state. It may still experience short-term position and attitude changes due to clamping rebound, structural micro-vibration, local slippage, or redistribution of force at the end of the supporting robot arm. If the insertion robot arm is started directly before the main workpiece is stable, subsequent detections of increased insertion resistance and insufficient insertion of the insertion component will make it difficult to determine whether these phenomena are caused by jamming of the insertion component itself or by the main workpiece still being in an unstable state. Therefore, the system includes:

[0060] The stable support state determination module is used to continuously observe the position, orientation, and force on the end effector of the main workpiece after the robotic arm completes initial support, confirming that the main workpiece has entered a stable support state, and establishing a stable support benchmark for subsequent anomaly detection. This includes:

[0061] After the robotic arm completes its initial support of the main workpiece, the end time of the support action is recorded, and from that moment on, the position data of the reference feature points of the main workpiece and the end force data of the robotic arm are continuously collected. It should be noted that the sampling period is set to [value missing]. From the moment the support action ends, select continuously Each sampling point constitutes a stable observation window, and the time length corresponding to the stable observation window is the sampling period. With the number of sampling points The product of.

[0062] The vision acquisition unit selects three fixed (not that the three points are permanently stationary in space, but that their positions on the main workpiece itself remain unchanged. For example, the three hole centers, three corner points, three marker points, or three stable structural feature points on the instrument panel frame can be selected. When the main workpiece moves as a whole, the three points will move with the main workpiece, but their structural positional relationship remains unchanged) and non-collinear (meaning the three points are not located on the same straight line, and the three points are not arranged in a straight line, but can form a triangle so that the three points can determine a reference plane and further calculate the attitude direction of the main workpiece) reference feature points of the main workpiece: Each reference feature point represents an actual structural position on the main workpiece, such as a hole center, corner point, or visual marker point, and is a three-dimensional position vector in the robot's base coordinate system. Using only one point only indicates whether the main workpiece has changed position, but cannot determine whether it has rotated. Using two points provides a direction, but still cannot fully reflect the attitude change of a plane. Using three non-collinear points allows for the simultaneous calculation of: the geometric center of the three points, representing the overall position of the main workpiece; and the normal vector of the reference plane formed by the three points, representing the attitude of the main workpiece. Then, based on these three points, the geometric center of the reference feature point group of the main workpiece at the r-th sampling time can be calculated. Multiple fixed reference feature points together characterize the overall spatial position of the main workpiece.

[0063] Furthermore, a reference plane for the main workpiece is established using three non-collinear reference feature points, and the unit normal vector of this reference plane is calculated:

[0064] ;

[0065] in, This is the unit normal vector of the reference plane of the main workpiece at the r-th sampling time. It is based on the geometric center of the reference feature point group of the main workpiece. Calculate the maximum positional fluctuation of the main workpiece within the stable observation window. This method quantifies the maximum positional change of the main workpiece within a certain observation period after the robotic arm completes its support, determining whether the main workpiece still exhibits swaying, slippage, or continuous displacement. 'r' represents the sampling sequence number within the stable observation window. To stabilize the geometric center of the main workpiece at the first sampling moment of the observation window.

[0066] Furthermore, by utilizing the first and second datum feature points among the three datum feature points, a unit vector of the datum direction within the surface of the main workpiece is established. It is used to characterize in-plane rotations that cannot be reflected by the normal vector of the reference plane alone.

[0067] Calculate the change angle of the normal to the reference plane Then calculate the angle of change of the in-plane reference direction. The larger of the two values ​​is taken as the change in the main workpiece posture at the r-th sampling time. . This represents the unit vector of the reference direction within the surface of the main workpiece at the first sampling moment.

[0068] Calculate the maximum attitude fluctuation of the main workpiece within the stable observation window: , This represents the unit normal vector of the reference plane of the main workpiece at the first sampling moment of the stable observation window. Indicates the maximum attitude change angle. ∠ denotes the inverse cosine operation, and · denotes the vector dot product.

[0069] In this embodiment, the first torque sensor (located between the end flange of the supporting robotic arm and the supporting fixture, used to collect the triaxial force and triaxial torque transmitted from the main workpiece to the end of the supporting robotic arm via the supporting fixture) obtains the force vector at the end of the supporting robotic arm. (Three directions) Calculate the stable average support force vector in the stable observation window. ;

[0070] Therefore, the maximum fluctuation of the end effector force of the supporting robotic arm is established: within a stable observation window, for each sampling time r, the vector of the end effector force of the supporting robotic arm at that sampling time is calculated. With the average support force vector within the stable observation window The difference vector between them ( - ), and calculate the Euclidean norm of the difference vector to obtain the change in the force at the end of the supporting robotic arm relative to the average supporting force at that sampling moment; subsequently, within the stable observation window containing all At each sampling time point, the maximum value of the change is selected as the maximum fluctuation of the end effector force of the supporting robotic arm within the stable observation window. It reflects the maximum deviation of the force at the end of the supporting robotic arm from the average supporting force state within the stable observation window, and is used to determine whether the main workpiece still has a situation of continuous force adjustment, local slippage or unstable clamping state after the initial support is completed.

[0071] This results in fluctuations at the maximum position of the main workpiece. The maximum attitude fluctuation of the main workpiece shall not exceed the upper limit of the allowable positional fluctuation. Not exceeding the upper limit of allowable attitude fluctuation, and supporting the maximum fluctuation of the end effector force of the robotic arm. If the support force fluctuation is not higher than the upper limit of the allowable support force, the main workpiece is determined to be in a stable support state. Otherwise, the support is maintained and the next stable observation window is entered for judgment, without initiating the axial advancement of the insertion robot arm.

[0072] The system also includes an initial contact recognition module, used to move the insertion robotic arm from the pre-insertion safe position after the stable support status judgment module determines that the main workpiece has been stably supported. The manipulator moves slowly towards the insertion inlet along the insertion direction, with a unit vector n. The end effector force vector acquired by the second torque sensor is transformed into the robot's base coordinate system according to the current end effector posture, resulting in the end effector force vector in the robot's base coordinate system. Calculate the axial insertion force at the k-th sampling time: .

[0073] in, : Axial insertion force along the insertion direction at the k-th sampling time : No. At each sampling time, the three-dimensional force vector at the end of the robotic arm is inserted. : Sampling sequence number during insertion process.

[0074] When axial insertion force In the When all sampling points within the contact confirmation window are above the contact force threshold, it is determined that the insert and the main workpiece have formed a true initial contact, and the contact start sampling time corresponding to the first formation of continuous contact is determined as the initial contact time. Save the position of the reference point of the insert in the robot's base coordinate system at the initial contact moment, and record it as the initial contact position. (That is, the location of the reference point of the insert when it first makes actual contact with the main workpiece). Among them, To determine the number of consecutive sampling points required for initial contact, axial insertion force was used. Identify the true initial contact and the initial contact location. This serves as the sole zero point for the subsequent actual axial advance. Therefore, the actual axial advance calculated later only reflects the true insertion distance of the insert after it enters the main workpiece, and does not include the free approach distance outside the insertion inlet.

[0075] The insertion state determination module calculates the actual axial advance of the insertion part relative to the initial contact position at the k-th sampling time after initial contact.

[0076] ;

[0077] Position of the insertion reference point at the k-th sampling time (The three-dimensional position vector of the reference point of the insert in the robot's base coordinate system) minus the initial contact position The spatial displacement vector of the insertion manipulator since the initial contact is obtained; the spatial displacement vector is projected onto the unit vector n in the insertion direction, and the resulting projection length is taken as the actual axial thrust at the k-th sampling time. Therefore, only the displacement generated by the insertion robot arm from the actual initial contact position along the actual insertion direction is retained to obtain the axial insertion distance of the insertion part into the main workpiece, avoiding the inclusion of lateral adjustment displacement and free approach displacement outside the insertion inlet in the insertion depth.

[0078] Using the k-th sampling time as the end time of the current state judgment window, continuously select time points backward from the current time, including the current time. Each sampling point, i.e. Read the actual axial thrust at the current moment. The actual axial thrust at the current moment Subtract the actual axial advance at the start of the window The actual increase in axial propulsion distance of the insert within the state judgment window is obtained and recorded as the axial propulsion increment. By comparing the actual axial advance at the start and end points of the same state judgment window, it is possible to determine whether the insert has generated effective advance within the continuous time period. This avoids judging the insert state based solely on the small displacement changes of two adjacent sampling points, and is suitable for distinguishing between normal continuous advance and a stuck state where the force increases but the advance is insufficient.

[0079] Calculate the average axial insertion force within the state judgment window: take the k-th sampling time as the end time of the current state judgment window, and continuously select from the current time backward. Each sampling point; read respectively Axial insertion force corresponding to each sampling point The average axial insertion force within the current state judgment window is obtained by summing all the axial insertion forces and dividing the sum by the number of sampling points. By averaging the axial insertion force at multiple consecutive sampling points within the state judgment window, the influence of force peaks caused by instantaneous friction, mechanical vibration, or short-term sensor disturbances at a single sampling moment is reduced, so that subsequent judgments of suspected insertion anomalies reflect a continuous stress state rather than a single, occasional instantaneous force change.

[0080] When axial propulsion increment Not lower than the lower limit of propulsion, and the average axial insertion force If the force is not higher than the abnormal insertion force threshold, the current state is determined as the continuous advancement state, and insertion is continued to be performed along the unit vector of the insertion direction.

[0081] When the average axial insertion force Not lower than the abnormal insertion force threshold, and axial advance increment Below the propulsion lower limit, and this combination of high axial insertion force and low axial propulsion increment continues to maintain the preset relationship. When a status judgment window appears, the current status is determined to be a suspected instrumentation abnormality, and the process proceeds to the next module.

[0082] When the average axial insertion force and axial advance increment do not meet the judgment conditions of the above continuous advance state or suspected insertion abnormal state, the current insertion process is maintained and the process continues to enter the next state judgment window for judgment.

[0083] To confirm the required number of consecutive abnormal state judgment windows for insertion anomalies, a force-propulsion result correspondence is established using axial propulsion increment and average axial insertion force. This ensures that a suspected insertion anomaly state only occurs when the axial insertion force remains consistently high while the actual propulsion of the insertion part remains insufficient, avoiding misjudgments caused by normal instantaneous friction peaks. Furthermore, the completion of the insertion robot arm's movement is not used as the basis for determining insertion completion, preventing a partially inserted state from being mistakenly judged as assembly completion.

[0084] It should be noted that when the current position of the insertion part reference point enters the preset proximity range of the insertion completion position, the final insertion completion confirmation module will be entered.

[0085] Furthermore, the abnormal alignment module, after the previous module has identified a suspected abnormal insertion state based on the average axial insertion force and axial propulsion increment within the current state judgment window—that is, the insertion robot arm is continuously applying a large axial insertion force, but the insertion part has not obtained effective axial propulsion corresponding to the force—will uniformly stop applying force along the original insertion path to prevent the abnormal state from escalating. The insertion robot arm will stop propulsing along the insertion direction and will withdraw in the opposite direction of the current insertion direction unit vector n to the initially established pre-insertion safety position, causing the insertion part to exit the current insertion contact area. After the insertion robot arm withdraws, the vision acquisition unit will reacquire the spatial position and spatial orientation of the main workpiece, and continuously acquire the position, orientation, and end force of the supporting robot arm within the preset stable observation window according to the method of the insertion state judgment module; if the position change, orientation change, and end force fluctuation of the supporting robot arm all meet the stable support conditions specified by the insertion state judgment module, the supporting robot arm will maintain its current support state. If the conditions are not met, the support robot arm will readjust its gripping or supporting state on the main workpiece, and after the adjustment is completed, the stability will be judged again according to the method of the insertion state judgment module until the main workpiece meets the stable support conditions.

[0086] After the main workpiece meets the stable support conditions, the process is repeated. The insertion entry center point, insertion completion position, theoretical insertion depth, insertion direction unit vector, and pre-insertion safety position are updated based on the main workpiece's current actual spatial position and attitude. This ensures that subsequent re-insertion uses the latest insertion space reference corresponding to the main workpiece's current actual state. Then, the inserted parts are re-aligned based on the lateral alignment error, including:

[0087] 1) The vision acquisition unit obtains the three-dimensional position of the center point of the front end of the current insert in the robot's base coordinate system and records it as... .

[0088] 2) Obtain the spatial error vector by subtracting the center point of the front end of the insert from the center point of the insert entry. .

[0089] 3) Since the insert is currently located outside the insert inlet, the spatial error vector... This includes both the front-to-back distance along the insertion direction and the lateral positional deviation perpendicular to the insertion direction. The front-to-back distance along the insertion direction is the normal advancing distance required for subsequent re-approach and should not be treated as a lateral alignment error. Therefore, the spatial error vector is calculated first. The axial component of the unit vector n in the insertion direction is then removed from the spatial error vector to obtain the lateral alignment error vector. :

[0090] ;

[0091] This calculation formula is used to eliminate the normal front-to-back distance along the insertion direction from the overall spatial deviation between the insert and the insertion port, retaining only the lateral positional deviation that may actually cause the insert to collide with the insertion port, cause local interference, or cause it to jam again.

[0092] 4) Calculate the Euclidean norm of the lateral alignment error vector to obtain the magnitude of the lateral alignment error: ;

[0093] It should be noted that the magnitude of the lateral alignment error It reflects the magnitude of the lateral deviation between the center point of the front end of the insert and the center point of the insert entry at the k-th sampling time.

[0094] 5) Based on the lateral alignment error vector The directional control of the insertion robot arm performs lateral position correction, moving the center point of the insertion part's front end laterally towards the center point of the insertion inlet. During this lateral position correction, the normal forward / backward distance along the insertion direction is not used as compensation for lateral error. After each lateral position correction, the vision acquisition unit re-acquires the position of the insertion part's front end center point. Then, recalculate the lateral alignment error according to steps 3) to 5). When the lateral alignment error is not higher than the upper limit of the allowable lateral alignment error, the lateral realignment of the insert is considered complete.

[0095] 6) When the lateral alignment error is not higher than the upper limit of the allowable lateral alignment error, the realignment is considered complete. At this time, the insertion robot arm moves to the updated pre-insertion safety position and returns to the initial contact recognition module to re-identify the initial contact position between the insertion part and the main workpiece. The actual axial advance is then recalculated based on the new initial contact position. The initial contact position and actual axial advance used in the previous abnormal insertion process are no longer used. The re-insertion process uses a new, true contact position as the zero point for axial advance, avoiding the accumulation of the advance distance, retraction distance, or lateral adjustment distance from the previous failed insertion into the new actual axial advance. The insert reference point is a pre-determined fixed position on the insert, representing the insert's position in the robot's base coordinate system. The insert front center point is the geometric center point located at the front of the insert's insertion end, representing the lateral alignment between the insert front and the insertion inlet. Their relative positions on the insert are fixed.

[0096] It should be noted that during the continuous advancement of the insertion status judgment module, or after lateral re-alignment by the abnormal alignment module and re-entry into the initial contact recognition module and insertion status judgment module, when the current position of the insertion part reference point has entered the preset proximity range of the insertion completion position, the final positioning confirmation is initiated. Specifically, an insertion completion confirmation module is also provided to confirm the insertion completion status based on the insertion completion position and the actual assembly gap, including:

[0097] Collect the current reference point position of the insert. And compare it with the completed insertion position to calculate the insertion completion position error:

[0098] ;

[0099] Determine whether the insert has been advanced to near the theoretical insertion depth.

[0100] A positioning reference position that reflects the complete insertion relationship is pre-determined on both the main workpiece and the insert component. The vision acquisition unit acquires the current positioning reference point of the main workpiece. and the current reference point for the insertion of the component. The reference points are all three-dimensional position vectors. Calculate the position difference vector between two positioning reference points. Then project this position difference vector onto the unit vector n in the insertion direction, and take the absolute value of the projection length to obtain the current axial assembly clearance between the main workpiece and the insertion part. It is used to extract the remaining gap along the insertion direction from the actual visual position of the main workpiece and the insert, reflecting whether the insert has achieved the final assembly relationship with the main workpiece as required by the design, and is not affected by lateral position differences.

[0101] Current axial assembly clearance By comparing the axial assembly clearance with the design target, the absolute value of the difference between the two is obtained, thus yielding the axial clearance error. .

[0102] This allows for comparison of the insertion completion position error. and axial clearance error :

[0103] When the insertion completion position error is not higher than the upper limit of the allowable completion position error, and the axial clearance error is not higher than the upper limit of the allowable axial clearance error, the current sampling time is determined as the time when the one-time completion condition is met. Let... To confirm the number of preset sampling points required to continuously meet the insertion conditions for complete insertion.

[0104] When the above two conditions are continuously maintained at the preset level When sampling points are reached, the current insertion of the insert is determined and the insertion task completion status is output.

[0105] If either the insertion completion position error or the axial clearance error exceeds the corresponding allowable error limit, the insertion completion status will not be output. In this case, the insertion status judgment module will continue to perform corresponding processing based on the current insertion status output: If the insertion status judgment module determines that the current state is still in a continuous advancing state, the insertion robot arm will continue to perform insertion along the insertion direction and continuously update the current position of the insertion part reference point, the actual axial advance amount, and the axial insertion force. If the insertion status judgment module again determines that the current state is in a suspected insertion abnormality state, it will enter an abnormal exit and realignment process, performing abnormal exit, insertion space reference update, and lateral realignment processing.

[0106] The above embodiments are only used to illustrate the technical solutions of the present invention; those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions described in the foregoing embodiments without causing the essence of the corresponding technical solutions to deviate from the protection scope of the technical solutions of the embodiments of the present invention.

Claims

1. An adaptive task planning and control system for a dual-arm composite robot, characterized in that, include: The insertion space establishment module determines the insertion entry center point, insertion completion position, insertion direction unit vector, and pre-insertion safety position based on the main workpiece. The stable support state judgment module is used to continuously acquire the position, posture and force at the end of the support robot arm of the main workpiece, and allow the insertion robot arm to be inserted after the corresponding fluctuations meet the support conditions. The initial contact recognition module controls the insertion robot arm to approach the main workpiece from the pre-insertion safety position along the insertion direction by a unit vector. It identifies the actual initial contact based on the axial insertion force obtained along the unit vector of the insertion direction and records the initial contact position. The insertion status judgment module takes the initial contact position as the zero point of advancement, determines the actual axial advancement amount based on the displacement of the unit vector along the insertion direction according to the position of the insertion reference point, and identifies the continuous advancement status or suspected insertion abnormal status based on the axial advancement increment and average axial insertion force in the continuous status judgment window. The abnormal alignment module controls the insertion robot arm to retract to the pre-insertion safety position when the insertion is suspected to be abnormal. It reconfirms the stable support status of the main workpiece, updates the insertion entry center point, insertion completion position, insertion direction unit vector and pre-insertion safety position, and performs re-alignment based on the spatial deviation between the front end center point of the insertion part and the insertion entry center point after removing the axial component, and re-identifies the initial contact position. The insertion completion confirmation module determines the insertion completion position error based on the reference point position of the insertion part and the insertion completion position, and determines the axial assembly clearance based on the positioning reference positions of the main workpiece and the insertion part, and confirms the insertion task completion status based on both.

2. The system according to claim 1, characterized in that, After the support robot arm completes the initial support, the stable support state judgment module establishes a stable observation window, and the vision acquisition unit continuously acquires at least three fixed and non-collinear reference feature points of the main workpiece set on the main workpiece. The geometric center is calculated based on the positions of all reference feature points of the main workpiece at each sampling time, and the spatial distance between the geometric center at each sampling time and the geometric center at the beginning sampling time of the stable observation window is calculated. The largest spatial distance within the stable observation window is selected as the maximum position fluctuation of the main workpiece.

3. The system according to claim 2, characterized in that, The stable support status determination module also includes: The reference plane of the main workpiece is determined using three fixed and non-collinear reference feature points of the main workpiece. The normal direction of the reference plane of the main workpiece is obtained by cross product of the spatial vectors formed by the lines connecting the reference feature points of different main workpieces. Then normalize the normal direction to obtain the unit normal vector of the reference plane; The in-plane reference direction unit vector is obtained by normalizing the direction from the first main workpiece reference feature point to the second main workpiece reference feature point. Calculate the angle between the unit normal vector of the reference plane at each sampling time and the unit normal vector of the reference plane at the beginning of the stable observation window, as well as the angle between the unit vector of the in-plane reference direction at each sampling time and the unit vector of the in-plane reference direction at the beginning of the stable observation window. The larger of the two included angles is taken as the change in the posture of the main workpiece at the corresponding sampling time, and the largest change in the posture of the main workpiece within the stable observation window is taken as the maximum posture fluctuation of the main workpiece.

4. The system according to claim 1, characterized in that, It also includes a first torque sensor located at the end of the supporting robotic arm, wherein: The first torque sensor continuously collects the force vector at the end of the supporting robotic arm, and the average of all the force vectors at the end of the supporting robotic arm within the stable observation window is used to obtain the stable average supporting force vector. Calculate the difference vector between the force vector at the end of the supporting robotic arm and the stable average supporting force vector at each sampling time, and calculate the Euclidean length of each difference vector. Select the maximum value from all Euclidean lengths as the maximum fluctuation of the force at the end of the supporting robotic arm.

5. The system according to claim 4, characterized in that, When the maximum position fluctuation of the main workpiece is not higher than the upper limit of the allowable position fluctuation, the maximum attitude fluctuation of the main workpiece is not higher than the upper limit of the allowable attitude fluctuation, and the maximum fluctuation of the end force of the supporting robotic arm is not higher than the upper limit of the allowable supporting force fluctuation, the main workpiece is determined to be in a stable supporting state.

6. The system according to claim 1, characterized in that, The initial contact recognition module controls the insertion robot arm to approach the insertion inlet center point at a low speed from the pre-insertion safety position along the insertion direction by a unit vector, wherein a second torque sensor is disposed at the end of the insertion robot arm; The end-effector force of the insertion robot arm collected by the second torque sensor is converted to the robot base coordinate system, and the end-effector force vector in the robot base coordinate system is projected onto the unit vector of the insertion direction to obtain the axial insertion force at each sampling moment. When the axial insertion force corresponding to a consecutive preset number of sampling points is not lower than the contact force threshold, it is determined that the insert and the main workpiece form a real initial contact. The initial contact time is determined by the moment when the continuous contact condition is first met, and the reference point position of the insert corresponding to the initial contact time is saved as the initial contact position.

7. The system according to claim 6, characterized in that, The method for determining the axial thrust increment is as follows: Subtract the spatial position of the reference point of the insert at the current sampling moment from the initial contact position to obtain the spatial displacement vector since the formation of the actual initial contact. The spatial displacement vector is projected onto the unit vector of the insertion direction, and the projection length is used as the actual axial advance at the current sampling moment. Using the current sampling time as the end time of the state judgment window, the actual axial thrust corresponding to the start and end sampling times of the state judgment window is read, and the difference between the two actual axial thrusts is used as the axial thrust increment.

8. The system according to claim 7, characterized in that, The arithmetic mean of all axial insertion forces within the state judgment window is used to obtain the average axial insertion force; when the axial advance increment is not lower than the advance lower limit and the average axial insertion force is not higher than the abnormal insertion force threshold, it is determined to be a continuous advance state; When the average axial insertion force is not lower than the abnormal insertion force threshold and the axial advance increment is lower than the advance lower limit, and the corresponding combination state continuously maintains a preset number of state judgment windows, it is determined to be a suspected abnormal insertion state.

9. The system according to claim 1, characterized in that, The method for realigning and re-identifying the initial contact position after removing the axial component and correcting the spatial deviation is as follows: Obtain the current center point of the front end of the insert, and form a spatial error vector based on the positional difference between the center point of the insert inlet and the center point of the front end of the insert; The projection of the spatial error vector onto the unit vector of the insertion direction is taken as the axial component. The axial component is removed from the spatial error vector to obtain the lateral alignment error vector. The Euclidean length of the lateral alignment error vector is used as the magnitude of the lateral alignment error. The insertion robot arm is controlled to perform lateral position correction along the direction indicated by the lateral alignment error vector until the magnitude of the lateral alignment error is not higher than the upper limit of the allowable lateral alignment error. The actual initial contact identification is re-executed, and the zero point of the actual axial thrust is re-established based on the re-identified initial contact position.

10. The system according to claim 1, characterized in that, Also includes: Obtain the current reference point position of the insert, calculate the Euclidean distance between the reference point position and the insertion completion position, and use it as the insertion completion position error; Obtain the positioning reference point of the main workpiece set on the main workpiece and the positioning reference point of the insert set on the insert, and obtain the positioning position difference vector based on the position difference between the main workpiece positioning reference point and the insert positioning reference point; Project the position difference vector onto the unit vector in the insertion direction and take the absolute value of the projection length to obtain the current axial assembly clearance; Compare the current axial assembly clearance with the design target axial assembly clearance, and use the absolute value of the difference between the two as the axial clearance error: When the insertion completion position error is not higher than the upper limit of the allowable completion position error and the axial clearance error is not higher than the upper limit of the allowable axial clearance error, and the preset number of sampling points are continuously maintained, the insertion task completion status is output.

Images