A robot medicine taking control method fusing visual positioning and force feedback

Abstract

The application relates to the technical field of robot control, and discloses a robot medicine taking control method fusing visual positioning and force feedback, which constructs an orthogonal projection matrix by extracting a point cloud gap vector, decouples a resultant force screw into a leading detachment force and an environmental interference shear force, removes camera self-motion interference in combination with a kinematics Jacobian matrix to obtain a real physical slip vector, maps a slip feature to a non-diagonal stiffness matrix of an impedance controller when the interference force and the slip amount reach a threshold value, drives a mechanical arm to produce a posture deflection to break static friction jamming, extracts an interference force change rate by using a momentum observer, performs asymmetric expansion on a damping matrix when the force is negatively dropped to absorb elastic potential energy and inhibit rebounding vibration, and performs time integration on the interference force by using a dead zone and a limiting mechanism to output a position compensation amount to update an expected detachment track. The application effectively solves the problems of rigidity jamming, unloading vibration and secondary physical interference in a dense medicine taking scene.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically to a robot drug dispensing control method that integrates visual positioning and force feedback. Background Technology

[0002] Hospital automated pharmacies commonly deploy multi-joint robotic arms to perform medication box grasping tasks. The cardboard boxes of medicine on pharmacy shelves are typically stacked densely side-by-side. When the robotic arm grips and pulls the target medication box away, the sidewall of the target box inevitably experiences strong physical compression and frictional interference with adjacent boxes. This densely stacked environment places stringent demands on the contact mechanics sensing and compliant control of the robotic arm's end effector under complex constraints.

[0003] Most existing automated medication dispensing systems employ an eye-to-hand configuration, integrating a camera and a six-axis torque sensor at the end effector of the robotic arm. The underlying motion framework is generally based on Cartesian impedance control. The stiffness matrix of a conventional impedance controller is set as a diagonal matrix, with each spatial degree of freedom responding independently. After completing visual positioning, the robotic arm performs a withdrawal action along a pre-planned straight, desired trajectory. When the torque sensor detects that the lateral contact force exceeds a set safety threshold, the impedance controller drives the robotic arm to perform a simple linear retraction along the direction of the force, or directly triggers the shutdown protection mechanism of the underlying actuator to avoid damaging the medication cartridge. Some systems incorporate visual feedback to monitor the relative motion of the surrounding environment and combine this with the magnitude of the contact force for simple logic-based start / stop control.

[0004] In handling scenarios with densely stacked, strong interference, existing technologies often rely on conventional diagonal stiffness matrices that only provide linear compliance in a single dimension. This makes it difficult to actively change the normal constraint angle of the contact surface, leading to the robotic arm easily getting stuck in a rigid static friction state. The end-effector camera, adjusting its position with the robotic arm, generates a self-motion visual component, which directly masks the actual physical sliding characteristics of adjacent medicine boxes. Directly performing numerical difference on discrete mechanical signals amplifies high-frequency noise exponentially, causing serious misjudgments of the physical unloading state by the controller. Therefore, this invention provides a robotic medicine-retrieval control method that integrates visual positioning and force feedback to address the shortcomings of existing technologies. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a robotic drug retrieval control method that integrates visual positioning and force feedback. This solves the problem of existing robotic drug retrieval control technologies encountering physical interference and static friction jamming when removing medication from densely stacked automated pharmacy shelves. Traditional Cartesian space impedance control methods, where each degree of freedom is independent, can only produce linear yielding along the force direction, making it difficult to actively change the normal constraint of the contact surface to release the jamming state. Furthermore, when the jamming state is instantly broken and physical unloading occurs, the accumulated elastic potential energy in the system causes the end effector to rebound and oscillate.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a robot drug retrieval control method integrating visual positioning and force feedback, comprising the following steps: S10, acquire point cloud data of the target medicine box, extract the gap vector between the target medicine box and adjacent medicine boxes, and construct an orthogonal projection matrix based on the gap vector; S20, control the gripper to hold the target medicine box and control the robotic arm to move along the desired extraction trajectory, obtain the resultant force spin, and use the orthogonal projection matrix to decouple the resultant force spin into the dominant extraction force and the environmental interference shear force; S30, calculate the original slip vector between the target medicine box and the adjacent medicine box, calculate the self-motion optical flow field of the depth camera by combining the joint velocity of the robotic arm and the Jacobian matrix, and subtract the self-motion optical flow field from the original slip vector to obtain the physical slip vector; S40, when both the environmental interference shear force and the physical slip vector reach a set threshold, the physical slip vector is interpolated to generate smooth curve data, and the smooth curve data is mapped to the off-diagonal elements of the stiffness matrix of the impedance controller to control the robotic arm to generate attitude deflection. S50, obtain the observed value of the rate of change of the environmental interference shear force; when the observed value of the rate of change is less than zero, increase the damping matrix parameter of the impedance controller according to the observed value of the rate of change. S60, perform time integration on the environmental interference shear force, and superimpose the integration result as a position compensation amount into the desired extraction trajectory to generate an updated desired extraction trajectory.

[0007] Preferably, step S10 further includes: The local point cloud data is preprocessed and the top surface point cloud of the target medicine box is extracted to establish a local task coordinate system with a defined macroscopic extraction direction; The surface curvature change rate of the local normal vector of the point cloud is calculated using a three-dimensional edge gradient operator. When the change rate of the local normal vector is greater than the preset surface curvature threshold and forms a linear concave region, the gap feature is identified, and a normalized gap vector is generated along the direction perpendicular to the side wall of the target medicine box and pointing to the adjacent medicine box. Calculate the difference matrix between the outer product matrix of the third-order identity matrix and the gap vector, and use the difference matrix as an orthogonal projection matrix for filtering out non-interference mechanical components along the gap direction in Cartesian space.

[0008] Preferably, in step S20, the step of decoupling the resultant spinor into the dominant extraction force and the environmental interference shear force using the orthogonal projection matrix further includes: The resultant force spinor in Cartesian space is read from the output of the six-axis torque sensor. The resultant force spinor is extracted and gravity-compensated to obtain the translational force component. The translational force component is then uniformly transformed to the local task coordinate system. Multiply the translational force component after coordinate system transformation by the orthogonal projection matrix to extract the external force along the normal direction of the gap sidewall in three-dimensional space, and generate the environmental interference shear force that quantifies the degree of lateral physical jamming. The dominant extraction force is calculated by subtracting the environmental interference shear force from the original translational force component, and the calculated dominant extraction force is fed back to the Z-axis motion control loop at the bottom of the robotic arm to dynamically maintain the dominant extraction action.

[0009] Preferably, in step S30, the step of calculating the self-motion optical flow field of the depth camera by combining the joint velocity of the robotic arm and the Jacobian matrix, and subtracting the self-motion optical flow field from the original slip vector to obtain the physical slip vector, includes: Extract the joint velocities aligned with the timestamp of the depth camera exposure image, substitute the aligned joint velocities into the positive kinematics Jacobian matrix of the robotic arm, and calculate the camera space spinor of the camera coordinate system in Cartesian space at the current moment. The intrinsic parameter matrix of the depth camera and the pixel depth value information of the corresponding image region are obtained to construct the image Jacobian matrix. A linear mapping relationship between the three-dimensional spatial velocity and the two-dimensional pixel motion velocity is established through the image Jacobian matrix. The camera spatial screw is mapped to generate the theoretical pixel displacement caused by the camera pose change on the image plane, and the output is a self-moving optical flow field. Subtracting the self-moving optical flow field generated by the kinematic model from the original slip vector output by the dense optical flow algorithm strips the parallax coupling effect caused by the impedance controller adapting to the environment and fine-tuning the robot arm pose, and outputs the true physical slip vector.

