A robot multi-modal fusion control method and device based on contact perception

Abstract

The application discloses a robot multimodal fusion control method and device based on contact sensing, and specifically comprises: a visual acquisition unit that acquires image information of an operation area; a depth auxiliary structure unit, the depth auxiliary structure adopts a monocular camera, a fisheye lens, and a plane mirror symmetrically arranged with the monocular camera, forms multi-view geometric constraints under the condition of a single camera by expanding the field of view and introducing a reflection view angle; a force / tactile acquisition unit that acquires physical interaction information in contact with an operation object; a synchronous acquisition and processing unit that uniformly acquires and data fusion processes data from the visual acquisition unit and the force / tactile acquisition unit, generates a motion control instruction for controlling the robot based on the fusion processing result, so as to realize operation control based on contact sensing.

Description

Technical Field

[0001] This invention relates to the field of robot perception and intelligent control technology, and mainly to a robot multimodal fusion control method and device based on contact perception. Background Technology

[0002] As robots evolve towards higher precision, greater flexibility, and greater adaptability to different scenarios, hand-based perception has progressed from single-modal perception to multi-modal fusion, with vision-inertial (VIO) fusion being the current mainstream technology. Typical industry solutions and their current status are as follows: 1. Vision-inertial fusion technology: Based on "camera + IMU", localization is achieved through EKF or BA algorithms, such as VINS-Mono, MSCKF and other tightly coupled algorithms, which have been widely used in robot localization and mapping, but rely on good visual observation conditions; 2. Tactile / force sensing applications: Tactile sensors (such as Tekscan Flexiforce) detect contact pressure, and six-dimensional force sensors (such as ATI Mini45) control grasping force, but they are mostly used as independent feedback signals and are not integrated with positioning. 3. Bottlenecks in depth perception solutions: High cost of binocular cameras (≥5000 RMB per unit), small field of view (≤ Monocular depth estimation relies on texture, with errors often ranging from 5% to 6%, making it difficult to balance low cost and high accuracy. 4. Multimodal fusion trend: Recent studies have confirmed that tactile and visual-inertial fusion can significantly improve the success rate of operations. For example, the OmniVTLA model, through visual-tactile fusion, increased the success rate of two-finger gripping from 75% to 96.9%.

[0003] The existing technology has the following quantifiable defects: 1. Poor robustness in occluded / low-texture scenes: Relying solely on visual-IMU fusion, visual feature points are lost when the field of view is occluded or texture is insufficient, and the positioning drift reaches 5-8cm within 10 seconds, which cannot meet the requirements of continuous operation. 2. Low cost-effectiveness of depth perception: Monocular depth estimation error is 5%-6%, while binocular cameras are expensive and have limited field of view, making it difficult to cover the entire hand operation area; 3. Loose and unfused framework of multimodal information: The tactile / force sensors (if configured) only output independent signals and do not participate in localization fusion, resulting in a disconnect between "localization" and "operation", and smooth objects are easy to slip when grasped; 4. Insufficient time synchronization accuracy: The synchronization error of software timestamp alignment is ≥5ms. The misalignment of multimodal data leads to the loss of fusion accuracy, which especially affects pose estimation in dynamic scenes. 5. Force / tactile sensation not converted into observation constraints: Mathematical models of tactile-pose and force-environment interaction have not been established, making it impossible to convert contact information into positioning correction quantities, resulting in weak dynamic operation posture fine-tuning capabilities. Summary of the Invention

[0004] To address the above shortcomings, this invention effectively controls positioning drift in occluded and low-texture scenes by fusing force-tactile and vision-IMU, overcoming the bottleneck of vision dependence; it adopts a "single camera + mirror" scheme to control hardware costs and reduce depth errors, achieving low-cost, high-precision depth; it achieves high-precision synchronization of multimodal data, solving the data misalignment problem and constructing a high-precision synchronization mechanism; it converts force-tactile signals into positioning observations, improving the grasping success rate and establishing a multimodal fusion framework. According to a first aspect of this invention, a multimodal fusion control device for robots based on contact perception is proposed, specifically comprising: The visual acquisition unit acquires image information of the operating area; A depth-assisted structural unit includes a monocular camera, a fisheye lens, and a plane mirror arranged symmetrically with the monocular camera. The fisheye lens expands the imaging field of view of the monocular camera, and the plane mirror introduces the reflection angle of the operating area into the monocular camera. The monocular camera receives multi-view image information formed by the expanded field of view and the reflection angle, and constructs multi-view geometric constraints. Force / tactile sensing unit acquires physical interaction information of contact with the object being manipulated; The synchronous acquisition and processing unit performs unified acquisition and data fusion processing on the data from the vision acquisition unit and the force / tactile acquisition unit, and generates motion control commands for controlling the robot based on the fusion processing results, so as to realize operation control based on contact perception.

[0005] Preferably, the multi-view geometric constraints are specifically parameter calibration, which includes camera-fisheye-mirror joint calibration, specifically including: solving for camera intrinsic distortion parameters based on the fisheye imaging model, wherein the model calculation of the relationship between the incident angle θ of the spatial point in the camera coordinate system and the imaging radius r is as follows: ; in, These are the radial distortion coefficients, r( ) represents the imaging radius; the parameter is solved by minimizing the reprojection error objective function.

[0006] Preferably, the parameter calibration further includes specular reflection geometry calibration, which is achieved by establishing the equation of the mirror plane. Given Ax + By + Cz + D = 0, and based on the planar mirror symmetry relationship, the real space point Preal and the virtual image point Pvirtual are compared with the plane. Symmetric, the mapping relationship is expressed as: ; The reflection transformation matrix H is determined by the mirror normal vectors A, B, and C and the bias D.

[0007] Preferably, the parameter calibration further includes IMU and camera time synchronization calibration, which is achieved by calculating the inertial angular velocity sequence. Compared with the angular velocity sequence estimated by the visual optical flow method The cross-correlation function determines the time deviation. t, the specific formula is: ; in, Represents the cross-correlation function. This represents the delay parameter, and t represents the time variable.

[0008] Preferably, the depth-assisted structural unit needs to resample the original fisheye image to remove geometric distortion. Specifically, this includes distortion correction mapping, which involves constructing an inverse mapping function to map the corrected pixel coordinates to the original distorted image coordinates, and using bilinear interpolation to obtain the grayscale value of the corrected image. The mapping relationship satisfies the following formula: ; in, Indicates bilinear interpolation. Represents grayscale value, Represents the inverse distortion mapping function. This represents the grayscale value of the original distorted image.

[0009] Preferably, the synchronous acquisition and processing unit establishes a unified spatial coordinate reference and time reference; a unified trigger source synchronously triggers the visual sensor within a preset acquisition period, and assigns microsecond-level timestamps to the inertial, tactile and mechanical sensing data, so that the data of each modality are acquired under the same time reference.

[0010] Preferably, the synchronous acquisition and processing unit further includes constructing a multimodal observation model to process the observation data and generate standardized multimodal observation data. The standardized multimodal observation data uses a hierarchical fusion algorithm to publish the fusion results in real time through a standard ROS interface.

[0011] Preferably, the multimodal observation model includes a visual observation model, an inertial observation model, a mechanical observation model, and a tactile observation model; when the total pressure exceeds a preset threshold, the tactile observation model calculates the contact point location based on the pressure distribution, using the following specific calculation formula: ; in, This represents the pressure value measured at the k-th contact point in the haptic array. This indicates the position of the contact point in the sensor coordinate system. , This represents the position coordinates of the k-th sensor. It represents the reference position in the world coordinate system.

[0012] Preferably, the mechanical observation model constructs residual terms through torque balance relationships, and its residual expression is: ; in, Represents the residual vector. Indicates contact force. Indicates contact force The generated torque This indicates the location of the reference point for torque calculation. Indicates the location of the contact point. This represents the rotation matrix of the machine's coordinate system.

[0013] Preferably, the hierarchical fusion algorithm performs joint state estimation on each observation model and outputs the system pose, contact state, and mechanical sensing results.

[0014] Preferably, the hierarchical fusion algorithm includes an online filtering layer and a back-end optimization layer, wherein the online filtering layer uses an extended Kalman filter for state prediction and updating, and its update formula is: ; in, Indicates Kalman gain, This represents the prior error covariance. Represents the Jacobian matrix. This represents the measurement noise covariance. This represents the state at time k predicted based on time k-1. This indicates the updated status after fusion measurement. This represents the sensor's measurement value at time k. This represents the expected measurement value calculated based on the predicted state.

[0015] The backend optimization layer constructs a joint objective function within a sliding window and solves for the optimal state using a nonlinear least squares method. Its objective function is: ; in, This represents the optimal state estimate. This indicates finding the parameter that minimizes x. This represents the visual reprojection residual. This represents the IMU pre-integration residual. Indicates tactile residuals, Indicates the residual of force perception. This represents the squared Mahalanobis distance. Represents the visual measurement covariance matrix. Represents the covariance matrix of tactile measurements. Let represent the force measurement covariance matrix, i,j represent the summation index, k represent the IMU segment summation index, m represent the tactile measurement summation index, and n represent the force measurement summation index.

[0016] According to a second aspect of the present invention, a robot based on a multimodal fusion control device is provided. The multimodal fusion control device based on contact perception is installed on the actuator at the end of the robot body. The synchronous acquisition and processing unit is used to control the robot body or the actuator to perform contact operations related to the operation object based on the fusion processing result of the multimodal sensing data.

[0017] According to a third aspect of the present invention, a multimodal fusion control method for robots based on contact perception is proposed, characterized in that it specifically includes: Acquire image information of the operation area; A depth-aid structure is constructed, comprising a monocular camera, a fisheye lens, and a plane mirror arranged symmetrically with the monocular camera. The fisheye lens expands the imaging field of view of the monocular camera, and the plane mirror introduces the reflection angle of the operating area into the monocular camera. The monocular camera receives multi-view image information formed by the expanded field of view and the reflection angle, and constructs multi-view geometric constraints. Obtain physical interaction information with the object being manipulated; Data from the vision acquisition unit and the force / tactile acquisition unit are uniformly acquired and fused. Based on the fusion processing results, motion control commands for controlling the robot are generated to achieve operation control based on contact perception.

[0018] According to a fourth aspect of the present invention, a computer program product is provided, on which one or more computer programs are stored, which, when executed by a computer processor, implement the method described above.

[0019] The above-described one or more technical solutions in the embodiments of this application have at least one of the following technical effects: Significantly improved localization robustness: Localization drift in occluded and low-texture scenes is greatly optimized, outperforming existing technologies; posture error is significantly reduced when grasping low-texture objects, resulting in greater accuracy.

[0020] Optimized cost-effectiveness of depth perception: The "camera + mirror" solution effectively controls hardware investment and has a greater cost advantage; the depth perception accuracy is better than the traditional monocular solution, achieving a balance between low cost and high accuracy.

[0021] Synchronization accuracy meets standards: Hardware synchronization is adopted, which greatly reduces synchronization error and significantly improves fusion accuracy compared to software synchronization; dynamic hand speed measurement is more accurate, and error control is effective.

[0022] Improved success rate: After force-tactile signals are involved in multimodal fusion, the success rate of grasping is greatly improved, especially for grasping smooth objects, which is close to the advanced level in the industry.

[0023] Strong scenario adaptability: No need to modify the hardware structure, it can be adapted to a variety of application scenarios simply by adjusting the calibration parameters; the adaptation process is more convenient and the adaptation-related costs are greatly reduced. Attached Figure Description

[0024] The accompanying drawings are included to provide a further understanding of the embodiments and are incorporated in and constitute a part of this specification. The drawings illustrate embodiments and, together with the description, serve to explain the principles of the invention. Other embodiments and many anticipated advantages of the embodiments will be readily recognized as they become better understood through reference to the following detailed description. Elements in the drawings are not necessarily to scale. The same reference numerals refer to corresponding similar parts.

[0025] Figure 1 A framework diagram of a contact-sensing-based robot multimodal fusion control device according to an embodiment of the present invention is shown.

[0026] Figure 2 A relative positional frame diagram of a depth-assisted structural device according to an embodiment of the present invention is shown.

[0027] Figure 3 An optical path diagram of a depth-assisted structure device according to an embodiment of the present invention is shown.

[0028] Figure 4 A flowchart of a contact-sensing-based multimodal fusion control method for robots according to an embodiment of the present invention is shown.

[0029] Figure 5 A schematic diagram of a multimodal data preprocessing flow according to an embodiment of the present invention is shown.

[0030] Figure 6 This is a schematic diagram of the structure of a computer system suitable for implementing the electronic devices of the present application embodiments. Detailed Implementation

[0031] The present application will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings.

[0032] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0033] Figure 1 A framework diagram of a contact-sensing-based robot multimodal fusion control device according to an embodiment of the present invention is shown, including: a vision acquisition unit 101, a depth auxiliary structure unit 102, a force / tactile acquisition unit 103, and a synchronous acquisition and processing unit 104. The vision acquisition unit 101 acquires actual image information of the operating area, and the depth auxiliary structure unit 102 performs multi-view geometric constraints for the vision acquisition, such as... Figure 2 As shown, the depth-assisting structural unit 102 includes a monocular camera 201, a fisheye lens 202, and a planar mirror 203 arranged symmetrically with the monocular camera. The fisheye lens 202 expands the imaging field of view of the monocular camera 201, and the planar mirror 203 introduces the reflection angle of the operating area into the monocular camera 201. The monocular camera 201 receives multi-view image information formed by the expanded field of view and the reflection angle, constructing multi-view geometric constraints by expanding the field of view and introducing the reflection angle, controlling hardware costs and reducing depth errors, such as... Figure 3 The diagram illustrates a 155° ultra-wide-angle optical path according to an embodiment of the present invention. It includes a main camera, left and right virtual cameras, and reflectors for light reflection. The reflectors reflect light from the left and right sides to the main camera. Combined with the left and right virtual cameras, they collectively cover a 155° ultra-wide-angle field of view, thereby achieving large-angle image acquisition. Furthermore, physical interaction information is acquired by the force / tactile sensing unit 103, and finally, a unified clock acquisition, data modeling, and data fusion processing are performed by the synchronous acquisition and processing unit 104. Based on the fusion processing results, motion control commands for controlling the robot are generated to achieve contact-based operation control.

