Micro-operation tail end system integrating multi-dimensional micro-force sensing and visual servo

Abstract

The invention relates to the technical field of robot micro-operation, and discloses a micro-operation tail end system integrating multi-dimensional micro-force sensing and visual servo, which comprises a data acquisition module used for synchronously acquiring binocular visual images and six-dimensional force data; the calibration registration module is used for establishing a multi-sensor unified coordinate system; the visual pose estimation module is used for realizing three-dimensional reconstruction and feature point extraction based on a deep neural network and structured light; the state fusion estimation module fuses vision and force information through an extended Kalman filter and judges a contact state; the cooperative control module is used for generating a motion instruction in combination with task priority and impedance control; the parameter self-adaptive module is used for identifying materials on line and self-adaptively adjusting force control parameters; and the motion control module adopts active damping control to suppress vibration. According to the invention, high-precision and multi-degree-of-freedom micro-operation can be realized in a narrow and limited space.

Description

Technical Field

[0001] This invention relates to the field of robot micromanipulation technology, and more specifically, to a micromanipulation end effector system that integrates multidimensional micro-force sensing and visual servoing. Background Technology

[0002] With the development of aerospace, precision manufacturing, and biomedicine, the demand for high-precision micro-manipulation in confined spaces is becoming increasingly urgent. Scenarios such as on-orbit satellite maintenance, precision assembly, and minimally invasive surgery present challenges including limited space, tiny objects, and stringent positioning and force control requirements, which traditional robotic systems struggle to meet.

[0003] Existing micro-operating systems mostly rely on a single sensor. Visual servoing can achieve precise positioning, but lacks force sensing, making it prone to damage upon contact; force control can sense contact force, but lacks visual guidance, making precise alignment difficult. Current major technical challenges include: difficulties in spatiotemporal synchronization of multi-sensor data, as differences in sampling frequency and data format make time alignment and coordinate system unification challenging; imperfect force-visual information fusion methods, where simple data superposition cannot fully utilize the complementarity of multimodal information, resulting in low state estimation accuracy; a lack of adaptability in control strategies, where fixed parameters are difficult to adapt to the contact characteristics of targets with different materials, easily leading to oscillations or slow response; and insufficient vibration suppression capabilities, as high-frequency vibrations in micro-operations affect accuracy and stability, and existing passive damping methods have limited effectiveness.

[0004] Therefore, there is an urgent need to develop a micro-manipulation end system that integrates multi-dimensional micro-force sensing and visual servoing to achieve deep fusion of force and visual information, intelligent identification of contact state, adaptive adjustment of control parameters, and active suppression of vibration, thereby meeting the needs of high-precision, multi-degree-of-freedom micro-manipulation tasks in confined spaces. Summary of the Invention

[0005] This invention provides a micro-manipulation end system that integrates multi-dimensional micro-force sensing and visual servoing, solving the technical problems in related technologies such as difficulty in spatiotemporal synchronization of multi-sensor data, imperfect force and visual information fusion methods, lack of adaptability in control strategies, and insufficient vibration suppression capabilities.

[0006] This invention provides a micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing, comprising: The data acquisition module is used to acquire data from multiple sensors according to the configuration parameters of the sensor module; The calibration and registration module is used to acquire multi-sensor data and establish a unified coordinate system to obtain multi-sensor data under the unified coordinate system. The visual pose estimation module is used to receive multi-sensor data in a unified coordinate system and perform 3D reconstruction to obtain a six-dimensional visual position error vector and target 3D pose information. The state fusion estimation module is used to receive the six-dimensional visual position error vector and the target's three-dimensional pose information to perform multimodal information fusion and obtain system state estimation and contact state identification. The collaborative control module receives system state estimates and contact state indicators, and generates control commands using task priority control and impedance control methods to obtain force vision-coordinated Cartesian space velocity control commands. The parameter adaptive module is used to receive contact status indicators and multi-sensor data in a unified coordinate system, identify material properties using a classification method, and obtain adjusted force control parameters. The motion control module receives speed control commands and force control parameters, and uses inverse kinematics and active damping control methods to drive the end motion, achieving stable and precise operation after vibration suppression.

[0007] In a preferred embodiment, the data acquisition module includes: Configure the hardware parameters of the binocular stereo vision camera and the structured light projection device. The structured light projection device uses a laser diode as the light source and generates regular dot matrix structured light through diffractive optical elements. Configure the sampling parameters of the six-dimensional force sensor, including sampling frequency, force measurement range and torque measurement range. The main controller generates a synchronization pulse signal, which is simultaneously sent to the external trigger interface of the binocular vision camera, the control interface of the structured light projection device, and the sampling trigger interface of the six-dimensional force sensor. The binocular vision camera simultaneously exposes the left and right image sensors, and the structured light pattern is superimposed on the binocular image. The six-dimensional force sensor starts the analog-to-digital converter to sample the output voltage of the strain bridge circuit. The system clock is used to add a timestamp to each set of data. The left eye image, right eye image, and six-dimensional force data with timestamps are organized into data frames and stored in a circular buffer.

[0008] In a preferred embodiment, the calibration and registration module includes: A hand-eye calibration method is used to establish the spatial transformation relationship between the visual coordinate system, the force sensor coordinate system, and the end effector coordinate system. The end effector coordinate system is defined as a unified reference coordinate system. The least squares optimization method is used to solve the rotation matrix and translation vector between the coordinate systems. The camera calibration parameters are used to correct the distortion of the binocular vision image. The camera calibration parameters include the intrinsic parameter matrix and distortion coefficients. The coordinates of each pixel in the image are remapped, and the gray value corresponding to the corrected pixel position is calculated by bilinear interpolation. Zero-point compensation and temperature compensation are performed on the six-dimensional force data. When the system starts up, the sensor output value under the condition of no external force is collected as the zero-point offset value. The output offset caused by temperature is calculated based on the working temperature measured by the temperature sensor and the temperature compensation coefficient. A coordinate system transformation method is used to unify all sensor data into the coordinate system of the end effector.

[0009] In a preferred embodiment, the visual pose estimation module includes: A deep convolutional neural network model with an encoder-decoder architecture is used to extract key feature points of the target operation. The encoder part consists of multiple convolutional layers and pooling layers to extract multi-scale features of the image, and the decoder part consists of multiple deconvolutional layers to generate feature point heatmaps. Non-maximum suppression processing is performed on the heatmaps to extract feature points in the heatmaps with a probability exceeding a preset probability threshold. The similarity of local feature descriptors is calculated using the normalized cross-correlation method for stereo matching. Matching points are searched under epipolar constraints. The disparity value is obtained by parabolic fitting of the cross-correlation curves around the matching points using the subpixel precision disparity estimation method. The three-dimensional coordinates of feature points are calculated using the triangulation principle to obtain a sparse three-dimensional point cloud. The structured light spot array is extracted from the binocular image and the spot coding is matched. The three-dimensional coordinates of the matched spot are calculated to obtain a dense three-dimensional point cloud. The sparse feature point cloud and the dense point cloud are then fused.

[0010] In a preferred embodiment, the visual pose estimation module further includes: A 3D CAD model of the target object is pre-built and converted into a 3D point cloud model. The iterative nearest point algorithm is used to register the model point cloud with the reconstructed point cloud. The iterative process includes nearest point search, corresponding point pair establishment, rigid body transformation solution and point cloud transformation update. The feature point alignment method is used to provide the initial pose for coarse registration and then fine registration is performed. Extract the position vector and attitude rotation matrix of the target from the final transformation matrix, calculate the position error vector and attitude error vector, and combine the position error vector and attitude error vector into a six-dimensional visual position error vector.

[0011] In a preferred embodiment, the state fusion estimation module includes: The system state vector is defined to include the position, velocity, attitude, and attitude angular velocity of the target object. The observation vector is defined to include the target pose obtained by visual measurement and the six-dimensional force obtained by force sensor measurement. The state equation describing the evolution of the system state over time and the observation equation describing the relationship between the observation vector and the state vector are established. An extended Kalman filter is used for multi-sensor data fusion. In the prediction step, the state prior estimate and covariance matrix at the current time are predicted based on the state estimate and state transition equation at the previous time step. In the update step, the observation residual and Kalman gain matrix are calculated, and the state estimate is updated based on the Kalman gain and observation residual. Three contact states are defined: free motion, contact approach, and stable contact. The composite amplitude and rate of change of the six-dimensional force are calculated. The composite amplitude is compared with a preset free motion threshold, and the rate of change is compared with a preset contact approach threshold to determine the contact state identifier of the current operation.

[0012] In a preferred embodiment, the collaborative control module includes: The visual position control mode corresponds to the free motion state, the position-force hybrid control mode corresponds to the contact approach state, and the force tracking control mode corresponds to the stable contact state. During the control mode transition, the control parameters are gradually adjusted to make the control output transition continuously and smoothly. Construct a control objective function that includes a position error minimization term and a force error minimization term. The priority of the two terms is balanced by a weighting coefficient. The weighting coefficient is determined according to the current control mode. In the position-force hybrid control mode, the weighting coefficients of the two terms are dynamically adjusted according to the contact force amplitude. The control objective function is expanded into a standard quadratic programming form, the decision variable is the Cartesian space velocity vector of the end effector, and the constraints include velocity limit constraints and acceleration limit constraints. The optimal velocity vector is calculated using a quadratic programming solver.