[0010] Preferably, in step S40, the step of interpolating the physical slip vector to generate smooth curve data when both the environmental interference shear force and the physical slip vector reach a set threshold further includes: The system monitors the magnitude of the environmental interference shear force and the magnitude of the physical slip vector in real time. When the magnitude of the environmental interference shear force exceeds a preset mechanical threshold and the magnitude of the physical slip vector exceeds a preset slip tolerance threshold, the system is determined to be in a physical jammed state under multi-constraint compression and a dynamic impedance parameter reconstruction mechanism is triggered. After the reconstruction mechanism is triggered, the polynomial manifold interpolation function is called to generate a trajectory, starting from the stiffness state of the current control cycle and ending at the target stiffness value mapped by the physical slip vector. Within the set visual update cycle time span, the velocity and acceleration boundary conditions at the start and end points are both zero, and the low-frequency discrete physical slip vector sampled across the clock domain is converted into smooth curve data of second-order continuous differentiability that matches the high-frequency cycle of the underlying impedance controller.

[0011] Preferably, in step S40, mapping the smooth curve data to the off-diagonal elements of the stiffness matrix of the impedance controller to control the robotic arm to generate attitude deflection further includes: The generated smooth curve data is substituted into the stiffness matrix of the impedance controller as a dynamic variable. Based on the orthogonal projection geometric relationship between the pixel displacement direction of the image plane and the spatial attitude deflection, a cross coupling coefficient is constructed between the translation axis and the orthogonal rotation axis to generate an off-diagonal stiffness matrix. As the underlying impedance controller operates the off-diagonal stiffness matrix, the linear drag of the environmental interference shear force continuously experienced along the translation axis is transformed into a torque input parameter in the orthogonal rotation direction; The transformed torque input parameter drives the end of the robotic arm to generate a corresponding compliant deflection displacement, continuously changing the spatial normal constraint angle of the contact surface between the target medicine box and the adjacent medicine box to break the physical deadlock.

[0012] Preferably, in step S50, obtaining the observed value of the rate of change of the environmental interference shear force includes: A generalized momentum observer based on integral form is constructed, and a state-space equation is constructed using the generalized momentum observer to perform closed-loop tracking and derivative-free observation of the environmental interference shear force output by the force perception decoupling process; The calculation is performed based on the pre-set positive definite diagonal observer gain matrix in the state space equation and the deviation between the real environmental interferometric shear force vector and the environmental interferometric shear force state observation vector. The smooth and phase-delay-free rate of change of shear force is directly extracted from the state-space equation as the observed rate of change.

[0013] Preferably, in step S50, when the observed rate of change is less than zero, the damping matrix parameter of the impedance controller is increased according to the observed rate of change, including: The magnitude and sign of the observed rate of change in each coordinate axis direction are monitored in real time. When it is determined that any component of the observed rate of change is less than the negative unloading drop judgment threshold, it is confirmed that the current force shows a negative jump and the robotic arm enters the dynamic friction unloading state. After entering the dynamic friction unloading state, based on the basic diagonal damping matrix under the steady state of the system, combined with the nonlinear damping expansion gain diagonal matrix and the asymmetric activation diagonal matrix, asymmetric damping expansion modulation is performed to generate an updated new desired damping matrix. In the process of generating the new expected damping matrix, a minimum value screening function is introduced, so that the damping term of the corresponding degree of freedom increases only in the square order of the gradient when the observed rate of change shows a negative gradient, thereby absorbing the system's sudden kinetic energy caused by the release of off-diagonal stiffness and suppressing rebound oscillations.

[0014] Preferably, increasing the damping matrix parameters of the impedance controller based on the observed rate of change further includes: When implementing asymmetric damped expansion modulation, an asymmetric activation diagonal matrix is ​​constructed to adjust the damping sensitivity of each spatial dimension. The first three main diagonal elements of the corresponding translational degree of freedom in the asymmetric activation diagonal matrix are assigned values ​​based on the negative jump gradient presented by the observed rate of change, so as to absorb the sudden kinetic energy in the translational direction. The last three main diagonal elements of the corresponding rotational degree of freedom in the asymmetric activated diagonal matrix are configured to be proportionally mapped to the translational components that generate cross-coupling, and the rotational direction elements that do not generate cross-coupling are kept at zero to suppress the rotational bounce that accompanies the release of off-diagonal stiffness.

[0015] Preferably, step S60 further includes: Extract the decoupled environmental interference shear force of the robotic arm during the historical time period within the extraction cycle, introduce a dead zone nonlinear function with a set error tolerance interval to preprocess the environmental interference shear force, and filter out the background zero-point drift and high-frequency white noise signal through the error tolerance interval; The effective environmental interference shear force after preprocessing is integrated by combining the set weight matrix and a saturation limiting function is configured outside the integration operation to limit the maximum safe offset boundary under continuous pressure. The continuous cumulative offset including position and attitude compensation parameters is calculated. The calculated cumulative offset is used as the position compensation amount. The cumulative offset is subtracted from the initially planned absolute straight line expected extraction trajectory vector to generate an updated expected extraction trajectory with a position offset far from the historical interference surface normal.

[0016] This invention provides a robot drug retrieval control method that integrates visual positioning and force feedback. It has the following beneficial effects: 1. This invention extracts the true physical slip vector by calculating and eliminating the self-moving optical flow field of the depth camera. When the environmental interference shear force and the physical slip vector reach a threshold, the slip characteristics are mapped to the off-diagonal elements of the stiffness matrix of the impedance controller. This control mechanism can convert the linear drag in the interference direction into the attitude deflection torque in the orthogonal direction, actively changing the spatial normal constraint angle of the contact surface between the target pillbox and adjacent pillboxes. This breaks the rigid static friction jamming in a densely stacked environment and solves the technical problem that traditional diagonal stiffness matrix impedance control is difficult to autonomously escape when a single degree of freedom is blocked.

[0017] 2. This invention constructs a generalized momentum observer based on integral form to perform derivative-free observation of environmental interference shear force. When a negative jump in the rate of change of shear force is detected, asymmetric expansion is performed on the damping matrix parameters of the impedance controller based on the negative force gradient. This data processing method avoids the high-frequency noise amplification effect introduced by direct numerical difference. At the instant of dynamic friction unloading upon disengagement, the sudden elastic potential energy released is absorbed by the higher-order nonlinear damping term added to the system, suppressing the rebound oscillation of the robotic arm end effector from the underlying dynamic level and maintaining the stability of the transient physical process.

[0018] 3. In this invention, when integrating the environmental interference shear force during decoupling to generate the position compensation amount for the desired extraction trajectory, a dead-zone nonlinear function and a saturation limiting function are introduced. This signal processing logic filters out sensor background zero-point drift interference, prevents integral divergence and position overshooting under continuous compression, and converts transient contact force into a stable spatial cumulative offset. This guides the robotic arm to maintain a spatial offset away from the historical interference surface during the later stage of the extraction stroke, eliminating the potential for secondary physical interference caused by the robotic arm returning along the initial straight path. Attached Figure Description