[0034] like Figure 4 The diagram illustrates a flowchart of a contact-sensing-based multimodal fusion control method for robots according to an embodiment of the present invention. The specific steps are as follows: S1. A monocular camera, a fisheye lens, and a plane mirror arranged symmetrically with the monocular camera are used. The fisheye lens expands the imaging field of view of the monocular camera, and the plane mirror introduces the reflection angle of the operating area into the monocular camera. The monocular camera receives multi-view image information formed by the expanded field of view and the reflection angle, constructs multi-view geometric constraints, and forms multi-view geometric constraints under single-camera conditions by expanding the field of view and introducing the reflection angle, thereby obtaining the depth information of the operating area. This step primarily involves establishing a unified spatiotemporal reference for the multi-sensor system and calibrating the intrinsic and extrinsic parameters of each sensor to eliminate system errors caused by hardware manufacturing and assembly. The specific implementation process is divided into three sub-steps: joint calibration of the vision system, time synchronization calibration, and force / tactile sensor calibration. These include: S1.1 Camera-fisheye-mirror joint calibration uses a high-precision checkerboard calibration plate with a specification of 9×12 grids and a grid spacing of 20mm. Keeping the camera fixed, move the calibration plate to cover the center and edge areas of the camera's field of view, and tilt and rotate it at different angles to acquire at least 20 clear images of the calibration plate.