[0013] In a preferred embodiment, the collaborative control module further includes: Define the desired impedance model, whose parameters include the inertia matrix, damping matrix, and stiffness matrix. Calculate the position correction caused by force deviation based on the desired impedance model. The force deviation is equal to the measured six-dimensional force minus the desired contact force. Discretize the dynamic equations of the impedance model to solve for the velocity correction. Superimpose the velocity correction onto the velocity command obtained from the quadratic programming solution.

[0014] In a preferred embodiment, the parameter adaptation module includes: When the contact state is identified as a contact approaching state or a stable contact state, the force response data acquisition program is started to continuously acquire six-dimensional force data and organize it into a force response curve. The force response curve is then processed with zero mean and bandpass filtered. The least squares parameter identification method is used to extract contact stiffness and damping characteristics. A linear regression relationship is established between the contact force response data and the velocity and position data. The contact stiffness coefficient and contact damping coefficient are solved and normalized. A fully connected feedforward neural network is used for material classification. The input layer corresponds to stiffness and damping features, and the output layer uses the Softmax activation function to output the probability distribution of each material category. The category with the highest probability is selected as the material category to be identified. A material parameter database is pre-established to store recommended force control parameters for each material category. The corresponding force control parameters are queried based on the identified material category. The parameter updates adopt a progressive method, using linear interpolation to transition over multiple consecutive control cycles.

[0015] In a preferred embodiment, the motion control module includes: The Jacobian matrix method is used to convert Cartesian space velocity commands into joint velocity vectors. For near-singular configurations, the damped least squares method is used to solve the problem. The velocity of each joint is checked to see if it exceeds the physical limit and is scaled accordingly. A numerical integration method is used to generate joint position control commands, and a dual-loop control structure for position and velocity is constructed. The position control loop calculates the joint position error and generates the desired velocity, while the velocity control loop applies a proportional-integral control law to the velocity error to generate joint torque commands. The six-dimensional force data is filtered out by a high-pass filter to remove the low-frequency static contact force component. The root mean square value of the signal is calculated to determine whether vibration exists. Fast Fourier transform is used for spectrum analysis to identify the main frequency component. Based on the vibration signal, the vibration velocity is estimated, and the reverse damping force is calculated to be equal to the negative damping coefficient multiplied by the vibration velocity. The damping coefficient is adaptively adjusted according to the vibration amplitude and frequency. The reverse damping force is converted into joint space damping torque and superimposed on the joint torque command.

[0016] The beneficial effects of this invention are as follows: By employing a hardware synchronization triggering mechanism to achieve time-synchronized data acquisition from a binocular vision camera, a structured light projection device, and a six-dimensional force sensor, and combining this with a hand-eye calibration method to establish a unified coordinate system for multiple sensors, the problem of spatiotemporal synchronization of multi-sensor data is solved. An extended Kalman filter is used to deeply fuse force and visual information, fully utilizing the complementary characteristics of multimodal information in the prediction and update steps, thereby improving the accuracy and robustness of system state estimation. By analyzing the composite amplitude and rate of change of the six-dimensional force, the contact state is determined, achieving accurate identification of three states: free motion, contact approach, and stable contact, providing a reliable basis for subsequent control mode switching. By employing a task priority control method to construct a unified control objective function that includes minimizing both position and force errors, and dynamically adjusting the weighting coefficients based on the contact state, a smooth transition from pure position control to pure force control is achieved. By using a system identification method to extract contact stiffness and damping characteristics, and utilizing a neural network to identify material types, the force control parameters are adaptively adjusted based on material properties, addressing the lack of adaptability in the control strategy. Furthermore, by using high-pass filtering and spectral analysis to detect high-frequency vibration signals, and generating a reverse damping force based on the vibration characteristics and superimposing it onto the joint torque command, active vibration suppression is achieved, improving operational stability and accuracy. Attached Figure Description

[0017] Figure 1 This is a block diagram of a micro-manipulation end system integrating multi-dimensional micro-force sensing and visual servoing according to the present invention. Figure 2 This is a flowchart of a micro-manipulation end system integrating multi-dimensional micro-force sensing and visual servoing according to the present invention. Detailed Implementation

[0018] The subject matter described herein will now be discussed with reference to exemplary embodiments. It should be understood that these embodiments are discussed only to enable those skilled in the art to better understand and implement the subject matter described herein, and changes may be made to the function and arrangement of the elements discussed without departing from the scope of this specification. Various processes or components may be omitted, substituted, or added as needed in the examples. Furthermore, some features described in the examples may be combined in other examples.

[0019] At least one embodiment of the present invention discloses a micro-manipulation end-effector system integrating multi-dimensional micro-force sensing and visual servoing, such as Figures 1 to 2 As shown, it includes the following steps: The data acquisition module is used to acquire data from multiple sensors according to the configuration parameters of the sensor module; S11, configures the hardware parameters of the integrated sensor module; Due to the geometric constraints of operating in confined spaces, a compact binocular stereo vision camera is used for hardware configuration. The baseline distance of the binocular camera is set between 20 mm and 40 mm, the camera resolution is configured as 1920×1200 pixels, the frame rate is set to 60 frames per second, the lens focal length is selected as 8 mm, and the field of view is configured as 65 degrees, ensuring clear images of the target within a working distance range of 20 mm to 150 mm. A structured light projection device is also configured, using a 660 nm red laser diode as the light source. Regular dot matrix structured light is generated through a diffractive optical element (DOE), with a dot matrix period set to 0.5 mm and a projection angle of 45 degrees, ensuring stable structured light patterns on targets with different surface reflectivity. The sampling parameters of the six-dimensional force sensor were configured, with the sampling frequency set to 1000 Hz, the force measurement range set to -20 N to +20 N, the torque measurement range set to -500 mN·m to +500 mN·m, the force resolution reaching 10 mN, and the torque resolution reaching 1 mN·m, thus obtaining sensor configuration parameters that meet the requirements of high-precision force sensing.

[0020] S12, synchronously triggers multi-sensor data acquisition; Based on the configured integrated sensor module, a hardware synchronous triggering mechanism is used to initiate data acquisition. The main controller generates a synchronization pulse signal with a period of 16.67 milliseconds, which is simultaneously sent to the external trigger interface of the binocular vision camera, the control interface of the structured light projection device, and the sampling trigger interface of the six-dimensional force sensor, ensuring that all sensors begin data acquisition at the same time. Upon receiving the synchronization pulse signal, the binocular vision camera simultaneously exposes both left and right image sensors. The exposure time is automatically adjusted according to ambient lighting conditions, ranging from 1 to 10 milliseconds. After exposure, the image data is transferred to the image buffer of the main controller via a USB 3.0 interface. Upon receiving the synchronization pulse signal, the structured light projection device continuously projects a structured light dot matrix using its laser diode during the binocular camera's exposure, ensuring that the structured light pattern is synchronously captured by the binocular camera. The structured light pattern is superimposed on the binocular image, and the structured light information can be extracted from the binocular image using image processing algorithms. After receiving the synchronization pulse signal, the six-dimensional force sensor starts the analog-to-digital converter (ADC) to sample the output voltage of the six strain bridge circuits. Each strain bridge corresponds to a force or torque component. The ADC has a conversion accuracy of 16 bits and a conversion time of 1 microsecond. After sampling, the digitized force data is transmitted to the force data buffer of the main controller through the SPI high-speed serial interface, resulting in binocular image pairs, structured light image data, and raw six-dimensional force data with completely consistent timestamps.

[0021] S13, Add timestamps and store raw sensor data; Based on the synchronously acquired binocular image data, structured light image data, and six-dimensional force raw data, a high-precision system clock is used to add microsecond-level timestamps to each data set. The timestamps record the precise moment data acquisition was triggered, with a resolution of 1 microsecond, ensuring accurate tracing of the data's temporal sequence during subsequent data processing. The time-stamped left-eye image, right-eye image, and six-dimensional force data are organized into a single data frame. The data frame structure includes a frame header identifier, a timestamp field, left-eye image data fields, right-eye image data fields, force data fields, and a checksum field. Since the structured light pattern is already superimposed on the binocular image, there is no need to store the structured light image separately; the structured light dot matrix information is extracted from the binocular image through image analysis during subsequent processing. The organized data frames are stored in the main controller's circular buffer, with a depth of 10 frames. When the buffer is full, the oldest data frame is automatically overwritten, ensuring the system can continuously acquire and process sensor data at a frequency of 60 Hz. Simultaneously, the data frames are transmitted to the host computer via a gigabit Ethernet interface for data recording and offline analysis, obtaining complete timestamped raw data from the multimodal sensors, including binocular vision raw images and six-dimensional force raw data, which serve as inputs for subsequent data preprocessing and coordinate system unification.