[0019] Figure 1 This is a flowchart of the method steps of the present invention; Figure 2 This is a system architecture diagram of the present invention; Figure 3 This is a comparison diagram of the time-domain response of the environmental interference shear force of the present invention; Figure 4 This is a schematic diagram illustrating the damping expansion and oscillation suppression effect during unloading of the present invention.

[0020] Among them, 10 is the visual and kinematic perception module; 20 is the force decoupling module; 30 is the manifold interpolation module; 40 is the stiffness modulation module; 50 is the damping expansion module; and 60 is the trajectory drift module. Detailed Implementation

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

[0022] See attached document Figure 1 , Figure 1 This is a system architecture diagram according to an embodiment of the present invention. The present invention provides a robot drug dispensing control system integrating visual positioning and force feedback. This system is applied to a multi-joint robotic arm with an eye-in-hand configuration. The end flange of the robotic arm is sequentially connected to a six-axis torque sensor, a gripper, and a depth camera. The system includes: a vision and kinematics perception module 10, a force decoupling module 20, a manifold interpolation module 30, a stiffness modulation module 40, a damping expansion module 50, and a trajectory drift module 60.

[0023] The vision and kinematics perception module 10 is used to acquire point cloud data of the target medicine box. Based on the point cloud data, the vision and kinematics perception module 10 extracts the gap vector between the target medicine box and adjacent medicine boxes. The vision and kinematics perception module 10 is also used to combine the joint encoder data of the robotic arm to calculate and remove the visual optical flow component generated by the movement of the robotic arm, and output the physical slip vector.

[0024] The force sensing decoupling module 20 is used to acquire the resultant force spinor collected by the six-axis torque sensor. The force sensing decoupling module 20 constructs an orthogonal projection matrix using the gap vector, and decouples the resultant force spinor into clamping force and environmental interference shear force.

[0025] The manifold interpolation module 30 is used to receive the physical slip vector output by the vision and kinematics perception module 10 and convert the discrete physical slip vector into continuous smooth curve data.

[0026] The stiffness modulation module 40 receives smooth curve data output by the manifold interpolation module 30 when the environmental interference shear force reaches a set threshold. The stiffness modulation module 40 maps this smooth curve data to the off-diagonal cross-coupling term of the impedance controller to generate the attitude deflection command of the robotic arm.

[0027] The damping expansion module 50 is used to calculate the rate of change of the environmental interference shear force through the momentum observer. When the environmental interference shear force decreases, the damping expansion module 50 increases the damping matrix parameters of the impedance controller.

[0028] The trajectory drift module 60 is used to perform integral calculations on the environmental interference shear force. Based on the integral result, the trajectory drift module 60 adjusts the desired extraction trajectory of the robotic arm.

[0029] See attached document Figure 2 , Figure 2 This is a flowchart of method steps according to an embodiment of the present invention. The present invention provides a robot drug retrieval control method integrating visual positioning and force feedback, comprising the following steps: S10, the vision and kinematics perception module 10 acquires the point cloud data of the target medicine box, extracts the gap vector between the target medicine box and adjacent medicine boxes, and constructs an orthogonal projection matrix; S20 controls the gripper to hold the target medicine box and controls the robotic arm to move along the desired extraction trajectory. The force decoupling module 20 obtains the resultant force screw and uses the orthogonal projection matrix to decouple the resultant force screw into the gripping force and the environmental interference shear force. S30, the vision and kinematics perception module 10 calculates the original slip vector of the target medicine box and the adjacent medicine boxes, combines the joint velocity of the robotic arm and the Jacobian matrix to calculate the self-motion optical flow field of the depth camera, and subtracts the self-motion optical flow field from the original slip vector to obtain the physical slip vector. S40, when both the environmental interference shear force and the physical slip vector reach the set threshold, the manifold interpolation module 30 interpolates the physical slip vector to generate smooth curve data, and the stiffness modulation module 40 maps the smooth curve data to the off-diagonal elements of the stiffness matrix of the impedance controller to control the robotic arm to generate attitude deflection. S50, the damping expansion module 50 acquires the observed rate of change of the environmental interference shear force. When the observed rate of change is less than zero, the damping expansion module 50 increases the damping matrix parameter of the impedance controller according to the observed rate of change. S60, the trajectory drift module 60 performs time integration on the environmental interference shear force obtained by decoupling, and superimposes the integration result as a position compensation amount into the expected extraction trajectory of the robotic arm to generate an updated expected extraction trajectory.

[0030] In this embodiment, step S10 controls the system to move the multi-joint robotic arm to a position above the target pillbox, and acquires local point cloud data including the target pillbox and its surrounding adjacent pillboxes using a depth camera mounted at the end of the robotic arm. To improve the accuracy of subsequent feature extraction, the vision and kinematics perception module 10 preprocesses the local point cloud data. This preprocessing step may include point cloud filtering and voxel downsampling. For the filtering and downsampling of point cloud data, those skilled in the art can use existing statistical filters or voxel grid filters. The specific algorithm implementation is well-known in the art and will not be described in detail here.

[0031] After completing the point cloud preprocessing, the vision and kinematics perception module 10 uses a plane fitting method based on the random sampling consensus algorithm to segment the top surface point cloud of the target medicine box from the preprocessed point cloud data and calculate the three-dimensional bounding box of the top surface point cloud.

[0032] As a preferred approach, the vision and kinematics perception module 10 uses the geometric center of the three-dimensional bounding box as the origin of the coordinate system to establish a local task coordinate system. When establishing the local task coordinate system, the vision and kinematics perception module 10 defines the direction of the normal vector perpendicular to the top surface of the target medicine box and pointing outward as the Z-axis, and sets the positive direction of the Z-axis as the macroscopic extraction direction for the subsequent medicine-retrieving action of the robotic arm, thereby providing a unified spatial reference benchmark for subsequent force perception decoupling.

[0033] To address interference characteristics in densely stacked environments, the vision and kinematics perception module 10 further processes local point cloud data to extract physical gap features. Specifically, the vision and kinematics perception module 10 employs a three-dimensional edge gradient operator to traverse the point cloud data surrounding the target medicine box. This three-dimensional edge gradient operator achieves edge detection by calculating the rate of change of surface curvature of the local normal vectors of the point cloud. It should be noted that when the rate of change of the local normal vectors is detected to be greater than a preset surface curvature threshold, forming a linear concave region, the vision and kinematics perception module 10 identifies this region as a gap feature between the sidewall of the target medicine box and the sidewall of an adjacent medicine box.

[0034] After identifying the gap feature, the vision and kinematics perception module 10 extracts the extension direction of the gap feature and generates a three-dimensional direction vector along a direction perpendicular to the side wall of the target medicine box and pointing towards the adjacent medicine box. To facilitate subsequent matrix operations, the vision and kinematics perception module 10 normalizes this three-dimensional direction vector to generate a normalized gap vector. ,and From a physical perspective, the gap vector The physical meaning of is the spatial normal of the target medicine box that may be subjected to physical compression and friction interference from adjacent medicine boxes during the extraction process. This vector accurately represents the directional attribute of environmental constraints.