[0035] Solving for camera-intrinsic distortion parameters based on a fisheye imaging model specifically includes: Let the coordinates of a point P(X,Y,Z) in space be given by the camera coordinate system. Define its incident angle θ and azimuth angle. as follows: ; To accurately fit the distortion, the imaging radius... An odd-degree polynomial model is used for approximation, and the specific formula is as follows: ; in, These are the radial distortion coefficients, r( ) represents the imaging radius; the parameter is solved by minimizing the reprojection error objective function.

[0036] Further introduction of tangential distortion correction terms and ,in Tangential distortion coefficient: ; Finally, image pixel coordinates With camera intrinsic parameter matrix (including focal length) and the main point The relationship between ) is represented as: ; The optimal parameter set is calculated by minimizing the reprojection error using the Levenberg-Marquardt algorithm. .

[0037] The parameters are solved by minimizing the reprojection error objective function, specifically as follows: ; in, Represents pixel observation value, Represents the coordinates of a point in space. This represents the set of parameters to be estimated.

[0038] S1.2 Specular reflection geometry calibration: Within the effective field of view of the specular reflection area, arrange... A set of infrared markers with known precise spatial coordinates, denoted as . , , The virtual image point after reflection by the mirror is captured by the camera. And extract its pixel coordinates from the image. Specific steps include: establishing the equation of the mirror plane. : Ax + By + Cz + D = 0, where, Let be a unit normal vector, satisfying Furthermore, based on the planar mirror symmetry relationship, the real space point Preal and the virtual image point Pvirtual are compared with each other across the plane. Symmetric, the mapping relationship is expressed as: ; The reflection transformation matrix H is determined by the mirror normal vectors A, B, and C and the bias D.

