A mechanical arm compliant gun delivery control method and device based on video and force sense fusion feedback

Abstract

The present application relates to electric vehicle charging auxiliary technical field, specifically to a kind of mechanical arm soft gun delivery control method and device based on video and force sensation fusion feedback, device includes sliding table, charging mechanical arm, end second perception module, end six-dimensional force / torque sensor, gun handle holding detection sensor, charging gun clamping mechanism and station end controller;Method is extracted by gravity and inertia compensation only by user applied external force, uses visual, holding detection and force sensation multi-source information fusion combination hysteresis logic to determine holding state, with abnormal branch finite state machine control handover whole process, in turn realize high stiffness load bearing, low stiffness compliance follow, phased unloading verification load bearing state and abnormal working condition safety let go of stage by stage.This application can accurately identify user holding steady state, reduce the risk of single signal misjudgment, realize gun body load smooth transfer, avoid charging gun drop, effectively improve the safety and smoothness of man-machine interaction.

Description

Technical Field

[0001] This invention relates to the field of electric vehicle charging assistance technology, specifically to a robotic arm compliant gun delivery control method and device based on video and force feedback fusion. Background Technology

[0002] With the development of fast charging technology for electric vehicles, the weight of high-power DC charging guns and their associated cables can reach several kilograms to over ten kilograms, leading to the emergence of semi-automatic charging gun delivery systems. This system uses a guide rail robotic arm to deliver the charging gun to the target delivery docking position outside the vehicle's charging port, where the user manually takes over and connects it. Significant technical challenges exist during this human-machine interaction: if the robotic arm releases its grip before the user has a firm grip, the heavy gun could easily fall and injure the user or damage the charging interface; if the robotic arm grips too tightly or releases too late, the user will have difficulty retrieving the gun smoothly, resulting in a poor user experience. Therefore, accurately judging the user's grip and achieving a smooth transfer of load are core issues for improving the safety and ease of use of the charging gun delivery system.

[0003] Currently, compliant control technology that integrates robot vision and force feedback has been applied in the industrial assembly field. For example, Chinese patent application CN121403414A discloses a robot control system based on the fusion of vision and force feedback. This system uses an artificial intelligence processing unit to analyze visual features and predict mechanical parameters, and combines this with an adaptive impedance control unit to dynamically fuse position commands and force feedback, achieving compliant control in precision assembly scenarios. Chinese patent application CN114131617A discloses an intelligent compliant control method for industrial robots. This method combines a position servo loop, a robot dynamics model, and an impedance control model to output position and force driving quantities respectively, improving the control compliance of the end effector. However, these technologies are all geared towards automated interaction scenarios between machines and workpieces. Their control objective is the precise assembly and processing of workpieces. They do not establish complete logic for user grip state recognition, gradual load transfer, and closed-loop verification for human-machine interaction scenarios. Furthermore, they do not consider the special needs of gravity compensation, fall protection, and personnel safety during charging gun handover, and therefore cannot be directly applied to human-machine handover scenarios where a robotic arm hands over a charging gun.

[0004] In summary, existing gun-handling technologies still have many shortcomings: First, the force control scheme for fully automatic gun insertion is based on machine docking and lacks modeling and judgment of the force applied by the user and the grip state, resulting in a mismatch between safety boundaries and handover logic; Second, relying solely on button commands or single sensor signals to confirm takeover cannot distinguish between touching and genuine gripping, leading to a high false judgment rate; Third, the direct release of the clamp lacks a load transition process, which can easily cause the charging gun to fall; Fourth, the lack of a fusion judgment mechanism for multi-source information such as vision, force, and grip detection makes it difficult to balance the safety and smoothness of the handover. Summary of the Invention

[0005] The purpose of this invention is to provide a robotic arm compliant gun delivery control method and device based on video and force feedback fusion, in order to solve the problems mentioned in the background art, such as the difficulty of accurately identifying the user's actual grip state in existing robotic arm gun delivery systems, the risk of falling due to direct release of the charging gun, and the lack of load smooth transfer and closed-loop verification mechanism, resulting in insufficient safety and smoothness of human-machine handover.

[0006] To achieve the above objectives, the present invention provides the following technical solution: A robotic arm compliant gun delivery control device based on video and force feedback fusion includes a slide table, a charging robotic arm, a charging gun clamping mechanism, an end-effector second sensing module, an end-effector six-dimensional force / torque sensor, a gun handle grip detection sensor, a joint current acquisition module, and a station controller. The charging robotic arm is mounted on a slide table. The charging gun clamping mechanism is located at the end of the charging robotic arm for clamping and fixing the charging gun. The end-effector six-dimensional force / torque sensor is connected in series between the end flange of the charging robotic arm and the charging gun clamping mechanism to collect the six-dimensional composite force rotation including the gun's own weight, inertial force, and external contact force. The handle grip detection sensor is located in the grip area of ​​the charging gun handle to output the user's grip contact status signal. The end-effector second sensing module is fixed to the side of the end of the charging robotic arm to collect images and depth information of the user's hand. The joint current acquisition module is connected to the drive circuit of each joint of the charging robotic arm to collect joint current to indirectly reflect joint torque. The station-end controller is connected to each of the above modules and is configured to: maintain the high-rigidity load-bearing posture and lifting force of the robotic arm after it delivers the charging gun to the target gun delivery docking position; perform time synchronization and fusion processing on visual and force information, and determine the user takeover status based on multi-source evidence of hand position, grip detection signal and external force characteristics; after determining that the user's grip is stable, control the robotic arm to switch to a low-rigidity compliant following mode, and reduce the lifting force and clamping force in stages according to the load sharing factor, and verify the user's acceptance status of the charging gun load in each stage; after the user completes the takeover, control the robotic arm to perform a safe yielding action along the avoidance direction; the device is used to assist the user in taking over the charging gun and does not drive the robotic arm to perform automatic gun insertion action.