[0022] The calibration and registration module is used to acquire multi-sensor data and establish a unified coordinate system to obtain multi-sensor data under the unified coordinate system. S21, Establish the multi-sensor coordinate system transformation relationship; Based on the timestamped binocular vision raw images and six-dimensional force raw data acquired from S1, a hand-eye calibration method is used to establish the spatial transformation relationship between the vision coordinate system, the force sensor coordinate system, and the end effector coordinate system. First, the end effector coordinate system is defined as a unified reference coordinate system, with its origin located at the actuator's operating endpoint. The X-axis points to the actuator's main operating direction, the Y-axis is perpendicular to the X-axis and points laterally, and the Z-axis is determined according to the right-hand rule. The six-dimensional force sensor coordinate system is defined, with its origin located at the sensor's geometric center, and its coordinate axes aligned with the sensor's force measurement axis. The binocular vision coordinate system is defined, with the optical center of the left eye camera as its origin. The Z-axis points to the operating scene along the camera's optical axis, the X-axis is parallel to the camera's baseline, and the Y-axis is vertically downward. A high-precision calibration block is used for hand-eye calibration. The calibration block has calibration patterns of known geometric dimensions machined on its surface, and standard force application points for force sensor calibration are mounted on the block. By manipulating a robotic arm to drive the end effector system to observe calibration blocks from multiple poses, in each pose, a binocular camera captures the calibration pattern to obtain image coordinates, and a six-dimensional force sensor applies a standard force of known magnitude and direction and records the force sensor readings. The least squares optimization method is used to solve for the rotation matrix and translation vector from the visual coordinate system to the end effector coordinate system, as well as the rotation matrix and translation vector from the force sensor coordinate system to the end effector coordinate system, resulting in a precise coordinate system transformation matrix between multiple sensors. The transformation accuracy reaches a translation error of 0.1 mm and a rotation error of 0.1 degrees.

[0023] S22, distortion correction of binocular vision images; Based on the established coordinate system transformation relationship and the original binocular vision images, image distortion correction is performed using camera calibration parameters. The camera calibration parameters are obtained through an offline calibration process. Calibration images are captured from multiple angles using a checkerboard calibration board, and the Zhang Zhengyou calibration method is used to calculate the camera's intrinsic parameter matrix and distortion coefficients. The intrinsic parameter matrix includes the camera's focal length parameters fx and fy, and principal point coordinate parameters cx and cy. The distortion coefficients include radial distortion coefficients k1, k2, and k3, and tangential distortion coefficients p1 and p2. Based on the acquired original left-eye image, a distortion correction model is used to remap the coordinates of each pixel in the image. First, the pixel coordinates are converted to normalized image plane coordinates. Then, the coordinate offset caused by distortion is calculated based on the distortion coefficients. Finally, the corrected pixel coordinates are obtained through inverse mapping. The bilinear interpolation method is used to calculate the grayscale value corresponding to the corrected pixel position, resulting in the distortion-corrected left-eye image. The same method is used to correct the distortion of the original right-eye image, resulting in the distortion-corrected right-eye image. The distortion-corrected image eliminates the geometric distortion caused by the lens, ensuring that straight lines in the image maintain their linearity in the pixel coordinate system. This improves the accuracy of subsequent stereo matching and 3D reconstruction, resulting in a preprocessed binocular image pair that meets the geometric constraints of stereo vision.

[0024] S23 performs zero-point compensation and temperature compensation on the six-dimensional force data; Based on the established coordinate system transformation relationship and the original six-dimensional force data, zero-point drift compensation and temperature compensation methods are used to preprocess the force data. The output value of the six-dimensional force sensor when no external force is applied is called the zero point. Due to the material properties of the strain gauges inside the sensor and the zero-point drift of the circuit, the zero-point value changes with time and temperature. When the system starts up, the end effector remains stationary and does not contact any object. At this time, the only external force on the end effector is its own weight, which is a known quantity. The output value of the six-dimensional force sensor in this state is collected, and the component of the actuator's own weight in the sensor coordinate system is subtracted from the sensor output value to obtain the current zero-point offset value. The zero-point offset value is subtracted from the original six-dimensional force data to complete the zero-point drift compensation. Further considering the influence of temperature on the sensor output, the operating temperature of the sensor is measured in real time by a temperature sensor installed on the sensor body. A temperature compensation model is established based on the temperature compensation coefficient calibrated by the sensor manufacturer. The temperature compensation coefficient describes the linear relationship between the sensor output and temperature. Based on the difference between the measured current temperature and the reference temperature, a temperature compensation model is used to calculate the output offset caused by temperature. The temperature offset is then subtracted from the zero-point compensated force data to obtain the temperature-compensated six-dimensional force data.

[0025] S24 converts multi-sensor data to a unified coordinate system; Based on temperature-compensated six-dimensional force data, preprocessed binocular image pairs, and a multi-sensor coordinate system transformation matrix, a coordinate system transformation method is used to unify all sensor data into the end effector coordinate system. For the six-dimensional force data, a rotation matrix is ​​used to transform the three-dimensional force vector and three-dimensional torque vector in the force sensor coordinate system to the end effector coordinate system. The transformation formula is that the force vector in the end effector coordinate system is equal to the rotation matrix from the force sensor to the end effector multiplied by the force vector in the sensor coordinate system. The torque vector transformation also needs to consider the additional torque caused by the translation of the coordinate system origin. The additional torque is equal to the cross product of the translation vector and the force vector, resulting in six-dimensional force data in a unified coordinate system. For binocular vision images, although the images themselves are represented in pixel coordinates, by recording the transformation matrix from the camera coordinate system to the end effector coordinate system, the coordinates of the three-dimensional feature points extracted from the images can be directly transformed to the end effector coordinate system. The transformed six-dimensional force data, preprocessed binocular image pairs, coordinate system transformation matrix, and timestamp information are organized into a unified data structure to obtain preprocessed sensor data in a unified coordinate system. This data serves as the input for subsequent visual feature extraction and 3D reconstruction, while the six-dimensional force data in the unified coordinate system serves as the input for subsequent multimodal information fusion.

[0026] The visual pose estimation module is used to receive multi-sensor data in a unified coordinate system and perform 3D reconstruction to obtain a six-dimensional visual position error vector and target 3D pose information. S31 uses a deep neural network to extract key feature points of the operation target; Based on preprocessed stereo images in a unified coordinate system, a deep convolutional neural network (DNN) model is used to extract key feature points of the target object. The DNN employs an encoder-decoder architecture. The encoder consists of multiple convolutional and pooling layers to extract multi-scale features from the image, while the decoder consists of multiple deconvolutional layers to restore the spatial resolution of the features and generate a feature point heatmap. The network model is pre-trained on a large-scale spacecraft component image dataset containing images of components under different lighting conditions, from different viewpoints, and with different surface materials. Each image is labeled with the locations of key feature points of the component, such as the center point of a bolt, the positioning hole of a circuit board, and the edge markers of an optical lens. Supervised learning is used for network training, with the loss function defined as the mean square error between the predicted and actual heatmaps. The network parameters are optimized using backpropagation. The preprocessed left-eye image is input into the trained DNN, and the network output is a feature point heatmap. Each pixel value in the heatmap represents the probability that the location is a key feature point. Non-maximum suppression (NMS) is applied to the heatmap, retaining the pixel with the highest probability in its local neighborhood as the feature point. The neighborhood window size is set to 5×5 pixels. Feature points with a probability exceeding a threshold are extracted from the heatmap. The threshold is set to 0.7, resulting in a set of key feature points for the target in the left eye image. The number of feature points is typically between 10 and 50. The right eye image is processed in the same way to obtain a set of key feature points for the target in the right eye image.

[0027] S32, perform binocular stereo matching to calculate the disparity of feature points; Based on the key feature point sets extracted from the left and right eye images, a feature descriptor matching method is used for stereo matching. For each key feature point in the left eye image, an image patch surrounding that feature point is extracted as a local feature descriptor, with the patch size set to 31×31 pixels, centered on the feature point. The normalized cross-correlation (NCC) method is used to calculate the similarity of the local feature descriptors. The normalized cross-correlation value ranges from -1 to +1, with values ​​closer to 1 indicating higher similarity. For a feature point in the left eye image, the best-matching feature point is searched on the corresponding epipolar line in the right eye image. The epipolar line constraint is determined by the stereo camera calibration parameters, and the search range is limited to a 3-pixel-wide strip near the epipolar line. The search length is determined based on the scene depth range; for working distances of 20 mm to 150 mm, the search length is set to 50 to 300 pixels. The normalized cross-correlation value between the left eye feature point and each candidate feature point on the right eye epipolar line is calculated. The candidate point with the largest cross-correlation value exceeding the matching threshold is selected as the matching point, with the matching threshold set to 0.8. For successfully matched feature point pairs, their disparity values ​​are calculated. The disparity value equals the x-coordinate of the left-eye feature point minus the x-coordinate of the right-eye matching point. The accuracy of the disparity value directly affects the depth accuracy of the 3D reconstruction. A sub-pixel precision disparity estimation method is adopted, which obtains sub-pixel level disparity values ​​by parabolic fitting of the cross-correlation curves around the matching points. The disparity precision reaches 0.1 pixels, resulting in high-precision disparity data for each pair of matched feature points.

[0028] S33, combining structured light information for 3D point cloud reconstruction; Based on the disparity data of matched feature points and camera calibration parameters, the 3D coordinates of the feature points are calculated using the principle of triangulation. The triangulation formula is based on the geometric relationship of the binocular camera. The depth value of a feature point equals the camera focal length multiplied by the baseline distance and then divided by the disparity value. The x-coordinate of the feature point in the camera coordinate system is equal to the pixel x-coordinate of the feature point in the left eye image minus the x-coordinate of the principal point, multiplied by the depth value, and then divided by the focal length. The y-coordinate is calculated similarly. This formula is used to calculate the 3D coordinates of each matched feature point in the left eye camera coordinate system, resulting in a sparse 3D point cloud based on binocular stereo vision. To enhance adaptability to targets with different surface reflectivity, dense 3D reconstruction is further performed by combining structured light information. The superimposed structured light point array is extracted from the preprocessed binocular image. Image processing methods are used to identify the position of light spots in the array. Light spot extraction employs threshold segmentation and connected component analysis. By setting a color threshold in the red light band, the structured light point array is separated from the background image. For metallic mirror reflective surfaces, the light spots may appear overexposed and bright. A local adaptive thresholding method and morphological processing are used to ensure accurate extraction of the center position of the light spots. Based on the known structured light dot matrix period and projection angle, a coding rule for the structured light dot matrix is ​​established, and dot encoding is achieved by identifying the position of each light spot in the dot matrix. Dots in the left-eye image are matched with dots in the right-eye image, and the matching process is constrained by the dot encoding information to reduce matching ambiguity. The disparity of the matched dots is calculated, and the three-dimensional coordinates of the dots are calculated using triangulation, resulting in a dense three-dimensional point cloud based on structured light, with a density of 10 to 50 dots per square millimeter. The sparse feature point cloud based on binocular stereo vision is fused with the dense point cloud based on structured light to obtain a complete three-dimensional point cloud containing geometric information of the target surface.