[0039] Wherein, the reflection transformation matrix The specific form is: ; Using the calibrated camera intrinsic parameters, the virtual image points in the image are back-projected into rays, combined with known real points. Coordinates, construct a system of geometric constraint equations and solve them. This allows us to determine the path of the reflected light.

[0040] S1.3. IMU and camera time synchronization calibration: The IMU and camera time synchronization calibration is performed using a rotary table method. The IMU and camera are rigidly connected to a precision rotary table, which is then controlled to perform a reciprocating sinusoidal motion at a frequency of approximately 0.5Hz. The specific steps are as follows: Collect the angular velocity sequence output by the IMU respectively and the camera angular velocity sequence calculated by visual optical flow method Assume there is a constant time delay between the two. That is, satisfying the relation .

[0041] By calculating the cross-correlation function of the two sequences. Determine the optimal time alignment point: ; in, Represents the cross-correlation function. This represents the delay parameter, and t represents the time variable.

[0042] Time Deviation That is, the value of the independent variable that maximizes the cross-correlation function: ; The calibrated time deviation accuracy is controlled within Within this range, and real-time compensation is performed in subsequent data collection stages.

[0043] S1.4, Tactile sensor calibration: For the tactile sensor array, apply a standard set of weights. Record the corresponding voltage response value for the standard pressure within the specified range.

[0044] To address the nonlinearity issue in tactile materials, a quadratic fitting model of "pressure-voltage" is established. Let... For output voltage, For calibrated pressure: ; Solve the coefficient vector using the least squares method. The objective function is to minimize the sum of squared residuals, which is expressed by the following formula: ; S1.5 Force sensor calibration: Apply standard loads to the force sensor using an ATI calibration stage, covering six degrees of freedom. Specifically, this includes: Establish the original signal vector of the sensor With the true force / torque vector Linear decoupling model between them: ; in, Here is the stiffness decoupling matrix. It is a zero-biased vector.

[0045] Solving the matrix through calibration experiments And verify its full-scale error: ; Here, FS represents the full-scale value, and this stiffness matrix will be written into the underlying driver of the controller for real-time force data calculation.

[0046] S2. The vision sensor is synchronously triggered by a unified trigger source within a preset acquisition period, and microsecond-level timestamps are assigned to the inertial, tactile and mechanical sensing data so that the data of each modality are acquired under the same time reference. Hardware-triggered synchronous acquisition and control; using an FPGA or MCU as the main controller to generate a unified clock reference. The acquisition frequency is set to... (For example ).

[0047] The main controller sends a trigger signal at the beginning of each cycle: 1. Send a hard trigger pulse to the industrial camera to control the start of exposure.

[0048] 2. Simultaneously record the global timestamp at that moment. .