[0035] The visual and kinematic perception module 10 is based on the extracted gap vector. Constructing an orthogonal projection matrix through mathematical matrix transformation The orthogonal projection matrix is ​​used to filter out non-interference mechanical components along the gap direction in Cartesian space, allowing the system to retain only the actual compression and friction forces perpendicular to the gap sidewalls. In this embodiment, the vision and kinematics perception module 10 calculates a third-order identity matrix and a gap vector. The difference matrix between the outer product matrices is used as the orthogonal projection matrix. Orthogonal projection matrix The specific calculation formula is as follows: ; In the formula, The dimension is The orthogonal projection matrix; The dimension is The identity matrix; The dimension is The gap vector; Represents the gap vector The transpose of the vector; The outer product matrix representing the gap vector. Orthogonal projection matrix. This constitutes a spatial mechanical dimension reduction mapping relationship of the robotic arm's drug-retrieving operation environment, and its output data will be directly provided to the force perception decoupling module 20 as the calculation basis for separating environmental interference forces.

[0036] In this embodiment, step S20 controls the gripper to close to hold the target medicine box, and at the same time generates the desired extraction trajectory based on the Z-axis of the local task coordinate system determined in step S10, and drives the robotic arm to continuously perform the medicine retrieval action along the desired extraction trajectory.

[0037] During the extraction process by the robotic arm, the end effector is subjected to complex environmental contact forces due to the dense stacking of adjacent medicine boxes. To accurately sense these dynamic mechanical states, as a preferred approach, the force sensing decoupling module 20 reads data output from the six-axis torque sensor according to the real-time communication cycle of the underlying control system. Its reading frequency is typically set between 500Hz and 2000Hz to ensure the dynamic stability of the underlying impedance control loop. This sensor data is expressed as a resultant force spinor in Cartesian space, which includes translational force components along the three spatial coordinate axes and rotational torque components about these three coordinate axes.

[0038] Since the physical interference during the drug dispensing process mainly manifests as compression and friction in the translational direction, the force decoupling module 20 extracts the resultant force spindle, retaining only the first three-dimensional translational force components. Before performing subsequent calculations, the system needs to eliminate the fixed deviations caused by the gripper itself and the gravity of the medicine box. For gravity compensation and base coordinate system transformation of the raw data from the torque sensor, those skilled in the art can use existing dynamic identification and homogeneous transformation matrices. The specific algorithm implementation is a well-known technology in this field and will not be elaborated here. Among them, the translational force components after gravity compensation must be uniformly transformed to the aforementioned established local task coordinate system to ensure the mathematical consistency and algorithm effectiveness of the subsequent spatial projection mapping process.

[0039] After obtaining the compensated and coordinate system-1 translational force components, the system needs to solve the problem of accurately identifying the mixed mechanical signals. In actual operation, this translational force component is a mixture of the supporting force required by the gripper to keep the medicine box from falling and the squeezing friction force of adjacent medicine boxes. In order to prevent the bottom impedance controller from misinterpreting the lateral friction force as a positive rigid collision and thus generating an incorrect reverse avoidance command, the system must mathematically decouple and separate these mixed forces.

[0040] Force perception decoupling module 20 calls the orthogonal projection matrix generated in the preceding steps. A spatial transformation is performed on the aforementioned translational force components. Specifically, during the calculation, the force decoupling module 20 extracts and aligns the coordinate system of the translational force components, then multiplies them by the orthogonal projection matrix. This allows for the extraction of external forces along the normal direction of the gap sidewall in three-dimensional space, generating environmental interference shear force. The specific formula for calculating environmental interference shear force is as follows: ; In the formula, This represents an environmental interference shear force vector with dimensions of 3×1. This represents an orthogonal projection matrix with dimensions of 3×3; This represents a 3×1 dimension translational force component vector. Through this orthogonal mapping process, environmental interference shear force... The degree of lateral physical jamming experienced by the target medicine box when it was removed was accurately quantified.

[0041] After separating the environmental interference shear force, the force-sensing decoupling module 20 further calculates the dominant extraction force that sustains the macroscopic extraction operation. By subtracting the environmental interference shear force from the original translational force component, the force-sensing decoupling module 20 calculates the dominant extraction force. The specific formula for calculating the dominant extraction force is as follows: ; In the formula, This represents the dominant extraction force vector with dimension 3×1. The effective dominant extraction force is then calculated. Subsequently, the control system feeds back the data to the Z-axis motion control loop at the bottom layer of the robotic arm, which is used to dynamically maintain the robotic arm's main-direction pulling motion against the weight of the medicine box and static friction, as well as environmental interference shear forces. This serves as the core mechanical variable reflecting the hard interference state in external space, which is then input into the subsequent control module.

[0042] During the extraction action performed by the robotic arm in step S30, a depth camera mounted on the end effector of the robotic arm moves synchronously with the gripper. In this embodiment, the vision and kinematics perception module 10 continuously acquires a sequence of texture images of the target pillbox and the surfaces of adjacent pillboxes at a set acquisition frame rate. Based on the continuous image frame sequence, the vision and kinematics perception module 10 calls a dense optical flow algorithm to calculate the motion field on the pixel plane of the two-dimensional image. For the specific calculation process of the dense optical flow algorithm, those skilled in the art can use existing methods such as Farneback optical flow calculation, which are well-known technologies in the field and will not be described in detail here.

[0043] By comparing the pixel changes between the current frame and the previous frame, the visual and kinematic perception module 10 extracts the pixel relative original slip vector corresponding to the boundary area between the target medicine box and the adjacent medicine box, and sets the relative original slip vector as .

[0044] Because the system hardware architecture adopts an eye-to-hand configuration, when the underlying impedance controller performs end-effector pose fine-tuning to adapt to the contact environment, the spatial coordinates of the depth camera in 3D space will change accordingly. This global pixel displacement caused by the camera's own motion is captured by the dense optical flow algorithm, resulting in the visual component of the robot arm's self-motion being mixed into the calculated relative original slip vector. In order to accurately isolate this interference component, the vision and kinematics perception module 10 introduces the robot arm's underlying kinematic parameters to implement theoretical self-motion optical flow field inversion.

[0045] As a preferred approach, the control system needs to handle the data synchronization problem between different hardware components. The vision and kinematics perception module 10 extracts the aligned joint velocity vectors of the robotic arm based on the timestamp of the depth camera's exposure image. Substituting the aligned joint velocity vectors into the robotic arm's forward kinematics Jacobian matrix, the vision and kinematics perception module 10 calculates the spatial spin of the camera coordinate system in Cartesian space at the current moment. The specific formula for calculating the camera spatial spin is: ; In the formula, Represents the camera space spinor of dimension 6×1, which includes the linear velocity along the three coordinate axes and the angular velocity about the three coordinate axes; Indicates the current joint angle of the robotic arm. The 6×n-dimensional Jacobian matrix below; Indicates the dimension of the robotic arm is The multi-joint velocity vector, where This represents the number of degrees of freedom of the robotic arm.

[0046] After calculating the camera's physical motion state in three-dimensional space, the system maps this state to the two-dimensional image domain to achieve physical dimension unification with the optical flow vector. The vision and kinematics perception module 10 acquires the intrinsic parameter matrix of the depth camera and the pixel depth values ​​of the corresponding image region. Using the image Jacobian matrix, the vision and kinematics perception module 10 establishes a linear mapping relationship between three-dimensional spatial velocity and two-dimensional pixel motion velocity. The vision and kinematics perception module 10 then calculates the self-moving optical flow vector using this image Jacobian matrix. The specific calculation formula is as follows: ; In the formula, This represents a self-moving optical flow vector with a dimension of 2×1; The image Jacobian matrix represents a 2×6 dimension matrix, whose elements consist of the pixel coordinates (u, v) of the target region in the image plane and the corresponding spatial depth value. Together with intrinsic parameters such as the focal length of the depth camera, this is determined. This projection process calculates the theoretical pixel displacement on the image plane caused by changes in the camera's pose, under the assumption of an absolutely static external environment.