[0029] S34, Estimate the position and orientation of the target object; Based on the reconstructed complete 3D point cloud, a pose estimation algorithm is used to calculate the position and orientation of the target in the end effector coordinate system. Pose estimation employs a model-matching method, pre-establishing a 3D CAD model of the target, which contains precise geometric dimensions and shape information. The CAD model is converted into a 3D point cloud model, with the point density of the point cloud model consistent with the reconstructed point cloud. The Iterative Closest Point (ICP) algorithm is used to register the model point cloud with the reconstructed point cloud. The ICP algorithm iteratively optimizes to find the rigid body transformation matrix that minimizes the distance between the two point clouds. The rigid body transformation matrix includes a rotation matrix and a translation vector, representing the transformation relationship from the model coordinate system to the scene coordinate system. The iterative process of the ICP algorithm includes four steps: nearest point search, corresponding point pair establishment, rigid body transformation solution, and point cloud transformation update. The iteration termination condition is set as a point cloud registration error less than 0.05 mm or the number of iterations exceeding 100. The ICP algorithm is sensitive to initial pose estimation; a feature point alignment method is used to provide the initial pose. Coarse registration is performed using the previously extracted key feature points and their corresponding feature points in the CAD model to obtain the initial transformation matrix. ICP fine registration is performed based on the initial transformation matrix to obtain a high-precision final transformation matrix. The position vector and attitude rotation matrix of the target are extracted from the final transformation matrix. The position vector represents the three-dimensional coordinates of the target's geometric center in the end effector coordinate system, and the attitude rotation matrix represents the rotation relationship between the target coordinate system and the end effector coordinate system. The attitude is represented by quaternions or Euler angles to obtain the complete pose information of the target object.

[0030] S35, calculate the visual position error vector and organize the output data; Based on the estimated target object pose information and the preset desired target pose, an error calculation method is used to obtain the visual position error vector. The desired target pose is given by the operation task planning module, describing the ideal position and attitude of the target object when completing the operation task. The desired pose includes the three-dimensional coordinates of the desired position and the rotation matrix of the desired attitude. The position error vector is calculated, which is equal to the target desired position vector minus the current estimated position vector. The position error vector contains three components: X, Y, and Z, in millimeters, representing the deviation between the current position and the desired position of the target. The attitude error is calculated. The rotation matrix of the current estimated attitude is denoted as Rc, and the rotation matrix of the target desired attitude is denoted as Rd. The attitude error rotation matrix Re is equal to Rd multiplied by the transpose of Rc. The attitude error rotation matrix is ​​converted into axis-angle representation to obtain the attitude error vector, which contains the rotation angle error about the X, Y, and Z axes, in degrees. The position error vector and the attitude error vector are combined into a six-dimensional visual position error vector, which fully describes the deviation between the current pose and the desired pose of the target object. The six-dimensional visual position error vector, the target's complete three-dimensional pose information including current position and current attitude, and timestamp information are organized into a structured visual perception data packet. The data packet adopts a standard data format to facilitate subsequent module parsing and use, resulting in complete visual perception output data. This data serves as the input for subsequent multimodal information fusion and contact state estimation steps.

[0031] The state fusion estimation module is used to receive the six-dimensional visual position error vector and the target's three-dimensional pose information to perform multimodal information fusion and obtain system state estimation and contact state identification. S41, construct the system state vector and observation vector; Based on the six-dimensional visual position error vector and target three-dimensional pose information received from S3, and the six-dimensional force data in a unified coordinate system received from S2, a state-space modeling method is used to construct the system state vector and observation vector. The system state vector is defined as follows: it includes the target object's position, velocity, attitude, and attitude angular velocity. The target position is represented by three-dimensional coordinates, the velocity by three-dimensional velocity components, the attitude by four components of a quaternion, and the attitude angular velocity by three-dimensional angular velocity components. The system state vector has 13 dimensions. The observation vector is defined as follows: it includes the target pose obtained from visual measurement and the six-dimensional forces obtained from force sensors. The visually measured target pose consists of 7 dimensions, namely the three-dimensional position coordinates and the attitude quaternion. The six-dimensional forces consist of 6 dimensions, namely a three-dimensional force vector and a three-dimensional moment vector. The observation vector has 13 dimensions. A system state equation is established to describe the evolution of the system state over time. The state equation is in discrete-time form; the current state vector is equal to the previous state vector after transformation by the state transition matrix plus process noise. The state transition matrix is ​​determined based on the rigid body kinematics equations. The position update equation is the current position equal to the previous position plus the previous velocity multiplied by the sampling period. The velocity update assumes a uniform motion model, i.e., the current velocity equals the previous velocity. The attitude update equation uses a quaternion differential equation; the current attitude quaternion equals the previous quaternion plus the quaternion differential multiplied by the sampling period. The quaternion differential is determined by the angular velocity vector. An observation equation is established to describe the relationship between the observation vector and the state vector. The pose observed visually directly corresponds to the position and attitude components in the state vector; the visual pose part of the observation equation is linear. The relationship between the six-dimensional force observed by the force sensor and the system state varies depending on the contact state. In the free motion state, the observed force is mainly sensor noise and end-effector self-weight, and is independent of the velocity and angular velocity in the state vector. In the contact state, the contact force is related to the end-effector's velocity relative to the target, contact stiffness, and contact damping. The contact force equals the contact stiffness multiplied by the position deviation plus the contact damping multiplied by the relative velocity. The force part in the observation equation is a nonlinear relationship, which needs to be linearized at the current state estimate to obtain a complete state-space model, including the state vector, observation vector, state equation, and observation equation.

[0032] S42 uses an extended Kalman filter for multi-sensor data fusion. Based on the constructed state-space model, the current observation vector, and the state estimate from the previous time step, an Extended Kalman Filter (EKF) is used for multi-sensor data fusion to achieve spatiotemporal synchronous fusion of force and visual information. The EKF is a generalization of the Kalman filter to nonlinear systems. It linearizes the nonlinear state equations and observation equations and estimates the system state recursively. The EKF algorithm consists of two stages: a prediction step and an update step. In the prediction step, based on the previous state estimate and the state transition equation, a priori state estimate for the current time step is predicted. This priori estimate equals the state transition matrix multiplied by the previous state estimate. Simultaneously, the state estimate covariance matrix is ​​predicted. The covariance matrix describes the uncertainty of the state estimate, and the covariance prediction formula is: current covariance equals the state transition matrix multiplied by the previous covariance, multiplied by the transpose of the state transition matrix, and then added to the process noise covariance matrix. In the update step, the prior state estimate is corrected based on the actual observation vector at the current time step. First, the observation residual is calculated, which equals the actual observation vector minus the predicted observation vector calculated by the observation equation. The predicted observation vector is obtained from the prior state estimate through the observation equation. The Kalman gain matrix is ​​calculated. The Kalman gain describes the weight of the observation information in correcting the state estimate. The calculation of the gain matrix involves the observation matrix, covariance matrix, and observation noise covariance matrix. The gain matrix is ​​equal to the covariance matrix multiplied by the transpose of the observation matrix, and then multiplied by the inverse of the observation residual covariance matrix. The state estimate is updated based on the Kalman gain and observation residuals. The posterior state estimate at the current time step is equal to the prior state estimate plus the Kalman gain multiplied by the observation residuals. Simultaneously, the state estimate covariance matrix is ​​updated. The updated covariance matrix is ​​equal to the identity matrix minus the Kalman gain multiplied by the observation matrix, and then multiplied by the prior covariance matrix, resulting in the optimal state estimate incorporating multi-sensor information.

[0033] S43, analyze the six-dimensional force change characteristics to determine the contact state; Based on the fused system state estimate and the six-dimensional force data in a unified coordinate system transmitted from S2, a contact state discrimination algorithm is used to identify the current contact state. Three contact states are defined: free motion state, contact approach state, and stable contact state. The free motion state indicates that the end effector is moving in space and has not yet made contact with the target object. In this state, the force measured by the six-dimensional force sensor is mainly the sensor's zero-point noise and the end effector's own gravity, and the force amplitude remains within a small threshold range. The contact approach state indicates that the end effector is approaching the target object and is about to make or has just made contact. At this state, the six-dimensional force begins to change significantly, and the rate of change of force increases significantly. The stable contact state indicates that the end effector has established stable contact with the target object. At this state, the six-dimensional force reaches a certain amplitude, and the rate of change tends to stabilize. Based on the six-dimensional force data at the current moment, the composite amplitude of the six-dimensional force is calculated. The composite amplitude is equal to the sum of the magnitudes of the three-dimensional force vector and the three-dimensional moment vector. The magnitude is calculated using the Euclidean norm. The synthesized amplitude is compared with a free motion threshold set to 0.5 Newtons. If the synthesized amplitude is less than the free motion threshold, the system is determined to be in a free motion state. If the synthesized amplitude is greater than or equal to the free motion threshold, the rate of change of the six-dimensional force is further calculated. The rate of change is equal to the current synthesized amplitude minus the previous synthesized amplitude, divided by the sampling period, in Newtons per second. The rate of change is then compared with a contact approach threshold set to 5 Newtons per second. If the rate of change is greater than the contact approach threshold, the system is determined to be in a contact approach state. If the synthesized amplitude is greater than a stable contact threshold and the rate of change is less than or equal to the contact approach threshold, the system is determined to be in a stable contact state, set to 1 Newton. This yields the contact state identifier for the current operation.