[0049] 3. Considering that IMUs and force sensors typically operate at higher frequencies (e.g., The system employs a "nearest neighbor search + interpolation" strategy for autonomous output, buffering the time before and after this moment. Non-visual data within. The specific algorithm is as follows: The timestamp of the image frame is Combined with the time deviation calibrated in step 1 Corrected image acquisition time for: ; The system will perform linear interpolation on the high-frequency IMU data near this correction time to obtain an angular velocity that is strictly aligned with the image. and acceleration .

[0050] By employing a step-by-step calibration mechanism, unified spatial coordinate and time references are established for vision, inertial, and force or tactile sensors, respectively. During the data acquisition phase, a hardware-triggered synchronization control and time deviation compensation mechanism is introduced, enabling data from different modalities to be processed under the same time axis and spatial reference system. This effectively eliminates systematic deviations caused by inconsistent sensor sampling frequencies, communication delays, and installation errors, and avoids time mismatch and spatial drift problems during the fusion of multimodal data, thereby improving the reliability and consistency of the fused input data from the data source.

[0051] S3, Standardized multimodal observation data, such as Figure 5As shown, it includes: visual data processing 501, inertial data processing 502, and tactile / force data processing 503; specifically, it includes: using a fisheye imaging model to correct wide-angle distortion, and combining the planar geometric relationship of the mirror to compensate for the reflection imaging path, and mapping the reflection image to observation data under a unified virtual camera model; and performing filtering preprocessing on the force / tactile sensing data.

[0052] S3.1 Image distortion correction and mirror ROI mapping, using the intrinsic parameters and distortion coefficients obtained in step S1. The original fisheye image is resampled to remove geometric distortion. Simultaneously, based on the mirror plane equation... Extract the effective pixels from the reflected area. The specific algorithm is as follows: S3.1.1, Distortion Mapping: Construct from corrected image coordinates coordinates of the original distorted image inverse mapping function For each corrected pixel, calculate its normalized planar coordinates. And introduce a distortion model: ; This allows us to obtain the original pixel positions. The grayscale value of that point is obtained through bilinear interpolation. : ; in, Indicates bilinear interpolation. Represents grayscale value, Represents the inverse distortion mapping function. This represents the grayscale value of the original distorted image.

[0053] S3.1.2, Mirror Virtual Image Projection: For pixels within the mirror area, they should be considered as images from a "virtual camera." Let the optical center of the virtual camera be... Its actual optical center Regarding mirrors The formula for calculating the symmetric point is: ; in This allows subsequent algorithms to directly process reflected images based on the virtual camera model, eliminating the need for repeated optical path inversion.

[0054] S3.2 Utilizing high-frequency IMU data (angular velocity) between two frames and acceleration The calculation system performs IMU pre-integration and pose update based on the pose increment of the current frame relative to the previous frame. The specific algorithm is as follows: Quaternions are used for attitude updates to avoid gimbal deadlock.

[0055] set up The attitude quaternion at time t is In tiny time intervals Inside, using angular velocity measurements Update your stance.

[0056] First, eliminate gyroscope bias. : ; Constructing angular velocity quaternion increments : ; Attitude update formula (using quaternion multiplication) ): ; For acceleration data, rotate it to the world coordinate system and subtract the gravity vector. The acceleration of motion is obtained: ; in This is the rotation matrix corresponding to the quaternion.

[0057] S3.3. To address the high-frequency electromagnetic noise present in the analog signals acquired by the tactile and force sensors, a digital low-pass filter is used for preprocessing, specifically including: The method employs a first-order recursive low-pass filter, which has low computational cost and is suitable for embedded real-time processing.

[0058] set up The original sampled value at time 1 is The filtered value is The filter coefficients are ( ): ; Among them, the filter coefficients From the cutoff frequency and sampling period Decide: ; For the six-dimensional force sensor data, the stiffness matrix obtained in step S1 also needs to be substituted. Converting voltage signals into physical force values: ;

[0059] After the above processing, the system outputs a multimodal data stream with time alignment, geometric distortion correction, and unified physical dimensions. .

[0060] S4. Based on the standardized multimodal observation data, construct a visual observation model, an inertial observation model, a tactile observation model, and a mechanical observation model respectively; After acquiring synchronized data, the system constructs a rigorous mathematical observation model based on the data characteristics of four different modalities: visual, inertial, tactile, and mechanical, providing residual constraints for subsequent state estimation.

[0061] S4.1 Construct a visual observation model and use the Kannala-Brandt parameters calibrated in step S1 to perform distortion correction on the fisheye image. Extract ORB (Oriented Fast and Rotated BRIEF) feature points and match them across consecutive frames. For specular reflection areas, combine the reflection geometry model and recover the 3D spatial coordinates of the feature points through triangulation. Specifically, this includes: Let the feature point in the world coordinate system be... The observed pixel coordinates in the current camera coordinate system are .

[0062] First, through camera pose Transform the point to the camera coordinate system: ; Based on the pinhole projection model (after distortion correction), the observation equation Defined as: ; in, Use Gaussian white noise and set the standard deviation. .