[0047] After obtaining the self-motion optical flow vector generated by the robotic arm's motion, the vision and kinematics perception module 10 performs a real physical slip vector extraction operation. The vision and kinematics perception module 10 subtracts the self-motion optical flow vector generated by the kinematic model mapping from the relative original slip vector output by the optical flow algorithm, thereby removing the parallax coupling effect caused by the robotic arm's pose change in the image. The specific formula for calculating the physical slip vector is as follows: ; In the formula, This represents the true physical slip vector after removing camera motion interference; Represents the relative original slip vector; This represents the self-moving optical flow vector. The physical slip vector output by the system is calculated through cross-modal fusion of kinematic data and visual features. This eliminates interference from the robotic arm's own posture adjustments. This data allows the control system to accurately determine whether adjacent medicine boxes are being dragged along due to frictional constraints, providing a robust basis for subsequent control modules to make state judgments.

[0048] Step S40: The motion control of the robotic arm's underlying layer is based on the Cartesian space impedance control law. In this embodiment, the dynamic equation of the impedance controller is specifically expressed as: ; In the formula, This represents the expected quality matrix with dimensions 6×6; This represents the desired damping matrix with dimensions 6×6; This represents the desired stiffness matrix with dimensions 6×6; This represents the six-degree-of-freedom pose deviation vector calculated by subtracting the actual end-effector pose from the desired reference pose. and Let represent the second and first derivatives of the pose deviation vector, respectively. This represents the external contact force experienced by the end effector of the robotic arm. During the normal no-load or straight-line withdrawal phase, the stiffness matrix... It is a diagonal matrix with non-zero elements on the main diagonal, and the degrees of freedom in each space are independent of each other.

[0049] To accurately identify the interference state of the medicine box during the extraction process, the system needs to jointly determine multimodal features, and the control unit needs to monitor the environmental interference shear force in real time. Magnitude and physical slip vector The modulus length. As a preferred approach, the control system internally presets a mechanical threshold characterizing the static friction limit and a slip tolerance threshold characterizing the physical drag of adjacent medicine boxes. The mechanical threshold can be obtained by offline calibration calculation of the surface friction coefficient of the medicine box packaging material, while the slip tolerance threshold is set based on the pixel resolution of the depth camera and the safety margin of the grasping operation. When the modulus length of the environmental interference shear force exceeds the preset mechanical threshold and the modulus length of the physical slip vector exceeds the preset slip tolerance threshold, the system determines that the robotic arm has fallen into a physical jammed state under multi-constraint compression, and then triggers the dynamic reconstruction mechanism of the impedance parameters.

[0050] Considering that the frequency of the physical slip vector output by the vision and kinematics perception module 10 is relatively low, while the operating cycle of the underlying impedance controller is in the high-frequency domain, directly writing the low-frequency discrete slip characteristics into the high-frequency updated stiffness matrix could easily cause a step change in the system stiffness parameters, resulting in high-frequency pulse excitation at the end of the robotic arm, and in severe cases, even triggering the overcurrent protection of the driver. To eliminate this numerical truncation effect caused by cross-clock domain sampling, the manifold interpolation module 30 performs differential smooth manifold interpolation processing on the physical slip vector.

[0051] In practice, the manifold interpolation module 30 calls a fifth-order polynomial manifold interpolation function. This interpolation function starts with the stiffness state of the current control cycle and ends with the target stiffness value mapped by the physical slip vector to generate the trajectory. Within the set visual update cycle time span, the manifold interpolation module 30 constrains the velocity and acceleration boundary conditions of the initial and final points to be zero, thereby constructing second-order continuously differentiable smooth curve data. For solving the polynomial boundary conditions and planning the trajectory smoothing, those skilled in the art can use existing time-optimal spline interpolation algorithms, the specific solution process of which is a well-known technology in the field and will not be elaborated here. The system uses this interpolation process to convert the low-frequency discrete physical slip vector into a smooth variable sequence that matches the high-frequency cycle.

[0052] After obtaining the smooth curve data, the stiffness modulation module 40 performs the core off-diagonal stiffness matrix adaptive mapping operation. The stiffness modulation module 40 uses the generated smooth curve data as a dynamic variable and substitutes it into the stiffness matrix of the impedance controller. In this process, cross-coupling coefficients are constructed between the translation Z-axis and the orthogonal rotation axes Rx and Ry to generate an off-diagonal stiffness matrix. The physical principle behind this cross-mapping is that, constrained by the camera projection geometry of the eye-hand configuration, a pose deflection Rx around the X-axis in space will produce a significant pixel displacement in the Y-direction of the image plane; similarly, a deflection Ry around the Y-axis will cause a displacement in the X-direction of the image. Based on this orthogonal correspondence, the specific formula for calculating the cross-coupling coefficients is as follows: ; ; In this formula, and They represent Stiffness matrix at any time Cross-coupling elements between the Z-axis and Rx-axis, and between the Z-axis and Ry-axis; Let represent the aforementioned fifth-order polynomial manifold interpolation function; and These represent the initial values ​​of the cross-coupled elements at the current time. and This represents the system's preset proportional gain scalar, whose specific value range is determined by the limit of the robotic arm's end-effector load capacity and the set joint compliance requirements. Represents the magnitude of the physical slip vector; and These represent the components of the physical slip vector on the X and Y axes of the image, respectively. This represents the sign function used to obtain the component direction.

[0053] As the underlying impedance controller receives and operates the off-diagonal stiffness matrix containing the aforementioned cross-coupling coefficients, when the robotic arm is continuously subjected to linear resistance from environmental interference shear forces along the Z-axis, the off-diagonal elements automatically convert this linear resistance into torque input parameters in the orthogonal rotation direction, driving the robotic arm's end effector to produce a corresponding compliant attitude deflection displacement. This adaptive attitude deflection, directly driven by visual slip, continuously alters the spatial normal constraint angle between the target pillbox and adjacent pillboxes. As the contact posture of the robotic arm's end effector changes, the relative static friction lock between the pillboxes is smoothly broken by the external physical force, allowing the robotic arm to release the mechanical lock and resume subsequent smooth extraction actions.

[0054] In step S50, based on the aforementioned off-diagonal stiffness matrix, the relative static friction lock between the medicine boxes is broken by external physical force, and the contact state between the end effector of the robotic arm and the environment will change from static friction to dynamic friction, resulting in transient dynamic friction unloading of the system. To capture this physical process, the damping expansion module 50 needs to obtain the rate of change of the environmental interference shear force in real time. In conventional robot control engineering, directly performing backward numerical difference on the discrete mechanical signal output by the torque sensor will amplify high-frequency electromagnetic noise and mechanical structure vibration noise. This amplified high-frequency noise can easily cause the control system to misjudge the unloading state.