[0007] Preferably, the station controller has a built-in gravity and inertia compensation calibration unit for offline identification of the equivalent mass, center of mass position, and sensor zero bias of the charging gun and cable, and online calculation and deduction of gravity and inertia components based on the current posture of the robotic arm, extracting the rotational force of external forces applied only by the user. The extracted external force satisfies the following conditions: ; in, The six-dimensional composite force spinor is directly acquired by the end-effector six-dimensional force / torque sensor; The current pose matrix obtained through the forward kinematics of the robotic arm The corresponding gravitational spin of the charging gun and cable is calculated by combining the offline identified equivalent mass and center of mass position with the real-time attitude. For terminal acceleration The calculated inertial force spinor and the terminal acceleration are obtained from joint velocity difference or kinematic estimation. The zero-bias spinor obtained from offline calibration of the sensor; the extracted As the core input for determining compliance control and takeover.

[0008] Preferably, the station-end controller has a built-in impedance control unit and uses an admittance control law to achieve compliant following. The control law satisfies the following: ; in, The preset desired inertia matrix, The preset desired damping matrix, The desired stiffness matrix is ​​preset. The deviation vector between the actual position and the reference position of the end point; the impedance control unit is configured to use high stiffness parameters in the load-bearing and holding stage and low stiffness parameters in the compliant following stage, and the stiffness along the user pulling direction is lower than that of the other degrees of freedom, so as to achieve anisotropic compliant characteristics.

[0009] Preferably, the station controller incorporates a finite state machine with hysteresis characteristics. The states sequentially include: load-bearing state, hand approach state, contact trigger state, grip confirmation state, compliant following state, phased unloading state, takeover completion state, safe yielding state, and abnormal state. State transitions are driven by multi-source fusion confidence, and the fusion confidence calculation satisfies the following: ; in, , , These are the pre-calibrated weighting coefficients for the visual modality, grip detection modality, and force perception modality, respectively. The confidence level, output by the second end-effector sensing module, is that the hand is located in the vicinity of the gun handle and is in a gripping gesture. This confidence level is calculated from the detection of key points of the hand and depth information. The activation level of the sensor outputting the grip detection of the gun handle; The confidence level is defined as the external force being in a reasonable direction and its amplitude exceeding a threshold; when Furthermore, when the set duration is stable and continuous, it switches to the hold confirmation state, and the exit judgment uses a hysteresis threshold lower than the entry threshold to avoid jitter.

[0010] Preferably, an emergency stop collision detection module is also included. The station-end controller has a built-in anomaly protection unit for estimating the external joint torque residuals based on a generalized momentum observer. The torque residual calculation satisfies the following: ; in, For the system momentum, The joint space inertia matrix of the robotic arm is calculated from the dynamic parameters of the robotic arm. The joint driving torque is calculated by the joint current acquisition module; The matrix represents the Coriolis force and the centrifugal force. This refers to the gravity term in the joint space. The observer integral gain is used; when the torque residual exceeds the threshold, the external force exceeds the limit, the pulling direction falls into the danger cone, or personnel enter the danger zone, the abnormal protection unit triggers the robotic arm to increase stiffness, pause, or retract in a safe direction, forming a triple redundant collision determination with the emergency stop collision detection module and the joint current acquisition module.

[0011] On the other hand, the present invention also provides a compliant gun delivery control method for a robotic arm based on video and force feedback fusion, which is executed by the aforementioned compliant gun delivery control device for a robotic arm based on video and force feedback fusion, and includes the following steps: S101 Positioning and Load-Bearing: Control the charging robotic arm to move along the slide and deliver the charging gun to the target gun delivery stop position, enter the high-rigidity load-bearing and holding mode, maintain the charging gun posture and lifting force, keep the gun head facing the charging port and maintain a safe distance from the vehicle body and the user; S102 Multimodal Signal Acquisition and Synchronization: The end-effector second sensing module at the end of the robotic arm acquires images and depth information of the user's hand, and the end-effector six-dimensional force / torque sensor acquires six-dimensional composite force spindle. At the same time, the opening and closing status of the charging gun clamping mechanism, the gun handle grip detection sensor signal and the joint current signal are acquired, and all signals are stamped with a unified timestamp and time alignment is completed according to the control cycle. S103 Force Compensation and Handling Status Fusion Judgment: The collected force signals are compensated for by gravity and inertia, and the rotation of the external force applied by the user is extracted; visual hand information, gun handle grip signal and external force characteristics are fused to determine whether the user is holding the charging gun and has reached a stable grip state. S104 Compliant Following and Phased Unloading: After determining that the user has a firm grip, the control robot arm switches to a low-stiffness compliant following mode, adaptively reducing the end stiffness along the user's pulling direction; at the same time, a load sharing factor is introduced to gradually reduce the lifting force of the robot arm and the clamping force of the clamping mechanism in stages, and at each stage, it is verified whether the charging gun load is borne by the user. S105 Safety Handling and Abnormal Handling: When the load sharing factor reaches 1 and the clamping mechanism is fully released, it is determined that the user has completed the takeover. The robotic arm is controlled to exit the gun delivery area along the avoidance direction and maintain a safe distance. If an abnormal working condition is detected during the handover, it will immediately trigger a pause, retraction, or re-prompt. The method is used to assist the user in taking over the charging gun without driving the robotic arm to perform the automatic gun insertion action.