[0034] S44, outputs fused state estimation and contact state information; Based on the optimal state estimation and contact state discrimination results from the extended Kalman filter output, a data organization method is used to generate fused information output. The target position, velocity, attitude, and angular velocity information from the optimal state estimation are extracted and organized into a target motion state data structure. The contact state identifier, the composite amplitude of six-dimensional forces, and the rate of change of six-dimensional forces are extracted and organized into a contact state data structure. Simultaneously, the six-dimensional visual position error vector and the six-dimensional force data in a unified coordinate system are retained for subsequent control. The target motion state data structure, the contact state data structure, the six-dimensional visual position error vector, and the six-dimensional force data are combined, and a timestamp of the current moment is added to form a complete fused information data frame. The fused information data frame contains all the perception information required by the system, providing accurate and reliable input for subsequent force-visual coordinated control. The fused information data frame is stored in the shared memory area of ​​the controller for the control algorithm module to read. At the same time, the fused information is sent to the host computer through the communication interface for real-time monitoring and data recording. The fused system state estimate, contact state identifier, six-dimensional visual position error vector, and six-dimensional force data in a unified coordinate system are obtained. This data serves as the input for the subsequent force-visual collaborative control command generation. Meanwhile, the contact state identifier and six-dimensional force data serve as the input for material recognition.

[0035] The collaborative control module receives system state estimates and contact state indicators, and generates control commands using task priority control and impedance control methods to obtain force vision-coordinated Cartesian space velocity control commands. S51, selects the control mode according to the contact state; Based on the contact state identifier received from S4, the fused system state estimate, the six-dimensional visual position error vector, and the six-dimensional force data in a unified coordinate system, a control mode selection strategy is adopted to determine the current control mode to be used. Three control modes are defined, corresponding to three contact states: visual position control mode for free motion, position-force hybrid control mode for contact approaching state, and force tracking control mode for stable contact state. When the contact state identifier is free motion, the visual position control mode is selected. This mode aims to minimize the visual position error, ignores force information, and uses visual servo control to quickly move the end effector to the desired target position. When the contact state identifier is contact approaching state, the position-force hybrid control mode is selected. This mode considers both position error and contact force, and achieves a smooth transition from pure position control to pure force control by adjusting the weight coefficients of position control and force control. When the contact state identifier is stable contact state, the force tracking control mode is selected. This mode aims to track the desired force with the contact force, maintaining a constant contact force between the end effector and the target or changing it according to a predetermined trajectory. It is suitable for tasks requiring precise force control, such as assembly and polishing. To avoid jitter during control mode switching, a smooth switching strategy is adopted. During the control mode transition, the control parameters are not changed abruptly, but are gradually adjusted over several consecutive control cycles to ensure a smooth transition of the control output and obtain the control mode identifier and corresponding control parameter configuration at the current moment.

[0036] S52, Construct the objective function for task priority control; Based on the selected control mode, visual position error vector, and six-dimensional force data in a unified coordinate system, a unified control objective function is constructed using a task priority allocation method. The control objective function adopts a weighted optimization form, including a position error minimization term and a force error minimization term, with their priorities balanced by weight coefficients. The position error minimization term is defined, aiming to minimize the weighted norm of the visual position error vector. The norm is in quadratic form, and the weighting matrix is ​​the position error weight matrix. The diagonal elements of the weight matrix correspond to the error weights in the three translational directions (X, Y, Z) and the three rotational directions (X, Y, Z), respectively. A larger weight value indicates a higher positional accuracy requirement in that direction. A force error minimization term is defined, the goal of which is to minimize the error between the actual contact force and the desired contact force. The desired contact force is determined by the operation task planning module according to the specific operation task. For cleaning and polishing tasks, the desired contact force is set to a constant slight contact force, such as 0.3 Newtons. For assembly and plugging tasks, the desired contact force changes dynamically according to the assembly stage. For free movement stages, the desired contact force is set to zero. The force error is equal to the desired six-dimensional force vector minus the currently measured six-dimensional force vector. The force error minimization term also adopts a weighted quadratic form, and the diagonal elements of the force error weight matrix correspond to the control accuracy requirements of the six force / torque components. The overall objective function is constructed by linearly combining the position error minimization term and the force error minimization term. The combination coefficients are determined according to the current control mode. In the visual position control mode, the weight coefficient of the position error term is set to 1, and the weight coefficient of the force error term is set to 0. In the force tracking control mode, the weight coefficient of the position error term is set to 0, and the weight coefficient of the force error term is set to 1. In the position-force hybrid control mode, the weight coefficients of the two terms are dynamically adjusted according to the contact force amplitude. The larger the contact force, the greater the weight of the force error term, and the smaller the contact force, the greater the weight of the position error term. The sum of the weight coefficients is normalized to 1, resulting in a unified control objective function that considers task priority.

[0037] S53, solves the Cartesian space velocity command; Based on the constructed control objective function and the current system state, a quadratic programming solver is used to calculate the optimal velocity command of the end effector in Cartesian space. The control objective function is expanded into a standard quadratic programming form, with the decision variables being the Cartesian space velocity vector of the end effector, containing six components: three-dimensional linear velocity and three-dimensional angular velocity. The objective function is a quadratic form of the decision variables, and the quadratic form matrix is ​​composed of the position error weight matrix, the force error weight matrix, and the system Jacobian matrix. Constraints include velocity and acceleration limits. The velocity limit constraint requires that the absolute value of the velocity in each direction in Cartesian space does not exceed the maximum velocity limit; the maximum linear velocity is set to 10 mm / s, and the maximum angular velocity is set to 5 degrees / s. The acceleration limit constraint requires that the acceleration obtained by dividing the difference between the velocity command of the current control cycle and the velocity command of the previous cycle by the sampling period does not exceed the maximum acceleration limit. This constraint ensures that the velocity command changes continuously, avoiding excessive acceleration that could cause shocks. The active set method or interior point method is used to solve this constrained quadratic programming problem. The solver output is the optimal velocity vector that satisfies all constraints and minimizes the objective function value. To improve control accuracy, a feedforward compensation term is added to the speed command. The feedforward compensation is based on the motion prediction of the target object. If the target object has a motion trend, the feedforward term compensates for the relative velocity change caused by the target motion, resulting in a Cartesian space speed command that includes feedforward compensation.

[0038] S54, with added impedance control characteristics; Based on the solved Cartesian space velocity command and the current six-dimensional force measurements, an impedance control method is used to endow the end effector with compliance characteristics, enabling it to exhibit the desired dynamic behavior during contact. The core idea of ​​impedance control is to establish a dynamic relationship between the contact force and the end effector position in Cartesian space, simulating the dynamic characteristics of a mass-spring-damped system. A desired impedance model is defined, with parameters including an inertia matrix, a damping matrix, and a stiffness matrix. All three matrices are 6×6 diagonal matrices, with diagonal elements corresponding to the inertia, damping, and stiffness parameters of the six degrees of freedom in Cartesian space, respectively. The impedance parameters are set according to the compliance requirements of the task. For tasks requiring high compliance, such as manipulating flexible materials, smaller stiffness values ​​and larger damping values ​​are set; for tasks requiring high stiffness, such as precision positioning, larger stiffness values ​​and smaller damping values ​​are set. Based on the desired impedance model, the position correction caused by force deviation is calculated. The force deviation equals the measured six-dimensional force minus the desired contact force. The position correction is obtained by solving the dynamic equation of the impedance model, which is: inertia matrix multiplied by acceleration, plus damping matrix multiplied by velocity, plus stiffness matrix multiplied by position deviation equals force deviation. This differential equation is discretized, and the position and velocity corrections are solved using either the Euler method or the Runge-Kutta method. The velocity correction is superimposed on the velocity command obtained from quadratic programming to obtain a final velocity command with impedance control characteristics. This velocity command enables the end effector to exhibit compliant interaction characteristics during contact, adapting to minor uncertainties in the target position and avoiding impacts and damage caused by rigid contact. This results in a force-vision coordinated Cartesian space velocity control command, which serves as the input for subsequent end effector motion.