[0055] In this embodiment, to eliminate the high-frequency noise amplification effect introduced by pure numerical difference, the damped expansion module 50 constructs a generalized momentum observer based on integral form. This generalized momentum observer performs closed-loop tracking and derivative-free observation of the environmental interference shear force by constructing a state-space equation, thereby outputting a smooth shear force change rate observation value. The core state update equation of the generalized momentum observer is as follows: ; ; In the formula, This represents a 3×1 dimension environmental interferometric shear force state observation vector; This represents the time derivative of the state observation vector; This represents the real-environment interference shear force vector input to the force perception decoupling module 20; This represents the observed rate of change of smooth shear force with dimension 3×1; The time derivative of the observed rate of change of smooth shear force; and These represent the gain matrices of the 3×3 positive definite diagonal observer. For the specific parameter tuning of the positive definite diagonal observer gain matrices, those skilled in the art can use the pole placement method to ensure that the observation error of the second-order system converges exponentially. The specific parameter calculation process is well-known in the art and will not be elaborated here. The system directly extracts smooth and phase-delay-free shear force change rate observations through this state observer. .

[0056] After obtaining the smooth shear force change rate observation, the damping expansion module 50 performs transient unloading drop judgment. The control system monitors the shear force change rate observation in real time. The magnitude and sign of the values ​​along each coordinate axis. As a preferred method, the system internally presets an unloading drop judgment threshold. This threshold is determined based on a combination of the maximum allowable acceleration jump of the grasped object and the inherent noise amplitude of the sensor itself, and is set to a positive number. When the observed rate of change of shear force is monitored... When any component is less than the negative unloading drop judgment threshold, the control system determines that the current environmental interference shear force shows a large negative jump, and the robotic arm officially enters the dynamic friction unloading state at the moment of jamming and disengagement in physical state.

[0057] After confirming that the system has entered the dynamic friction unloading state, the damping expansion module 50 implements asymmetric damping expansion modulation on the damping parameters of the impedance controller. From a physical dynamics perspective, when the stuck state is broken by attitude deflection, the system's elastic potential energy, originally stored in the off-diagonal stiffness matrix, is released instantaneously. If the basic constant damping parameters are maintained, this elastic potential energy will cause violent rebound oscillations at the end of the robotic arm and the gripped medicine box, increasing the risk of the target object falling. Therefore, the damping expansion module 50 dynamically increases the corresponding elements in the diagonal damping matrix of the impedance controller exponentially according to the magnitude of the negative gradient of the observed shear force change rate. The specific calculation formula for asymmetric damping expansion is as follows: ; In the formula, This represents the new expected damping matrix with dimensions of 6×6 after the update; This represents the basic 6×6 diagonal damping matrix of the system under steady-state conditions. This represents a 6×6 nonlinear damping expansion gain diagonal matrix, where the diagonal elements represent the system's sensitivity to damping adjustments in each degree of freedom. This represents a 6×6 asymmetric activation diagonal matrix. The first three main diagonal elements of this asymmetric activation diagonal matrix... The calculation logic is as follows: ; In the formula, The observed value of the rate of change of shear force The Each component. Simultaneously, to suppress the rotational bounce accompanying off-diagonal stiffness release, the last three main diagonal elements of this asymmetric activation diagonal matrix are... It is configured to be proportional to the translational component that produces the coupling association. For example, the damping expansion coefficient of rotation about the X-axis. Set as The proportional mapping value (in the Z-axis translation direction) is used, while the other rotational direction elements that do not produce cross-coupling remain zero. This is due to the use of a minimum value filtering function. This damping expansion mechanism is activated only when the shear force drops negatively, and remains zero when the force increases or remains constant, thus exhibiting a physical asymmetric regulation characteristic.

[0058] During the execution phase of the control cycle, the underlying impedance controller receives the updated desired damping matrix. The generated high-order nonlinear damping term increases dramatically on a quadratic basis with increasing unloading speed, effectively consuming the sudden kinetic energy generated by the release of elastic potential energy in the end effector. This nonlinear damping term significantly suppresses the rebound oscillation of the end effector, ensuring the dynamic stability of the cartridge removal process in complex constraint environments.

[0059] In step S60, under the conventional impedance control framework, when the external interference force disappears, the robotic arm will spontaneously tend towards and return to the initially planned desired extraction trajectory under the action of the rigid restoring force. In a densely stacked drug retrieval scenario, this return action poses a risk of secondary interference, that is, when the robotic arm continues to extract along the original initial path, it is easy to touch the obstacle surface on the same side again. To eliminate this engineering hazard, the trajectory drift module 60 intervenes in the control process, and performs dynamic spatial compensation for the system's desired reference trajectory through a time integration mechanism.

[0060] In this embodiment, the trajectory drift module 60 extracts the decoupled environmental interference shear force of the robotic arm in real time over the entire extraction cycle. Considering that the sensor background zero-point drift and high-frequency white noise in the industrial field will cause the system to accumulate drift error under long-term integration calculation, resulting in the robotic arm deviating from the safe working space, as a preferred method, the trajectory drift module 60 introduces a dead-zone nonlinear function to preprocess the environmental interference shear force signal before performing time integration calculation.

[0061] Specifically, the dead zone nonlinear function An error tolerance range was set. When the interference force component falls within this interval, the function output is zero; when the interference force component exceeds this interval, the function outputs the actual interference force and the set threshold. The difference. This setting is specifically designed to filter out minute noise signals whose amplitude is within the set dead zone threshold range, ensuring that the system retains only the effective interference force generated by real physical contact and compression, thereby avoiding the integral divergence problem of the algorithm under no-load conditions.

[0062] For the pre-processed effective environmental interference shear force, the trajectory drift module 60 performs time integration calculations based on the set weight matrix to calculate the continuous cumulative offset. To prevent the integral term from increasing infinitely under continuous pressure, causing the robotic arm to deviate from the physical safety boundary, this embodiment configures a saturation limiting function outside the integration calculation to constrain the position offset command. The specific formula for calculating the cumulative offset is as follows: ; In this formula, express The cumulative offset vector calculated at each time step contains position and attitude compensation parameters in Cartesian space; Represents the saturation limiting function; This represents the maximum safety offset vector set by the system. Its upper threshold is determined based on the minimum physical gap between adjacent medicine boxes and the external dimensions of the grippers. This represents the positive definite diagonal weight matrix set by the system, used to adjust the sensitivity of the robotic arm to trajectory drift response to historical contact forces. The range of values ​​for its matrix parameters is determined by the physical boundary constraints of the drug retrieval space. Indicates in The environmental interference shear force vector acquired at any time; Represents a dead-zone nonlinear function; Indicates the time from the start of the withdrawal action to the present. The integral time variable at time step.

[0063] After obtaining the cumulative offset in the time dimension, the trajectory drift module 60 performs the desired baseline trajectory drift compensation operation. The trajectory drift module 60 uses this cumulative offset as a position compensation amount and directly superimposes it onto the desired extraction trajectory at the bottom layer of the robotic arm. Specifically, the trajectory drift module 60 subtracts this cumulative offset vector from the initially planned straight-line desired extraction trajectory vector to generate the updated desired extraction trajectory. The specific formula for calculating the updated desired extraction trajectory is as follows: ; In the formula, express The expected extracted trajectory vector updated at each time step; This represents the expected trajectory vector of the absolute straight line planned during the task initialization phase.

[0064] As the reference of the underlying impedance controller is modified in real time by this equation, the robotic arm's macroscopic extraction path generates a position offset away from the normal of the historical interference surface in the three-dimensional work space. This position offset guides the robotic arm to avoid physically stuck areas during subsequent extraction strokes. Through this trajectory adaptive adjustment mechanism based on multimodal force perception history, the system overcomes the shortcomings of conventional force perception impedance control in position maintenance, ensuring that the target pillbox can smoothly and safely detach completely from the densely stacked constraint space.

[0065] To better understand the technical solution of this invention, the following description is based on a specific application scenario.