[0012] Preferably, in step S103, before determining that the user has reached a stable grip state, a grip stability assessment step is also included: calculating the grip stability. Grip stability is characterized by the amplitude of external force, the smoothness of external force, the number of contact points, the consistency of pulling direction, and the duration of holding, and must satisfy the following: ; in, The magnitude of the external force applied to the user is calculated from the compensated external force spinor. To set the time-domain variance of forces inside and outside the window, reflecting the stability of the forces; The number of valid contacts output by the gun grip detection array; The cosine of the angle between the pulling direction and the preset reasonable direction is calculated from the external force direction vector and the preset direction vector; The duration of the holding state; the phased unloading step will only proceed after the holding stability reaches the preset threshold and is maintained for the set window duration.

[0013] Preferably, in step S104, during the phased unloading process, the lifting force and clamping force of the robotic arm decrease smoothly according to the load sharing factor, respectively satisfying: ; ; in, This is the load-sharing factor, with an initial value of 0, representing the full load-bearing capacity of the robotic arm; The equivalent mass of the charging gun and cable obtained from offline calibration. It is the acceleration due to gravity; The initial clamping force, To release the critical clamping force; After each load-sharing step is completed, the charging gun is checked for a downward trend using external force signals: if a sudden change in vertical external force or downward displacement is detected, the current unloading step is reversed, full load is restored, and the user is notified; if the user continues to bear the load, the load-sharing factor is increased further until... Release the charging gun clamping mechanism at the time.

[0014] As a preferred option, in step S105, abnormal working conditions include over-limit pulling, reverse pulling, collision detection, and personnel intrusion into the danger zone; among them, the collision detection adopts a sensorless detection method based on a generalized momentum observer, combined with joint current acquisition and emergency stop collision detection module to form a triple redundancy judgment; when an abnormal working condition is detected, the end stiffness of the robotic arm is immediately increased and the movement is paused, and if necessary, it is retracted in a safe direction away from the user and the vehicle body. After the abnormality is resolved and the gripping state is reconfirmed, the handover process is resumed.

[0015] As a preferred approach, the entire handover process follows human-machine collaboration safety constraints: the peak contact force and movement speed of the end are limited throughout the process, and the closer the personnel are to the charging gun, the lower the upper limit of the end movement speed; when personnel enter the preset danger zone, all gun-handling related actions are immediately stopped; when all fusion judgment and unloading steps do not meet the conditions, the process is downgraded to maintaining the load and prompting the user for a safety status, prioritizing the safety of the handover.

[0016] Compared with the prior art, the beneficial effects of the present invention are: 1. This invention utilizes a multi-source information fusion judgment mechanism combining visual hand perception, grip detection, and force detection, along with hysteresis judgment logic, to effectively distinguish between brief user touch and stable grip states. This reduces the probability of misjudgment caused by a single sensor signal and avoids abnormal conditions such as premature or delayed release. Furthermore, by employing a gravity and inertia compensation method for end-effector forces, it extracts external forces applied solely by the user as the judgment criterion, further improving the accuracy of handover state recognition and ensuring the safety of human-machine handover from the judgment level.

[0017] 2. This invention adopts a dual-mode switching strategy of high-rigidity load-bearing maintenance and low-rigidity compliant following, combined with anisotropic stiffness settings, to maintain the stability of the charging gun's posture while reducing the force resistance when the user pulls the gun out; by using a load-sharing factor to achieve phased and gradual unloading, and performing load-bearing closed-loop verification at each unloading stage, the load on the gun body can be smoothly transferred from the robotic arm to the user, avoiding the risk of the charging gun suddenly falling from the process level and improving the smoothness of the handover process.

[0018] 3. This invention constructs a multi-layered anomaly protection mechanism that includes force over-limit detection, generalized momentum observer collision detection, and personnel intrusion detection in dangerous areas. Combined with the anomaly degradation logic and safety yield strategy of the state machine, it follows the human-machine collaboration safety constraints throughout the process. It can quickly trigger pause, rollback, or degradation processing under abnormal operating conditions, improve the system's operational reliability and environmental adaptability in real site environments, and meet the relevant safety specifications for human-machine collaboration. Attached Figure Description

[0019] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are explained in detail together with the embodiments of the invention, but do not constitute a limitation thereof.

[0020] Figure 1 This is a structural diagram of the robotic arm compliant gun delivery control system based on video and force feedback of the present invention; Figure 2 This is a flowchart illustrating the compliant gun delivery method for a robotic arm based on video and force feedback according to the present invention. Figure 3 The present invention aims to jointly determine the control state by combining video recognition of hand position / grip gesture with force detection of the direction of force on the gun handle; Figure 4 This is a schematic diagram of the compliant following and gradual unloading process of the present invention; Figure 5 This is a schematic diagram illustrating the pause / retreat / re-prompt actions under abnormal scenarios such as collision, reverse / over-limit pulling, and personnel entering a danger zone in this invention. Detailed Implementation

[0021] The technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0022] like Figures 1-5 As shown, the present invention relates to a robotic arm compliant gun delivery control method and device based on video and force feedback fusion, which is applicable to the human-machine handover process in the semi-automatic charging scenario of electric vehicles. The robotic arm compliant gun delivery control device based on video and force feedback fusion includes a slide table 2, a charging robotic arm 3, a charging gun clamping mechanism 4, a charging gun 5, an end-effector second sensing module 8, an end-effector six-dimensional force / torque sensor 19, a gun handle grip detection sensor 21, a joint current acquisition module 20, an emergency stop collision detection module 24, and a station controller 16.

[0023] The slide table 2 is a horizontal guide rail structure, installed on the ground or column of the charging station, used to support the charging robotic arm 3 and drive it to move horizontally to adjust the spatial position of the charging gun. The charging robotic arm 3 is a multi-degree-of-freedom serial robotic arm, with its base fixed on the sliding platform of the slide table 2. It has both position control and force control modes, and its end effector is connected to the actuator via a flange.