[0063] S4.2 Construct an inertial observation model, pre-integrate the raw data from the accelerometer and gyroscope, and calculate the two frames of images ( Frame and The relative pose changes, velocity changes, and zero-bias updates between frames are used to compress high-frequency IMU data into low-frequency constraints. Specifically, this includes: Using the median integration method, let the IMU measurement value be... and The pre-integral quantity (relative motion) is defined as follows: Relative rotation increment: ,in, This represents the angular velocity measured at time t. The oblique-symmetric matrix function representing angular velocity. Indicates the instantaneous rotation increment. This indicates that the gyroscope has zero bias. Indicates within the time interval [ , Points are awarded.

[0064] Relative velocity increment: ,in, This represents the raw measurement value of the accelerometer. R represents the zero bias of the accelerometer, and R represents the rotation matrix.

[0065] Relative displacement increment: .

[0066] Model setting angular velocity noise Acceleration noise .

[0067] S4.3 Construct a tactile observation model and set a pressure threshold. When total pressure is detected When the contact point is reached, it is determined to be in a contact state. The location of the contact point is estimated using the centroid method (CoP) of the pressure distribution of the sensor array. Specifically, this includes: Let the tactile array be the first The coordinates of each contact point are The measured pressure value is The position of the contact point in the sensor coordinate system. The calculation is as follows: ; in, This represents the pressure value measured at the k-th contact point in the haptic array. This indicates the position of the contact point in the sensor coordinate system. , This represents the position coordinates of the k-th sensor. It represents the reference position in the world coordinate system.

[0068] The standard deviation of the observed noise is set to .

[0069] S4.4 Construct a force observation model and set the trigger threshold as force. or torque When the conditions are met, contact dynamics constraints are triggered. A Jacobian matrix is ​​established to correlate the measured values ​​with the hand's end-effector pose, used to correct the pose. Specifically, this includes: Assuming the contact point is a rigid connection, the contact force The generated torque With contact point position vector Related: ; Constructing mechanical residual equations : ; in, Represents the residual vector. Indicates contact force. Indicates contact force The generated torque This indicates the location of the reference point for torque calculation. Indicates the location of the contact point. This represents the rotation matrix of the machine's coordinate system.

[0070] Setting the standard deviation of force observation noise .

[0071] S5. A hierarchical fusion strategy is adopted to perform joint state estimation on each observation model, and output the system pose, contact state and mechanical sensing results.

[0072] S5.1, Online Layer: High-frequency state estimation is performed using the Extended Kalman Filter (EKF), specifically including: S5.1.1, State Vector: ; S5.1.2 Prediction Steps: Using IMU state equations Make a prediction: ; in Here is the state transition matrix. Let be the process noise covariance.

[0073] S5.1.3, Update Steps: Calculate Kalman gain And update the status: ; in, Indicates Kalman gain, This represents the prior error covariance. Represents the Jacobian matrix. This represents the measurement noise covariance. This represents the state at time k predicted based on time k-1. This indicates the updated status after fusion measurement. This represents the sensor's measurement value at time k. This represents the expected measurement value calculated based on the predicted state.

[0074] S5.1.4 Dynamic Weight Adjustment: Calculate the observation residuals: .

[0075] Set threshold If visual residual : ; The system stability is maintained by increasing the visual noise covariance (reducing the weight) and decreasing the force / tactile noise covariance (increasing the weight).

[0076] During the online filtering process, the observation noise covariance matrix of different modes is dynamically adjusted according to the magnitude of the observation residuals in order to change the weight ratio of each mode in the state update.

[0077] S5.2, Backend layer: Maintain a sliding window containing 20 keyframes, construct a factor graph for joint optimization, specifically including: Construct the overall objective function Find the optimal state Minimize the sum of weighted Mahalanobis distances: ; in, This represents the optimal state estimate. This indicates finding the parameter that minimizes x. This represents the visual reprojection residual. This represents the IMU pre-integration residual. Indicates tactile residuals, Indicates the residual of force perception. This represents the squared Mahalanobis distance. Represents the visual measurement covariance matrix. Represents the covariance matrix of tactile measurements. Let represent the force measurement covariance matrix, i,j represent the summation index, k represent the IMU segment summation index, m represent the tactile measurement summation index, and n represent the force measurement summation index.

[0078] Solve iteratively using the Levenberg-Marquardt (LM) algorithm: ; in It is an approximate Hessian matrix. After optimization, drift can be controlled within 10 seconds. .

[0079] S6. The system finally publishes the fusion results in real time through the standard ROS (Robot Operating System) interface.

[0080] The system encapsulates the data to Frequently publish Topic messages, including pose, contact status, and force data.

[0081] Data format specifications: 1) High-precision hand pose: Publish message type `geometry_msgs / PoseStamped`.

[0082] ; 2) Full access to status information: Custom message type `ContactState`: `bool is_contact`: Contact determination (True / False).

[0083] `geometry_msgs / Point contact_point`: 3D coordinates in the world frame.

[0084] `float32 pressure_value`: Scalar pressure value.

[0085] 3) Six-dimensional force / torque data: Publish message type `geometry_msgs / WrenchStamped`.

[0086] Output the actual contact force after gravity compensation: ; in For end-load quality, This is the gravity vector.

[0087] Through the above processing and fusion, the pose, contact state and mechanical data finally output by the present invention are strictly aligned in time, maintain a unified coordinate description in space, and have consistent physical dimensions, which can be directly used for downstream control, interaction or decision-making modules.

[0088] Compared to traditional fusion methods based on a single modality or simple time alignment, this invention can significantly reduce pose drift error and contact judgment error under long-term operation and complex contact scenarios, thereby improving the overall perception accuracy and operational stability of the system.

[0089] Based on the data fusion method described above, according to another aspect of the present invention, a series of hardware device structures adapted to robot control are provided, and the modules are shown in Table 1 below: Table 1 Hardware Module Devices

[0090] Module connection relationship: 1. The synchronous control module triggers the camera via the SPI interface, connects to the IMU via I2C, connects to the force sensor via the CAN bus, and connects to the tactile sensor via analog input; 2. The data processing module receives the "timestamp-sensor data" integrated packet via Ethernet; 3. The relative positions of the mirror, camera, and grippers are calibrated at the factory to ensure that the reflected image covers the operating area.

[0091] The hardware device of this invention achieves multi-view depth perception under low-cost conditions by introducing a combination structure of a fisheye lens and a symmetrical reflector under single-camera conditions. At the same time, it achieves high-precision synchronous acquisition of multimodal sensing data through a unified hardware triggering and timestamp distribution mechanism. Furthermore, by integrating vision, inertial, tactile, and mechanical sensors into a unified spatiotemporal device, it significantly improves the perception stability and reliability in complex operation and contact scenarios. Based on the above fusion processing results, motion control commands for controlling the robot are generated to achieve contact-based operation control.

[0092] The following is for reference. Figure 6 It shows a schematic diagram of the structure of a computer system 600 suitable for implementing electronic devices according to embodiments of the present application. Figure 6 The electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.

[0093] like Figure 6 As shown, the computer system 600 includes a central processing unit (CPU) 601, which can perform various appropriate actions and processes based on programs stored in read-only memory (ROM) 602 or programs loaded from storage section 608 into random access memory (RAM) 603. The RAM 603 also stores various programs and data required for the operation of the system 600. The CPU 601, ROM 602, and RAM 603 are interconnected via a bus 604. An input / output (I / O) interface 605 is also connected to the bus 604.