[0039] The parameter adaptive module is used to receive contact status indicators and multi-sensor data in a unified coordinate system, identify material properties using a classification method, and obtain adjusted force control parameters. S61, collect force response data during the stable contact phase; Based on the contact state identifier received from S4 and the six-dimensional force data in a unified coordinate system, the system first determines whether the current operation involves contact. If the contact state identifier remains in a free motion state, it indicates that the operation does not involve contact, and the system skips the material identification process in step S6, directly using the default conservative force control parameter configuration to pass to step S7, ensuring that the system can work normally under various operating scenarios. If the operation involves contact, a data acquisition method is used to obtain the force response curve of the contact phase for material identification. The contact state identifier is monitored, and when the contact state changes from a free motion state to a contact approach state or a stable contact state, the force response data acquisition program is started to ensure that force response characteristics can be collected in a timely manner after contact occurs. During the duration of the contact state, six-dimensional force data is continuously acquired at a sampling frequency of 1000 Hz, with an acquisition duration of 0.5 seconds, for a total of 500 force data sample points. The acquired force data is organized into a force response curve according to the time series, and the force response curve contains six curves corresponding to the changes of three-dimensional force and three-dimensional torque over time. The force response curves are preprocessed to remove DC bias, calculate the mean of each curve, and subtract the mean from the original curves to obtain a force response curve with zero mean. The preprocessed force response curves are then filtered using a Butterworth bandpass filter with a passband range of 1 Hz to 100 Hz. This preserves the dynamic characteristics of the force response while removing low-frequency drift and high-frequency noise, resulting in force response curve data for material identification.

[0040] S62, extract contact stiffness and damping characteristics; Based on the collected force response curve data, contact stiffness and damping characteristics are extracted using a system identification method. The dynamic characteristics of the contact process can be described by a second-order system model, which includes three parameters: mass, damping, and stiffness. The mass parameter is mainly determined by the inertia of the end effector and is a known quantity. The parameters that need to be identified are contact stiffness and contact damping. Using a least squares parameter identification method, a linear regression relationship is established between the contact force response data, velocity data, and position data. The regression equation is: contact force equals stiffness coefficient multiplied by position deviation plus damping coefficient multiplied by velocity. The position deviation and velocity data are obtained from the system state estimate fused in step S4, which includes the position and velocity information of the target object. At the start of force response data acquisition, the initial position of the end effector is recorded as the initial contact position, and the position deviation is the difference between the current position and the initial contact position. A regression matrix is ​​constructed, with each row corresponding to a sampling time. Each row contains the position deviation and velocity at that time as independent variables, and the corresponding contact force as the dependent variable. The least squares method is used to solve for the regression coefficients, minimizing the mean square error between the predicted contact force and the actual measured contact force, thus obtaining the contact stiffness coefficient and contact damping coefficient. For anisotropic materials, the force response in the X, Y, and Z directions is identified, yielding stiffness coefficients and damping coefficients in these three directions. The identification results are then normalized: the stiffness coefficient is divided by the reference stiffness value to obtain the normalized stiffness, and the damping coefficient is divided by the critical damping value to obtain the normalized damping. The normalized eigenvalues ​​range from 0 to 2, facilitating subsequent feature classification, resulting in the contact stiffness eigenvector and damping eigenvector.

[0041] S63 uses a neural network to classify and identify material categories; Based on the extracted contact stiffness and damping feature vectors, a trained material recognition neural network is used for material classification. The neural network employs a fully connected feedforward network structure, consisting of an input layer, two hidden layers, and an output layer. The input layer has 6 neurons, corresponding to stiffness features in three directions and damping features in three directions. The first hidden layer contains 20 neurons, and the second hidden layer contains 10 neurons, both using the ReLU activation function. The output layer has the same number of neurons as the number of material categories, defining five common material categories: rigid metals, flexible rubber, brittle ceramics, composite materials, and optical glass. The output layer uses the Softmax activation function, outputting the probability distribution for each category. The neural network is trained on a force response dataset labeled with material categories. This dataset contains contact experimental data for different materials, with each sample containing stiffness and damping features and the corresponding material category label. Training uses a cross-entropy loss function and the Adam optimizer, and the network parameters converge after multiple iterations. The currently extracted stiffness and damping feature vectors are input into the trained neural network, and the network calculates the probability distribution of the output layer during forward propagation, selecting the category with the highest probability as the identified material category. At the same time, the probability value of the category is recorded as the recognition confidence level. When the confidence level is greater than 0.8, the recognition result is considered reliable. When the confidence level is less than 0.8, the material is considered uncertain. The material category identifier and confidence level are obtained.

[0042] S64, adaptively adjusts force control parameters according to material type; Based on the identified material category and confidence level, a parameter query and interpolation method is used to adaptively adjust the force control parameters. A material parameter database is pre-established, storing recommended force control parameters for each material category. These parameters include the proportional gain, integral gain, and derivative gain of the force controller, as well as the stiffness and damping parameters for impedance control. For rigid metals, a larger force control gain and larger impedance stiffness are recommended to ensure accurate force tracking performance. For flexible rubber, a smaller force control gain and smaller impedance stiffness are recommended to avoid system oscillation. For brittle ceramics and optical glass, a medium force control gain combined with larger impedance damping is recommended to ensure smooth contact and avoid impact. Based on the identified material category, the corresponding force control parameters are queried from the parameter database. If the identification confidence level is greater than 0.8, the queried parameters are used directly. If the identification confidence level is less than 0.8, a conservative parameter strategy is adopted, selecting parameters from multiple candidate material categories and performing a weighted average, with weights allocated according to the identification probability of each category. The force control parameters obtained from the query or calculation are updated to the force controller and impedance controller. The parameter update adopts a gradual method, without immediately jumping the parameter value. Instead, it is a linear interpolation transition over 10 consecutive control cycles. The parameter value of the current cycle is equal to the parameter value of the previous cycle plus one-tenth of the parameter change, ensuring smooth parameter update and avoiding control jitter. The resulting adaptively adjusted force control parameters, including force control gain and impedance parameters, serve as the configuration input for subsequent execution of end motion and force control.

[0043] The motion control module receives speed control commands and force control parameters, and uses inverse kinematics and active damping control methods to drive the end motion, achieving stable and precise operation after vibration suppression. S71 converts Cartesian space velocity commands into joint space control quantities; Based on the force-vision coordinated Cartesian space velocity commands received from S5 and the adaptively adjusted force control parameters received from S6, an inverse kinematics method is used to convert the Cartesian space velocity commands into control quantities in the joint space of the robotic arm. The kinematic relationship of the robotic arm describes the mapping between joint space and Cartesian space. The forward kinematic equation maps joint angles to end-effector pose, and the inverse kinematic equation maps end-effector pose to joint angles. For inverse kinematics at the velocity level, the Jacobian matrix method is used. The Jacobian matrix describes the linear relationship between joint velocity and Cartesian space velocity; the Cartesian space velocity vector is equal to the Jacobian matrix multiplied by the joint velocity vector. Based on the current joint angle configuration of the robotic arm, the Jacobian matrix is ​​calculated. The Jacobian matrix is ​​calculated using differential or geometric methods. For a 6-DOF robotic arm, the Jacobian matrix is ​​a 6×6 matrix. The Cartesian space velocity command is denoted as the desired velocity vector. The joint velocity vector is solved so that the Jacobian matrix multiplied by the joint velocity vector equals the desired velocity vector. For non-singular configurations, the Jacobian matrix is ​​invertible, and the joint velocity vector is equal to the inverse of the Jacobian matrix multiplied by the desired velocity vector. For near-singular configurations or redundant robotic arms, the Jacobian matrix is ​​either non-invertible or non-unique. Damped least squares or pseudo-inverse methods are used to solve for it. Damped least squares adds a damping term to the objective function to avoid singularities. The joint velocity vector is equal to the transpose of the Jacobian matrix multiplied by the transpose of the Jacobian matrix, plus the damping coefficient multiplied by the inverse of the identity matrix, and then multiplied by the desired velocity vector. The damping coefficient is adaptively adjusted based on the condition number of the Jacobian matrix. After obtaining the joint velocity vector, joint velocity constraints are considered. It is checked whether the velocity of each joint exceeds the physical limitations of the robotic arm. If it does, the joint velocity vector is scaled proportionally while keeping the velocity vector direction unchanged. The largest joint velocity component is scaled to the limit value, and other components are scaled proportionally to obtain a joint velocity vector that satisfies the joint constraints.

[0044] S72 generates joint position and speed control commands; Based on the solved joint velocity vector and the current joint angle, a numerical integration method is used to generate joint position control commands. The joint position is updated using discrete-time integration. The desired joint angle in the current control cycle equals the joint angle of the previous cycle plus the joint velocity vector multiplied by the control cycle. The control cycle is 0.001 seconds, corresponding to a control frequency of 1000 Hz. The desired joint angle is used as the joint position control command, and the joint velocity vector is used as the joint velocity feedforward command. For each joint, a dual-loop control structure for position and velocity is constructed, with an outer loop for position control and an inner loop for velocity control. The position control loop calculates the joint position error, which equals the desired joint angle minus the current actual joint angle. The actual joint angle is measured by the joint encoder. A proportional control law is applied to the position error to generate the desired velocity for the velocity control loop, which equals the position proportional gain multiplied by the position error. The desired velocity of the velocity control loop is added to the velocity feedforward command to obtain the total desired joint velocity. The velocity control loop calculates the joint velocity error, which equals the desired joint velocity minus the current actual joint velocity. The actual joint velocity is measured by the joint encoder differential or a velocity sensor. A proportional-integral (PI) control law is applied to the speed error to generate joint torque or current commands. The torque command equals the speed proportional gain multiplied by the speed error, plus the speed integral gain multiplied by the integral of the speed error. The torque commands are output to the drivers of each joint. The drivers control the motors to output corresponding driving torques according to the torque commands, thus obtaining the joint control commands that drive the robotic arm to move.