[0024] A six-dimensional force / torque sensor 19 is connected in series between the end flange of the charging robotic arm 3 and the charging gun clamping mechanism 4, with a sampling frequency of not less than 1kHz, to collect the six-dimensional composite force spinor, which includes the weight of the gun body, inertial force, and external contact force. The charging gun clamping mechanism 4 is fixed to the output end of the six-dimensional force / torque sensor 19 and has a built-in clamping driver 22 for clamping or releasing the handle of the charging gun 5. The clamping force is continuously adjustable. The charging gun 5 is a DC fast charging gun for electric vehicles. A handle grip detection sensor 21 is installed in the handle area, and a charging cable is connected to the tail of the gun body.

[0025] The gun handle grip detection sensor 21, employing a capacitive array or strain gauge array sensor, is positioned in the grip area of ​​the charging gun 5 handle to detect the user's hand contact position, contact area, and grip pressure, outputting a grip activation signal. The end-effector second sensing module 8, fixed to the side of the charging robotic arm 3, includes an RGB camera and a depth camera, operating at 30-60fps, to acquire images and depth information of the user's hand, outputting the hand key point positions and gesture recognition results.

[0026] The joint current acquisition module 20 is connected to the servo drive circuit of each joint of the charging robotic arm 3, and collects the current signal of each joint motor in real time for indirect calculation of joint drive torque. The emergency stop collision detection module 24 is a safety detection unit independent of the main control system, connected to the robotic arm's emergency stop circuit, and used for emergency stop when a collision is triggered. The station-side controller 16 is the industrial control unit on the station side, which is connected to all the above modules. It has a built-in data synchronization unit, gravity compensation unit, impedance control unit, state machine decision unit, and abnormal protection unit, and executes the entire compliant gun delivery control logic.

[0027] Coordinate system definition and transformation The system defines four coordinate systems and achieves data unification through calibration and kinematic calculations: Base system B: The origin is located at the center of the base of the charging robotic arm 3, fixed to the slide table 2, and serves as the global reference coordinate system; End-effector E: The origin is located at the measurement center of the end-effector six-dimensional force / torque sensor 19 and moves with the end of the robotic arm; End-point camera system C: The origin is located at the optical center of the second sensing module 8 at the end, and is aligned with the camera imaging plane; H-type grip system: The origin is located at the grip reference point of the charging gun 5 grip, and the grip direction is defined along the grip axis.

[0028] The transformation between coordinate systems is unified through hand-eye calibration and forward kinematics: the homogeneous transformation matrix from the end-tool frame to the end-camera frame is obtained through hand-eye calibration. The homogeneous transformation matrix from the base system to the end effector system is obtained through forward kinematics calculations of the charging robotic arm 3. Furthermore, all sensor data is uniformly converted and expressed in the base system.

[0029] Gravity and inertia compensation: To extract the external forces applied solely by the user, the gun's own weight, inertial forces, and sensor zero bias must be subtracted from the original sensor measurements. The compensation process consists of two stages: offline calibration and online calculation.

[0030] Offline calibration phase: The charging robotic arm 3 is controlled to traverse several different postures within the workspace. In each posture, the output data of the six-dimensional force / torque sensor 19 at the end is collected while stationary. The equivalent mass of the charging gun 5 and the cable is identified by the least squares method. Centroid position vector and sensor zero bias spinor .

[0031] Online operation phase: Calculate the current posture rotation matrix based on the current joint angles of the charging robotic arm 3. The gravity spinor under the current attitude is calculated by combining offline calibration parameters. Simultaneously based on the terminal acceleration Calculate the spinor of inertial force In low-speed operation scenarios, the inertial force term can be ignored. The final formula for calculating the external force spinor is: ; in, The six-dimensional composite force spinor is collected in real time by the end-effector six-dimensional force / torque sensor 19; The formula for calculation is: , It is the vector of gravitational acceleration; The final acceleration is calculated from the end-effector acceleration and the equivalent mass. The end-effector acceleration is estimated by joint velocity difference or kinematic observer. Zero bias of the sensor calibrated offline. Extracted... This serves as the core input for subsequent compliance control and takeover decisions.

[0032] Multimodal signal acquisition and time synchronization Visual signal acquisition: The second sensing module 8 at the end acquires RGB images and depth maps in real time. It outputs the pixel coordinates of hand key points using a hand key point detection algorithm, and calculates the three-dimensional positions of the palm and fingertips under the end-sensor system by combining the depth map with camera intrinsic parameters. It simultaneously recognizes hand opening and closing states and grip gestures, and outputs visual confidence scores. .

[0033] Force and state signal acquisition: The end effector six-dimensional force / torque sensor 19 acquires six-dimensional force / torque data at high frequency; the joint current acquisition module 20 synchronously acquires the current of each joint and converts it into joint driving torque. The grip detection sensor 21 outputs the grip contact state and activation level. The charging gun clamping mechanism 4 provides feedback on the current opening / closing status and clamping force.

[0034] Time synchronization and hand-gun relative motion calculation: A unified system timestamp is applied to all modal signals. For signals with different sampling frequencies, nearest neighbor interpolation or linear interpolation is used to align all data to a unified control period. The control period is set to 1-4ms to ensure that visual, force, and clamping state signals are strictly corresponding in time.

[0035] The position of the grip reference point under the base system Obtained by transforming the positive kinematics of the robotic arm with the gun handle system; calculate the Euclidean distance between the hand position and the gun handle reference point. and approach speed The calculation formula is: ; ; in, The time derivative of the distance is given, and a positive approach velocity indicates that the hand is moving closer to the gun handle.