[0094] The following components are connected to I / O interface 605: an input section 606 including a keyboard, mouse, etc.; an output section 607 including a liquid crystal display (LCD) and speakers, etc.; a storage section 608 including a hard disk, etc.; and a communication section 609 including a network interface card such as a LAN card and a modem, etc. The communication section 609 performs communication processing via a network such as the Internet. A drive 610 is also connected to I / O interface 605 as needed. A removable medium 611, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on drive 610 as needed so that computer programs read from it can be installed into storage section 608 as needed.

[0095] Specifically, according to embodiments of this disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of this disclosure include a computer program product comprising a computer program carried on a computer-readable storage medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via communication section 609, and / or installed from removable medium 611. When the computer program is executed by central processing unit (CPU) 601, it performs the functions defined in the methods of this application. It should be noted that the computer-readable storage medium of this application can be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. The computer-readable storage medium can be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: electrical connections having one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this application, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in connection with an instruction execution system, apparatus, or device. In this application, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals can take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium can also be any computer-readable storage medium other than a computer-readable storage medium that can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. Program code contained on a computer-readable storage medium may be transmitted using any suitable medium, including but not limited to: wireless, wire, optical fiber, RF, etc., or any suitable combination thereof.

[0096] Computer program code for performing the operations of this application can be written in one or more programming languages ​​or a combination thereof. Programming languages ​​include object-oriented programming languages—such as Java, Smalltalk, and C++—as well as conventional procedural programming languages—such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0097] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0098] The modules described in the embodiments of this application can be implemented in software or in hardware.

[0099] In another aspect, this application also provides a computer-readable storage medium, which may be included in the electronic device described in the above embodiments; or it may exist independently and not assembled into the electronic device. The aforementioned computer-readable storage medium carries one or more programs, which, when executed by the electronic device, cause the electronic device to: a vision acquisition unit, acquiring image information of the operating area; a depth-assisted structure unit, wherein the depth-assisted structure employs a monocular camera, a fisheye lens, and a plane mirror symmetrically arranged with the monocular camera, thereby expanding the field of view and introducing a reflection angle to form multi-view geometric constraints under single-camera conditions; a force / tactile acquisition unit, acquiring physical interaction information of contact with the operating object; and a synchronous acquisition and processing unit, performing unified acquisition and data fusion processing on the data from the vision acquisition unit and the force / tactile acquisition unit, and generating motion control commands for controlling the robot based on the fusion processing results to achieve contact-based operation control.

[0100] The above description is merely a preferred embodiment of this application and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in this application is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features with similar functions disclosed in this application.

Claims

1. A multimodal fusion control device for robots based on contact perception, characterized in that, include: The visual acquisition unit acquires image information of the operating area; A depth-assisted structural unit includes a monocular camera, a fisheye lens, and a plane mirror arranged symmetrically with the monocular camera. The fisheye lens expands the imaging field of view of the monocular camera, and the plane mirror introduces the reflection angle of the operating area into the monocular camera. The monocular camera receives multi-view image information formed by the expanded field of view and the reflection angle, and constructs multi-view geometric constraints. Force / tactile sensing unit acquires physical interaction information of contact with the object being manipulated; The synchronous acquisition and processing unit performs unified acquisition and data fusion processing on the data from the vision acquisition unit and the force / tactile acquisition unit, and generates motion control commands for controlling the robot based on the fusion processing results, so as to realize operation control based on contact perception.

2. The robot multimodal fusion control device according to claim 1, characterized in that, The multi-view geometric constraints are specifically parameter calibration, which includes camera-fisheye-mirror joint calibration. Specifically, this includes solving for camera intrinsic distortion parameters based on a fisheye imaging model. The model calculation of the relationship between the incident angle θ and the imaging radius r of a spatial point in the camera coordinate system is as follows: in, These are the radial distortion coefficients, r( ) represents the imaging radius; the parameter is solved by minimizing the reprojection error objective function.

3. The robot multimodal fusion control device according to claim 2, characterized in that, The parameter calibration also includes specular reflection geometry calibration, which is achieved by establishing the equation of the mirror plane. Given Ax + By + Cz + D = 0, and based on the planar mirror symmetry relationship, the real space point Preal and the virtual image point Pvirtual are compared with the plane. Symmetric, the mapping relationship is expressed as: The reflection transformation matrix H is determined by the mirror normal vectors A, B, and C and the bias D.

4. The robot multimodal fusion control device according to claim 2, characterized in that, The parameter calibration also includes IMU and camera time synchronization calibration, which is achieved by calculating the inertial angular velocity sequence. Compared with the angular velocity sequence estimated by the visual optical flow method The cross-correlation function determines the time deviation. t, the specific formula is: in, Represents the cross-correlation function. This represents the delay parameter, and t represents the time variable.

5. The robot multimodal fusion control device according to claim 1, characterized in that, The depth-assisted structural unit requires resampling of the original fisheye image to remove geometric distortion. Specifically, this includes distortion correction mapping, which involves constructing an inverse mapping function to map the corrected pixel coordinates to the original distorted image coordinates, and using bilinear interpolation to obtain the grayscale values ​​of the corrected image. The mapping relationship satisfies the following formula: in, Indicates bilinear interpolation. Represents grayscale value, Represents the inverse distortion mapping function. This represents the grayscale value of the original distorted image.