[0066] In this embodiment, a robotic medication dispensing control system integrating visual positioning and force feedback is deployed in an automated, high-density pharmacy in a hospital outpatient department. The medications are densely stacked rectangular cardboard boxes. The robotic arm is a six-degree-of-freedom collaborative arm, with a parallel electric gripper, a six-axis torque sensor, and a global shutter depth camera at its end effector. The underlying impedance control algorithm runs on a real-time industrial computer with a sampling rate of 1000Hz, while the visual optical flow algorithm runs on an independent image processing unit with a sampling rate of 30Hz.

[0067] In a typical medication retrieval operation, the target medication box, due to compression during manual refilling, forms a tight physical bond with the adjacent medication box on its right. The control system drives the gripper to hold the target medication box and perform a detachment action along the Z-axis at a set speed of 0.05 m / s.

[0068] In the initial stage of extraction, strong static friction interference occurs between the sidewall of the target medicine box and the sidewall of the adjacent medicine box. The force perception decoupling module 20 reads sensor data at high frequency and, after decoupling through an orthogonal projection matrix, calculates that the environmental interference shear force along the right normal direction continuously increases. At the same time, the vision and kinematic perception module 10 eliminates the visual parallax of the robotic arm's movement along the Z-axis through the Jacobian matrix, accurately extracting the physical slip vector of the adjacent medicine box in the X-axis direction of the image plane, indicating that the adjacent medicine box is being dragged along by the target medicine box.

[0069] When the environmental interference shear force exceeds the set 15N static friction threshold and the physical slip vector exceeds the slip tolerance of 5 pixels, the system determines that physical jamming has occurred. The manifold interpolation module 30 immediately converts the physical slip vector into a smooth curve, and the stiffness modulation module 40 accordingly smoothly increases the cross-coupling stiffness term of the impedance controller from 0 to the set value. Under the action of this off-diagonal stiffness matrix, the robotic arm, while maintaining the Z-axis pull-out force, spontaneously generates a small compliant deflection of about 3 degrees around the Y-axis. This attitude deflection changes the parallel contact surface between the pillboxes, transforming it into line contact, thereby physically disrupting the static friction lock state.

[0070] At the instant static friction is broken, the environmental interference shear force drops rapidly. The momentum observer keenly detects this negative mechanical jump, for example, the rate of change reaches -50 N / s. The damping expansion module 50 is then triggered, exponentially increasing the damping parameters of the corresponding rotational and translational degrees of freedom. This instantly absorbs the elastic potential energy during the robot arm's posture return to center, allowing the medicine box to smoothly disengage from the jamming point without any high-frequency vibration.

[0071] During the subsequent extraction process, the trajectory drift module 60 integrates the accumulated environmental interference shear force over time and, after saturation limiting, generates a positional offset of approximately 4 mm in the X-axis direction of the desired extraction trajectory. The robotic arm continues extraction with this spatial offset, completely avoiding the physical interference area on the right and safely completing the drug retrieval operation.

[0072] This experiment was designed for comparative verification on the same hardware platform and in a densely stacked environment.

[0073] Traditional method group: adopts the standard diagonal matrix impedance control method, each spatial degree of freedom responds independently, does not introduce visual slip monitoring and cross stiffness mapping, and keeps the damping parameters constant.

[0074] The method group of this invention adopts the complete control strategy proposed in this invention, which integrates visual optical flow denoising, off-diagonal stiffness adaptive modulation, asymmetric damped expansion, and integral trajectory drift.

[0075] The target pillbox was subjected to 50 consecutive extraction tests under the same initial compression conditions. The peak value of the maximum environmental interference shear force, the peak value of the end acceleration oscillation at the moment of separation, and the rate of adjacent pillboxes falling off were recorded during the test.

[0076] See attached Figure 3 and attached Figure 4 In traditional methods, because the robotic arm can only perform one-dimensional compliant retreat along a straight line, it is difficult to change the physical constraint state of the contact surface. The environmental interference shear force increases linearly and sharply in the early stage of extraction, with an average maximum peak value reaching 32.4 N, which can easily cause deformation or tearing of the medicine box. In the method of this invention, when the shear force reaches a set monitoring threshold, the off-diagonal stiffness matrix drives the robotic arm to generate an orthogonal orientation twist, effectively unloading the lateral normal pressure. Experimental data shows that the average maximum environmental interference shear force peak value of the method of this invention is limited to 16.8 N, a significant reduction compared to traditional methods, effectively protecting the integrity of the medicine packaging.

[0077] To assess system stability at the moment of disengagement from jamming, time-domain acceleration data of the end effector along the Z-axis was extracted and compared. In the traditional method group, the accumulated elastic potential energy is released instantaneously at the moment of disengagement, resulting in a peak-to-peak acceleration of up to 4.5 m / s². 2The high-frequency decaying oscillation lasts for approximately 0.6 seconds. The method of this invention benefits from an asymmetric damped expansion mechanism; when the momentum observer detects a force-induced drop, the damping matrix parameters increase instantaneously. Experimental curves show that the peak value of the terminal acceleration jump is strictly suppressed to 1.2 m / s². 2 Within 0.1 seconds, it returns to a stable state, eliminating the rebound oscillation phenomenon of the end effector.

[0078] In 50 intensive medication retrieval tests, the traditional method group, due to its difficulty in effectively resolving static friction deadlock and the lack of a trajectory offset mechanism to prevent secondary jamming, resulted in adjacent medicine boxes being dragged off the shelf 14 times. The method group of this invention, through precise intervention of visual sliding features and dynamic spatial compensation of the trajectory drift module 60, successfully retrieved the target medicine box in all 50 tests, with zero instances of adjacent medicine boxes falling off, achieving a 100% success rate.

Claims

1. A robot drug-dispensing control method integrating visual positioning and force feedback, characterized in that, Includes the following steps: S10, acquire point cloud data of the target medicine box, extract the gap vector between the target medicine box and adjacent medicine boxes, and construct an orthogonal projection matrix based on the gap vector; S20, control the gripper to hold the target medicine box and control the robotic arm to move along the desired extraction trajectory, obtain the resultant force spin, and use the orthogonal projection matrix to decouple the resultant force spin into the dominant extraction force and the environmental interference shear force; S30, calculate the original slip vector between the target medicine box and the adjacent medicine box, calculate the self-motion optical flow field of the depth camera by combining the joint velocity of the robotic arm and the Jacobian matrix, and subtract the self-motion optical flow field from the original slip vector to obtain the physical slip vector; S40, when both the environmental interference shear force and the physical slip vector reach a set threshold, the physical slip vector is interpolated to generate smooth curve data, and the smooth curve data is mapped to the off-diagonal elements of the stiffness matrix of the impedance controller to control the robotic arm to generate attitude deflection. S50, obtain the observed rate of change of the environmental interference shear force; when the observed rate of change is less than zero, increase the damping matrix parameter of the impedance controller according to the observed rate of change. S60, perform time integration on the environmental interference shear force, and superimpose the integration result as a position compensation amount into the desired extraction trajectory to generate an updated desired extraction trajectory.

2. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, Step S10 further includes: The local point cloud data is preprocessed and the top surface point cloud of the target medicine box is extracted to establish a local task coordinate system with a defined macroscopic extraction direction; The surface curvature change rate of the local normal vector of the point cloud is calculated using a three-dimensional edge gradient operator. When the change rate of the local normal vector is greater than the preset surface curvature threshold and forms a linear concave region, the gap feature is identified, and a normalized gap vector is generated along the direction perpendicular to the side wall of the target medicine box and pointing to the adjacent medicine box. Calculate the difference matrix between the outer product matrix of the third-order identity matrix and the gap vector, and use the difference matrix as an orthogonal projection matrix for filtering out non-interference mechanical components along the gap direction in Cartesian space.

3. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S20, the step of decoupling the resultant spinor into the dominant extraction force and the environmental interference shear force using the orthogonal projection matrix further includes: The resultant force spinor in Cartesian space is read from the output of the six-axis torque sensor. The resultant force spinor is extracted and gravity-compensated to obtain the translational force component. The translational force component is then uniformly transformed to the local task coordinate system. Multiply the translational force component after coordinate system transformation by the orthogonal projection matrix to extract the external force along the normal direction of the gap sidewall in three-dimensional space, and generate the environmental interference shear force that quantifies the degree of lateral physical jamming. The dominant extraction force is calculated by subtracting the environmental interference shear force from the original translational force component, and the calculated dominant extraction force is fed back to the Z-axis motion control loop at the bottom of the robotic arm to dynamically maintain the dominant extraction action.

4. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S30, the step of calculating the self-motion optical flow field of the depth camera by combining the joint velocity of the robotic arm and the Jacobian matrix, and subtracting the self-motion optical flow field from the original slip vector to obtain the physical slip vector, includes: Extract the joint velocities aligned with the timestamp of the depth camera exposure image, substitute the aligned joint velocities into the positive kinematics Jacobian matrix of the robotic arm, and calculate the camera space spinor of the camera coordinate system in Cartesian space at the current moment. The intrinsic parameter matrix of the depth camera and the pixel depth value information of the corresponding image region are obtained to construct the image Jacobian matrix. A linear mapping relationship between the three-dimensional spatial velocity and the two-dimensional pixel motion velocity is established through the image Jacobian matrix. The camera spatial screw is mapped to generate the theoretical pixel displacement caused by the camera pose change on the image plane, and the output is a self-moving optical flow field. Subtracting the self-moving optical flow field generated by the kinematic model from the original slip vector output by the dense optical flow algorithm strips the parallax coupling effect caused by the impedance controller adapting to the environment and fine-tuning the robot arm pose, and outputs the true physical slip vector.

5. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S40, the step of interpolating the physical slip vector to generate smooth curve data when both the environmental interference shear force and the physical slip vector reach a set threshold further includes: The system monitors the magnitude of the environmental interference shear force and the magnitude of the physical slip vector in real time. When the magnitude of the environmental interference shear force exceeds a preset mechanical threshold and the magnitude of the physical slip vector exceeds a preset slip tolerance threshold, the system is determined to be in a physical jammed state under multi-constraint compression and a dynamic impedance parameter reconstruction mechanism is triggered. After the reconstruction mechanism is triggered, the polynomial manifold interpolation function is called to generate a trajectory, starting from the stiffness state of the current control cycle and ending at the target stiffness value mapped by the physical slip vector. Within the set visual update cycle time span, the velocity and acceleration boundary conditions at the start and end points are both zero, and the low-frequency discrete physical slip vector sampled across the clock domain is converted into smooth curve data of second-order continuous differentiability that matches the high-frequency cycle of the underlying impedance controller.

6. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S40, mapping the smoothed curve data to the off-diagonal elements of the stiffness matrix of the impedance controller to control the robotic arm to generate attitude deflection further includes: The generated smooth curve data is substituted into the stiffness matrix of the impedance controller as a dynamic variable. Based on the orthogonal projection geometric relationship between the pixel displacement direction of the image plane and the spatial attitude deflection, a cross coupling coefficient is constructed between the translation axis and the orthogonal rotation axis to generate an off-diagonal stiffness matrix. As the underlying impedance controller operates the off-diagonal stiffness matrix, the linear drag of the environmental interference shear force continuously experienced along the translation axis is transformed into a torque input parameter in the orthogonal rotation direction; The transformed torque input parameter drives the end of the robotic arm to generate a corresponding compliant deflection displacement, continuously changing the spatial normal constraint angle of the contact surface between the target medicine box and the adjacent medicine box to break the physical deadlock.

7. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S50, obtaining the observed rate of change of the environmental interference shear force includes: A generalized momentum observer based on integral form is constructed, and a state-space equation is constructed using the generalized momentum observer to perform closed-loop tracking and derivative-free observation of the environmental interference shear force output by the force perception decoupling process; The calculation is performed based on the pre-set positive definite diagonal observer gain matrix in the state space equation and the deviation between the real environmental interferometric shear force vector and the environmental interferometric shear force state observation vector. The smooth and phase-delay-free rate of change of shear force is directly extracted from the state-space equation as the observed rate of change.

8. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, In step S50, when the observed rate of change is less than zero, the damping matrix parameter of the impedance controller is increased according to the observed rate of change, including: The magnitude and sign of the observed rate of change in each coordinate axis direction are monitored in real time. When it is determined that any component of the observed rate of change is less than the negative unloading drop judgment threshold, it is confirmed that the current force shows a negative jump and the robotic arm enters the dynamic friction unloading state. After entering the dynamic friction unloading state, based on the basic diagonal damping matrix under the steady state of the system, combined with the nonlinear damping expansion gain diagonal matrix and the asymmetric activation diagonal matrix, asymmetric damping expansion modulation is performed to generate an updated new desired damping matrix. In the process of generating the new expected damping matrix, a minimum value screening function is introduced, so that the damping term of the corresponding degree of freedom increases only in the square order of the gradient when the observed rate of change shows a negative gradient, thereby absorbing the system's sudden kinetic energy caused by the release of off-diagonal stiffness and suppressing rebound oscillations.

9. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 8, characterized in that, The step of increasing the damping matrix parameters of the impedance controller based on the observed rate of change further includes: When implementing asymmetric damped expansion modulation, an asymmetric activation diagonal matrix is ​​constructed to adjust the damping sensitivity of each spatial dimension. The first three main diagonal elements of the corresponding translational degree of freedom in the asymmetric activation diagonal matrix are assigned values ​​based on the negative jump gradient presented by the observed rate of change, so as to absorb the sudden kinetic energy in the translational direction. The last three main diagonal elements of the corresponding rotational degree of freedom in the asymmetric activated diagonal matrix are configured to be proportionally mapped to the translational components that generate cross-coupling, and the rotational direction elements that do not generate cross-coupling are kept at zero to suppress the rotational bounce that accompanies the release of off-diagonal stiffness.

10. The robot drug-retrieving control method integrating visual positioning and force feedback according to claim 1, characterized in that, The step S60 further includes: Extract the decoupled environmental interference shear force of the robotic arm during the historical time period within the extraction cycle, introduce a dead zone nonlinear function with a set error tolerance interval to preprocess the environmental interference shear force, and filter out the background zero-point drift and high-frequency white noise signal through the error tolerance interval; The effective environmental interference shear force after preprocessing is integrated by combining the set weight matrix and a saturation limiting function is configured outside the integration operation to limit the maximum safe offset boundary under continuous pressure. The continuous cumulative offset including position and attitude compensation parameters is calculated. The calculated cumulative offset is used as the position compensation amount. The cumulative offset is subtracted from the initially planned absolute straight line expected extraction trajectory vector to generate an updated expected extraction trajectory with a position offset far from the historical interference surface normal.

Images