[0036] The station controller 16 has a built-in finite state machine with hysteresis to manage the state transition of the entire gun handover. The states include, in sequence, the position and load-bearing state, the hand approach state, the contact trigger state, the grip confirmation state, the compliant following state, the phased unloading state, the takeover completion state, the safe yielding state, and the abnormal state. Any state can trigger an abnormal state transition.

[0037] Fusion confidence calculation: State switching is based on multi-source fusion confidence. The driving force and fusion confidence score are obtained by weighted summation of the confidence scores from the visual modality, grip detection modality, and force perception modality. The calculation formula is as follows: ; in, , , The weight coefficients for the visual modality, grip detection modality, and force perception modality are determined through offline calibration and scene testing. For visual confidence, a high value is used when the hand is within the neighborhood of the gun handle and is in a gripping gesture; otherwise, a low value is used. The activation level output by the grip detection sensor 21 is 0-1, with a value ranging from 0 to 1. The larger the contact area and the higher the grip pressure, the larger the value. For force perception confidence, a high value is taken when the external force is along a preset reasonable direction and the amplitude exceeds a set threshold; otherwise, a low value is taken.

[0038] Hysteresis Detection Mechanism: To avoid frequent state switching caused by signal jitter, a hysteresis detection mechanism is adopted: a threshold for steady grip is set. Withholding the exit threshold And satisfy .when And the set duration is stable and continuous. When the state switches to the hold-and-confirm state; when already in the hold-and-confirm state and Only then will it revert to the contact-triggered state.

[0039] Grip stability assessment: Before proceeding with phased unloading, a grip stability assessment must be conducted to calculate the grip stability. Taking into account the amplitude of the external force, the stability of the force, the number of contact points, the consistency of direction, and the duration of the hold, the calculation formula is as follows: ; in, The amplitude of the external force applied to the user is determined by the compensated external force spinor. The force components are obtained by taking the modulus; To set the time-domain variance of internal and external forces within a time window, the smaller the variance, the more stable the grip; The number of valid contacts output by the gun grip detection array; The cosine of the angle between the pulling direction and the preset reasonable direction is calculated by the dot product of the external force direction vector and the preset reasonable direction vector. The duration of the grip. Only when the grip is stable... Only after maintaining the set window duration will the phased uninstallation process be allowed.

[0040] Compliant following control strategy: The station controller 16 has a built-in impedance control unit, which uses an admittance control law to achieve end-effector compliance characteristics, making the robotic arm respond to external forces in a manner similar to a spring-damped-mass system. The control law expression is as follows: ; in, The preset desired inertia matrix, The preset desired damping matrix, The desired stiffness matrix is ​​preset. This is the deviation vector between the actual position of the end and the reference position.

[0041] The system is configured with two sets of control parameters to adapt to the needs of different stages: Load-bearing stage: High rigidity parameters are adopted to ensure the stability of the charging gun 5 posture and the lifting force, resist the weight of the gun body and prevent it from falling. Compliant following phase: Low stiffness parameters are used, and the stiffness along the user's pulling direction is significantly lower than that of the other degrees of freedom, to achieve anisotropic compliant characteristics, allowing the user to easily pull the gun body to adjust its position while maintaining the stability of the gun head's orientation.

[0042] Phased unloading and takeover verification: Once user holding is confirmed to be stable, the system enters a phased unloading state, using load sharing factors. Gradually transfer the load on the gun body from the robotic arm to the user, while simultaneously reducing the clamping force of the gripping mechanism. Load sharing factor. The initial value is 0, indicating that the robotic arm fully bears the weight of the gun; the final value is 1, indicating that the user fully bears the weight of the gun.

[0043] The lifting force of the robotic arm and the clamping force of the gripping mechanism decrease smoothly with the load sharing factor, and the calculation formulas are as follows: ; ; in, The equivalent mass of the charging gun and cable calibrated offline. It is the acceleration due to gravity; This provides initial clamping force to ensure the gun body does not loosen; To release the critical clamping force, the clamping mechanism is completely released at this point.

[0044] The uninstallation process uses a step-by-step incremental approach, with each step incrementing the amount of data. Take a value of 0.1-0.2. After each step of load sharing is completed, perform a closed-loop load verification: through external force rotation. The vertical component is used to determine if the charging gun 5 has a downward trend. If a sudden change in the vertical external force or a downward displacement is detected, it indicates that the user is not effectively bearing the load. The current unloading step is immediately reversed, the full load-bearing state is restored, and a prompt message is issued. If the user is continuously and stably bearing the load, the load-sharing factor is further increased. When the grip is stable, the charging gun clamping mechanism 4 is fully unlocked and released, allowing the user to take over.

[0045] Anomaly protection and security take precedence Triple Redundancy Collision Detection: The station controller 16 has a built-in anomaly protection unit to detect and handle abnormal conditions such as over-limit pulling, reverse pulling, collisions, and personnel intrusion into dangerous areas during the handover process. Collision detection adopts a sensorless detection method based on a generalized momentum observer, combined with joint current acquisition and emergency stop collision detection module 24 to form triple redundancy judgment.

[0046] The generalized momentum observer estimates the external torque residual by the integral deviation between the system momentum and the driving torque. The calculation formula is: ; in, For the system momentum, The joint space inertia matrix of the robotic arm is calculated from the dynamic parameters of the robotic arm. The joint driving torque is calculated by the joint current acquisition module 20; The matrix represents the Coriolis force and the centrifugal force. This refers to the gravity term in the joint space. The observer integral gain. When the external torque residual... If the threshold is exceeded, a collision is determined to have occurred.