[0045] S73 detects high-frequency vibration signals; Based on real-time force data measured by a six-dimensional force sensor, high-pass filtering and spectral analysis are used to detect high-frequency vibration signals during operation. Each component of the six-dimensional force data is passed through a high-pass filter with a cutoff frequency set to 20 Hz to filter out low-frequency static contact force components and retain high-frequency vibration signal components. Time-domain analysis is performed on the filtered signal to calculate the root mean square (RMS) value, which reflects the magnitude of vibration energy. A vibration detection threshold of 0.2 Newtons is set; if the RMS value of a force component exceeds the threshold, vibration is determined to exist in that direction. Further spectral analysis is performed on the vibration signal, using a Fast Fourier Transform (FFT) to convert the time-domain signal to the frequency-domain signal, obtaining the spectral distribution of the vibration signal. The dominant frequency component in the spectrum is identified; the dominant frequency corresponds to the frequency with the highest vibration energy, and the dominant frequency value and its corresponding amplitude are recorded. The analysis examines whether the dominant frequency is close to the system's natural frequency. The system's natural frequency is determined through offline modal analysis experiments. For the terminal system of this invention, the main natural frequency is distributed in the range of 80 Hz to 150 Hz. When the deviation between the detected dominant vibration frequency and the natural frequency is less than 5 Hz, the system is determined to resonate, requiring enhanced vibration suppression. Vibration suppression filter parameters are designed based on the dominant frequency, yielding vibration characteristic information including vibration direction, vibration amplitude, and vibration frequency.

[0046] S74 generates and superimposes a reverse damping force control command. Based on the detected vibration characteristics, an active damping control algorithm is used to generate a reverse damping force control command. The principle of active damping control is to apply a damping force opposite to the vibration velocity through an actuator, thereby consuming vibration energy and suppressing vibration. Based on the vibration signal, the vibration velocity is estimated by numerically differentiating the vibration signal. To avoid noise amplification during differentiation, a center difference method combined with low-pass filtering is used. The vibration velocity components in each direction are calculated. For directions where vibration is detected, a reverse damping force is calculated. The reverse damping force equals a negative damping coefficient multiplied by the vibration velocity. The damping coefficient is adaptively adjusted according to the vibration amplitude and frequency; it increases when the vibration amplitude is large and when the vibration frequency is close to the natural frequency. The adjustment of the damping coefficient uses a fuzzy control rule, with the input being the vibration amplitude and frequency deviation, and the output being the damping coefficient adjustment amount. The calculated reverse damping force is organized into a six-dimensional damping force vector. For directions without vibration, the damping force component is set to zero. The six-dimensional damping force vector is converted into a damping torque in joint space using the Jacobian matrix transpose method. The joint space damping torque is equal to the Jacobian matrix transpose multiplied by the six-dimensional damping force vector. The joint space damping torque is then superimposed on the joint torque command generated in step S72 using a direct addition method. The final joint torque command is equal to the original joint torque command plus the damping torque, resulting in the final joint control command that includes vibration suppression.

[0047] S75 drives the end effector to move and provides feedback on the operation effect; Based on final joint control commands that include vibration suppression, joint actuators drive the movement of each joint of the robotic arm to achieve precise operation of the end effector. After receiving torque commands, the joint actuators control the motors to output corresponding torques to drive the joints to rotate. During the drive process, joint encoders measure the angles and angular velocities of each joint in real time, generating joint state feedback. Through forward kinematics calculations, the joint states are converted into the actual pose and velocity of the end effector, obtaining the end effector motion state. Simultaneously, a six-dimensional force sensor continuously measures the contact force, and a binocular vision camera continuously acquires images of the operation scene; these real-time sensor data constitute feedback information on the operation effect. The end effector motion state, contact force data, and visual image data are transmitted to the main controller, which evaluates the operation effect, including position accuracy, force control accuracy, and operation stability. Position accuracy is evaluated by comparing the deviation between the desired pose and the actual pose; force control accuracy is evaluated by comparing the deviation between the desired contact force and the actual contact force; and operation stability is evaluated by analyzing vibration amplitude and control error fluctuations. If the operation effect meets the task requirements, the operation is successfully completed; if the operation effect does not meet the requirements, the main controller adjusts the control parameters or plans a new operation strategy and re-executes the operation process. The binocular vision images and six-dimensional force data acquired in this control cycle are used as inputs for the next control cycle and fed back to step S1 for data acquisition and processing again, forming a complete closed-loop control. The system continuously executes all steps from S1 to S7 at a frequency of 60 Hz, responds to changes in the operating environment in real time, continuously optimizes operating performance, and obtains a stable and precise operating effect after vibration suppression, thus completing high-precision multi-degree-of-freedom micro-operation tasks in confined spaces.

[0048] The micro-manipulation end-effector system integrating multi-dimensional micro-force sensing and visual servoing of this invention has been practically applied and verified in a precision maintenance mission inside a certain type of on-orbit satellite. This maintenance mission involved cleaning and calibrating optical lenses for scientific payloads inside the satellite, replacing electronic equipment modules, and disassembling and assembling precision fasteners. The operating space was limited, with the smallest gap between devices being only 8 centimeters. The size of the manipulated objects ranged from bolts with a diameter of 3 millimeters to circuit board modules with a diameter of 15 centimeters. Positioning accuracy was required to be within 50 micrometers, and contact force control accuracy was required to be within 0.5 Newtons.

[0049] Application Scenario Description: The system's end effector carries specialized tools into the confined space of the cabin. First, a binocular vision system identifies the position and orientation of the target optical lens. The lens surface is made of highly reflective glass, resulting in strong specular reflection under the complex lighting conditions inside the cabin. The system employs binocular stereo vision combined with structured light technology. It extracts feature points from the lens edge using a deep neural network, calculates disparity through binocular stereo matching, and reconstructs a dense 3D point cloud using structured light lattice information. The ICP algorithm is then used to estimate the lens pose, successfully reconstructing the 3D pose with a positioning accuracy of 30 micrometers. During the cleaning operation, the cleaning tool needs to maintain a constant, slight contact force with the lens surface to avoid scratching the delicate optical surface. The system monitors the contact force in real time using a six-dimensional force sensor. An extended Kalman filter fuses force and visual information to determine the contact state. In free movement, a visual position control mode guides the tool towards the lens surface. When a contact approach is detected, it switches to a position-force hybrid control mode. After contact stabilizes, it switches to a force tracking control mode, maintaining the contact force at a stable 0.3 Newtons with a force control standard deviation of 0.05 Newtons. During the cleaning process, the system collects force response data during the stable contact phase. It extracts contact stiffness and damping characteristics using a system identification method, and employs a neural network to classify and identify the lens's optical glass material. Based on the material type, it adaptively adjusts the force control parameters, setting a smaller impedance stiffness value and a larger impedance damping value to ensure the tool has good compliance, preventing excessive contact force even with slight deviations in lens position. After cleaning, lens calibration is performed by finely adjusting the calibration screw on the lens holder, requiring an adjustment accuracy at the arcsecond level. The system uses force feedback to precisely control the screw's rotation angle, while simultaneously measuring changes in the lens's optical axis direction using a vision system to achieve closed-loop calibration. The calibration accuracy reaches 3 arcseconds, meeting the mission requirements.

[0050] In electronic device module replacement tasks, it is necessary to remove the old module and insert the new module. There are multiple pin connections between the module and the socket, and the insertion / removal process requires precise and stable force control. The system extracts key feature points of the module using a binocular vision system, and uses binocular stereo matching and 3D reconstruction technology to identify the module's position and orientation, planning the insertion / removal path. When removing the old module, the system uses a force tracking control mode, gradually increasing the pulling force and monitoring force changes in real time through a six-dimensional force sensor. When a sudden decrease in force is detected, indicating that the module has detached from the socket, the system judges the change in contact state based on the force change rate and immediately stops the pulling action to avoid impact. When inserting the new module, the system uses a visual position control mode to guide precise alignment between the module and the socket, achieving an alignment accuracy of 0.1 mm. Then, it switches to a position-force hybrid control mode for slow insertion. An extended Kalman filter fuses visual position information and force information, monitoring changes in insertion force. When the insertion force reaches a preset value, it indicates that the module is fully inserted. The entire insertion / removal process is smooth and impact-free, with no damage to the module or socket.

[0051] In the fastener assembly / disassembly task, it is necessary to remove 3 mm diameter bolts made of titanium alloy. The required installation torque is 2 N·m, and the control accuracy must be within 0.1 N·m. The system uses a deep neural network to extract key feature points of the bolt, calculates parallax through binocular stereo matching, and reconstructs the three-dimensional position of the bolt to guide the tightening tool to align with the bolt. During the tightening process, the system monitors the tightening torque through a six-dimensional force sensor, adopts a force tracking control mode, and generates Cartesian space velocity control commands based on the task priority control objective function. These commands are then converted into joint space control quantities through inverse kinematics to drive the end effector movement, achieving precise tightening. Simultaneously, an active damping control method is used to detect and suppress high-frequency vibrations during operation. Tightening stops when the target torque is reached. The actual measured torque deviation is within 0.08 N·m, meeting assembly requirements. When removing the bolt, the system continuously monitors the removal torque and uses force vision-based collaborative control to avoid excessive force that could damage the threads, ensuring a smooth and damage-free removal process.

[0052] The system's performance data in this maintenance task is shown in Tables 1 and 2: Table 1. Data on optical lens cleaning and calibration tasks

[0053] Table 2. Data on Electronic Module Replacement and Fastener Removal / Installation Tasks