6. The robot multimodal fusion control device according to claim 1, characterized in that, The synchronous acquisition and processing unit establishes a unified spatial coordinate reference and time reference; a unified trigger source synchronously triggers the vision sensor within a preset acquisition period, and assigns microsecond-level timestamps to the inertial, tactile and mechanical sensing data, so that the data of each modality are acquired under the same time reference.

7. The robot multimodal fusion control device according to claim 1, characterized in that, The synchronous acquisition and processing unit also includes constructing a multimodal observation model to process the observation data and generate standardized multimodal observation data. The standardized multimodal observation data uses a hierarchical fusion algorithm to publish the fusion results in real time through the standard ROS interface.

8. The robot multimodal fusion control device according to claim 7, characterized in that, The multimodal observation model includes a visual observation model, an inertial observation model, a mechanical observation model, and a tactile observation model. When the total pressure exceeds a preset threshold, the tactile observation model calculates the contact point location based on the pressure distribution. The specific calculation formula is as follows: in, This represents the pressure value measured at the k-th contact point in the haptic array. This indicates the position of the contact point in the sensor coordinate system. , This represents the position coordinates of the k-th sensor. It represents the reference position in the world coordinate system.

9. The robot multimodal fusion control device according to claim 8, characterized in that, The mechanical observation model constructs residual terms through torque balance relationships, and its residual expression is as follows: in, Represents the residual vector. Indicates contact force. Indicates contact force The generated torque This indicates the location of the reference point for torque calculation. Indicates the location of the contact point. This represents the rotation matrix of the body coordinate system.

10. The robot multimodal fusion control device according to claim 7, characterized in that, The hierarchical fusion algorithm performs joint state estimation on each observation model and outputs the system pose, contact state, and mechanical sensing results.

11. The robot multimodal fusion control device according to claim 10, characterized in that, The hierarchical fusion algorithm includes an online filtering layer and a back-end optimization layer. The online filtering layer uses an extended Kalman filter for state prediction and updating, and its update formula is as follows: in, Indicates Kalman gain, This represents the prior error covariance. Represents the Jacobian matrix. This represents the measurement noise covariance. This represents the state at time k predicted based on time k-1. This indicates the updated status after fusion measurement. This represents the sensor's measurement value at time k. This represents the expected measurement value calculated based on the predicted state. The backend optimization layer constructs a joint objective function within a sliding window and solves for the optimal state using a nonlinear least squares method. Its objective function is: in, This represents the optimal state estimate. This indicates finding the parameter that minimizes x. This represents the visual reprojection residual. This represents the IMU pre-integration residual. Indicates tactile residuals, Indicates the residual of force perception. This represents the squared Mahalanobis distance. Represents the visual measurement covariance matrix. Represents the covariance matrix of tactile measurements. Let represent the force measurement covariance matrix, i,j represent the summation index, k represent the IMU segment summation index, m represent the tactile measurement summation index, and n represent the force measurement summation index.

12. A robot based on a multimodal fusion control device, characterized in that, The contact-sensing-based robot multimodal fusion control device as described in any one of claims 1-11 is installed on the actuator at the end of the robot body, wherein the synchronous acquisition and processing unit is used to control the robot body or the actuator to perform contact operations related to the operation object based on the fusion processing results of the multimodal sensing data.

13. A multimodal fusion control method for robots based on contact perception, characterized in that, Specifically, it includes: Acquire image information of the operation area; A depth-aid structure is constructed, comprising a monocular camera, a fisheye lens, and a plane mirror arranged symmetrically with the monocular camera. The fisheye lens expands the imaging field of view of the monocular camera, and the plane mirror introduces the reflection angle of the operating area into the monocular camera. The monocular camera receives multi-view image information formed by the expanded field of view and the reflection angle, and constructs multi-view geometric constraints. Obtain physical interaction information with the object being manipulated; Data from the vision acquisition unit and the force / tactile acquisition unit are uniformly acquired and fused. Based on the fusion processing results, motion control commands for controlling the robot are generated to achieve operation control based on contact perception.

14. The robot multimodal fusion control method according to claim 13, characterized in that, The multi-view geometric constraints are specifically parameter calibration, which includes camera-fisheye-mirror joint calibration, specular reflection geometric calibration, and IMU-camera time synchronization calibration.

15. The robot multimodal fusion control method according to claim 13, characterized in that, In the process of forming multi-view geometric constraints under single-camera conditions, it is necessary to resample the original fisheye image to remove geometric distortion. Specifically, this includes distortion correction mapping, which involves constructing an inverse mapping function to map the corrected pixel coordinates to the original distorted image coordinates, and using bilinear interpolation to obtain the grayscale values ​​of the corrected image. The mapping relationship satisfies the following formula: in, Indicates bilinear interpolation. Represents grayscale value, Represents the inverse distortion mapping function. This represents the grayscale value of the original distorted image.

16. The robot multimodal fusion control method according to claim 13, characterized in that, The unified acquisition and processing of the acquired data specifically includes: establishing a unified spatial coordinate reference and time reference; synchronously triggering the visual sensor by a unified trigger source within a preset acquisition period, and allocating microsecond-level timestamps to the inertial, tactile and mechanical sensing data, so that the data of each modality are acquired under the same time reference.

17. The robot multimodal fusion control method according to claim 13, characterized in that, The unified collection and processing of the acquired data also includes: constructing a multimodal observation model to process the observation data, generating standardized multimodal observation data, and using a hierarchical fusion algorithm to publish the fusion results in real time through a standard ROS interface.

18. A computer program product, characterized in that, It stores a computer program that, when executed by a processor, implements the method as described in any one of claims 13-17.

Images