[0047] Anomaly Handling and Safety Priority: Anomaly Handling Logic: When an excessive external force is detected, the pulling direction falls into a danger cone, a collision is triggered, or personnel enter a danger zone, the end effector stiffness of the robotic arm is immediately increased and all movements are suspended. If necessary, it is retracted in a safe direction away from the user and the vehicle body, while issuing a warning message. Normal handover procedures are resumed only after the abnormal condition is resolved and the user's grip is reconfirmed.

[0048] Once the user has taken over, the system enters a safe yielding state, controlling the charging robotic arm 3 to slowly exit the gun delivery area along the avoidance direction, avoiding the user and charging cable, and returning to a safe standby position, maintaining a safe distance and speed limit from personnel throughout the process.

[0049] The complete execution flow of the compliant gun delivery control method of the present invention is as follows: S101 Position and Load Bearing: Control the charging robotic arm 3 to move along the slide table 2, deliver the charging gun 5 to the target gun delivery stop 12, enter the high rigidity load bearing and holding mode, maintain the charging gun posture and lifting force, keep the gun head facing the charging port 10, and maintain a safe distance from the target vehicle 9 body and the user.

[0050] S102 Multimodal Signal Acquisition and Synchronization: The user's hand image and depth information are acquired through the second sensing module 8 at the end, and the six-dimensional composite force spindle is acquired through the six-dimensional force / torque sensor 19 at the end. At the same time, the opening and closing status of the charging gun clamping mechanism 4, the signal of the gun handle grip detection sensor 21, and the joint current signal are acquired. All signals are stamped with a unified timestamp and time alignment is completed according to the control cycle.

[0051] S103 Force Compensation and Control Status Fusion Determination: Gravity and inertia compensation are applied to the collected force signals to extract the spinor force of the external force applied by the user. It integrates visual hand information, gun handle grip signals, and external force characteristics, and uses a hysteresis-based fusion judgment logic to determine whether the user is holding the charging gun and has achieved a stable grip, while also assessing grip stability.

[0052] S104 Compliant Following and Phased Unloading: After determining that the user has a firm grip, the control robot arm switches to a low-stiffness compliant following mode, adaptively reducing the end effector stiffness along the user's pulling direction; at the same time, a load-sharing factor is introduced. The lifting force of the robotic arm and the clamping force of the clamping mechanism are gradually reduced in stages. After each stage is completed, it is checked whether the load of the charging gun has been effectively accepted by the user. If it has not been accepted, the process is reversed and a prompt is displayed.

[0053] S105 Safety Handling and Abnormal Handling: When the load sharing factor reaches 1 and the clamping mechanism is fully released, it is determined that the user has completed the takeover. The robotic arm is controlled to exit the gun delivery area along the avoidance direction and maintain a safe distance. If an abnormal working condition is detected at any time during the handover process, the pause, retraction or re-prompt is immediately triggered.

[0054] The entire handover process strictly adheres to human-machine collaboration safety constraints: the peak contact force and movement speed of the end are limited throughout the process, and the closer the personnel are to the charging gun, the lower the upper limit of the end movement speed; when personnel enter the preset danger zone, all gun-handling related actions are immediately stopped; when all fusion judgment and unloading steps do not meet the conditions, the process is downgraded to maintaining the load and prompting the user for a safety status, prioritizing the safety of the handover.

[0055] After the charging robotic arm 3 is in position, it maintains the posture and lifting force of the charging gun 5 with high rigidity. The lifting force is equal to the total weight of the gun and the cable. When the user's hand enters the 80mm neighborhood of the handle and the grip gesture is recognized, the state switches to the hand proximity state. When the handle grip detection sensor 21 is activated and the compensated external force has an amplitude of ≥8N along a reasonable direction and lasts for 0.5s, the fusion confidence reaches the grip stability threshold, and the state switches to the grip stability confirmation state.

[0056] Once the grip stability meets the standard, the system switches to a low-stiffness compliant mode, with the pulling direction stiffness set to 150 N / m. The load sharing factor increases in 5 steps, with each step incrementing by 0.2, and after each step, the vertical force is checked to ensure there is no downward trend. When the load sharing factor reaches 1, the gripping driver 22 drives the charging gun gripping mechanism 4 to unlock and release, and the robotic arm retreats to a safe yielding posture, completing the handover.

[0057] After the second step of the phased unloading process is completed, a sudden increase in the vertical component of the external force is detected, exceeding the expected value corresponding to the current load distribution. This indicates that the user has not effectively supported the load and the gun body is showing a downward trend. The system immediately reverts to this unloading step, restoring the full-lift state, and issues a voice prompt. Once the user's grip is stable and the fusion confidence level returns to the target, the phased unloading process is restarted.

[0058] During compliant following, the torque residual output by the generalized momentum observer exceeded the collision threshold, and a sudden change occurred in the joint current, leading the system to determine a collision. The system immediately increased the end effector stiffness of the robotic arm and paused movement, then slightly retracted along a safe direction away from the vehicle body, issuing a warning message. Once the collision factors were eliminated and the user's grip was reconfirmed, the handover process resumed.

[0059] This invention utilizes a multi-source information fusion judgment mechanism combining visual hand perception, grip detection, and force detection, along with hysteresis judgment logic, to effectively distinguish between brief user touches and stable grip states. This reduces the probability of misjudgments caused by single sensor signals and avoids abnormal conditions such as premature or delayed release. Furthermore, by employing a gravity and inertia compensation method for end-effector forces, it extracts external forces applied solely by the user as the judgment criterion, further improving the accuracy of takeover state recognition and ensuring the safety of human-machine handover from the judgment level.

[0060] This invention employs a dual-mode switching strategy of high-rigidity load-bearing maintenance and low-rigidity compliant following, combined with anisotropic stiffness settings, to maintain the stability of the charging gun's posture while reducing the resistance to the user's pulling force when removing the gun. By using a load-sharing factor to achieve phased and gradual unloading, and performing load-bearing closed-loop verification at each unloading stage, the load on the gun body can be smoothly transferred from the robotic arm to the user, avoiding the risk of the charging gun suddenly falling from the process level and improving the smoothness of the handover process.