[0054] Application example data demonstrates that the micromanipulation end effector system of this invention successfully completed high-precision, multi-degree-of-freedom precision manipulation tasks in confined spaces, achieving a positioning accuracy stable within 50 micrometers and a contact force control accuracy within 0.5 Newtons, meeting the stringent requirements of the aerospace field. The system's force-vision collaborative control, material adaptive recognition, and active vibration suppression technologies exhibit excellent performance and robustness in practical applications, validating the effectiveness and practicality of the invention's technical solutions. This system is not only suitable for the aerospace field but also has broad application prospects in industrial precision manufacturing, biomedical micromanipulation, and other areas.

[0055] The embodiments of the present invention have been described above. However, the embodiments are not limited to the specific implementation methods described above. The specific implementation methods described above are merely illustrative and not restrictive. Those skilled in the art can make more equivalent embodiments under the guidance of the present embodiments, and all of them are within the protection scope of the present embodiments.

Claims

1. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing, characterized in that, include: The data acquisition module is used to acquire data from multiple sensors according to the configuration parameters of the sensor module; The calibration and registration module is used to acquire multi-sensor data and establish a unified coordinate system to obtain multi-sensor data under the unified coordinate system. The visual pose estimation module is used to receive multi-sensor data in a unified coordinate system and perform 3D reconstruction to obtain a six-dimensional visual position error vector and target 3D pose information. The state fusion estimation module is used to receive the six-dimensional visual position error vector and the target's three-dimensional pose information to perform multimodal information fusion and obtain system state estimation and contact state identification. The collaborative control module receives system state estimates and contact state indicators, and generates control commands using task priority control and impedance control methods to obtain force vision-coordinated Cartesian space velocity control commands. The parameter adaptive module is used to receive contact status indicators and multi-sensor data in a unified coordinate system, identify material properties using a classification method, and obtain adjusted force control parameters. The motion control module receives speed control commands and force control parameters, and uses inverse kinematics and active damping control methods to drive the end motion, achieving stable and precise operation after vibration suppression.

2. The micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The data acquisition module includes: Configure the hardware parameters of the binocular stereo vision camera and the structured light projection device. The structured light projection device uses a laser diode as a light source and generates regular dot matrix structured light through diffractive optical elements. Configure the sampling parameters of the six-dimensional force sensor, including sampling frequency, force measurement range and torque measurement range. The main controller generates a synchronous pulse signal, which is simultaneously sent to the external trigger interface of the binocular vision camera, the control interface of the structured light projection device, and the sampling trigger interface of the six-dimensional force sensor. The binocular vision camera simultaneously exposes the left and right image sensors, and the structured light pattern is superimposed on the binocular image. The six-dimensional force sensor starts the analog-to-digital converter to sample the output voltage of the strain bridge circuit. The system clock is used to add a timestamp to each set of data. The left eye image, right eye image, and six-dimensional force data with timestamps are organized into data frames and stored in a circular buffer.

3. The micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The calibration and registration module includes: A hand-eye calibration method is used to establish the spatial transformation relationship between the visual coordinate system, the force sensor coordinate system, and the end effector coordinate system. The end effector coordinate system is defined as a unified reference coordinate system. The least squares optimization method is used to solve the rotation matrix and translation vector between each coordinate system. The camera calibration parameters are used to correct the distortion of the binocular vision image. The camera calibration parameters include the intrinsic parameter matrix and distortion coefficients. The coordinates of each pixel in the image are remapped, and the gray value corresponding to the pixel position after correction is calculated by bilinear interpolation. Zero-point compensation and temperature compensation are performed on the six-dimensional force data. When the system starts up, the sensor output value under the condition of no external force is collected as the zero-point offset value. The output offset caused by temperature is calculated based on the working temperature measured by the temperature sensor and the temperature compensation coefficient. A coordinate system transformation method is used to unify all sensor data into the coordinate system of the end effector.

4. The micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The visual pose estimation module includes: A deep convolutional neural network model with an encoder-decoder architecture is used to extract key feature points of the target operation. The encoder part consists of multiple convolutional layers and pooling layers to extract multi-scale features of the image, and the decoder part consists of multiple deconvolutional layers to generate feature point heatmaps. Non-maximum suppression processing is performed on the heatmaps to extract feature points in the heatmaps with a probability exceeding a preset probability threshold. The similarity of local feature descriptors is calculated using the normalized cross-correlation method for stereo matching. Matching points are searched under epipolar constraints. The disparity value is obtained by parabolic fitting of the cross-correlation curves around the matching points using the subpixel precision disparity estimation method. The three-dimensional coordinates of feature points are calculated using the triangulation principle to obtain a sparse three-dimensional point cloud. The structured light spot array is extracted from the binocular image and the spot coding is matched. The three-dimensional coordinates of the matched spot are calculated to obtain a dense three-dimensional point cloud. The sparse feature point cloud and the dense point cloud are then fused.

5. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 4, characterized in that, The visual pose estimation module also includes: A 3D CAD model of the target object is pre-built and converted into a 3D point cloud model. The iterative nearest point algorithm is used to register the model point cloud with the reconstructed point cloud. The iterative process includes nearest point search, corresponding point pair establishment, rigid body transformation solution and point cloud transformation update. The feature point alignment method is used to provide the initial pose for coarse registration and then fine registration is performed. Extract the position vector and attitude rotation matrix of the target from the final transformation matrix, calculate the position error vector and attitude error vector, and combine the position error vector and attitude error vector into a six-dimensional visual position error vector.

6. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The state fusion estimation module includes: The system state vector is defined to include the position, velocity, attitude, and attitude angular velocity of the target object. The observation vector is defined to include the target pose obtained by visual measurement and the six-dimensional force obtained by force sensor measurement. The state equation describing the evolution of the system state over time and the observation equation describing the relationship between the observation vector and the state vector are established. An extended Kalman filter is used for multi-sensor data fusion. In the prediction step, the state prior estimate and covariance matrix at the current time are predicted based on the state estimate and state transition equation at the previous time step. In the update step, the observation residual and Kalman gain matrix are calculated, and the state estimate is updated based on the Kalman gain and observation residual. Three contact states are defined: free motion, contact approach, and stable contact. The composite amplitude and rate of change of the six-dimensional force are calculated. The composite amplitude is compared with a preset free motion threshold, and the rate of change is compared with a preset contact approach threshold to determine the contact state identifier of the current operation.

7. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The collaborative control module includes: The visual position control mode corresponds to the free motion state, the position-force hybrid control mode corresponds to the contact approach state, and the force tracking control mode corresponds to the stable contact state. During the control mode transition, the control parameters are gradually adjusted to make the control output transition continuously and smoothly. Construct a control objective function that includes a position error minimization term and a force error minimization term. The priority of the two terms is balanced by a weighting coefficient. The weighting coefficient is determined according to the current control mode. In the position-force hybrid control mode, the weighting coefficients of the two terms are dynamically adjusted according to the contact force amplitude. The control objective function is expanded into a standard quadratic programming form, the decision variable is the Cartesian space velocity vector of the end effector, and the constraints include velocity limit constraints and acceleration limit constraints. The optimal velocity vector is calculated using a quadratic programming solver.

8. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 7, characterized in that, The collaborative control module also includes: Define the desired impedance model, whose parameters include the inertia matrix, damping matrix, and stiffness matrix. Calculate the position correction caused by force deviation based on the desired impedance model. The force deviation is equal to the measured six-dimensional force minus the desired contact force. Discretize the dynamic equations of the impedance model to solve for the velocity correction. Superimpose the velocity correction onto the velocity command obtained from the quadratic programming solution.

9. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The parameter adaptive module includes: When the contact state is identified as a contact approaching state or a stable contact state, the force response data acquisition program is started to continuously acquire six-dimensional force data and organize it into a force response curve. The force response curve is then processed with zero mean and bandpass filtered. The least squares parameter identification method is used to extract contact stiffness and damping characteristics. A linear regression relationship is established between the contact force response data and the velocity and position data. The contact stiffness coefficient and contact damping coefficient are solved and normalized. A fully connected feedforward neural network is used for material classification. The input layer corresponds to stiffness and damping features, and the output layer uses the Softmax activation function to output the probability distribution of each material category. The category with the highest probability is selected as the material category to be identified. A material parameter database is pre-established to store recommended force control parameters for each material category. The corresponding force control parameters are queried based on the identified material category. The parameter updates adopt a progressive method, using linear interpolation to transition over multiple consecutive control cycles.

10. A micro-manipulation end effector system integrating multi-dimensional micro-force sensing and visual servoing according to claim 1, characterized in that, The motion control module includes: The Jacobian matrix method is used to convert Cartesian space velocity commands into joint velocity vectors. For near-singular configurations, the damped least squares method is used to solve the problem. The velocity of each joint is checked to see if it exceeds the physical limit and is scaled accordingly. A numerical integration method is used to generate joint position control commands, and a dual-loop control structure for position and velocity is constructed. The position control loop calculates the joint position error and generates the desired velocity, while the velocity control loop applies a proportional-integral control law to the velocity error to generate joint torque commands. The six-dimensional force data is filtered out by a high-pass filter to remove the low-frequency static contact force component. The root mean square value of the signal is calculated to determine whether vibration exists. Fast Fourier transform is used for spectrum analysis to identify the main frequency component. Based on the vibration signal, the vibration velocity is estimated, and the reverse damping force is calculated to be equal to the negative damping coefficient multiplied by the vibration velocity. The damping coefficient is adaptively adjusted according to the vibration amplitude and frequency. The reverse damping force is converted into joint space damping torque and superimposed on the joint torque command.

Images