[0061] This invention constructs a multi-layered anomaly protection mechanism that includes force over-limit detection, generalized momentum observer collision detection, and personnel intrusion detection in dangerous areas. Combined with the anomaly degradation logic and safety yield strategy of the state machine, it follows the human-machine collaboration safety constraints throughout the process. It can quickly trigger pause, rollback, or degradation processing under abnormal operating conditions, improve the system's operational reliability and environmental adaptability in real site environments, and meet the relevant safety specifications for human-machine collaboration.

[0062] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely preferred examples and are not intended to limit the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of the present invention is defined by the appended claims and their equivalents.

Claims

1. A robotic arm compliant gun delivery control device based on video and force feedback fusion, characterized in that, It includes a slide table (2), a charging robotic arm (3), a charging gun clamping mechanism (4), an end-effector second sensing module (8), an end-effector six-dimensional force / torque sensor (19), a gun handle grip detection sensor (21), a joint current acquisition module (20), and a station controller (16). The charging robotic arm (3) is mounted on the slide table (2), and the charging gun clamping mechanism (4) is located at the end of the charging robotic arm (3) for clamping and fixing the charging gun (5); the end six-dimensional force / torque sensor (19) is connected in series between the end flange of the charging robotic arm (3) and the charging gun clamping mechanism (4) for collecting the six-dimensional composite force rotation including the weight of the gun body, inertial force and external contact force; the gun handle grip detection sensor (21) is arranged in the grip area of ​​the gun handle of the charging gun (5) for outputting the user grip contact state signal; the end second sensing module (8) is fixed on the side of the end of the charging robotic arm (3) for collecting the image and depth information of the user's hand; The joint current acquisition module (20) is connected to the drive circuit of each joint of the charging robotic arm (3) to acquire joint current in order to indirectly reflect joint torque. The station controller (16) is connected to the above modules and is configured to: maintain the high rigidity load-bearing posture and lifting force of the robotic arm after the charging robotic arm (3) delivers the charging gun (5) to the target gun delivery docking position (12); perform time synchronization and fusion processing on visual information and force information, and determine the user takeover status based on multi-source evidence of hand position, grip detection signal and external force characteristics; after determining that the user grip is stable, control the robotic arm to switch to low rigidity compliant following mode, and reduce the lifting force and clamping force in stages according to the load sharing factor, and verify the user's acceptance status of the charging gun load in each stage; After the user takes over, the control robot arm performs a safe yielding action in the avoidance direction; the device is used to assist the user in taking over the charging gun, without driving the robot arm to perform an automatic gun insertion action.

2. The robotic arm compliant gun delivery control device based on video and force feedback fusion feedback according to claim 1, characterized in that, The station controller (16) has a built-in gravity and inertia compensation calibration unit, which is used to identify the equivalent mass, center of mass position and sensor zero bias of the charging gun and cable offline, and calculate and deduct the gravity and inertia components online according to the current posture of the robotic arm, and extract the external force rotation applied only by the user. The external force extraction satisfies: ； in, The six-dimensional composite force spinor is directly acquired by the end six-dimensional force / torque sensor (19); The current pose matrix obtained through the forward kinematics of the robotic arm The corresponding gravitational spin of the charging gun and cable is calculated by combining the offline identified equivalent mass and center of mass position with the real-time attitude. For terminal acceleration The calculated inertial force spinor and the terminal acceleration are obtained from joint velocity difference or kinematic estimation. The zero-bias spinor obtained from offline calibration of the sensor; the extracted As the core input for determining compliance control and takeover.

3. The robotic arm compliant gun delivery control device based on video and force feedback fusion feedback according to claim 1, characterized in that, The station-end controller (16) has a built-in impedance control unit and uses an admittance control law to achieve compliant following. The control law satisfies: ； in, The preset desired inertia matrix, The preset desired damping matrix, The desired stiffness matrix is ​​preset. The deviation vector between the actual position and the reference position of the end point; the impedance control unit is configured to use high stiffness parameters in the load-bearing and holding stage and low stiffness parameters in the compliant following stage, and the stiffness along the user pulling direction is lower than that of the other degrees of freedom, so as to achieve anisotropic compliant characteristics.

4. The robotic arm compliant gun delivery control device based on video and force feedback fusion feedback according to claim 1, characterized in that, The station controller (16) has a built-in finite state machine with hysteresis characteristics. The states include, in sequence, the bearing state, the hand approach state, the contact trigger state, the grip confirmation state, the compliant following state, the phased unloading state, the takeover completion state, the safe yielding state, and the abnormal state. The state switching is driven by multi-source fusion confidence, and the fusion confidence calculation satisfies: ； in, , , These are the pre-calibrated weighting coefficients for the visual modality, grip detection modality, and force perception modality, respectively. The confidence level of the hand being located in the vicinity of the gun handle and exhibiting a gripping gesture, output by the second end perception module (8), is calculated from the hand key point detection and depth information. The activation level of the grip detection sensor (21) is the output of the gun handle. The confidence level is defined as the external force being in a reasonable direction and its amplitude exceeding a threshold; when Furthermore, when the set duration is stable and continuous, it switches to the hold confirmation state, and the exit judgment uses a hysteresis threshold lower than the entry threshold to avoid jitter.

5. The robotic arm compliant gun delivery control device based on video and force feedback fusion feedback according to claim 1, characterized in that, It also includes an emergency stop collision detection module (24), and the station controller (16) has a built-in anomaly protection unit for estimating the external joint torque residuals based on the generalized momentum observer. The torque residual calculation satisfies: ； in, For the system momentum, The joint space inertia matrix of the robotic arm is calculated from the dynamic parameters of the robotic arm; The joint driving torque is calculated by the joint current acquisition module (20); The matrix represents the Coriolis force and the centrifugal force. This refers to the gravity term in the joint space. The observer integral gain is used for the following: when the torque residual exceeds the threshold, the external force exceeds the limit, the pulling direction falls into the danger cone, or personnel enter the danger zone, the abnormal protection unit triggers the robotic arm to increase stiffness, pause, or retract along the safe direction, forming a triple redundant collision judgment with the emergency stop collision detection module (24) and the joint current acquisition module (20).

6. A compliant gun-handling control method for a robotic arm based on video and force feedback fusion, executed using the compliant gun-handling control device for a robotic arm based on video and force feedback fusion as described in any one of claims 1-5, characterized in that, Includes the following steps: S101 Load-bearing in place: Control the charging robotic arm (3) to move along the slide (2) and deliver the charging gun (5) to the target gun delivery stop (12), enter the high rigidity load-bearing holding mode, maintain the charging gun posture and lifting force, keep the gun head facing the charging port (10) and maintain a safe distance from the vehicle body and the user. S102 Multimodal Signal Acquisition and Synchronization: The user's hand image and depth information are acquired through the end-effector second sensing module (8) at the end of the robotic arm, and the six-dimensional composite force spindle is acquired through the end-effector six-dimensional force / torque sensor (19). At the same time, the opening and closing status of the charging gun clamping mechanism (4), the gun handle grip detection sensor (21) signal and the joint current signal are acquired, and all signals are stamped with a unified timestamp and time alignment is completed according to the control cycle. S103 Force Compensation and Handling Status Fusion Judgment: The collected force signals are compensated for by gravity and inertia, and the rotation of the external force applied by the user is extracted; visual hand information, gun handle grip signal and external force characteristics are fused to determine whether the user is holding the charging gun and has reached a stable grip state. S104 Compliant Following and Phased Unloading: After determining that the user has a firm grip, the control robot arm switches to a low-stiffness compliant following mode, adaptively reducing the end-effector stiffness along the user's pulling direction; At the same time, a load-sharing factor is introduced to gradually reduce the lifting force of the robotic arm and the clamping force of the clamping mechanism in stages, and to verify whether the load of the charging gun is borne by the user in each stage. S105 Safety Handling and Abnormal Handling: When the load sharing factor reaches 1 and the clamping mechanism is fully released, it is determined that the user has completed the takeover. The robotic arm is controlled to exit the gun delivery area along the avoidance direction and maintain a safe distance. If an abnormal working condition is detected during the handover, it will immediately trigger a pause, retraction, or re-prompt. The method is used to assist the user in taking over the charging gun without driving the robotic arm to perform the automatic gun insertion action.

7. The robotic arm compliant gun delivery control method based on video and force feedback fusion feedback according to claim 6, characterized in that, In step S103, before determining that the user has reached a stable grip state, a grip stability assessment step is also included: calculating the grip stability. Grip stability is characterized by the amplitude of external force, the smoothness of external force, the number of contact points, the consistency of pulling direction, and the duration of holding, and must satisfy the following: ； in, The magnitude of the external force applied to the user is calculated from the compensated external force spinor. To set the time-domain variance of forces inside and outside the window, reflecting the stability of the forces; The number of valid contacts output by the gun grip detection array; The cosine of the angle between the pulling direction and the preset reasonable direction is calculated from the external force direction vector and the preset direction vector; The duration of the holding state; the phased unloading step will only proceed after the holding stability reaches the preset threshold and is maintained for the set window duration.

8. The robotic arm compliant gun delivery control method based on video and force feedback fusion feedback according to claim 6, characterized in that, In step S104, during the phased unloading process, the lifting force and clamping force of the robotic arm decrease smoothly according to the load sharing factor, respectively satisfying: ； ； in, This is the load-sharing factor, with an initial value of 0, representing the full load-bearing capacity of the robotic arm; The equivalent mass of the charging gun and cable obtained from offline calibration. It is the acceleration due to gravity; The initial clamping force, To release the critical clamping force; After each load-sharing step is completed, the charging gun is checked for a downward trend using external force signals: if a sudden change in vertical external force or downward displacement is detected, the current unloading step is reversed, full load is restored, and the user is notified; if the user continues to bear the load, the load-sharing factor is increased further until... Release the charging gun clamping mechanism (4) at the time.

9. The robotic arm compliant gun delivery control method based on video and force feedback fusion feedback according to claim 6, characterized in that, In step S105, abnormal working conditions include over-limit pulling, reverse pulling, collision detection, and personnel intrusion into the danger zone. Among them, the collision detection adopts a sensorless detection method based on a generalized momentum observer, combined with joint current acquisition and emergency stop collision detection module to form a triple redundancy judgment. When an abnormal working condition is detected, the stiffness of the robotic arm end is immediately increased and the movement is paused. If necessary, it is retracted in a safe direction away from the user and the vehicle body. After the abnormality is resolved and the gripping status is reconfirmed, the handover process is resumed.

10. The robotic arm compliant gun delivery control method based on video and force feedback fusion feedback according to claim 6, characterized in that, The entire handover process follows human-machine collaboration safety constraints: the peak contact force and movement speed of the end are limited throughout the process, and the closer the personnel are to the charging gun, the lower the upper limit of the end movement speed; when personnel enter the preset danger zone, all gun handover-related actions are immediately stopped; when all fusion judgment and unloading steps do not meet the conditions, the process is downgraded to maintaining the load and prompting the user for a safety status, prioritizing the safety of the handover.

Images